diff --git a/.clang-format b/.clang-format index 338ce6b7f392..46923aae03d2 100644 --- a/.clang-format +++ b/.clang-format @@ -1,6 +1,6 @@ # Commented out parameters are those with the same value as base LLVM style. # We can uncomment them if we want to change their value, or enforce the -# chosen value in case the base style changes (last sync: Clang 18.1.8). +# chosen value in case the base style changes (last sync: Clang 17.0.6). BasedOnStyle: LLVM AccessModifierOffset: -4 AlignAfterOpenBracket: DontAlign @@ -10,28 +10,24 @@ AlignAfterOpenBracket: DontAlign # AcrossEmptyLines: false # AcrossComments: false # AlignCompound: false -# AlignFunctionPointers: false # PadOperators: true # AlignConsecutiveBitFields: # Enabled: false # AcrossEmptyLines: false # AcrossComments: false # AlignCompound: false -# AlignFunctionPointers: false # PadOperators: false # AlignConsecutiveDeclarations: # Enabled: false # AcrossEmptyLines: false # AcrossComments: false # AlignCompound: false -# AlignFunctionPointers: false # PadOperators: false # AlignConsecutiveMacros: # Enabled: false # AcrossEmptyLines: false # AcrossComments: false # AlignCompound: false -# AlignFunctionPointers: false # PadOperators: false # AlignConsecutiveShortCaseStatements: # Enabled: false @@ -45,15 +41,14 @@ AlignTrailingComments: OverEmptyLines: 0 # AllowAllArgumentsOnNextLine: true AllowAllParametersOfDeclarationOnNextLine: false -# AllowBreakBeforeNoexceptSpecifier: Never # AllowShortBlocksOnASingleLine: Never # AllowShortCaseLabelsOnASingleLine: false -# AllowShortCompoundRequirementOnASingleLine: true # AllowShortEnumsOnASingleLine: true # AllowShortFunctionsOnASingleLine: All # AllowShortIfStatementsOnASingleLine: Never # AllowShortLambdasOnASingleLine: All # AllowShortLoopsOnASingleLine: false +# AlwaysBreakAfterDefinitionReturnType: None # AlwaysBreakAfterReturnType: None # AlwaysBreakBeforeMultilineStrings: false # AlwaysBreakTemplateDeclarations: MultiLine @@ -81,8 +76,7 @@ AllowAllParametersOfDeclarationOnNextLine: false # SplitEmptyFunction: true # SplitEmptyRecord: true # SplitEmptyNamespace: true -# BreakAdjacentStringLiterals: true -# BreakAfterAttributes: Leave +# BreakAfterAttributes: Never # BreakAfterJavaFieldAnnotations: false # BreakArrays: true # BreakBeforeBinaryOperators: None @@ -171,7 +165,6 @@ PackConstructorInitializers: NextLine # PenaltyBreakComment: 300 # PenaltyBreakFirstLessLess: 120 # PenaltyBreakOpenParenthesis: 0 -# PenaltyBreakScopeResolution: 500 # PenaltyBreakString: 1000 # PenaltyBreakTemplateDeclaration: 10 # PenaltyExcessCharacter: 1000000 @@ -188,7 +181,6 @@ RemoveSemicolon: true # RequiresExpressionIndentation: OuterScope # SeparateDefinitionBlocks: Leave # ShortNamespaceLines: 1 -# SkipMacroDefinitionBody: false # SortIncludes: CaseSensitive # SortJavaStaticImport: Before # SortUsingDeclarations: LexicographicNumeric @@ -202,6 +194,7 @@ RemoveSemicolon: true # SpaceBeforeCtorInitializerColon: true # SpaceBeforeInheritanceColon: true # SpaceBeforeJsonColon: false +# SpaceBeforeParens: ControlStatements # SpaceBeforeParensOptions: # AfterControlStatements: true # AfterForeachMacros: true @@ -209,7 +202,6 @@ RemoveSemicolon: true # AfterFunctionDefinitionName: false # AfterIfMacros: true # AfterOverloadedOperator: false -# AfterPlacementOperator: true # AfterRequiresInClause: false # AfterRequiresInExpression: false # BeforeNonEmptyParentheses: false @@ -219,10 +211,8 @@ RemoveSemicolon: true # SpacesBeforeTrailingComments: 1 # SpacesInAngles: Never # SpacesInContainerLiterals: true -## Godot TODO: We'll want to use a min of 1, but we need to see how to fix -## our comment capitalization at the same time. SpacesInLineCommentPrefix: - Minimum: 0 + Minimum: 0 # We want a minimum of 1 for comments, but allow 0 for disabled code. Maximum: -1 # SpacesInParens: Never # SpacesInParensOptions: diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 6c8a8ef91981..9de8b0153038 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,10 +2,6 @@ # Each line is a file pattern followed by one or more owners. # Owners can be @users, @org/teams or emails. -# Buildsystem (Before everything to be overwritten) - -* @godotengine/buildsystem - # Core /core/ @godotengine/core @@ -20,8 +16,6 @@ # Drivers -/drivers/ @godotengine/_systems - ## Audio /drivers/alsa/ @godotengine/audio /drivers/alsamidi/ @godotengine/audio @@ -42,7 +36,7 @@ /drivers/vulkan/ @godotengine/rendering ## OS -/drivers/unix/ @godotengine/_platforms +/drivers/unix/ @godotengine/linux-bsd /drivers/windows/ @godotengine/windows ## Misc @@ -50,9 +44,9 @@ # Editor -/editor/ @godotengine/_editor /editor/**/*2d* @godotengine/2d-editor /editor/**/*3d* @godotengine/3d-editor +/editor/**/*audio* @godotengine/audio /editor/**/*code* @godotengine/script-editor /editor/**/*debugger* @godotengine/debugger /editor/**/*dock* @godotengine/docks @@ -75,11 +69,6 @@ # Modules -/modules/ @godotengine/_engine -/modules/**/doc_classes/ @godotengine/_engine @godotengine/documentation -/modules/**/icons/ @godotengine/_engine @godotengine/usability -/modules/**/tests/ @godotengine/_engine @godotengine/tests - ## Audio (+ video) /modules/interactive_music/ @godotengine/audio /modules/interactive_music/doc_classes/ @godotengine/audio @godotengine/documentation @@ -95,6 +84,7 @@ ## Import /modules/astcenc/ @godotengine/import /modules/basis_universal/ @godotengine/import +/modules/bcdec/ @godotengine/import /modules/betsy/ @godotengine/import /modules/bmp/ @godotengine/import /modules/cvtt/ @godotengine/import @@ -121,6 +111,7 @@ /modules/mbedtls/tests/ @godotengine/network @godotengine/tests /modules/multiplayer/ @godotengine/network /modules/multiplayer/doc_classes/ @godotengine/network @godotengine/documentation +/modules/multiplayer/tests/ @godotengine/network @godotengine/tests /modules/upnp/ @godotengine/network /modules/upnp/doc_classes/ @godotengine/network @godotengine/documentation /modules/webrtc/ @godotengine/network @@ -131,6 +122,7 @@ ## Physics /modules/godot_physics_2d/ @godotengine/physics /modules/godot_physics_3d/ @godotengine/physics +/modules/jolt_physics/ @godotengine/physics ## Rendering /modules/glslang/ @godotengine/rendering @@ -189,7 +181,6 @@ # Platform -/platform/ @godotengine/_platforms /platform/android/ @godotengine/android /platform/android/doc_classes/ @godotengine/android @godotengine/documentation /platform/ios/ @godotengine/ios @@ -205,7 +196,6 @@ # Scene -/scene/ @godotengine/_systems @godotengine/core /scene/2d/ @godotengine/2d-nodes /scene/2d/physics/ @godotengine/2d-nodes @godotengine/physics /scene/3d/ @godotengine/3d-nodes @@ -215,20 +205,23 @@ /scene/debugger/ @godotengine/debugger /scene/gui/ @godotengine/gui-nodes /scene/main/ @godotengine/core -/scene/resources/font.* @godotengine/gui-nodes -/scene/resources/text_line.* @godotengine/gui-nodes -/scene/resources/text_paragraph.* @godotengine/gui-nodes -/scene/resources/visual_shader*.* @godotengine/shaders +/scene/resources/2d/ @godotengine/2d-nodes +/scene/resources/3d/ @godotengine/3d-nodes +/scene/resources/animated* @godotengine/animation +/scene/resources/animation* @godotengine/animation +/scene/resources/audio* @godotengine/audio +/scene/resources/font* @godotengine/gui-nodes +/scene/resources/shader* @godotengine/shaders +/scene/resources/text_* @godotengine/gui-nodes +/scene/resources/visual_shader* @godotengine/shaders /scene/theme/ @godotengine/gui-nodes /scene/theme/icons/ @godotengine/gui-nodes @godotengine/usability # Servers -/servers/ @godotengine/_systems /servers/**/audio_* @godotengine/audio /servers/**/camera_* @godotengine/xr /servers/**/debugger_* @godotengine/debugger -/servers/**/display_* @godotengine/_platforms /servers/**/navigation_* @godotengine/navigation /servers/**/physics_* @godotengine/physics /servers/**/rendering_* @godotengine/rendering @@ -237,7 +230,6 @@ /servers/audio/ @godotengine/audio /servers/camera/ @godotengine/xr /servers/debugger/ @godotengine/debugger -/servers/display/ @godotengine/_platforms /servers/navigation/ @godotengine/navigation /servers/rendering/ @godotengine/rendering /servers/text/ @godotengine/gui-nodes @@ -253,6 +245,7 @@ # Buildsystem (After everything to catch all) +/*.* @godotengine/buildsystem *.py @godotengine/buildsystem SConstruct @godotengine/buildsystem SCsub @godotengine/buildsystem diff --git a/.github/actions/godot-deps/action.yml b/.github/actions/godot-deps/action.yml index bd9a1f55ed35..d9b95e844841 100644 --- a/.github/actions/godot-deps/action.yml +++ b/.github/actions/godot-deps/action.yml @@ -10,7 +10,7 @@ inputs: default: x64 scons-version: description: The SCons version to use. - default: 4.8.0 + default: 4.8.1 runs: using: composite @@ -29,3 +29,7 @@ runs: python -c "import sys; print(sys.version)" python -m pip install scons==${{ inputs.scons-version }} scons --version + + - name: Setup problem matchers + shell: bash + run: echo ::add-matcher::misc/utility/problem-matchers.json diff --git a/.github/workflows/android_builds.yml b/.github/workflows/android_builds.yml index 950e1e51cc4e..2803ff4b5e88 100644 --- a/.github/workflows/android_builds.yml +++ b/.github/workflows/android_builds.yml @@ -25,18 +25,21 @@ jobs: target: editor tests: false sconsflags: arch=arm64 production=yes swappy=yes + cache-limit: 1 - name: Template arm32 (target=template_release, arch=arm32) cache-name: android-template-arm32 target: template_release tests: false sconsflags: arch=arm32 swappy=yes + cache-limit: 1 - name: Template arm64 (target=template_release, arch=arm64) cache-name: android-template-arm64 target: template_release tests: false sconsflags: arch=arm64 swappy=yes + cache-limit: 1 steps: - name: Checkout @@ -77,6 +80,7 @@ jobs: platform: android target: ${{ matrix.target }} tests: ${{ matrix.tests }} + scons-cache-limit: ${{ matrix.cache-limit }} - name: Save Godot build cache uses: ./.github/actions/godot-cache-save diff --git a/.github/workflows/godot_cpp_test.yml b/.github/workflows/godot_cpp_test.yml index af99a4b03576..ae2fc9388c37 100644 --- a/.github/workflows/godot_cpp_test.yml +++ b/.github/workflows/godot_cpp_test.yml @@ -21,7 +21,9 @@ jobs: - name: Checkout uses: actions/checkout@v4 with: - sparse-checkout: .github + sparse-checkout: | + .github + misc/utility/problem-matchers.json - name: Checkout godot-cpp uses: actions/checkout@v4 @@ -34,9 +36,6 @@ jobs: - name: Setup Python and SCons uses: ./.github/actions/godot-deps - - name: Setup GCC problem matcher - uses: ammaraskar/gcc-problem-matcher@master - - name: Download GDExtension interface and API dump uses: ./.github/actions/download-artifact with: diff --git a/.github/workflows/ios_builds.yml b/.github/workflows/ios_builds.yml index 8513429f9a63..270204ebe507 100644 --- a/.github/workflows/ios_builds.yml +++ b/.github/workflows/ios_builds.yml @@ -37,6 +37,7 @@ jobs: platform: ios target: template_release tests: false + scons-cache-limit: 1 - name: Save Godot build cache uses: ./.github/actions/godot-cache-save diff --git a/.github/workflows/linux_builds.yml b/.github/workflows/linux_builds.yml index bd4e2856e398..193942a1bb3a 100644 --- a/.github/workflows/linux_builds.yml +++ b/.github/workflows/linux_builds.yml @@ -10,6 +10,7 @@ env: DOTNET_NOLOGO: true DOTNET_CLI_TELEMETRY_OPTOUT: true TSAN_OPTIONS: suppressions=misc/error_suppressions/tsan.txt + UBSAN_OPTIONS: suppressions=misc/error_suppressions/ubsan.txt concurrency: group: ci-${{ github.actor }}-${{ github.head_ref || github.run_number }}-${{ github.ref }}-linux @@ -35,6 +36,7 @@ jobs: proj-conv: true api-compat: true artifact: true + cache-limit: 1 - name: Editor with doubles and GCC sanitizers (target=editor, tests=yes, dev_build=yes, scu_build=yes, precision=double, use_asan=yes, use_ubsan=yes, linker=gold) cache-name: linux-editor-double-sanitizers @@ -49,6 +51,7 @@ jobs: api-dump: true # Skip 2GiB artifact speeding up action. artifact: false + cache-limit: 7 - name: Editor with clang sanitizers (target=editor, tests=yes, dev_build=yes, use_asan=yes, use_ubsan=yes, use_llvm=yes, linker=lld) cache-name: linux-editor-llvm-sanitizers @@ -61,6 +64,7 @@ jobs: artifact: false # Test our oldest supported SCons/Python versions on one arbitrary editor build. legacy-scons: true + cache-limit: 7 - name: Editor with ThreadSanitizer (target=editor, tests=yes, dev_build=yes, use_tsan=yes, use_llvm=yes, linker=lld) cache-name: linux-editor-thread-sanitizer @@ -71,6 +75,7 @@ jobs: build-mono: false # Skip 2GiB artifact speeding up action. artifact: false + cache-limit: 5 - name: Template w/ Mono (target=template_release, tests=yes) cache-name: linux-template-mono @@ -80,6 +85,7 @@ jobs: build-mono: false tests: true artifact: true + cache-limit: 1 - name: Minimal template (target=template_release, tests=yes, everything disabled) cache-name: linux-template-minimal @@ -88,6 +94,7 @@ jobs: bin: ./bin/godot.linuxbsd.template_release.x86_64 tests: true artifact: true + cache-limit: 1 steps: - name: Checkout @@ -133,9 +140,6 @@ jobs: python-version: 3.8 scons-version: 4.0 - - name: Setup GCC problem matcher - uses: ammaraskar/gcc-problem-matcher@master - - name: Compilation uses: ./.github/actions/godot-build with: @@ -143,6 +147,7 @@ jobs: platform: linuxbsd target: ${{ matrix.target }} tests: ${{ matrix.tests }} + scons-cache-limit: ${{ matrix.cache-limit }} - name: Save Godot build cache uses: ./.github/actions/godot-cache-save diff --git a/.github/workflows/macos_builds.yml b/.github/workflows/macos_builds.yml index fcf4b00afb18..3fedc2a5c9a9 100644 --- a/.github/workflows/macos_builds.yml +++ b/.github/workflows/macos_builds.yml @@ -25,6 +25,7 @@ jobs: target: editor tests: true bin: ./bin/godot.macos.editor.universal + cache-limit: 1 - name: Template (target=template_release, tests=yes) cache-name: macos-template @@ -32,6 +33,7 @@ jobs: tests: true sconsflags: debug_symbols=no bin: ./bin/godot.macos.template_release.universal + cache-limit: 1 steps: - name: Checkout @@ -59,6 +61,7 @@ jobs: platform: macos target: ${{ matrix.target }} tests: ${{ matrix.tests }} + scons-cache-limit: 0 # Only cap on second run to avoid purging unnecessarily - name: Compilation (arm64) uses: ./.github/actions/godot-build @@ -67,6 +70,7 @@ jobs: platform: macos target: ${{ matrix.target }} tests: ${{ matrix.tests }} + scons-cache-limit: ${{ matrix.cache-limit }} - name: Save Godot build cache uses: ./.github/actions/godot-cache-save diff --git a/.github/workflows/static_checks.yml b/.github/workflows/static_checks.yml index 9b326cb43e90..e832faaca477 100644 --- a/.github/workflows/static_checks.yml +++ b/.github/workflows/static_checks.yml @@ -56,7 +56,7 @@ jobs: - name: Class reference schema checks run: | - xmllint --noout --schema doc/class.xsd doc/classes/*.xml modules/*/doc_classes/*.xml platform/*/doc_classes/*.xml + xmllint --quiet --noout --schema doc/class.xsd doc/classes/*.xml modules/*/doc_classes/*.xml platform/*/doc_classes/*.xml - name: Run C compiler on `gdextension_interface.h` run: | diff --git a/.github/workflows/web_builds.yml b/.github/workflows/web_builds.yml index 9ed84757690a..dac6270b2bd6 100644 --- a/.github/workflows/web_builds.yml +++ b/.github/workflows/web_builds.yml @@ -67,6 +67,7 @@ jobs: platform: web target: ${{ matrix.target }} tests: ${{ matrix.tests }} + scons-cache-limit: 0.5 - name: Save Godot build cache uses: ./.github/actions/godot-cache-save diff --git a/.github/workflows/windows_builds.yml b/.github/workflows/windows_builds.yml index 384284b6047e..c407ae12d099 100644 --- a/.github/workflows/windows_builds.yml +++ b/.github/workflows/windows_builds.yml @@ -31,6 +31,7 @@ jobs: sconsflags: debug_symbols=no vsproj=yes vsproj_gen_only=no windows_subsystem=console bin: ./bin/godot.windows.editor.x86_64.exe compiler: msvc + cache-limit: 2 - name: Editor w/ clang-cl (target=editor, tests=yes, use_llvm=yes) cache-name: windows-editor-clang @@ -39,6 +40,7 @@ jobs: sconsflags: debug_symbols=no windows_subsystem=console use_llvm=yes bin: ./bin/godot.windows.editor.x86_64.llvm.exe compiler: clang + cache-limit: 1 - name: Template (target=template_release, tests=yes) cache-name: windows-template @@ -47,6 +49,7 @@ jobs: sconsflags: debug_symbols=no bin: ./bin/godot.windows.template_release.x86_64.console.exe compiler: msvc + cache-limit: 2 - name: Template w/ GCC (target=template_release, tests=yes, use_mingw=yes) cache-name: windows-template-gcc @@ -56,6 +59,7 @@ jobs: sconsflags: debug_symbols=no use_mingw=yes bin: ./bin/godot.windows.template_release.x86_64.console.exe compiler: gcc + cache-limit: 1 steps: - name: Checkout @@ -86,14 +90,6 @@ jobs: - name: Extract pre-built ANGLE static libraries run: Expand-Archive -Force angle/angle.zip ${{ github.workspace }}/ - - name: Setup MSVC problem matcher - if: matrix.compiler == 'msvc' - uses: ammaraskar/msvc-problem-matcher@master - - - name: Setup GCC problem matcher - if: matrix.compiler != 'msvc' - uses: ammaraskar/gcc-problem-matcher@master - - name: Compilation uses: ./.github/actions/godot-build with: @@ -101,6 +97,7 @@ jobs: platform: windows target: ${{ matrix.target }} tests: ${{ matrix.tests }} + scons-cache-limit: ${{ matrix.cache-limit }} - name: Save Godot build cache uses: ./.github/actions/godot-cache-save diff --git a/.gitignore b/.gitignore index 0dcf79c4604c..22a1d53a8791 100644 --- a/.gitignore +++ b/.gitignore @@ -36,8 +36,8 @@ compile_commands.json platform/windows/godot_res.res # Ninja build files -build.ninja -.ninja +*.ninja +.ninja/ run_ninja_env.bat # Generated by Godot binary @@ -216,6 +216,7 @@ xcuserdata/ *.xcscmblueprint *.xccheckout *.xcodeproj/* +!misc/misc/dist/ios_xcode/godot_ios.xcodeproj/project.pbxproj ############################## ### Visual Studio specific ### diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 2fa0493544bd..77418d45b03c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -184,10 +184,11 @@ repos: .*\.patch$| .*\.out$| modules/gdscript/tests/scripts/parser/features/mixed_indentation_on_blank_lines\.gd$| - modules/gdscript/tests/scripts/parser/warnings/empty_file_newline_comment\.notest\.gd$| - modules/gdscript/tests/scripts/parser/warnings/empty_file_newline\.notest\.gd$| + modules/gdscript/tests/scripts/parser/warnings/empty_file_newline_comment\.norun\.gd$| + modules/gdscript/tests/scripts/parser/warnings/empty_file_newline\.norun\.gd$| platform/android/java/editor/src/main/java/com/android/.*| - platform/android/java/lib/src/com/google/.* + platform/android/java/lib/src/com/google/.*| + tests/data/.*\.bin$ ) - id: dotnet-format diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt index 2ece06eebbfb..d2d9794b9112 100644 --- a/COPYRIGHT.txt +++ b/COPYRIGHT.txt @@ -101,6 +101,13 @@ Copyright: 2007, Starbreeze Studios 2007-2014, Juan Linietsky, Ariel Manzur License: Expat and Zlib +Files: ./modules/jolt_physics/spaces/jolt_temp_allocator.cpp +Comment: Jolt Physics +Copyright: 2021, Jorrit Rouwe + 2014-present, Godot Engine contributors + 2007-2014, Juan Linietsky, Ariel Manzur +License: Expat + Files: ./modules/lightmapper_rd/lm_compute.glsl Comment: Joint Non-Local Means (JNLM) denoiser Copyright: 2020, Manuel Prandini @@ -186,7 +193,7 @@ License: MPL-2.0 Files: ./thirdparty/clipper2/ Comment: Clipper2 -Copyright: 2010-2023, Angus Johnson +Copyright: 2010-2024, Angus Johnson License: BSL-1.0 Files: ./thirdparty/cvtt/ @@ -240,6 +247,11 @@ Comment: Noto Sans font Copyright: 2012, Google Inc. License: OFL-1.1 +Files: ./thirdparty/fonts/Vazirmatn*.woff2 +Comment: Vazirmatn font +Copyright: 2015, The Vazirmatn Project Authors. +License: OFL-1.1 + Files: ./thirdparty/freetype/ Comment: The FreeType Project Copyright: 1996-2023, David Turner, Robert Wilhelm, and Werner Lemberg. @@ -289,6 +301,11 @@ Comment: International Components for Unicode Copyright: 2016-2024, Unicode, Inc. License: Unicode +Files: ./thirdparty/jolt_physics/ +Comment: Jolt Physics +Copyright: 2021, Jorrit Rouwe +License: Expat + Files: ./thirdparty/jpeg-compressor/ Comment: jpeg-compressor Copyright: 2012, Rich Geldreich @@ -334,6 +351,11 @@ Comment: WebP codec Copyright: 2010, Google Inc. License: BSD-3-clause +Files: ./thirdparty/manifold/ +Comment: Manifold +Copyright: 2020-2024, The Manifold Authors +License: Apache-2.0 + Files: ./thirdparty/mbedtls/ Comment: Mbed TLS Copyright: The Mbed TLS Contributors diff --git a/SConstruct b/SConstruct index 234a6624f1cc..50e83dcfdb8c 100644 --- a/SConstruct +++ b/SConstruct @@ -227,7 +227,6 @@ opts.Add(BoolVariable("use_volk", "Use the volk library to load the Vulkan loade opts.Add(BoolVariable("disable_exceptions", "Force disabling exception handling code", True)) opts.Add("custom_modules", "A list of comma-separated directory paths containing custom modules to build.", "") opts.Add(BoolVariable("custom_modules_recursive", "Detect custom modules recursively for each specified path.", True)) -opts.Add(BoolVariable("swappy", "Use Swappy Frame Pacing Library in Android builds.", False)) # Advanced options opts.Add( @@ -383,8 +382,7 @@ if env["platform"] not in platform_list: # Add platform-specific options. if env["platform"] in platform_opts: - for opt in platform_opts[env["platform"]]: - opts.Add(opt) + opts.AddVariables(*platform_opts[env["platform"]]) # Platform-specific flags. # These can sometimes override default options, so they need to be processed @@ -440,12 +438,11 @@ for name, path in modules_detected.items(): else: enabled = False - opts.Add(BoolVariable("module_" + name + "_enabled", "Enable module '%s'" % (name,), enabled)) + opts.Add(BoolVariable(f"module_{name}_enabled", f"Enable module '{name}'", enabled)) # Add module-specific options. try: - for opt in config.get_opts(env["platform"]): - opts.Add(opt) + opts.AddVariables(*config.get_opts(env["platform"])) except AttributeError: pass @@ -504,14 +501,19 @@ else: # Disable assert() for production targets (only used in thirdparty code). env.Append(CPPDEFINES=["NDEBUG"]) +# This is not part of fast_unsafe because the only downside it has compared to +# the default is that SCons won't mark files that were changed in the last second +# as different. This is unlikely to be a problem in any real situation as just booting +# up scons takes more than that time. +# Renamed to `content-timestamp` in SCons >= 4.2, keeping MD5 for compat. +env.Decider("MD5-timestamp") + # SCons speed optimization controlled by the `fast_unsafe` option, which provide # more than 10 s speed up for incremental rebuilds. # Unsafe as they reduce the certainty of rebuilding all changed files, so it's # enabled by default for `debug` builds, and can be overridden from command line. # Ref: https://github.com/SCons/scons/wiki/GoFastButton if methods.get_cmdline_bool("fast_unsafe", env.dev_build): - # Renamed to `content-timestamp` in SCons >= 4.2, keeping MD5 for compat. - env.Decider("MD5-timestamp") env.SetOption("implicit_cache", 1) env.SetOption("max_drift", 60) @@ -580,7 +582,7 @@ env.Append(RCFLAGS=env.get("rcflags", "").split()) # Feature build profile env.disabled_classes = [] if env["build_profile"] != "": - print('Using feature build profile: "{}"'.format(env["build_profile"])) + print(f'Using feature build profile: "{env["build_profile"]}"') import json try: @@ -592,7 +594,7 @@ if env["build_profile"] != "": for c in dbo: env[c] = dbo[c] except json.JSONDecodeError: - print_error('Failed to open feature build profile: "{}"'.format(env["build_profile"])) + print_error(f'Failed to open feature build profile: "{env["build_profile"]}"') Exit(255) # 'dev_mode' and 'production' are aliases to set default options if they haven't been @@ -711,6 +713,12 @@ elif env.msvc: ) Exit(255) +# Default architecture flags. +if env["arch"] == "x86_32": + if env.msvc: + env.Append(CCFLAGS=["/arch:SSE2"]) + else: + env.Append(CCFLAGS=["-msse2"]) # Set optimize and debug_symbols flags. # "custom" means do nothing and let users set their own optimization flags. @@ -734,9 +742,15 @@ if env.msvc: env.Append(CCFLAGS=["/Od"]) else: if env["debug_symbols"]: - # Adding dwarf-4 explicitly makes stacktraces work with clang builds, - # otherwise addr2line doesn't understand them - env.Append(CCFLAGS=["-gdwarf-4"]) + if env["platform"] == "windows": + if methods.using_clang(env): + env.Append(CCFLAGS=["-gdwarf-4"]) # clang dwarf-5 symbols are broken on Windows. + else: + env.Append(CCFLAGS=["-gdwarf-5"]) # For gcc, only dwarf-5 symbols seem usable by libbacktrace. + else: + # Adding dwarf-4 explicitly makes stacktraces work with clang builds, + # otherwise addr2line doesn't understand them + env.Append(CCFLAGS=["-gdwarf-4"]) if methods.using_emcc(env): # Emscripten only produces dwarf symbols when using "-g3". env.Append(CCFLAGS=["-g3"]) @@ -818,44 +832,41 @@ elif env.msvc: # Configure compiler warnings if env.msvc and not methods.using_clang(env): # MSVC - if env["warnings"] == "no": - env.Append(CCFLAGS=["/w"]) - else: - if env["warnings"] == "extra": - env.Append(CCFLAGS=["/W4"]) - elif env["warnings"] == "all": - # C4458 is like -Wshadow. Part of /W4 but let's apply it for the default /W3 too. - env.Append(CCFLAGS=["/W3", "/w34458"]) - elif env["warnings"] == "moderate": - env.Append(CCFLAGS=["/W2"]) - # Disable warnings which we don't plan to fix. - - env.Append( - CCFLAGS=[ - "/wd4100", # C4100 (unreferenced formal parameter): Doesn't play nice with polymorphism. - "/wd4127", # C4127 (conditional expression is constant) - "/wd4201", # C4201 (non-standard nameless struct/union): Only relevant for C89. - "/wd4244", # C4244 C4245 C4267 (narrowing conversions): Unavoidable at this scale. - "/wd4245", - "/wd4267", - "/wd4305", # C4305 (truncation): double to float or real_t, too hard to avoid. - "/wd4324", # C4820 (structure was padded due to alignment specifier) - "/wd4514", # C4514 (unreferenced inline function has been removed) - "/wd4714", # C4714 (function marked as __forceinline not inlined) - "/wd4820", # C4820 (padding added after construct) - ] - ) + # Disable warnings which we don't plan to fix. + disabled_warnings = [ + "/wd4100", # C4100 (unreferenced formal parameter): Doesn't play nice with polymorphism. + "/wd4127", # C4127 (conditional expression is constant) + "/wd4201", # C4201 (non-standard nameless struct/union): Only relevant for C89. + "/wd4244", # C4244 C4245 C4267 (narrowing conversions): Unavoidable at this scale. + "/wd4245", + "/wd4267", + "/wd4305", # C4305 (truncation): double to float or real_t, too hard to avoid. + "/wd4324", # C4820 (structure was padded due to alignment specifier) + "/wd4514", # C4514 (unreferenced inline function has been removed) + "/wd4714", # C4714 (function marked as __forceinline not inlined) + "/wd4820", # C4820 (padding added after construct) + ] + + if env["warnings"] == "extra": + env.Append(CCFLAGS=["/W4"] + disabled_warnings) + elif env["warnings"] == "all": + # C4458 is like -Wshadow. Part of /W4 but let's apply it for the default /W3 too. + env.Append(CCFLAGS=["/W3", "/w34458"] + disabled_warnings) + elif env["warnings"] == "moderate": + env.Append(CCFLAGS=["/W2"] + disabled_warnings) + else: # 'no' + # C4267 is particularly finicky & needs to be explicitly disabled. + env.Append(CCFLAGS=["/w", "/wd4267"]) if env["werror"]: env.Append(CCFLAGS=["/WX"]) env.Append(LINKFLAGS=["/WX"]) + else: # GCC, Clang common_warnings = [] if methods.using_gcc(env): common_warnings += ["-Wshadow", "-Wno-misleading-indentation"] - if cc_version_major == 7: # Bogus warning fixed in 8+. - common_warnings += ["-Wno-strict-overflow"] if cc_version_major < 11: # Regression in GCC 9/10, spams so much in our variadic templates # that we need to outright disable it. @@ -931,7 +942,7 @@ env.module_icons_paths = [] env.doc_class_path = platform_doc_class_path for name, path in modules_detected.items(): - if not env["module_" + name + "_enabled"]: + if not env[f"module_{name}_enabled"]: continue sys.path.insert(0, path) env.current_module = name @@ -1044,13 +1055,13 @@ if env["compiledb"]: if env["ninja"]: if env.scons_version < (4, 2, 0): - print_error("The `ninja=yes` option requires SCons 4.2 or later, but your version is %s." % scons_raw_version) + print_error(f"The `ninja=yes` option requires SCons 4.2 or later, but your version is {scons_raw_version}.") Exit(255) SetOption("experimental", "ninja") env["NINJA_FILE_NAME"] = env["ninja_file"] env["NINJA_DISABLE_AUTO_RUN"] = not env["ninja_auto_run"] - env.Tool("ninja", "build.ninja") + env.Tool("ninja", env["ninja_file"]) # Threads if env["threads"]: @@ -1112,7 +1123,7 @@ atexit.register(print_elapsed_time) def purge_flaky_files(): - paths_to_keep = ["build.ninja"] + paths_to_keep = [env["ninja_file"]] for build_failure in GetBuildFailures(): path = build_failure.node.path if os.path.isfile(path) and path not in paths_to_keep: diff --git a/core/config/engine.cpp b/core/config/engine.cpp index aac048e93f7e..250f39b08a32 100644 --- a/core/config/engine.cpp +++ b/core/config/engine.cpp @@ -248,6 +248,9 @@ String Engine::get_architecture_name() const { return "ppc"; #endif +#elif defined(__loongarch64) + return "loongarch64"; + #elif defined(__wasm__) #if defined(__wasm64__) return "wasm64"; diff --git a/core/config/project_settings.cpp b/core/config/project_settings.cpp index fa0e4037ba3f..30cdd789a448 100644 --- a/core/config/project_settings.cpp +++ b/core/config/project_settings.cpp @@ -194,7 +194,7 @@ String ProjectSettings::localize_path(const String &p_path) const { return cwd.replace_first(res_path, "res://"); } else { - int sep = path.rfind("/"); + int sep = path.rfind_char('/'); if (sep == -1) { return "res://" + path; } @@ -262,6 +262,12 @@ String ProjectSettings::globalize_path(const String &p_path) const { return p_path.replace("res:/", resource_path); } return p_path.replace("res://", ""); + } else if (p_path.begins_with("uid://")) { + const String path = ResourceUID::uid_to_path(p_path); + if (!resource_path.is_empty()) { + return path.replace("res:/", resource_path); + } + return path.replace("res://", ""); } else if (p_path.begins_with("user://")) { String data_dir = OS::get_singleton()->get_user_data_dir(); if (!data_dir.is_empty()) { @@ -300,7 +306,7 @@ bool ProjectSettings::_set(const StringName &p_name, const Variant &p_value) { } { // Feature overrides. - int dot = p_name.operator String().find("."); + int dot = p_name.operator String().find_char('.'); if (dot != -1) { Vector s = p_name.operator String().split("."); @@ -435,7 +441,7 @@ void ProjectSettings::_get_property_list(List *p_list) const { for (const _VCSort &E : vclist) { String prop_info_name = E.name; - int dot = prop_info_name.find("."); + int dot = prop_info_name.find_char('.'); if (dot != -1 && !custom_prop_info.has(prop_info_name)) { prop_info_name = prop_info_name.substr(0, dot); } @@ -508,16 +514,19 @@ void ProjectSettings::_convert_to_last_version(int p_from_version) { } } } - if (p_from_version <= 5) { - // Converts the device in events from -1 (emulated events) to -3 (all events). + if (p_from_version == 5) { + // Converts the device in events from -3 to -1. + // -3 was introduced in GH-97707 as a way to prevent a clash in device IDs, but as reported in GH-99243, this leads to problems. + // -3 was used during dev-releases, so this conversion helps to revert such affected projects. + // This conversion doesn't affect any other projects, since -3 is not used otherwise. for (KeyValue &E : props) { if (String(E.key).begins_with("input/")) { Dictionary action = E.value.variant; Array events = action["events"]; for (int i = 0; i < events.size(); i++) { Ref ev = events[i]; - if (ev.is_valid() && ev->get_device() == -1) { // -1 was the previous value (GH-97707). - ev->set_device(InputEvent::DEVICE_ID_ALL_DEVICES); + if (ev.is_valid() && ev->get_device() == -3) { + ev->set_device(-1); } } } @@ -740,7 +749,7 @@ Error ProjectSettings::_load_settings_binary(const String &p_path) { cs[slen] = 0; f->get_buffer((uint8_t *)cs.ptr(), slen); String key; - key.parse_utf8(cs.ptr()); + key.parse_utf8(cs.ptr(), slen); uint32_t vlen = f->get_32(); Vector d; @@ -1092,7 +1101,7 @@ Error ProjectSettings::save_custom(const String &p_path, const CustomMap &p_cust String category = E.name; String name = E.name; - int div = category.find("/"); + int div = category.find_char('/'); if (div < 0) { category = ""; @@ -1507,7 +1516,11 @@ ProjectSettings::ProjectSettings() { GLOBAL_DEF("display/window/frame_pacing/android/enable_frame_pacing", true); GLOBAL_DEF(PropertyInfo(Variant::INT, "display/window/frame_pacing/android/swappy_mode", PROPERTY_HINT_ENUM, "pipeline_forced_on,auto_fps_pipeline_forced_on,auto_fps_auto_pipeline"), 2); - custom_prop_info["rendering/driver/threads/thread_model"] = PropertyInfo(Variant::INT, "rendering/driver/threads/thread_model", PROPERTY_HINT_ENUM, "Single-Unsafe,Single-Safe,Multi-Threaded"); +#ifdef DISABLE_DEPRECATED + custom_prop_info["rendering/driver/threads/thread_model"] = PropertyInfo(Variant::INT, "rendering/driver/threads/thread_model", PROPERTY_HINT_ENUM, "Safe:1,Separate"); +#else + custom_prop_info["rendering/driver/threads/thread_model"] = PropertyInfo(Variant::INT, "rendering/driver/threads/thread_model", PROPERTY_HINT_ENUM, "Unsafe (deprecated),Safe,Separate"); +#endif GLOBAL_DEF("physics/2d/run_on_separate_thread", false); GLOBAL_DEF("physics/3d/run_on_separate_thread", false); @@ -1550,6 +1563,7 @@ ProjectSettings::ProjectSettings() { GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/staging_buffer/block_size_kb", PROPERTY_HINT_RANGE, "4,2048,1,or_greater"), 256); GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/staging_buffer/max_size_mb", PROPERTY_HINT_RANGE, "1,1024,1,or_greater"), 128); GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/staging_buffer/texture_upload_region_size_px", PROPERTY_HINT_RANGE, "1,256,1,or_greater"), 64); + GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/staging_buffer/texture_download_region_size_px", PROPERTY_HINT_RANGE, "1,256,1,or_greater"), 64); GLOBAL_DEF_RST(PropertyInfo(Variant::BOOL, "rendering/rendering_device/pipeline_cache/enable"), true); GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "rendering/rendering_device/pipeline_cache/save_chunk_size_mb", PROPERTY_HINT_RANGE, "0.000001,64.0,0.001,or_greater"), 3.0); GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/vulkan/max_descriptors_per_pool", PROPERTY_HINT_RANGE, "1,256,1,or_greater"), 64); diff --git a/core/core_bind.cpp b/core/core_bind.cpp index 925551d933fe..66bf5f41e14e 100644 --- a/core/core_bind.cpp +++ b/core/core_bind.cpp @@ -577,6 +577,11 @@ String OS::get_cache_dir() const { return ::OS::get_singleton()->get_cache_path(); } +String OS::get_temp_dir() const { + // Exposed as `get_temp_dir()` instead of `get_temp_path()` for consistency with other exposed OS methods. + return ::OS::get_singleton()->get_temp_path(); +} + bool OS::is_debug_build() const { #ifdef DEBUG_ENABLED return true; @@ -705,6 +710,7 @@ void OS::_bind_methods() { ClassDB::bind_method(D_METHOD("get_config_dir"), &OS::get_config_dir); ClassDB::bind_method(D_METHOD("get_data_dir"), &OS::get_data_dir); ClassDB::bind_method(D_METHOD("get_cache_dir"), &OS::get_cache_dir); + ClassDB::bind_method(D_METHOD("get_temp_dir"), &OS::get_temp_dir); ClassDB::bind_method(D_METHOD("get_unique_id"), &OS::get_unique_id); ClassDB::bind_method(D_METHOD("get_keycode_string", "code"), &OS::get_keycode_string); diff --git a/core/core_bind.h b/core/core_bind.h index d013e348bd87..3b0442bd65a0 100644 --- a/core/core_bind.h +++ b/core/core_bind.h @@ -262,6 +262,7 @@ class OS : public Object { String get_config_dir() const; String get_data_dir() const; String get_cache_dir() const; + String get_temp_dir() const; Error set_thread_name(const String &p_name); ::Thread::ID get_thread_caller_id() const; diff --git a/core/core_constants.cpp b/core/core_constants.cpp index 25da49fa5c3e..cdb4f2c800d8 100644 --- a/core/core_constants.cpp +++ b/core/core_constants.cpp @@ -678,6 +678,7 @@ void register_global_constants() { BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_HIDE_QUATERNION_EDIT); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_PASSWORD); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_TOOL_BUTTON); + BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_ONESHOT); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_MAX); BIND_CORE_BITFIELD_FLAG(PROPERTY_USAGE_NONE); diff --git a/core/core_string_names.h b/core/core_string_names.h index d4ba9110c30e..be3bc38e70ad 100644 --- a/core/core_string_names.h +++ b/core/core_string_names.h @@ -34,61 +34,56 @@ #include "core/string/string_name.h" class CoreStringNames { - friend void register_core_types(); - friend void unregister_core_types(); + inline static CoreStringNames *singleton = nullptr; +public: static void create() { singleton = memnew(CoreStringNames); } static void free() { memdelete(singleton); singleton = nullptr; } - CoreStringNames(); - -public: _FORCE_INLINE_ static CoreStringNames *get_singleton() { return singleton; } - static CoreStringNames *singleton; - - StringName free_; // "free", conflict with C++ keyword. - StringName changed; - StringName script; - StringName script_changed; - StringName _iter_init; - StringName _iter_next; - StringName _iter_get; - StringName get_rid; - StringName _to_string; - StringName _custom_features; + const StringName free_ = StaticCString::create("free"); // free would conflict with C++ keyword. + const StringName changed = StaticCString::create("changed"); + const StringName script = StaticCString::create("script"); + const StringName script_changed = StaticCString::create("script_changed"); + const StringName _iter_init = StaticCString::create("_iter_init"); + const StringName _iter_next = StaticCString::create("_iter_next"); + const StringName _iter_get = StaticCString::create("_iter_get"); + const StringName get_rid = StaticCString::create("get_rid"); + const StringName _to_string = StaticCString::create("_to_string"); + const StringName _custom_features = StaticCString::create("_custom_features"); - StringName x; - StringName y; - StringName z; - StringName w; - StringName r; - StringName g; - StringName b; - StringName a; - StringName position; - StringName size; - StringName end; - StringName basis; - StringName origin; - StringName normal; - StringName d; - StringName h; - StringName s; - StringName v; - StringName r8; - StringName g8; - StringName b8; - StringName a8; + const StringName x = StaticCString::create("x"); + const StringName y = StaticCString::create("y"); + const StringName z = StaticCString::create("z"); + const StringName w = StaticCString::create("w"); + const StringName r = StaticCString::create("r"); + const StringName g = StaticCString::create("g"); + const StringName b = StaticCString::create("b"); + const StringName a = StaticCString::create("a"); + const StringName position = StaticCString::create("position"); + const StringName size = StaticCString::create("size"); + const StringName end = StaticCString::create("end"); + const StringName basis = StaticCString::create("basis"); + const StringName origin = StaticCString::create("origin"); + const StringName normal = StaticCString::create("normal"); + const StringName d = StaticCString::create("d"); + const StringName h = StaticCString::create("h"); + const StringName s = StaticCString::create("s"); + const StringName v = StaticCString::create("v"); + const StringName r8 = StaticCString::create("r8"); + const StringName g8 = StaticCString::create("g8"); + const StringName b8 = StaticCString::create("b8"); + const StringName a8 = StaticCString::create("a8"); - StringName call; - StringName call_deferred; - StringName bind; - StringName notification; - StringName property_list_changed; + const StringName call = StaticCString::create("call"); + const StringName call_deferred = StaticCString::create("call_deferred"); + const StringName bind = StaticCString::create("bind"); + const StringName notification = StaticCString::create("notification"); + const StringName property_list_changed = StaticCString::create("property_list_changed"); }; #define CoreStringName(m_name) CoreStringNames::get_singleton()->m_name diff --git a/core/crypto/crypto.h b/core/crypto/crypto.h index c19e6b6773bb..dc4441c9b88c 100644 --- a/core/crypto/crypto.h +++ b/core/crypto/crypto.h @@ -155,6 +155,10 @@ class ResourceFormatLoaderCrypto : public ResourceFormatLoader { virtual void get_recognized_extensions(List *p_extensions) const override; virtual bool handles_type(const String &p_type) const override; virtual String get_resource_type(const String &p_path) const override; + + // Treat certificates as text files, do not generate a `*.{crt,key,pub}.uid` file. + virtual ResourceUID::ID get_resource_uid(const String &p_path) const override { return ResourceUID::INVALID_ID; } + virtual bool has_custom_uid_support() const override { return true; } }; class ResourceFormatSaverCrypto : public ResourceFormatSaver { diff --git a/core/debugger/engine_debugger.cpp b/core/debugger/engine_debugger.cpp index 6d2868cef2fd..a9f87ad825f9 100644 --- a/core/debugger/engine_debugger.cpp +++ b/core/debugger/engine_debugger.cpp @@ -163,7 +163,7 @@ void EngineDebugger::initialize(const String &p_uri, bool p_skip_breakpoints, co for (int i = 0; i < p_breakpoints.size(); i++) { const String &bp = p_breakpoints[i]; - int sp = bp.rfind(":"); + int sp = bp.rfind_char(':'); ERR_CONTINUE_MSG(sp == -1, vformat("Invalid breakpoint: '%s', expected file:line format.", bp)); singleton_script_debugger->insert_breakpoint(bp.substr(sp + 1, bp.length()).to_int(), bp.substr(0, sp)); diff --git a/core/debugger/local_debugger.cpp b/core/debugger/local_debugger.cpp index 5e79a6d39525..a5d807f66bec 100644 --- a/core/debugger/local_debugger.cpp +++ b/core/debugger/local_debugger.cpp @@ -171,7 +171,7 @@ void LocalDebugger::debug(bool p_can_continue, bool p_is_error_breakpoint) { } else { String key_value = line.get_slicec(' ', 1); - int value_pos = key_value.find("="); + int value_pos = key_value.find_char('='); if (value_pos < 0) { print_line("Error: Invalid set format. Use: set key=value"); @@ -344,7 +344,7 @@ Pair LocalDebugger::to_breakpoint(const String &p_line) { String breakpoint_part = p_line.get_slicec(' ', 1); Pair breakpoint; - int last_colon = breakpoint_part.rfind(":"); + int last_colon = breakpoint_part.rfind_char(':'); if (last_colon < 0) { print_line("Error: Invalid breakpoint format. Expected [source:line]"); return breakpoint; diff --git a/core/debugger/remote_debugger.cpp b/core/debugger/remote_debugger.cpp index a02354b3777e..f8e42e6d92be 100644 --- a/core/debugger/remote_debugger.cpp +++ b/core/debugger/remote_debugger.cpp @@ -338,7 +338,7 @@ void RemoteDebugger::_send_stack_vars(List &p_names, List &p_va } Error RemoteDebugger::_try_capture(const String &p_msg, const Array &p_data, bool &r_captured) { - const int idx = p_msg.find(":"); + const int idx = p_msg.find_char(':'); r_captured = false; if (idx < 0) { // No prefix, unknown message. return OK; @@ -610,7 +610,7 @@ void RemoteDebugger::poll_events(bool p_is_idle) { ERR_CONTINUE(arr[1].get_type() != Variant::ARRAY); const String cmd = arr[0]; - const int idx = cmd.find(":"); + const int idx = cmd.find_char(':'); bool parsed = false; if (idx < 0) { // Not prefix, use scripts capture. capture_parse("core", cmd, arr[1], parsed); diff --git a/core/debugger/remote_debugger_peer.cpp b/core/debugger/remote_debugger_peer.cpp index 793db7330fdd..499b221ce6a7 100644 --- a/core/debugger/remote_debugger_peer.cpp +++ b/core/debugger/remote_debugger_peer.cpp @@ -223,8 +223,8 @@ RemoteDebuggerPeer *RemoteDebuggerPeerTCP::create(const String &p_uri) { String debug_host = p_uri.replace("tcp://", ""); uint16_t debug_port = 6007; - if (debug_host.contains(":")) { - int sep_pos = debug_host.rfind(":"); + if (debug_host.contains_char(':')) { + int sep_pos = debug_host.rfind_char(':'); debug_port = debug_host.substr(sep_pos + 1).to_int(); debug_host = debug_host.substr(0, sep_pos); } diff --git a/core/extension/extension_api_dump.cpp b/core/extension/extension_api_dump.cpp index 7263cafdf3cb..9429ad1ef8a4 100644 --- a/core/extension/extension_api_dump.cpp +++ b/core/extension/extension_api_dump.cpp @@ -1205,7 +1205,7 @@ Dictionary GDExtensionAPIDump::generate_extension_api(bool p_include_docs) { if (F.name.begins_with("_")) { continue; //hidden property } - if (F.name.contains("/")) { + if (F.name.contains_char('/')) { // Ignore properties with '/' (slash) in the name. These are only meant for use in the inspector. continue; } diff --git a/core/extension/gdextension.cpp b/core/extension/gdextension.cpp index 05ba114a9656..258b01542e26 100644 --- a/core/extension/gdextension.cpp +++ b/core/extension/gdextension.cpp @@ -154,7 +154,7 @@ class GDExtensionMethodBind : public MethodBind { } virtual bool is_vararg() const override { - return false; + return vararg; } #ifdef TOOLS_ENABLED diff --git a/core/extension/gdextension_interface.cpp b/core/extension/gdextension_interface.cpp index 203f7960dc53..85d53c31ecaa 100644 --- a/core/extension/gdextension_interface.cpp +++ b/core/extension/gdextension_interface.cpp @@ -700,6 +700,91 @@ static GDExtensionTypeFromVariantConstructorFunc gdextension_get_variant_to_type ERR_FAIL_V_MSG(nullptr, "Getting Variant conversion function with invalid type"); } +static GDExtensionVariantGetInternalPtrFunc gdextension_variant_get_ptr_internal_getter(GDExtensionVariantType p_type) { + switch (p_type) { + case GDEXTENSION_VARIANT_TYPE_BOOL: + return reinterpret_cast(static_cast(VariantInternal::get_bool)); + case GDEXTENSION_VARIANT_TYPE_INT: + return reinterpret_cast(static_cast(VariantInternal::get_int)); + case GDEXTENSION_VARIANT_TYPE_FLOAT: + return reinterpret_cast(static_cast(VariantInternal::get_float)); + case GDEXTENSION_VARIANT_TYPE_STRING: + return reinterpret_cast(static_cast(VariantInternal::get_string)); + case GDEXTENSION_VARIANT_TYPE_VECTOR2: + return reinterpret_cast(static_cast(VariantInternal::get_vector2)); + case GDEXTENSION_VARIANT_TYPE_VECTOR2I: + return reinterpret_cast(static_cast(VariantInternal::get_vector2i)); + case GDEXTENSION_VARIANT_TYPE_RECT2: + return reinterpret_cast(static_cast(VariantInternal::get_rect2)); + case GDEXTENSION_VARIANT_TYPE_RECT2I: + return reinterpret_cast(static_cast(VariantInternal::get_rect2i)); + case GDEXTENSION_VARIANT_TYPE_VECTOR3: + return reinterpret_cast(static_cast(VariantInternal::get_vector3)); + case GDEXTENSION_VARIANT_TYPE_VECTOR3I: + return reinterpret_cast(static_cast(VariantInternal::get_vector3i)); + case GDEXTENSION_VARIANT_TYPE_TRANSFORM2D: + return reinterpret_cast(static_cast(VariantInternal::get_transform2d)); + case GDEXTENSION_VARIANT_TYPE_VECTOR4: + return reinterpret_cast(static_cast(VariantInternal::get_vector4)); + case GDEXTENSION_VARIANT_TYPE_VECTOR4I: + return reinterpret_cast(static_cast(VariantInternal::get_vector4i)); + case GDEXTENSION_VARIANT_TYPE_PLANE: + return reinterpret_cast(static_cast(VariantInternal::get_plane)); + case GDEXTENSION_VARIANT_TYPE_QUATERNION: + return reinterpret_cast(static_cast(VariantInternal::get_quaternion)); + case GDEXTENSION_VARIANT_TYPE_AABB: + return reinterpret_cast(static_cast(VariantInternal::get_aabb)); + case GDEXTENSION_VARIANT_TYPE_BASIS: + return reinterpret_cast(static_cast(VariantInternal::get_basis)); + case GDEXTENSION_VARIANT_TYPE_TRANSFORM3D: + return reinterpret_cast(static_cast(VariantInternal::get_transform)); + case GDEXTENSION_VARIANT_TYPE_PROJECTION: + return reinterpret_cast(static_cast(VariantInternal::get_projection)); + case GDEXTENSION_VARIANT_TYPE_COLOR: + return reinterpret_cast(static_cast(VariantInternal::get_color)); + case GDEXTENSION_VARIANT_TYPE_STRING_NAME: + return reinterpret_cast(static_cast(VariantInternal::get_string_name)); + case GDEXTENSION_VARIANT_TYPE_NODE_PATH: + return reinterpret_cast(static_cast(VariantInternal::get_node_path)); + case GDEXTENSION_VARIANT_TYPE_RID: + return reinterpret_cast(static_cast(VariantInternal::get_rid)); + case GDEXTENSION_VARIANT_TYPE_OBJECT: + return reinterpret_cast(static_cast(VariantInternal::get_object)); + case GDEXTENSION_VARIANT_TYPE_CALLABLE: + return reinterpret_cast(static_cast(VariantInternal::get_callable)); + case GDEXTENSION_VARIANT_TYPE_SIGNAL: + return reinterpret_cast(static_cast(VariantInternal::get_signal)); + case GDEXTENSION_VARIANT_TYPE_DICTIONARY: + return reinterpret_cast(static_cast(VariantInternal::get_dictionary)); + case GDEXTENSION_VARIANT_TYPE_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_BYTE_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_byte_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_INT32_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_int32_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_INT64_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_int64_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_FLOAT32_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_float32_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_FLOAT64_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_float64_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_STRING_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_string_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_VECTOR2_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_vector2_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_VECTOR3_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_vector3_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_COLOR_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_color_array)); + case GDEXTENSION_VARIANT_TYPE_PACKED_VECTOR4_ARRAY: + return reinterpret_cast(static_cast(VariantInternal::get_vector4_array)); + case GDEXTENSION_VARIANT_TYPE_NIL: + case GDEXTENSION_VARIANT_TYPE_VARIANT_MAX: + ERR_FAIL_V_MSG(nullptr, "Getting Variant get internal pointer function with invalid type."); + } + ERR_FAIL_V_MSG(nullptr, "Getting Variant get internal pointer function with invalid type."); +} + // ptrcalls static GDExtensionPtrOperatorEvaluator gdextension_variant_get_ptr_operator_evaluator(GDExtensionVariantOperator p_operator, GDExtensionVariantType p_type_a, GDExtensionVariantType p_type_b) { return (GDExtensionPtrOperatorEvaluator)Variant::get_ptr_operator_evaluator(Variant::Operator(p_operator), Variant::Type(p_type_a), Variant::Type(p_type_b)); @@ -1625,6 +1710,7 @@ void gdextension_setup_interface() { REGISTER_INTERFACE_FUNC(variant_can_convert_strict); REGISTER_INTERFACE_FUNC(get_variant_from_type_constructor); REGISTER_INTERFACE_FUNC(get_variant_to_type_constructor); + REGISTER_INTERFACE_FUNC(variant_get_ptr_internal_getter); REGISTER_INTERFACE_FUNC(variant_get_ptr_operator_evaluator); REGISTER_INTERFACE_FUNC(variant_get_ptr_builtin_method); REGISTER_INTERFACE_FUNC(variant_get_ptr_constructor); diff --git a/core/extension/gdextension_interface.h b/core/extension/gdextension_interface.h index 374dbfd07115..b401acef8295 100644 --- a/core/extension/gdextension_interface.h +++ b/core/extension/gdextension_interface.h @@ -198,6 +198,7 @@ typedef struct { typedef void (*GDExtensionVariantFromTypeConstructorFunc)(GDExtensionUninitializedVariantPtr, GDExtensionTypePtr); typedef void (*GDExtensionTypeFromVariantConstructorFunc)(GDExtensionUninitializedTypePtr, GDExtensionVariantPtr); +typedef void *(*GDExtensionVariantGetInternalPtrFunc)(GDExtensionVariantPtr); typedef void (*GDExtensionPtrOperatorEvaluator)(GDExtensionConstTypePtr p_left, GDExtensionConstTypePtr p_right, GDExtensionTypePtr r_result); typedef void (*GDExtensionPtrBuiltInMethod)(GDExtensionTypePtr p_base, const GDExtensionConstTypePtr *p_args, GDExtensionTypePtr r_return, int p_argument_count); typedef void (*GDExtensionPtrConstructor)(GDExtensionUninitializedTypePtr p_base, const GDExtensionConstTypePtr *p_args); @@ -841,7 +842,7 @@ typedef void (*GDExtensionInterfaceMemFree)(void *p_ptr); * * Logs an error to Godot's built-in debugger and to the OS terminal. * - * @param p_description The code trigging the error. + * @param p_description The code triggering the error. * @param p_function The function name where the error occurred. * @param p_file The file where the error occurred. * @param p_line The line where the error occurred. @@ -855,7 +856,7 @@ typedef void (*GDExtensionInterfacePrintError)(const char *p_description, const * * Logs an error with a message to Godot's built-in debugger and to the OS terminal. * - * @param p_description The code trigging the error. + * @param p_description The code triggering the error. * @param p_message The message to show along with the error. * @param p_function The function name where the error occurred. * @param p_file The file where the error occurred. @@ -870,7 +871,7 @@ typedef void (*GDExtensionInterfacePrintErrorWithMessage)(const char *p_descript * * Logs a warning to Godot's built-in debugger and to the OS terminal. * - * @param p_description The code trigging the warning. + * @param p_description The code triggering the warning. * @param p_function The function name where the warning occurred. * @param p_file The file where the warning occurred. * @param p_line The line where the warning occurred. @@ -884,7 +885,7 @@ typedef void (*GDExtensionInterfacePrintWarning)(const char *p_description, cons * * Logs a warning with a message to Godot's built-in debugger and to the OS terminal. * - * @param p_description The code trigging the warning. + * @param p_description The code triggering the warning. * @param p_message The message to show along with the warning. * @param p_function The function name where the warning occurred. * @param p_file The file where the warning occurred. @@ -899,7 +900,7 @@ typedef void (*GDExtensionInterfacePrintWarningWithMessage)(const char *p_descri * * Logs a script error to Godot's built-in debugger and to the OS terminal. * - * @param p_description The code trigging the error. + * @param p_description The code triggering the error. * @param p_function The function name where the error occurred. * @param p_file The file where the error occurred. * @param p_line The line where the error occurred. @@ -913,7 +914,7 @@ typedef void (*GDExtensionInterfacePrintScriptError)(const char *p_description, * * Logs a script error with a message to Godot's built-in debugger and to the OS terminal. * - * @param p_description The code trigging the error. + * @param p_description The code triggering the error. * @param p_message The message to show along with the error. * @param p_function The function name where the error occurred. * @param p_file The file where the error occurred. @@ -1383,6 +1384,23 @@ typedef GDExtensionVariantFromTypeConstructorFunc (*GDExtensionInterfaceGetVaria */ typedef GDExtensionTypeFromVariantConstructorFunc (*GDExtensionInterfaceGetVariantToTypeConstructor)(GDExtensionVariantType p_type); +/** + * @name variant_get_ptr_internal_getter + * @since 4.4 + * + * Provides a function pointer for retrieving a pointer to a variant's internal value. + * Access to a variant's internal value can be used to modify it in-place, or to retrieve its value without the overhead of variant conversion functions. + * It is recommended to cache the getter for all variant types in a function table to avoid retrieval overhead upon use. + * + * @note Each function assumes the variant's type has already been determined and matches the function. + * Invoking the function with a variant of a mismatched type has undefined behavior, and may lead to a segmentation fault. + * + * @param p_type The Variant type. + * + * @return A pointer to a type-specific function that returns a pointer to the internal value of a variant. Check the implementation of this function (gdextension_variant_get_ptr_internal_getter) for pointee type info of each variant type. + */ +typedef GDExtensionVariantGetInternalPtrFunc (*GDExtensionInterfaceGetVariantGetInternalPtrFunc)(GDExtensionVariantType p_type); + /** * @name variant_get_ptr_operator_evaluator * @since 4.1 diff --git a/core/input/gamecontrollerdb.txt b/core/input/gamecontrollerdb.txt index 7150911e75b6..50a7db5efdea 100644 --- a/core/input/gamecontrollerdb.txt +++ b/core/input/gamecontrollerdb.txt @@ -3,6 +3,7 @@ # Windows 03000000300f00000a01000000000000,3 In 1 Conversion Box,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b8,x:b3,y:b0,platform:Windows, +03000000fa190000918d000000000000,3 In 1 Conversion Box,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b8,x:b3,y:b0,platform:Windows, 03000000fa2d00000100000000000000,3dRudder Foot Motion Controller,leftx:a0,lefty:a1,rightx:a5,righty:a2,platform:Windows, 03000000d0160000040d000000000000,4Play Adapter,a:b1,b:b3,back:b4,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,leftshoulder:b6,leftstick:b14,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b15,righttrigger:b9,rightx:a3,righty:a4,start:b5,x:b0,y:b2,platform:Windows, 03000000d0160000050d000000000000,4Play Adapter,a:b1,b:b3,back:b4,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,leftshoulder:b6,leftstick:b14,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b15,righttrigger:b9,rightx:a3,righty:a4,start:b5,x:b0,y:b2,platform:Windows, @@ -19,6 +20,7 @@ 03000000801000000900000000000000,8BitDo F30 Arcade Stick,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a2,righty:a5,start:b11,x:b3,y:b4,platform:Windows, 03000000c82d00001038000000000000,8BitDo F30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a5,start:b11,x:b4,y:b3,platform:Windows, 03000000c82d00000090000000000000,8BitDo FC30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b4,y:b3,platform:Windows, +05000000c82d00006a28000000000000,8BitDo GameCube,a:b0,b:b3,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b9,paddle2:b8,rightshoulder:b10,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b1,y:b4,platform:Windows, 03000000c82d00001251000000000000,8BitDo Lite 2,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b4,y:b3,platform:Windows, 03000000c82d00001151000000000000,8BitDo Lite SE,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b4,y:b3,platform:Windows, 03000000c82d00000150000000000000,8BitDo M30,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:b7,rightx:a3,righty:a5,start:b11,x:b4,y:b3,platform:Windows, @@ -72,6 +74,8 @@ 03000000c82d00000260000000000000,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Windows, 03000000c82d00000261000000000000,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Windows, 03000000c82d00001230000000000000,8BitDo Ultimate,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,paddle1:b2,paddle2:b5,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, +03000000c82d00001b30000000000000,8BitDo Ultimate 2C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,paddle1:b5,paddle2:b2,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, +03000000c82d00001d30000000000000,8BitDo Ultimate 2C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,paddle1:b5,paddle2:b2,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 03000000c82d00001530000000000000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 03000000c82d00001630000000000000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 03000000c82d00001730000000000000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, @@ -108,13 +112,14 @@ 03000000830500000160000000000000,Arcade,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,lefttrigger:b7,leftx:a0,lefty:a1,rightshoulder:b2,righttrigger:b3,x:b4,y:b4,platform:Windows, 03000000120c0000100e000000000000,Armor 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 03000000490b00004406000000000000,ASCII Seamic Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,lefttrigger:b7,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b2,start:b9,x:b3,y:b4,platform:Windows, -03000000869800002500000000000000,Astro C40 TR PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000869800002500000000000000,Astro C40 TR PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000a30c00002700000000000000,Astro City Mini,a:b2,b:b1,back:b8,dpdown:+a4,dpleft:-a3,dpright:+a3,dpup:-a4,rightshoulder:b4,righttrigger:b5,start:b9,x:b3,y:b0,platform:Windows, 03000000a30c00002800000000000000,Astro City Mini,a:b2,b:b1,back:b8,leftx:a3,lefty:a4,rightshoulder:b4,righttrigger:b5,start:b9,x:b3,y:b0,platform:Windows, 03000000050b00000579000000000000,ASUS ROG Kunai 3,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 03000000050b00000679000000000000,ASUS ROG Kunai 3,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,misc1:b15,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 03000000503200000110000000000000,Atari VCS Classic Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,start:b3,platform:Windows, 03000000503200000210000000000000,Atari VCS Modern Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,rightx:a3,righty:a4,start:b9,x:b2,y:b3,platform:Windows, +03000000380800001889000000000000,AtGames Legends Gamer Pro,a:b1,b:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b13,lefttrigger:b14,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, 030000008a3500000102000000000000,Backbone One,a:b4,b:b5,back:b14,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b16,leftshoulder:b10,leftstick:b17,lefttrigger:b12,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b18,righttrigger:b13,rightx:a3,righty:a4,start:b15,x:b7,y:b8,platform:Windows, 030000008a3500000201000000000000,Backbone One,a:b4,b:b5,back:b14,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b16,leftshoulder:b10,leftstick:b17,lefttrigger:b12,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b18,righttrigger:b13,rightx:a3,righty:a4,start:b15,x:b7,y:b8,platform:Windows, 030000008a3500000302000000000000,Backbone One,a:b4,b:b5,back:b14,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b16,leftshoulder:b10,leftstick:b17,lefttrigger:b12,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b18,righttrigger:b13,rightx:a3,righty:a4,start:b15,x:b7,y:b8,platform:Windows, @@ -124,7 +129,7 @@ 03000000c01100001352000000000000,Battalife Joystick,a:b6,b:b7,back:b2,leftshoulder:b0,leftx:a0,lefty:a1,rightshoulder:b1,start:b3,x:b4,y:b5,platform:Windows, 030000006f0e00003201000000000000,Battlefield 4 PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000ad1b000001f9000000000000,BB 070,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, -03000000d62000002a79000000000000,BDA PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000d62000002a79000000000000,BDA PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000bc2000005250000000000000,Beitong G3,a:b0,b:b1,back:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b11,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b5,righttrigger:b9,rightx:a3,righty:a4,start:b15,x:b3,y:b4,platform:Windows, 030000000d0500000208000000000000,Belkin Nostromo N40,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:b7,rightx:a5,righty:a2,start:b9,x:b2,y:b3,platform:Windows, 03000000bc2000006012000000000000,Betop 2126F,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Windows, @@ -141,8 +146,8 @@ 030000006b1400000209000000000000,Bigben,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 030000006b1400000055000000000000,Bigben PS3 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Windows, 030000006b1400000103000000000000,Bigben PS3 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b2,platform:Windows, -03000000120c0000200e000000000000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000210e000000000000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, +03000000120c0000200e000000000000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000210e000000000000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000120c0000f10e000000000000,Brook PS2 Adapter,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 03000000120c0000310c000000000000,Brook Super Converter,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,start:b9,x:b3,y:b0,platform:Windows, 03000000d81d00000b00000000000000,Buffalo BSGP1601 Series,a:b5,b:b3,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b8,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b9,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b13,x:b4,y:b2,platform:Windows, @@ -224,7 +229,7 @@ 030000004c0e00001035000000000000,Gamester,a:b0,b:b1,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b11,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Windows, 030000000d0f00001110000000000000,GameStick Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,rightx:a2,righty:a5,start:b11,x:b3,y:b4,platform:Windows, 0300000047530000616d000000000000,GameStop,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Windows, -03000000c01100000140000000000000,GameStop PS4 Fun Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000c01100000140000000000000,GameStop PS4 Fun Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000b62500000100000000000000,Gametel GT004 01,a:b3,b:b0,dpdown:b10,dpleft:b9,dpright:b8,dpup:b11,leftshoulder:b4,rightshoulder:b5,start:b7,x:b1,y:b2,platform:Windows, 030000008f0e00001411000000000000,Gamo2 Divaller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000120c0000a857000000000000,Gator Claw,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, @@ -258,7 +263,7 @@ 030000000d0f00002500000000000000,Hori Fighting Commander 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00002d00000000000000,Hori Fighting Commander 3 Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00005f00000000000000,Hori Fighting Commander 4 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -030000000d0f00005e00000000000000,Hori Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +030000000d0f00005e00000000000000,Hori Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000000d0f00008400000000000000,Hori Fighting Commander 5,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00006201000000000000,Hori Fighting Commander Octa,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00006401000000000000,Hori Fighting Commander Octa,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:+a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:-a2,start:b7,x:b2,y:b3,platform:Windows, @@ -266,7 +271,7 @@ 030000000d0f00008600000000000000,Hori Fighting Commander Xbox 360,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 030000000d0f0000ba00000000000000,Hori Fighting Commander Xbox 360,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 030000000d0f00008800000000000000,Hori Fighting Stick mini 4 (PS3),a:b1,b:b2,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b8,x:b0,y:b3,platform:Windows, -030000000d0f00008700000000000000,Hori Fighting Stick mini 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, +030000000d0f00008700000000000000,Hori Fighting Stick mini 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000000d0f00001000000000000000,Hori Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00003200000000000000,Hori Fightstick 3W,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f0000c000000000000000,Hori Fightstick 4,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, @@ -276,8 +281,8 @@ 030000000d0f00002100000000000000,Hori Fightstick V3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00002700000000000000,Hori Fightstick V3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f0000a000000000000000,Hori Grip TAC4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b13,x:b0,y:b3,platform:Windows, -030000000d0f0000a500000000000000,Hori Miku Project Diva X HD PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -030000000d0f0000a600000000000000,Hori Miku Project Diva X HD PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, +030000000d0f0000a500000000000000,Hori Miku Project Diva X HD PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +030000000d0f0000a600000000000000,Hori Miku Project Diva X HD PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000000d0f00000101000000000000,Hori Mini Hatsune Miku FT,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00005400000000000000,Hori Pad 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00000900000000000000,Hori Pad 3 Turbo,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, @@ -311,18 +316,20 @@ 030000000d0f00001300000000000000,Horipad 3W,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00005500000000000000,Horipad 4 FPS,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f00006e00000000000000,Horipad 4 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -030000000d0f00006600000000000000,Horipad 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +030000000d0f00006600000000000000,Horipad 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000000d0f00004200000000000000,Horipad A,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 03000000ad1b000001f5000000000000,Horipad EXT2,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b11,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 030000000d0f0000ee00000000000000,Horipad Mini 4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f0000c100000000000000,Horipad Nintendo Switch Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 030000000d0f0000f600000000000000,Horipad Nintendo Switch Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Windows, 030000000d0f00006700000000000000,Horipad One,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, +030000000d0f00009601000000000000,Horipad Steam,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,misc1:b2,paddle1:b15,paddle2:b5,paddle3:b19,paddle4:b18,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 030000000d0f0000dc00000000000000,Horipad Switch,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 03000000242e00000b20000000000000,Hyperkin Admiral N64 Controller,+rightx:b11,+righty:b13,-rightx:b8,-righty:b12,a:b1,b:b0,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b14,leftx:a0,lefty:a1,rightshoulder:b5,start:b9,platform:Windows, 03000000242e0000ff0b000000000000,Hyperkin N64 Adapter,a:b1,b:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightx:a2,righty:a3,start:b9,platform:Windows, 03000000790000004e95000000000000,Hyperkin N64 Controller Adapter,a:b1,b:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b7,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b5,rightx:a5,righty:a2,start:b9,platform:Windows, 03000000242e00006a48000000000000,Hyperkin RetroN Sq,a:b3,b:b7,back:b5,dpdown:+a4,dpleft:-a0,dpright:+a0,dpup:-a4,leftshoulder:b0,rightshoulder:b1,start:b4,x:b2,y:b6,platform:Windows, +03000000242f00000a20000000000000,Hyperkin Scout,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,rightshoulder:b5,start:b9,x:b2,y:b3,platform:Windows, 03000000242e00006a38000000000000,Hyperkin Trooper 2,a:b0,b:b1,back:b4,leftshoulder:b2,leftx:a0,lefty:a1,rightshoulder:b3,start:b5,platform:Windows, 03000000d81d00000e00000000000000,iBuffalo AC02 Arcade Joystick,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b7,leftx:a0,lefty:a1,rightshoulder:b2,rightstick:b11,righttrigger:b3,rightx:a2,righty:a5,start:b8,x:b4,y:b5,platform:Windows, 03000000d81d00000f00000000000000,iBuffalo BSGP1204 Series,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Windows, @@ -377,22 +384,22 @@ 03000000380700006352000000000000,Mad Catz CTRLR,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 03000000380700006652000000000000,Mad Catz CTRLR,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b0,y:b3,platform:Windows, 03000000380700005032000000000000,Mad Catz Fightpad Pro PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -03000000380700005082000000000000,Mad Catz Fightpad Pro PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000380700005082000000000000,Mad Catz Fightpad Pro PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000380700008031000000000000,Mad Catz FightStick Alpha PS3 ,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000003807000038b7000000000000,Mad Catz Fightstick TE,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b8,rightshoulder:b5,righttrigger:b9,start:b7,x:b2,y:b3,platform:Windows, 03000000380700008433000000000000,Mad Catz Fightstick TE S PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -03000000380700008483000000000000,Mad Catz Fightstick TE S PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000380700008483000000000000,Mad Catz Fightstick TE S PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000380700008134000000000000,Mad Catz Fightstick TE2 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b7,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b4,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -03000000380700008184000000000000,Mad Catz Fightstick TE2 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b5,leftstick:b10,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000380700008184000000000000,Mad Catz Fightstick TE2 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b5,leftstick:b10,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000380700006252000000000000,Mad Catz Micro CTRLR,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b0,y:b3,platform:Windows, 03000000380700008232000000000000,Mad Catz PlayStation Brawlpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000380700008731000000000000,Mad Catz PlayStation Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 030000003807000056a8000000000000,Mad Catz PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000380700001888000000000000,Mad Catz SFIV Fightstick PS3,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b5,lefttrigger:b7,leftx:a0,lefty:a1,rightshoulder:b4,righttrigger:b6,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Windows, -03000000380700008081000000000000,Mad Catz SFV Arcade Fightstick Alpha PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000380700008081000000000000,Mad Catz SFV Arcade Fightstick Alpha PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000380700001847000000000000,Mad Catz Street Fighter 4 Xbox 360 FightStick,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b8,rightshoulder:b5,righttrigger:b9,start:b7,x:b2,y:b3,platform:Windows, 03000000380700008034000000000000,Mad Catz TE2 PS3 Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -03000000380700008084000000000000,Mad Catz TE2 PS4 Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000380700008084000000000000,Mad Catz TE2 PS4 Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000002a0600001024000000000000,Matricom,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b2,y:b3,platform:Windows, 030000009f000000adbb000000000000,MaxJoypad Virtual Controller,a:b1,b:b2,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b8,x:b3,y:b0,platform:Windows, 03000000250900000128000000000000,Mayflash Arcade Stick,a:b1,b:b2,back:b8,leftshoulder:b0,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b3,righttrigger:b7,start:b9,x:b5,y:b6,platform:Windows, @@ -405,6 +412,7 @@ 03000000242f00007300000000000000,Mayflash Magic NS,a:b1,b:b4,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b0,y:b3,platform:Windows, 0300000079000000d218000000000000,Mayflash Magic NS,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Windows, 03000000d620000010a7000000000000,Mayflash Magic NS,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, +03000000242e0000f500000000000000,Mayflash N64 Adapter,a:b2,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightx:a2,righty:a5,start:b9,platform:Windows, 03000000242f0000f400000000000000,Mayflash N64 Controller Adapter,a:b1,b:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightx:a2,righty:a5,start:b9,platform:Windows, 03000000790000007918000000000000,Mayflash N64 Controller Adapter,a:b1,b:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,lefttrigger:b9,leftx:a0,lefty:a1,righttrigger:b7,rightx:a3,righty:a2,start:b8,platform:Windows, 030000008f0e00001030000000000000,Mayflash Saturn Adapter,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b5,lefttrigger:b7,rightshoulder:b6,righttrigger:b2,start:b9,x:b3,y:b4,platform:Windows, @@ -436,11 +444,12 @@ 03000000250900006688000000000000,MP-8866 Super Dual Box,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,platform:Windows, 03000000091200004488000000000000,MUSIA PlayStation 2 Input Display,a:b0,b:b2,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b8,leftstick:b6,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b9,rightstick:b7,righttrigger:b11,rightx:a2,righty:a3,start:b5,x:b1,y:b3,platform:Windows, 03000000f70600000100000000000000,N64 Adaptoid,+rightx:b2,+righty:b1,-rightx:b4,-righty:b5,a:b0,b:b3,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,leftshoulder:b6,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b7,start:b8,platform:Windows, +030000006f0e00001311000000000000,N64 Controller,+rightx:b10,+righty:b3,-rightx:b0,-righty:b11,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b9,platform:Windows, 030000006b140000010c000000000000,Nacon GC 400ES,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Windows, -030000006b1400001106000000000000,Nacon Revolution 3 PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +030000006b1400001106000000000000,Nacon Revolution 3 PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 0300000085320000170d000000000000,Nacon Revolution 5 Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 0300000085320000190d000000000000,Nacon Revolution 5 Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, -030000006b140000100d000000000000,Nacon Revolution Infinity PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, +030000006b140000100d000000000000,Nacon Revolution Infinity PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000006b140000080d000000000000,Nacon Revolution Unlimited Pro Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000bd12000001c0000000000000,Nebular,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a5,righty:a2,start:b9,x:b3,y:b0,platform:Windows, 03000000eb0300000000000000000000,NeGcon Adapter,a:a2,b:b13,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,lefttrigger:a4,leftx:a1,righttrigger:b11,start:b3,x:a3,y:b12,platform:Windows, @@ -475,6 +484,7 @@ 03000000790000002201000000000000,PC Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Windows, 030000006f0e00008501000000000000,PDP Fightpad Pro GameCube Controller,a:b1,b:b0,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Windows, 030000006f0e00000901000000000000,PDP PS3 Versus Fighting,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, +030000006f0e00008901000000000000,PDP Realmz Nintendo Switch Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 030000008f0e00004100000000000000,PlaySega,a:b1,b:b0,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,lefttrigger:b7,rightshoulder:b5,righttrigger:b2,start:b8,x:b4,y:b3,platform:Windows, 03000000666600006706000000000000,PlayStation Adapter,a:b2,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,leftshoulder:b6,leftstick:b9,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b10,righttrigger:b5,rightx:a2,righty:a3,start:b11,x:b3,y:b0,platform:Windows, 03000000e30500009605000000000000,PlayStation Adapter,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,platform:Windows, @@ -483,11 +493,13 @@ 03000000f0250000c183000000000000,PlayStation Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000d9040000160f000000000000,PlayStation Controller Adapter,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b3,y:b0,platform:Windows, 030000004c0500003713000000000000,PlayStation Vita,a:b1,b:b2,back:b8,dpdown:b13,dpleft:b15,dpright:b14,dpup:b12,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b5,rightx:a3,righty:a4,start:b9,x:b0,y:b3,platform:Windows, -03000000d620000011a7000000000000,PowerA Core Plus GameCube Controller,a:b1,b:b0,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Windows, +03000000d620000011a7000000000000,PowerA Core Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000dd62000015a7000000000000,PowerA Fusion Nintendo Switch Arcade Stick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000d620000012a7000000000000,PowerA Fusion Nintendo Switch Fight Pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000dd62000016a7000000000000,PowerA Fusion Pro Nintendo Switch Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000d620000013a7000000000000,PowerA Nintendo Switch Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, +03000000d62000002640000000000000,PowerA OPS Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, +03000000d62000003340000000000000,PowerA OPS Pro Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 03000000d62000006dca000000000000,PowerA Pro Ex,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 0300000062060000d570000000000000,PowerA PS3 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000d620000014a7000000000000,PowerA Spectra Nintendo Switch Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, @@ -521,28 +533,28 @@ 030000008f0e00000300000000000000,PS3 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b3,y:b0,platform:Windows, 030000008f0e00001431000000000000,PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000ba2200002010000000000000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a5,righty:a2,start:b9,x:b3,y:b2,platform:Windows, -03000000120c00000807000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000111e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000121e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000130e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000150e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000180e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000181e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000191e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c00001e0e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000a957000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000aa57000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000f21c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000f31c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000f41c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000f51c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120c0000f70e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000120e0000120c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000160e0000120c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -030000001a1e0000120c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -030000004c050000a00b000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000120c00000807000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000111e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000121e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000130e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000150e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000180e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000181e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000191e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c00001e0e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000a957000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000aa57000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000f21c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000f31c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000f41c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000f51c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120c0000f70e000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000120e0000120c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +03000000160e0000120c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +030000001a1e0000120c000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +030000004c050000a00b000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000004c050000c405000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, -030000004c050000cc09000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +030000004c050000cc09000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000004c0500005f0e000000000000,PS5 Access Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b14,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000004c050000e60c000000000000,PS5 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b14,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000004c050000f20d000000000000,PS5 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b14,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, @@ -557,7 +569,7 @@ 03000000300f00001210000000000000,Qanba Joystick Plus,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,rightshoulder:b5,start:b9,x:b2,y:b3,platform:Windows, 03000000341a00000104000000000000,Qanba Joystick Q4RAF,a:b5,b:b6,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b0,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b3,righttrigger:b7,start:b9,x:b1,y:b2,platform:Windows, 03000000222c00000223000000000000,Qanba Obsidian Arcade Stick PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -03000000222c00000023000000000000,Qanba Obsidian Arcade Stick PS4,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000222c00000023000000000000,Qanba Obsidian Arcade Stick PS4,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 030000008a2400006682000000000000,R1 Mobile Controller,a:b3,b:b1,back:b7,leftx:a0,lefty:a1,start:b6,x:b4,y:b0,platform:Windows, 03000000086700006626000000000000,RadioShack,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b3,y:b0,platform:Windows, 03000000ff1100004733000000000000,Ramox FPS Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b9,x:b3,y:b0,platform:Windows, @@ -585,10 +597,11 @@ 030000009b2800002b00000000000000,Raphnet Wii Classic Adapter,a:b1,b:b4,back:b2,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,guide:b10,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a3,righty:a4,start:b3,x:b0,y:b5,platform:Windows, 030000009b2800002c00000000000000,Raphnet Wii Classic Adapter,a:b1,b:b4,back:b2,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,guide:b10,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a3,righty:a4,start:b3,x:b0,y:b5,platform:Windows, 030000009b2800008000000000000000,Raphnet Wii Classic Adapter,a:b1,b:b4,back:b2,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,guide:b10,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a3,righty:a4,start:b3,x:b0,y:b5,platform:Windows, +03000000790000008f18000000000000,Rapoo Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b3,y:b0,platform:Windows, 03000000321500000003000000000000,Razer Hydra,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a2,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 03000000f8270000bf0b000000000000,Razer Kishi,a:b6,b:b7,back:b16,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b18,leftshoulder:b12,leftstick:b19,lefttrigger:b14,leftx:a0,lefty:a1,rightshoulder:b13,rightstick:b20,righttrigger:b15,rightx:a3,righty:a4,start:b17,x:b9,y:b10,platform:Windows, 03000000321500000204000000000000,Razer Panthera PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, -03000000321500000104000000000000,Razer Panthera PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000321500000104000000000000,Razer Panthera PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000321500000010000000000000,Razer Raiju,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 03000000321500000507000000000000,Razer Raiju Mobile,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 03000000321500000707000000000000,Razer Raiju Mobile,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, @@ -596,7 +609,7 @@ 03000000321500000a10000000000000,Razer Raiju TE,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000321500000410000000000000,Razer Raiju UE,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 03000000321500000910000000000000,Razer Raiju UE,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000321500000011000000000000,Razer Raion PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000321500000011000000000000,Razer Raion PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000321500000009000000000000,Razer Serval,+lefty:+a2,-lefty:-a1,a:b0,b:b1,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,leftx:a0,rightshoulder:b5,rightstick:b9,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 03000000921200004547000000000000,Retro Bit Sega Genesis Controller Adapter,a:b0,b:b1,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,lefttrigger:b7,rightshoulder:b5,righttrigger:b2,start:b6,x:b3,y:b4,platform:Windows, 03000000790000001100000000000000,Retro Controller,a:b1,b:b2,back:b8,dpdown:+a4,dpleft:-a3,dpright:+a3,dpup:-a4,leftshoulder:b6,lefttrigger:b7,rightshoulder:b4,righttrigger:b5,start:b9,x:b0,y:b3,platform:Windows, @@ -653,7 +666,7 @@ 03000000830500006120000000000000,Sanwa Smart Grip II,a:b0,b:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,x:b1,y:b3,platform:Windows, 03000000c01100000051000000000000,Satechi Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a5,start:b11,x:b3,y:b4,platform:Windows, 030000004f04000028b3000000000000,Score A,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, -03000000952e00002577000000000000,Scuf PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000952e00002577000000000000,Scuf PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000a30c00002500000000000000,Sega Genesis Mini 3B Controller,a:b2,b:b1,dpdown:+a4,dpleft:-a3,dpright:+a3,dpup:-a4,righttrigger:b5,start:b9,platform:Windows, 03000000a30c00002400000000000000,Sega Mega Drive Mini 6B Controller,a:b2,b:b1,dpdown:+a4,dpleft:-a3,dpright:+a3,dpup:-a4,rightshoulder:b4,righttrigger:b5,start:b9,x:b3,y:b0,platform:Windows, 03000000d804000086e6000000000000,Sega Multi Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b7,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b2,start:b8,x:b3,y:b4,platform:Windows, @@ -663,7 +676,7 @@ 03000000b40400000a01000000000000,Sega Saturn Controller,a:b0,b:b1,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,lefttrigger:b7,rightshoulder:b5,righttrigger:b2,start:b8,x:b3,y:b4,platform:Windows, 030000003b07000004a1000000000000,SFX,a:b0,b:b2,back:b7,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b5,start:b8,x:b1,y:b3,platform:Windows, 03000000f82100001900000000000000,Shogun Bros Chameleon X1,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,platform:Windows, -03000000120c00001c1e000000000000,SnakeByte 4S PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000120c00001c1e000000000000,SnakeByte 4S PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000140300000918000000000000,SNES Controller,a:b0,b:b1,back:b6,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b7,x:b2,y:b3,platform:Windows, 0300000081170000960a000000000000,SNES Controller,a:b4,b:b0,back:b2,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b3,x:b5,y:b1,platform:Windows, 03000000811700009d0a000000000000,SNES Controller,a:b0,b:b4,back:b2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b3,x:b1,y:b5,platform:Windows, @@ -678,7 +691,7 @@ 03000000d11800000094000000000000,Stadia Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:b12,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:b11,rightx:a3,righty:a4,start:b9,x:b2,y:b3,platform:Windows, 03000000de280000fc11000000000000,Steam Virtual Gamepad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 03000000de280000ff11000000000000,Steam Virtual Gamepad,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:+a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:-a2,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, -03000000120c0000160e000000000000,Steel Play Metaltech PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, +03000000120c0000160e000000000000,Steel Play Metaltech PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 03000000110100001914000000000000,SteelSeries,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftstick:b13,lefttrigger:b6,leftx:a0,lefty:a1,rightstick:b14,righttrigger:b7,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, 03000000381000001214000000000000,SteelSeries Free,a:b0,b:b1,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Windows, 03000000110100003114000000000000,SteelSeries Stratus Duo,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a3,righty:a4,start:b11,x:b3,y:b4,platform:Windows, @@ -738,10 +751,11 @@ 03000000bd12000012d0000000000000,USB Controller,a:b0,b:b1,back:b6,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b5,start:b7,x:b2,y:b3,platform:Windows, 03000000ff1100004133000000000000,USB Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a4,righty:a2,start:b9,x:b3,y:b0,platform:Windows, 03000000632500002305000000000000,USB Vibration Joystick,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Windows, +03000000882800000305000000000000,V5 Game Pad,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,x:b2,y:b3,platform:Windows, 03000000790000001a18000000000000,Venom,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Windows, 03000000790000001b18000000000000,Venom Arcade Joystick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, -030000006f0e00000302000000000000,Victrix PS4 Pro Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, -030000006f0e00000702000000000000,Victrix PS4 Pro Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Windows, +030000006f0e00000302000000000000,Victrix PS4 Pro Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, +030000006f0e00000702000000000000,Victrix PS4 Pro Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,touchpad:b13,x:b0,y:b3,platform:Windows, 0300000034120000adbe000000000000,vJoy Device,a:b0,b:b1,back:b15,dpdown:b6,dpleft:b7,dpright:b8,dpup:b5,guide:b16,leftshoulder:b9,leftstick:b13,lefttrigger:b11,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b14,righttrigger:b12,rightx:a3,righty:a4,start:b4,x:b2,y:b3,platform:Windows, 03000000120c0000ab57000000000000,Warrior Joypad JS083,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Windows, 030000007e0500003003000000000000,Wii U Pro,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,leftshoulder:b6,leftstick:b11,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b12,righttrigger:b5,rightx:a2,righty:a3,start:b9,x:b3,y:b2,platform:Windows, @@ -769,7 +783,6 @@ 03000000120c00000a88000000000000,Xbox Controller,a:b0,b:b1,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b11,rightx:a2,righty:a4,start:b6,x:b2,y:b3,platform:Windows, 03000000120c00001088000000000000,Xbox Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:a2~,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5~,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 030000002a0600002000000000000000,Xbox Controller,a:b0,b:b1,back:b13,dpdown:b9,dpleft:b10,dpright:b11,dpup:b8,leftshoulder:b5,leftstick:b14,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b15,righttrigger:b7,rightx:a2,righty:a5,start:b12,x:b2,y:b3,platform:Windows, -03000000300f00008888000000000000,Xbox Controller,a:b0,b:b1,back:b7,dpdown:b13,dpleft:b10,dpright:b11,dpup:b12,leftshoulder:b5,leftstick:b8,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b9,righttrigger:b11,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Windows, 03000000380700001645000000000000,Xbox Controller,a:b0,b:b1,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b5,leftstick:b8,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b9,righttrigger:b11,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Windows, 03000000380700002645000000000000,Xbox Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b11,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 03000000380700003645000000000000,Xbox Controller,a:b0,b:b1,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b5,leftstick:b8,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b9,righttrigger:b11,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Windows, @@ -778,7 +791,6 @@ 030000005e0400008502000000000000,Xbox Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 030000005e0400008702000000000000,Xbox Controller,a:b0,b:b1,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b5,leftstick:b8,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b9,righttrigger:b7,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Windows, 030000005e0400008902000000000000,Xbox Controller,a:b0,b:b1,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b10,leftstick:b8,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b9,righttrigger:b4,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Windows, -030000000d0f00006300000000000000,Xbox One Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b9,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 030000005e0400000c0b000000000000,Xbox One Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 030000005e040000d102000000000000,Xbox One Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, 030000005e040000dd02000000000000,Xbox One Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Windows, @@ -812,6 +824,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000c82d00000951000000010000,8BitDo Dogbone,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b11,platform:Mac OS X, 03000000c82d00000090000001000000,8BitDo FC30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a5,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Mac OS X, 03000000c82d00001038000000010000,8BitDo FC30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Mac OS X, +03000000c82d00006a28000000010000,8BitDo GameCube,a:b0,b:b3,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b9,paddle2:b8,rightshoulder:b10,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b1,y:b4,platform:Mac OS X, 03000000c82d00001251000000010000,8BitDo Lite 2,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Mac OS X, 03000000c82d00001251000000020000,8BitDo Lite 2,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Mac OS X, 03000000c82d00001151000000010000,8BitDo Lite SE,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Mac OS X, @@ -853,6 +866,8 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000c82d00000260000001000000,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Mac OS X, 03000000c82d00000261000000010000,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Mac OS X, 03000000c82d00001230000000010000,8BitDo Ultimate,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b2,paddle2:b5,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, +03000000c82d00001b30000001000000,8BitDo Ultimate 2C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b5,paddle2:b2,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, +03000000c82d00001d30000001000000,8BitDo Ultimate 2C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b5,paddle2:b2,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 03000000c82d00001530000001000000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 03000000c82d00001630000001000000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 03000000c82d00001730000001000000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, @@ -871,18 +886,19 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000050b00000045000031000000,ASUS Gamepad,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Mac OS X, 03000000050b00000579000000010000,ASUS ROG Kunai 3,a:b0,b:b1,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b14,leftshoulder:b6,leftstick:b15,lefttrigger:a5,leftx:a0,lefty:a1,misc1:b42,paddle1:b9,paddle2:b11,rightshoulder:b7,rightstick:b16,righttrigger:a4,rightx:a2,righty:a3,start:b13,x:b3,y:b4,platform:Mac OS X, 03000000050b00000679000000010000,ASUS ROG Kunai 3,a:b0,b:b1,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b14,leftshoulder:b6,leftstick:b15,lefttrigger:a5,leftx:a0,lefty:a1,misc1:b23,rightshoulder:b7,rightstick:b16,righttrigger:a4,rightx:a2,righty:a3,start:b13,x:b3,y:b4,platform:Mac OS X, +03000000503200000110000045010000,Atari VCS Classic,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b3,start:b2,platform:Mac OS X, 03000000503200000110000047010000,Atari VCS Classic Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b3,start:b2,platform:Mac OS X, 03000000503200000210000047010000,Atari VCS Modern Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b9,leftshoulder:b4,leftstick:b6,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a4,rightx:a2,righty:a3,start:b8,x:b2,y:b3,platform:Mac OS X, 030000008a3500000102000000010000,Backbone One,a:b0,b:b1,back:b16,dpdown:b11,dpleft:b13,dpright:b12,dpup:b10,guide:b17,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a2,righty:a3~,start:b15,x:b2,y:b3,platform:Mac OS X, 030000008a3500000201000000010000,Backbone One,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 030000008a3500000202000000010000,Backbone One,a:b0,b:b1,back:b16,dpdown:b11,dpleft:b13,dpright:b12,dpup:b10,guide:b17,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a2,righty:a3~,start:b15,x:b2,y:b3,platform:Mac OS X, 030000008a3500000402000000010000,Backbone One,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, -030000008a3500000302000000010000,Backbone One PlayStationÆ Edition,a:b0,b:b1,back:b16,dpdown:b11,dpleft:b13,dpright:b12,dpup:b10,guide:b17,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a2,righty:a3~,start:b15,x:b2,y:b3,platform:Mac OS X, +030000008a3500000302000000010000,Backbone One PlayStation Edition,a:b0,b:b1,back:b16,dpdown:b11,dpleft:b13,dpright:b12,dpup:b10,guide:b17,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a2,righty:a3~,start:b15,x:b2,y:b3,platform:Mac OS X, 03000000c62400001a89000000010000,BDA MOGA XP5-X Plus,a:b0,b:b1,back:b12,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b14,leftshoulder:b6,leftstick:b15,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b16,righttrigger:a4,rightx:a2,righty:a3,start:b13,x:b3,y:b4,platform:Mac OS X, 03000000c62400001b89000000010000,BDA MOGA XP5-X Plus,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, -03000000d62000002a79000000010000,BDA PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, -03000000120c0000200e000000010000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, -03000000120c0000210e000000010000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, +03000000d62000002a79000000010000,BDA PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +03000000120c0000200e000000010000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +03000000120c0000210e000000010000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 030000008305000031b0000000000000,Cideko AK08b,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, 03000000d8140000cecf000000000000,Cthulhu,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Mac OS X, 03000000260900008888000088020000,Cyber Gadget GameCube Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:a5,rightx:a2,righty:a3~,start:b7,x:b2,y:b3,platform:Mac OS X, @@ -897,7 +913,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000ac0500001a06000002020000,GameSir-T3 2.02,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b15,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 03000000ad1b000001f9000000000000,Gamestop BB070 X360 Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, 0500000047532047616d657061640000,GameStop Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Mac OS X, -03000000c01100000140000000010000,GameStop PS4 Fun Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, +03000000c01100000140000000010000,GameStop PS4 Fun Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 030000006f0e00000102000000000000,GameStop Xbox 360 Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, 03000000ff1100003133000007010000,GameWare PC Control Pad,a:b2,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b8,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:b7,rightx:a3,righty:a4,start:b11,x:b3,y:b0,platform:Mac OS X, 030000007d0400000540000001010000,Gravis Eliminator Pro,a:b1,b:b2,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Mac OS X, @@ -906,13 +922,13 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000000d0f00002d00000000100000,Hori Fighting Commander 3 Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f00005f00000000000000,Hori Fighting Commander 4 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f00005f00000000010000,Hori Fighting Commander 4 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, -030000000d0f00005e00000000000000,Hori Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, -030000000d0f00005e00000000010000,Hori Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, +030000000d0f00005e00000000000000,Hori Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +030000000d0f00005e00000000010000,Hori Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 030000000d0f00008400000000010000,Hori Fighting Commander PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f00008500000000010000,Hori Fighting Commander PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, 03000000341a00000302000014010000,Hori Fighting Stick Mini,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f00008800000000010000,Hori Fighting Stick mini 4 (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,rightstick:b11,righttrigger:b7,start:b9,x:b0,y:b3,platform:Mac OS X, -030000000d0f00008700000000010000,Hori Fighting Stick mini 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,rightstick:b11,righttrigger:b7,start:b9,x:b0,y:b3,platform:Mac OS X, +030000000d0f00008700000000010000,Hori Fighting Stick mini 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,rightstick:b11,righttrigger:b7,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 030000000d0f00004d00000000000000,Hori Gem Pad 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f00003801000008010000,Hori PC Engine Mini Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,start:b9,platform:Mac OS X, 030000000d0f00009200000000010000,Hori Pokken Tournament DX Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Mac OS X, @@ -920,7 +936,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000000d0f00000002000017010000,Hori Split Pad Fit,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f00000002000015010000,Hori Switch Split Pad Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f00006e00000000010000,Horipad 4 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, -030000000d0f00006600000000010000,Horipad 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, +030000000d0f00006600000000010000,Horipad 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 030000000d0f00006600000000000000,Horipad FPS Plus 4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f0000ee00000000010000,Horipad Mini 4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, 030000000d0f0000c100000072050000,Horipad Nintendo Switch Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, @@ -943,8 +959,8 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000006d04000018c2000000010000,Logitech RumblePad 2,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3~,start:b9,x:b0,y:b3,platform:Mac OS X, 03000000380700005032000000010000,Mad Catz PS3 Fightpad Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, 03000000380700008433000000010000,Mad Catz PS3 Fightstick TE S Plus,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, -03000000380700005082000000010000,Mad Catz PS4 Fightpad Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, -03000000380700008483000000010000,Mad Catz PS4 Fightstick TE S Plus,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, +03000000380700005082000000010000,Mad Catz PS4 Fightpad Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +03000000380700008483000000010000,Mad Catz PS4 Fightstick TE S Plus,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 03000000790000000600000007010000,Marvo GT-004,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Mac OS X, 030000008f0e00001330000011010000,Mayflash Controller Adapter,a:b2,b:b4,back:b16,dpdown:h0.8,dpleft:h0.2,dpright:h0.1,dpup:h0.4,leftshoulder:b12,lefttrigger:b16,leftx:a0,lefty:a2,rightshoulder:b14,rightx:a6~,righty:a4,start:b18,x:b0,y:b6,platform:Mac OS X, 03000000790000004318000000010000,Mayflash GameCube Adapter,a:b4,b:b0,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,lefttrigger:a12,leftx:a0,lefty:a4,rightshoulder:b28,righttrigger:a16,rightx:a20,righty:a8,start:b36,x:b8,y:b12,platform:Mac OS X, @@ -988,10 +1004,10 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000004c0500006802000000000000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,platform:Mac OS X, 030000004c0500006802000000010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,platform:Mac OS X, 030000004c0500006802000072050000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,platform:Mac OS X, -030000004c050000a00b000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, -030000004c050000c405000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, -030000004c050000c405000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, -030000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, +030000004c050000a00b000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +030000004c050000c405000000000000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +030000004c050000c405000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +030000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 0300004b4c0500005f0e000000010000,PS5 Access Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b14,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 030000004c050000e60c000000010000,PS5 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b14,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 030000004c050000f20d000000010000,PS5 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b14,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, @@ -1004,10 +1020,10 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000009b2800008000000022020000,Raphnet Wii Classic Adapter,a:b1,b:b4,back:b2,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,guide:b10,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a3,righty:a4,start:b3,x:b0,y:b5,platform:Mac OS X, 030000008916000000fd000000000000,Razer Onza TE,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, 03000000321500000204000000010000,Razer Panthera PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Mac OS X, -03000000321500000104000000010000,Razer Panthera PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, +03000000321500000104000000010000,Razer Panthera PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 03000000321500000010000000010000,Razer Raiju,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, 03000000321500000507000001010000,Razer Raiju Mobile,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b21,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, -03000000321500000011000000010000,Razer Raion PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, +03000000321500000011000000010000,Razer Raion PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 03000000321500000009000000020000,Razer Serval,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a4,rightx:a2,righty:a3,start:b7,x:b2,y:b3,platform:Mac OS X, 030000003215000000090000163a0000,Razer Serval,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a4,rightx:a2,righty:a3,start:b7,x:b2,y:b3,platform:Mac OS X, 0300000032150000030a000000000000,Razer Wildcat,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, @@ -1031,8 +1047,8 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000b40400000a01000000000000,Sega Saturn,a:b0,b:b1,back:b5,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,guide:b2,leftshoulder:b6,rightshoulder:b7,start:b8,x:b3,y:b4,platform:Mac OS X, 030000003512000021ab000000000000,SFC30 Joystick,a:b1,b:b0,back:b10,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b11,x:b4,y:b3,platform:Mac OS X, 0300000000f00000f100000000000000,SNES RetroPort,a:b2,b:b3,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b5,rightshoulder:b7,start:b6,x:b0,y:b1,platform:Mac OS X, -030000004c050000a00b000000000000,Sony DualShock 4 Adapter,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, -030000004c050000cc09000000000000,Sony DualShock 4 V2,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, +030000004c050000a00b000000000000,Sony DualShock 4 Adapter,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +030000004c050000cc09000000000000,Sony DualShock 4 V2,a:b1,b:b2,back:b13,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 03000000d11800000094000000010000,Stadia Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a4,rightx:a2,righty:a3,start:b7,x:b2,y:b3,platform:Mac OS X, 030000005e0400008e02000001000000,Steam Virtual Gamepad,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, 03000000110100002014000000000000,SteelSeries Nimbus,a:b0,b:b1,dpdown:b9,dpleft:b11,dpright:b10,dpup:b8,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1~,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3~,x:b2,y:b3,platform:Mac OS X, @@ -1062,8 +1078,8 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000100800000100000000000000,Twin USB Joystick,a:b4,b:b2,back:b16,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b12,leftstick:b20,lefttrigger:b8,leftx:a0,lefty:a2,rightshoulder:b14,rightstick:b22,righttrigger:b10,rightx:a6,righty:a4,start:b18,x:b6,y:b0,platform:Mac OS X, 03000000632500002605000000010000,Uberwith Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 03000000151900005678000010010000,Uniplay U6,a:b3,b:b6,back:b25,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b17,leftstick:b31,lefttrigger:b21,leftx:a1,lefty:a3,rightshoulder:b19,rightstick:b33,righttrigger:b23,rightx:a4,righty:a5,start:b27,x:b11,y:b13,platform:Mac OS X, -030000006f0e00000302000025040000,Victrix PS4 Pro Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Mac OS X, -030000006f0e00000702000003060000,Victrix PS4 Pro Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Mac OS X, +030000006f0e00000302000025040000,Victrix PS4 Pro Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, +030000006f0e00000702000003060000,Victrix PS4 Pro Fightstick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,touchpad:b13,x:b0,y:b3,platform:Mac OS X, 050000005769696d6f74652028303000,Wii Remote,a:b4,b:b5,back:b7,dpdown:b3,dpleft:b0,dpright:b1,dpup:b2,guide:b8,leftshoulder:b11,lefttrigger:b12,leftx:a0,lefty:a1,start:b6,x:b10,y:b9,platform:Mac OS X, 050000005769696d6f74652028313800,Wii U Pro Controller,a:b16,b:b15,back:b7,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b8,leftshoulder:b19,leftstick:b23,lefttrigger:b21,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b24,righttrigger:b22,rightx:a2,righty:a3,start:b6,x:b18,y:b17,platform:Mac OS X, 030000005e0400008e02000000000000,Xbox 360 Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, @@ -1078,7 +1094,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000005e040000d102000000000000,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, 030000005e040000dd02000000000000,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, 030000005e040000e002000000000000,Xbox One Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Mac OS X, -030000005e040000e002000003090000,Xbox One Controller,a:b0,b:b1,back:b16,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b15,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, +030000005e040000e002000003090000,Xbox One Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Mac OS X, 030000005e040000e302000000000000,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, 030000005e040000ea02000000000000,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b4,leftstick:b6,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b8,x:b2,y:b3,platform:Mac OS X, 030000005e040000fd02000003090000,Xbox One Controller,a:b0,b:b1,back:b16,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b15,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, @@ -1088,7 +1104,9 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000005e040000130b000009050000,Xbox Series Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,misc1:b15,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 030000005e040000130b000013050000,Xbox Series Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 030000005e040000130b000015050000,Xbox Series Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, +030000005e040000130b000007050000,Xbox Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 030000005e040000130b000017050000,Xbox Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, +030000005e040000130b000022050000,Xbox Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 030000005e040000220b000017050000,Xbox Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Mac OS X, 03000000172700004431000029010000,XiaoMi Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b15,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a6,rightx:a2,righty:a5,start:b11,x:b3,y:b4,platform:Mac OS X, 03000000120c0000100e000000010000,Zeroplus P4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Mac OS X, @@ -1096,10 +1114,12 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, # Linux 03000000c82d00000031000011010000,8BitDo Adapter,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, +03000000c82d00000631000000010000,8BitDo Adapter 2,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000c82d00000951000000010000,8BitDo Dogbone,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b11,platform:Linux, 03000000021000000090000011010000,8BitDo FC30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 03000000c82d00000090000011010000,8BitDo FC30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a5,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 05000000c82d00001038000000010000,8BitDo FC30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, +05000000c82d00006a28000000010000,8BitDo GameCube,a:b0,b:b3,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b9,paddle2:b8,rightshoulder:b10,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b1,y:b4,platform:Linux, 03000000c82d00001251000011010000,8BitDo Lite 2,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a5,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 05000000c82d00001251000000010000,8BitDo Lite 2,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 03000000c82d00001151000011010000,8BitDo Lite SE,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, @@ -1107,11 +1127,13 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000c82d00000151000000010000,8BitDo M30,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:b7,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000c82d00000650000011010000,8BitDo M30,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 05000000c82d00005106000000010000,8BitDo M30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b8,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:b7,start:b11,x:b3,y:b4,platform:Linux, +03000000c82d00000a20000000020000,8BitDo M30 Xbox,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,start:b7,x:b2,y:b3,platform:Linux, 03000000c82d00002090000011010000,8BitDo Micro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 05000000c82d00002090000000010000,8BitDo Micro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 03000000c82d00000451000000010000,8BitDo N30,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b11,platform:Linux, 03000000c82d00001590000011010000,8BitDo N30 Pro 2,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 05000000c82d00006528000000010000,8BitDo N30 Pro 2,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, +03000000c82d00006928000011010000,8BitDo N64,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a2,righty:a3,start:b11,platform:Linux, 05000000c82d00006928000000010000,8BitDo N64,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b9,rightx:a2,righty:a3,start:b11,platform:Linux, 05000000c82d00002590000001000000,8BitDo NEOGEO,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000008000000210000011010000,8BitDo NES30,a:b1,b:b2,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, @@ -1133,6 +1155,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000c82d00000331000011010000,8BitDo Receiver,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 03000000c82d00000431000011010000,8BitDo Receiver,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 03000000c82d00002867000000010000,8BitDo S30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b8,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:b7,rightx:a2,righty:a3,start:b10,x:b3,y:b4,platform:Linux, +03000000c82d00000060000011010000,8BitDo SF30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 05000000c82d00000060000000010000,8BitDo SF30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 05000000c82d00000061000000010000,8BitDo SF30 Pro,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b2,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 030000003512000012ab000010010000,8BitDo SFC30,a:b2,b:b1,back:b6,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b7,x:b3,y:b0,platform:Linux, @@ -1151,12 +1174,16 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 05000000c82d00000261000000010000,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 05000000202800000900000000010000,8BitDo SNES30,a:b1,b:b0,back:b10,dpdown:b122,dpleft:b119,dpright:b120,dpup:b117,leftshoulder:b6,rightshoulder:b7,start:b11,x:b4,y:b3,platform:Linux, 05000000c82d00001230000000010000,8BitDo Ultimate,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, +03000000c82d00000a31000014010000,8BitDo Ultimate 2C,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +03000000c82d00001d30000011010000,8BitDo Ultimate 2C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b5,paddle2:b2,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, +05000000c82d00001b30000001000000,8BitDo Ultimate 2C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b5,paddle2:b2,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000c82d00001530000011010000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000c82d00001630000011010000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000c82d00001730000011010000,8BitDo Ultimate C,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000c82d00001130000011010000,8BitDo Ultimate Wired,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,misc1:b26,paddle1:b24,paddle2:b25,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, +03000000c82d00000631000010010000,8BitDo Ultimate Wireless,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000c82d00000760000011010000,8BitDo Ultimate Wireless,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, -03000000c82d00001230000011010000,8BitDo Ultimate Wireless,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, +03000000c82d00001230000011010000,8BitDo Ultimate Wireless,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b2,paddle2:b5,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000c82d00001330000011010000,8BitDo Ultimate Wireless,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,misc1:b26,paddle1:b23,paddle2:b19,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000c82d00000631000014010000,8BitDo Ultimate Wireless Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000c82d00000121000011010000,8BitDo Xbox One SN30 Pro,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, @@ -1164,7 +1191,6 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 05000000a00500003232000001000000,8BitDo Zero,a:b0,b:b1,back:b10,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b11,x:b3,y:b4,platform:Linux, 05000000a00500003232000008010000,8BitDo Zero,a:b0,b:b1,back:b10,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b11,x:b3,y:b4,platform:Linux, 03000000c82d00001890000011010000,8BitDo Zero 2,a:b1,b:b0,back:b10,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b11,x:b4,y:b3,platform:Linux, -050000005e040000e002000030110000,8BitDo Zero 2,a:b0,b:b1,back:b6,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,start:b7,x:b2,y:b3,platform:Linux, 05000000c82d00003032000000010000,8BitDo Zero 2,a:b1,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,rightx:a2,righty:a3,start:b11,x:b4,y:b3,platform:Linux, 03000000c01100000355000011010000,Acrux Gamepad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 030000006f0e00008801000011010000,Afterglow Deluxe Nintendo Switch Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, @@ -1201,7 +1227,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000008a3500000302000011010000,Backbone One,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 030000008a3500000402000011010000,Backbone One,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000c62400001b89000011010000,BDA MOGA XP5X Plus,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, -03000000d62000002a79000011010000,BDA PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000d62000002a79000011010000,BDA PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000c21100000791000011010000,Be1 GC101 Controller 1.03,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Linux, 03000000c31100000791000011010000,Be1 GC101 Controller 1.03,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 030000005e0400008e02000003030000,Be1 GC101 Xbox 360,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, @@ -1209,14 +1235,15 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 05000000bc2000000055000001000000,Betop AX1 BFM,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000bc2000006412000011010000,Betop Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b30,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Linux, 030000006b1400000209000011010000,Bigben,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -03000000120c0000200e000011010000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, -03000000120c0000210e000011010000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, +03000000120c0000200e000011010000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, +03000000120c0000210e000011010000,Brook Mars PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000120c0000f70e000011010000,Brook Universal Fighting Board,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,rightstick:b11,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, 03000000e82000006058000001010000,Cideko AK08b,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Linux, 03000000af1e00002400000010010000,Clockwork Pi DevTerm,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,start:b9,x:b3,y:b0,platform:Linux, 030000000b0400003365000000010000,Competition Pro,a:b0,b:b1,back:b2,leftx:a0,lefty:a1,start:b3,platform:Linux, 03000000260900008888000000010000,Cyber Gadget GameCube Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b6,righttrigger:a5,rightx:a2,righty:a3~,start:b7,x:b2,y:b3,platform:Linux, 03000000a306000022f6000011010000,Cyborg V3 Rumble,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:+a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:-a3,rightx:a2,righty:a4,start:b9,x:b0,y:b3,platform:Linux, +030000005e0400008e02000002010000,Data Frog S80,a:b1,b:b0,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b3,y:b2,platform:Linux, 03000000791d00000103000010010000,Dual Box Wii Classic Adapter,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Linux, 030000006f0e00003001000001010000,EA Sports PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 03000000c11100000191000011010000,EasySMX,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Linux, @@ -1260,17 +1287,16 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 06000000adde0000efbe000002010000,Hidromancer Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000d81400000862000011010000,HitBox PS3 PC Analog Mode,a:b1,b:b2,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,guide:b9,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b12,x:b0,y:b3,platform:Linux, 03000000c9110000f055000011010000,HJC Gamepad,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Linux, -030000000d0f00000d00000000010000,Hori,a:b0,b:b6,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b3,rightshoulder:b7,start:b9,x:b1,y:b2,platform:Linux, 030000000d0f00006d00000020010000,Hori EDGE 301,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:+a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:+a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000000d0f00008400000011010000,Hori Fighting Commander,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, 030000000d0f00005f00000011010000,Hori Fighting Commander 4 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -030000000d0f00005e00000011010000,Hori Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +030000000d0f00005e00000011010000,Hori Fighting Commander 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000000d0f00005001000009040000,Hori Fighting Commander OCTA Xbox One,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000000d0f00008500000010010000,Hori Fighting Commander PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 030000000d0f00008600000002010000,Hori Fighting Commander Xbox 360,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Linux, 030000000d0f00003701000013010000,Hori Fighting Stick Mini,a:b1,b:b0,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,lefttrigger:a2,rightshoulder:b5,righttrigger:a5,start:b7,x:b3,y:b2,platform:Linux, 030000000d0f00008800000011010000,Hori Fighting Stick mini 4 (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,rightstick:b11,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, -030000000d0f00008700000011010000,Hori Fighting Stick mini 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,rightshoulder:b5,rightstick:b11,righttrigger:a4,start:b9,x:b0,y:b3,platform:Linux, +030000000d0f00008700000011010000,Hori Fighting Stick mini 4 (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,rightshoulder:b5,rightstick:b11,righttrigger:a4,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000000d0f00001000000011010000,Hori Fightstick 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, 03000000ad1b000003f5000033050000,Hori Fightstick VX,+leftx:h0.2,+lefty:h0.4,-leftx:h0.8,-lefty:h0.1,a:b0,b:b1,back:b8,guide:b10,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b2,y:b3,platform:Linux, 030000000d0f00004d00000011010000,Hori Gem Pad 3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, @@ -1286,7 +1312,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000000d0f00008501000017010000,Hori Split Pad Fit,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000000d0f00008501000015010000,Hori Switch Split Pad Pro,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000000d0f00006e00000011010000,Horipad 4 PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -030000000d0f00006600000011010000,Horipad 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +030000000d0f00006600000011010000,Horipad 4 PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000000d0f0000ee00000011010000,Horipad Mini 4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, 030000000d0f0000c100000011010000,Horipad Nintendo Switch Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 030000000d0f00006700000001010000,Horipad One,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, @@ -1337,12 +1363,12 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 05000000380700006652000025010000,Mad Catz CTRLR,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 03000000380700008532000010010000,Mad Catz Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b5,rightshoulder:b6,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, 03000000380700005032000011010000,Mad Catz Fightpad Pro PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -03000000380700005082000011010000,Mad Catz Fightpad Pro PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000380700005082000011010000,Mad Catz Fightpad Pro PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000ad1b00002ef0000090040000,Mad Catz Fightpad SFxT,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,lefttrigger:a2,rightshoulder:b5,righttrigger:a5,start:b7,x:b2,y:b3,platform:Linux, 03000000380700008034000011010000,Mad Catz Fightstick PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -03000000380700008084000011010000,Mad Catz Fightstick PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000380700008084000011010000,Mad Catz Fightstick PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000380700008433000011010000,Mad Catz Fightstick TE S PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -03000000380700008483000011010000,Mad Catz Fightstick TE S PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000380700008483000011010000,Mad Catz Fightstick TE S PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000380700001888000010010000,Mad Catz Joystick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 03000000380700003888000010010000,Mad Catz Joystick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:a0,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 03000000380700001647000010040000,Mad Catz Xbox 360 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, @@ -1385,6 +1411,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 050000005e040000050b000003090000,Microsoft Xbox One Elite 2,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a6,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a5,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 050000005e0400008e02000030110000,Microsoft Xbox One Elite 2,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,paddle1:b11,paddle2:b13,paddle3:b12,paddle4:b14,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000120b00000b050000,Microsoft Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +060000005e040000120b000001050000,Microsoft Xbox Series X Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000030000000300000002000000,Miroof,a:b1,b:b0,back:b6,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b5,start:b7,x:b3,y:b2,platform:Linux, 03000000790000001c18000010010000,Mobapad Chitu HD,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:b9,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 050000004d4f435554452d3035335800,Mocute 053X,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Linux, @@ -1398,9 +1425,13 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 05000000c62400001a89000000010000,MOGA XP5X Plus,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000250900006688000000010000,MP8866 Super Dual Box,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,platform:Linux, 030000005e0400008e02000010020000,MSI GC20 V2,a:b0,b:b1,back:b6,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +03000000f70600000100000000010000,N64 Adaptoid,+rightx:b2,+righty:b1,-rightx:b4,-righty:b5,a:b0,b:b3,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,leftshoulder:b6,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b7,start:b8,platform:Linux, +030000006f0e00001311000011010000,N64 Controller,+rightx:b10,+righty:b3,-rightx:b0,-righty:b11,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b9,platform:Linux, 030000006b1400000906000014010000,Nacon Asymmetric Wireless PS4 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000006b140000010c000010010000,Nacon GC 400ES,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Linux, 03000000853200000706000012010000,Nacon GC-100,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +0300000085320000170d000011010000,Nacon Revolution 5 Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, +0300000085320000190d000011010000,Nacon Revolution 5 Pro,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000000d0f00000900000010010000,Natec Genesis P44,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 030000004f1f00000800000011010000,NeoGeo PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, 0300000092120000474e000000010000,NeoGeo X Arcade Stick,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,start:b9,x:b3,y:b2,platform:Linux, @@ -1461,7 +1492,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000004c0500003713000011010000,PlayStation Vita,a:b1,b:b2,back:b8,dpdown:b13,dpleft:b15,dpright:b14,dpup:b12,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b5,rightx:a3,righty:a4,start:b9,x:b0,y:b3,platform:Linux, 03000000c62400000053000000010000,PowerA,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000c62400003a54000001010000,PowerA 1428124-01,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, -03000000d620000011a7000011010000,PowerA Core Plus Gamecube Controller,a:b1,b:b0,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Linux, +03000000d620000011a7000011010000,PowerA Core Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 03000000dd62000015a7000011010000,PowerA Fusion Nintendo Switch Arcade Stick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 03000000d620000012a7000011010000,PowerA Fusion Nintendo Switch Fight Pad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,misc1:b13,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 03000000d62000000140000001010000,PowerA Fusion Pro 2 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, @@ -1476,7 +1507,9 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000c62400001a54000001010000,PowerA Xbox One Mini Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000d62000000240000001010000,PowerA Xbox One Spectra Infinity,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000d62000000f20000001010000,PowerA Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b7,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +03000000d62000000b20000001010000,PowerA Xbox Series X Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000006d040000d2ca000011010000,Precision Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, +03000000250900000017000010010000,PS/SS/N64 Adapter,a:b1,b:b2,dpdown:b14,dpleft:b15,dpright:b13,dpup:b12,leftshoulder:b5,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b7,rightx:a2~,righty:a3,start:b8,platform:Linux, 03000000ff1100004133000010010000,PS2 Controller,a:b2,b:b1,back:b8,leftshoulder:b6,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b5,start:b9,x:b3,y:b0,platform:Linux, 03000000341a00003608000011010000,PS3 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, 030000004c0500006802000010010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,platform:Linux, @@ -1492,18 +1525,18 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 050000004c0500006802000000810000,PS3 Controller,a:b0,b:b1,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,platform:Linux, 05000000504c415953544154494f4e00,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,platform:Linux, 060000004c0500006802000000010000,PS3 Controller,a:b14,b:b13,back:b0,dpdown:b6,dpleft:b7,dpright:b5,dpup:b4,guide:b16,leftshoulder:b10,leftstick:b1,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b11,rightstick:b2,righttrigger:b9,rightx:a2,righty:a3,start:b3,x:b15,y:b12,platform:Linux, -030000004c050000a00b000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +030000004c050000a00b000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000004c050000a00b000011810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,platform:Linux, -030000004c050000c405000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +030000004c050000c405000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000004c050000c405000011810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,platform:Linux, -030000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, -030000004c050000cc09000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +030000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, +030000004c050000cc09000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000004c050000cc09000011810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,platform:Linux, -03000000c01100000140000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, -050000004c050000c405000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000c01100000140000011010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, +050000004c050000c405000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 050000004c050000c405000000810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,platform:Linux, 050000004c050000c405000001800000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,platform:Linux, -050000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +050000004c050000cc09000000010000,PS4 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 050000004c050000cc09000000810000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,platform:Linux, 050000004c050000cc09000001800000,PS4 Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b11,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:a5,rightx:a3,righty:a4,start:b9,x:b3,y:b2,platform:Linux, 0300004b4c0500005f0e000011010000,PS5 Access Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b14,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, @@ -1515,25 +1548,29 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 050000004c050000f20d000000010000,PS5 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000300f00001211000011010000,Qanba Arcade Joystick,a:b2,b:b0,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b5,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,righttrigger:b6,start:b9,x:b1,y:b3,platform:Linux, 03000000222c00000225000011010000,Qanba Dragon Arcade Joystick (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -03000000222c00000025000011010000,Qanba Dragon Arcade Joystick (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, -03000000222c00000020000011010000,Qanba Drone Arcade PS4 Joystick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,rightshoulder:b5,righttrigger:a4,start:b9,x:b0,y:b3,platform:Linux, +03000000222c00000025000011010000,Qanba Dragon Arcade Joystick (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, +03000000222c00001220000011010000,Qanba Drone 2 Arcade Joystick (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000222c00001020000011010000,Qanba Drone 2 Arcade Joystick (PS5),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000222c00000020000011010000,Qanba Drone Arcade PS4 Joystick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,rightshoulder:b5,righttrigger:a4,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000300f00001210000010010000,Qanba Joystick Plus,a:b0,b:b1,back:b8,leftshoulder:b5,lefttrigger:b7,leftx:a0,lefty:a1,rightshoulder:b4,righttrigger:b6,start:b9,x:b2,y:b3,platform:Linux, 03000000222c00000223000011010000,Qanba Obsidian Arcade Joystick (PS3),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -03000000222c00000023000011010000,Qanba Obsidian Arcade Joystick (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000222c00000023000011010000,Qanba Obsidian Arcade Joystick (PS4),a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000009b2800000300000001010000,Raphnet 4nes4snes,a:b0,b:b4,back:b2,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,start:b3,x:b1,y:b5,platform:Linux, 030000009b2800004200000001010000,Raphnet Dual NES Adapter,a:b0,b:b1,back:b2,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,start:b3,platform:Linux, +0300132d9b2800006500000000000000,Raphnet GameCube Adapter,a:b0,b:b7,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b2,righttrigger:b5,rightx:a3,righty:a4,start:b3,x:b1,y:b8,platform:Linux, +0300132d9b2800006500000001010000,Raphnet GameCube Adapter,a:b0,b:b7,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b2,righttrigger:b5,rightx:a3,righty:a4,start:b3,x:b1,y:b8,platform:Linux, 030000009b2800003200000001010000,Raphnet GC and N64 Adapter,a:b0,b:b7,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b2,righttrigger:b5,rightx:a3,righty:a4,start:b3,x:b1,y:b8,platform:Linux, 030000009b2800006000000001010000,Raphnet GC and N64 Adapter,a:b0,b:b7,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b2,righttrigger:b5,rightx:a3,righty:a4,start:b3,x:b1,y:b8,platform:Linux, 030000009b2800008000000020020000,Raphnet Wii Classic Adapter,a:b1,b:b4,back:b2,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,leftshoulder:b6,rightshoulder:b7,start:b3,x:b0,y:b5,platform:Linux, 03000000f8270000bf0b000011010000,Razer Kishi,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 030000008916000001fd000024010000,Razer Onza Classic Edition,a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000321500000204000011010000,Razer Panthera PS3,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b0,y:b3,platform:Linux, -03000000321500000104000011010000,Razer Panthera PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, -03000000321500000810000011010000,Razer Panthera PS4 Evo Arcade Stick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b13,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000321500000104000011010000,Razer Panthera PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, +03000000321500000810000011010000,Razer Panthera PS4 Evo Arcade Stick,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b13,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000321500000010000011010000,Razer Raiju,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, 03000000321500000507000000010000,Razer Raiju Mobile,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b21,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 05000000321500000a10000001000000,Razer Raiju Tournament Edition,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b13,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, -03000000321500000011000011010000,Razer Raion PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +03000000321500000011000011010000,Razer Raion PS4 Fightpad,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 030000008916000000fe000024010000,Razer Sabertooth,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000c6240000045d000024010000,Razer Sabertooth,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000c6240000045d000025010000,Razer Sabertooth,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, @@ -1541,6 +1578,8 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 050000003215000000090000163a0000,Razer Serval,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a4,rightx:a2,righty:a3,start:b7,x:b2,y:b3,platform:Linux, 0300000032150000030a000001010000,Razer Wildcat,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 03000000321500000b10000011010000,Razer Wolverine PS5 Controller,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, +030000000d0f0000c100000010010000,Retro Bit Legacy16,a:b1,b:b2,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,guide:b12,leftshoulder:b4,lefttrigger:b6,misc1:b13,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, +030000000d0f0000c100000072056800,Retro Bit Legacy16,a:b1,b:b0,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,guide:b5,leftshoulder:b9,lefttrigger:+a4,misc1:b11,rightshoulder:b10,righttrigger:+a5,start:b6,x:b3,y:b2,platform:Linux, 03000000790000001100000010010000,Retro Controller,a:b1,b:b2,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,lefttrigger:b7,rightshoulder:b4,righttrigger:b5,start:b9,x:b0,y:b3,platform:Linux, 0300000003040000c197000011010000,Retrode Adapter,a:b0,b:b4,back:b2,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,rightshoulder:b7,start:b3,x:b1,y:b5,platform:Linux, 190000004b4800000111000000010000,RetroGame Joypad,a:b1,b:b0,back:b8,dpdown:b14,dpleft:b15,dpright:b16,dpup:b13,leftshoulder:b4,leftstick:b11,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b12,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Linux, @@ -1567,6 +1606,9 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000a306000020f6000011010000,Saitek PS2700 Rumble,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a4,start:b9,x:b0,y:b3,platform:Linux, 05000000e804000000a000001b010000,Samsung EIGP20,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b15,leftshoulder:b6,leftx:a0,lefty:a1,rightshoulder:b7,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000d81d00000e00000010010000,Savior,a:b0,b:b1,back:b8,leftshoulder:b6,leftstick:b10,lefttrigger:b7,leftx:a0,lefty:a1,rightshoulder:b2,rightstick:b11,righttrigger:b3,start:b9,x:b4,y:b5,platform:Linux, +03000000952e00004b43000011010000,Scuf Envision,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b9,righttrigger:a4,rightx:a2,righty:a5,start:b7,x:b2,y:b3,platform:Linux, +03000000952e00004d43000011010000,Scuf Envision,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b9,righttrigger:a4,rightx:a2,righty:a5,start:b7,x:b2,y:b3,platform:Linux, +03000000952e00004e43000011010000,Scuf Envision,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b4,leftstick:b8,lefttrigger:a3,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b9,righttrigger:a4,rightx:a2,righty:a5,start:b7,x:b2,y:b3,platform:Linux, 03000000a30c00002500000011010000,Sega Genesis Mini 3B Controller,a:b2,b:b1,dpdown:+a4,dpleft:-a3,dpright:+a3,dpup:-a4,righttrigger:b5,start:b9,platform:Linux, 03000000790000001100000011010000,Sega Saturn,a:b1,b:b2,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b6,lefttrigger:b7,rightshoulder:b5,righttrigger:b4,start:b9,x:b0,y:b3,platform:Linux, 03000000790000002201000011010000,Sega Saturn,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,lefttrigger:b5,rightshoulder:b6,righttrigger:b7,start:b9,x:b2,y:b3,platform:Linux, @@ -1578,7 +1620,8 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000bc2000000055000010010000,Shanwan Gamepad,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 03000000f025000021c1000010010000,Shanwan Gioteck PS3 Controller,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b3,y:b0,platform:Linux, 03000000341a00000908000010010000,SL6566,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Linux, -050000004c050000cc09000001000000,Sony DualShock 4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, +030000004b2900000430000011000000,Snakebyte Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +050000004c050000cc09000001000000,Sony DualShock 4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 03000000ff000000cb01000010010000,Sony PlayStation Portable,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b5,start:b7,x:b2,y:b3,platform:Linux, 03000000250900000500000000010000,Sony PS2 pad with SmartJoy Adapter,a:b2,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a2,righty:a3,start:b8,x:b3,y:b0,platform:Linux, 030000005e0400008e02000073050000,Speedlink Torid,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, @@ -1648,8 +1691,8 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000100800000300000010010000,USB Gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b3,y:b0,platform:Linux, 03000000790000000600000007010000,USB gamepad,a:b2,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:b6,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:b7,rightx:a3,righty:a4,start:b9,x:b3,y:b0,platform:Linux, 03000000790000001100000000010000,USB Gamepad,a:b2,b:b1,back:b8,dpdown:a0,dpleft:a1,dpright:a2,dpup:a4,start:b9,platform:Linux, -030000006f0e00000302000011010000,Victrix Pro Fightstick PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, -030000006f0e00000702000011010000,Victrix Pro Fightstick PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,x:b0,y:b3,platform:Linux, +030000006f0e00000302000011010000,Victrix Pro Fightstick PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, +030000006f0e00000702000011010000,Victrix Pro Fightstick PS4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:b6,rightshoulder:b5,righttrigger:b7,start:b9,touchpad:b13,x:b0,y:b3,platform:Linux, 05000000ac0500003232000001000000,VR Box Controller,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b6,leftstick:b10,lefttrigger:b4,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b11,righttrigger:b5,rightx:a3,righty:a2,start:b9,x:b2,y:b3,platform:Linux, 05000000434f4d4d414e440000000000,VX Gaming Command Series,a:b0,b:b1,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftstick:b10,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a3,start:b9,x:b2,y:b3,platform:Linux, 0000000058626f782033363020576900,Xbox 360 Controller,a:b0,b:b1,back:b14,dpdown:b11,dpleft:b12,dpright:b13,dpup:b10,guide:b7,leftshoulder:b4,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Linux, @@ -1660,6 +1703,7 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000005e040000a102000000010000,Xbox 360 Controller,a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000a102000007010000,Xbox 360 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000a102000030060000,Xbox 360 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +030000006f0e00001503000000020000,Xbox 360 Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e0400008e02000000010000,Xbox 360 EasySMX,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000a102000014010000,Xbox 360 Receiver,a:b0,b:b1,back:b6,dpdown:b14,dpleft:b11,dpright:b12,dpup:b13,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e0400000202000000010000,Xbox Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b5,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b2,rightstick:b9,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b3,y:b4,platform:Linux, @@ -1679,12 +1723,14 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 030000005e040000ea02000011050000,Xbox One S Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 060000005e040000ea0200000b050000,Xbox One S Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 060000005e040000ea0200000d050000,Xbox One S Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +060000005e040000ea02000016050000,Xbox One S Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000120b000001050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000120b000005050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000120b000007050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000120b000009050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000120b00000d050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, -030000005e040000120b00000f050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:b13,dpleft:b14,dpright:b15,dpup:b12,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +030000005e040000120b00000f050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +030000005e040000120b000015050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000130b000005050000,Xbox Series Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 050000005e040000130b000001050000,Xbox Series Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 050000005e040000130b000005050000,Xbox Series Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, @@ -1696,10 +1742,10 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 060000005e040000120b000007050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 060000005e040000120b00000b050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 060000005e040000120b00000f050000,Xbox Series Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, -050000005e040000130b000017050000,Xbox Series X Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,misc1:b15,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, -060000005e040000120b00000d050000,Xbox Series X Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000120b000011050000,Xbox Series X Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 030000005e040000120b000014050000,Xbox Series X Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, +050000005e040000130b000017050000,Xbox Series X Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,misc1:b15,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, +060000005e040000120b00000d050000,Xbox Series X Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,misc1:b11,rightshoulder:b5,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,start:b7,x:b2,y:b3,platform:Linux, 050000005e040000200b000013050000,Xbox Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 050000005e040000200b000017050000,Xbox Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, 050000005e040000220b000017050000,Xbox Wireless Controller,a:b0,b:b1,back:b10,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b6,leftstick:b13,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b7,rightstick:b14,righttrigger:a4,rightx:a2,righty:a3,start:b11,x:b3,y:b4,platform:Linux, @@ -1712,181 +1758,184 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 03000000120c0000101e000011010000,Zeroplus P4,a:b1,b:b2,back:b8,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b12,leftshoulder:b4,leftstick:b10,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b5,rightstick:b11,righttrigger:a4,rightx:a2,righty:a5,start:b9,x:b0,y:b3,platform:Linux, # Android -38653964633230666463343334313533,8BitDo Adapter,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -36666264316630653965636634386234,8BitDo Adapter 2,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38653964633230666463343334313533,8BitDo Adapter,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +36666264316630653965636634386234,8BitDo Adapter 2,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 38426974446f20417263616465205374,8BitDo Arcade Stick,a:b0,b:b1,back:b15,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,guide:b5,leftshoulder:b9,lefttrigger:a4,rightshoulder:b10,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -61393962646434393836356631636132,8BitDo Arcade Stick,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, -64323139346131306233636562663738,8BitDo Arcade Stick,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b19,y:b2,platform:Android, -64643565386136613265663236636564,8BitDo Arcade Stick,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, -33313433353539306634656436353432,8BitDo Dogbone,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -38426974446f20446f67626f6e65204d,8BitDo Dogbone,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b6,platform:Android, -34343439373236623466343934376233,8BitDo FC30 Pro,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b28,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b29,righttrigger:b7,start:b5,x:b30,y:b2,platform:Android, -38426974446f2038426974446f204c69,8BitDo Lite,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -30643332373663313263316637356631,8BitDo Lite 2,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38426974446f204c6974652032000000,8BitDo Lite 2,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -62656331626461363634633735353032,8BitDo Lite 2,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38393936616436383062666232653338,8BitDo Lite SE,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38426974446f204c6974652053450000,8BitDo Lite SE,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -39356430616562366466646636643435,8BitDo Lite SE,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +61393962646434393836356631636132,8BitDo Arcade Stick,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, +64323139346131306233636562663738,8BitDo Arcade Stick,a:b0,b:b1,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b19,y:b2,platform:Android, +64643565386136613265663236636564,8BitDo Arcade Stick,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, +33313433353539306634656436353432,8BitDo Dogbone,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38426974446f20446f67626f6e65204d,8BitDo Dogbone,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b6,platform:Android, +34343439373236623466343934376233,8BitDo FC30 Pro,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b28,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b4,rightstick:b29,righttrigger:b7,start:b5,x:b30,y:b2,platform:Android, +38426974446f204e4743204d6f646b69,8BitDo GameCube,a:b0,b:b2,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,paddle1:b18,paddle2:b17,rightshoulder:b15,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b1,y:b3,platform:Android, +38426974446f2038426974446f204c69,8BitDo Lite,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +30643332373663313263316637356631,8BitDo Lite 2,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +38426974446f204c6974652032000000,8BitDo Lite 2,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +62656331626461363634633735353032,8BitDo Lite 2,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +38393936616436383062666232653338,8BitDo Lite SE,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +38426974446f204c6974652053450000,8BitDo Lite SE,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +39356430616562366466646636643435,8BitDo Lite SE,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, 05000000c82d000006500000ffff3f00,8BitDo M30,a:b1,b:b0,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,guide:b17,leftshoulder:b9,lefttrigger:a5,rightshoulder:b10,righttrigger:a4,start:b6,x:b3,y:b2,platform:Android, 05000000c82d000051060000ffff3f00,8BitDo M30,a:b1,b:b0,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,guide:b17,leftshoulder:b9,lefttrigger:a4,rightshoulder:b10,righttrigger:a5,start:b6,x:b3,y:b2,platform:Android, -32323161363037623637326438643634,8BitDo M30,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -33656266353630643966653238646264,8BitDo M30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:a5,start:b10,x:b19,y:b2,platform:Android, -38426974446f204d3330204d6f646b69,8BitDo M30,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -39366630663062373237616566353437,8BitDo M30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,start:b6,x:b2,y:b3,platform:Android, -64653533313537373934323436343563,8BitDo M30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:a4,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,start:b6,x:b2,y:b3,platform:Android, -66356438346136366337386437653934,8BitDo M30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,start:b18,x:b19,y:b2,platform:Android, -66393064393162303732356665666366,8BitDo M30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:a5,start:b6,x:b2,y:b3,platform:Android, -38426974446f204d6963726f2067616d,8BitDo Micro,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,lefttrigger:a4,leftx:b0,lefty:b1,rightshoulder:b10,righttrigger:a5,rightx:b2,righty:b3,start:b6,x:b3,y:b2,platform:Android, -61653365323561356263373333643266,8BitDo Micro,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,lefttrigger:a4,leftx:b0,lefty:b1,rightshoulder:b10,righttrigger:a5,rightx:b2,righty:b3,start:b6,x:b3,y:b2,platform:Android, -62613137616239666338343866326336,8BitDo Micro,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,lefttrigger:a4,leftx:b0,lefty:b1,rightshoulder:b10,righttrigger:a5,rightx:b2,righty:b3,start:b6,x:b3,y:b2,platform:Android, -33663431326134333366393233616633,8BitDo N30,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b6,platform:Android, -38426974446f204e3330204d6f646b69,8BitDo N30,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b6,platform:Android, +32323161363037623637326438643634,8BitDo M30,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +33656266353630643966653238646264,8BitDo M30,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:a5,start:b10,x:b19,y:b2,platform:Android, +38426974446f204d3330204d6f646b69,8BitDo M30,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +39366630663062373237616566353437,8BitDo M30,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,start:b6,x:b2,y:b3,platform:Android, +64653533313537373934323436343563,8BitDo M30,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:a4,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,start:b6,x:b2,y:b3,platform:Android, +66356438346136366337386437653934,8BitDo M30,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,start:b18,x:b19,y:b2,platform:Android, +66393064393162303732356665666366,8BitDo M30,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:a5,start:b6,x:b2,y:b3,platform:Android, +38426974446f204d6963726f2067616d,8BitDo Micro,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,lefttrigger:a4,leftx:b0,lefty:b1,rightshoulder:b10,righttrigger:a5,rightx:b2,righty:b3,start:b6,x:b3,y:b2,platform:Android, +61653365323561356263373333643266,8BitDo Micro,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,lefttrigger:a4,leftx:b0,lefty:b1,rightshoulder:b10,righttrigger:a5,rightx:b2,righty:b3,start:b6,x:b3,y:b2,platform:Android, +62613137616239666338343866326336,8BitDo Micro,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,lefttrigger:a4,leftx:b0,lefty:b1,rightshoulder:b10,righttrigger:a5,rightx:b2,righty:b3,start:b6,x:b3,y:b2,platform:Android, +33663431326134333366393233616633,8BitDo N30,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b6,platform:Android, +38426974446f204e3330204d6f646b69,8BitDo N30,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftx:a0,lefty:a1,rightx:a2,righty:a3,start:b6,platform:Android, 05000000c82d000015900000ffff3f00,8BitDo N30 Pro 2,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, 05000000c82d000065280000ffff3f00,8BitDo N30 Pro 2,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b17,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38323035343766666239373834336637,8BitDo N64,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,platform:Android, -38426974446f204e3634204d6f646b69,8BitDo N64,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,platform:Android, -32363135613966656338666638666237,8BitDo NEOGEO,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -35363534633333373639386466346631,8BitDo NEOGEO,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -38426974446f204e454f47454f204750,8BitDo NEOGEO,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -39383963623932353561633733306334,8BitDo NEOGEO,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38323035343766666239373834336637,8BitDo N64,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,platform:Android, +38426974446f204e3634204d6f646b69,8BitDo N64,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,platform:Android, +32363135613966656338666638666237,8BitDo NEOGEO,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +35363534633333373639386466346631,8BitDo NEOGEO,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38426974446f204e454f47454f204750,8BitDo NEOGEO,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +39383963623932353561633733306334,8BitDo NEOGEO,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000000220000000900000ffff3f00,8BitDo NES30 Pro,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, 050000002038000009000000ffff3f00,8BitDo NES30 Pro,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38313433643131656262306631373166,8BitDo P30,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -38326536643339353865323063616339,8BitDo P30,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -38426974446f2050333020636c617373,8BitDo P30,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -35376664343164386333616535333434,8BitDo Pro 2,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b17,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b18,righttrigger:a5,rightx:a3,start:b10,x:b19,y:b2,platform:Android, -38426974446f2038426974446f205072,8BitDo Pro 2,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38426974446f2050726f203200000000,8BitDo Pro 2,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -62373739366537363166326238653463,8BitDo Pro 2,a:b1,b:b0,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b3,y:b2,platform:Android, -38386464613034326435626130396565,8BitDo Receiver,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38426974446f2038426974446f205265,8BitDo Receiver,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -66303230343038613365623964393766,8BitDo Receiver,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38426974446f20533330204d6f646b69,8BitDo S30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:a4,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -66316462353561376330346462316137,8BitDo S30,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:a4,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38313433643131656262306631373166,8BitDo P30,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38326536643339353865323063616339,8BitDo P30,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38426974446f2050333020636c617373,8BitDo P30,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +35376664343164386333616535333434,8BitDo Pro 2,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b17,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b18,righttrigger:a5,rightx:a3,start:b10,x:b19,y:b2,platform:Android, +38426974446f2038426974446f205072,8BitDo Pro 2,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +38426974446f2050726f203200000000,8BitDo Pro 2,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +61333362366131643730353063616330,8BitDo Pro 2,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +62373739366537363166326238653463,8BitDo Pro 2,a:b1,b:b0,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b3,y:b2,platform:Android, +38386464613034326435626130396565,8BitDo Receiver,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +38426974446f2038426974446f205265,8BitDo Receiver,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +66303230343038613365623964393766,8BitDo Receiver,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +38426974446f20533330204d6f646b69,8BitDo S30,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:a4,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +66316462353561376330346462316137,8BitDo S30,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:a4,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b9,righttrigger:b10,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 05000000c82d000000600000ffff3f00,8BitDo SF30 Pro,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b15,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b16,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, 05000000c82d000000610000ffff3f00,8BitDo SF30 Pro,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38426974646f20534633302050726f00,8BitDo SF30 Pro,a:b1,b:b0,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b16,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b2,y:b17,platform:Android, +38426974646f20534633302050726f00,8BitDo SF30 Pro,a:b1,b:b0,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b16,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b2,y:b17,platform:Android, 61623334636338643233383735326439,8BitDo SFC30,a:b0,b:b1,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b3,rightshoulder:b31,start:b5,x:b30,y:b2,platform:Android, 05000000c82d000012900000ffff3f00,8BitDo SN30,a:b1,b:b0,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b3,y:b2,platform:Android, 05000000c82d000062280000ffff3f00,8BitDo SN30,a:b1,b:b0,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b3,y:b2,platform:Android, -38316230613931613964356666353839,8BitDo SN30,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38426974446f20534e3330204d6f646b,8BitDo SN30,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -65323563303231646531383162646335,8BitDo SN30,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -35383531346263653330306238353131,8BitDo SN30 PP,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38316230613931613964356666353839,8BitDo SN30,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +38426974446f20534e3330204d6f646b,8BitDo SN30,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +65323563303231646531383162646335,8BitDo SN30,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +35383531346263653330306238353131,8BitDo SN30 PP,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 05000000c82d000001600000ffff3f00,8BitDo SN30 Pro,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, 05000000c82d000002600000ffff0f00,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b17,leftshoulder:b9,leftstick:b7,lefttrigger:b15,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b16,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -36653638656632326235346264663661,8BitDo SN30 Pro Plus,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b19,y:b2,platform:Android, -38303232393133383836366330346462,8BitDo SN30 Pro Plus,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b17,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b18,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, -38346630346135363335366265656666,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -38426974446f20534e33302050726f2b,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -536f6e7920436f6d707574657220456e,8BitDo SN30 Pro Plus,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -66306331643531333230306437353936,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +36653638656632326235346264663661,8BitDo SN30 Pro Plus,a:b0,b:b1,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b19,y:b2,platform:Android, +38303232393133383836366330346462,8BitDo SN30 Pro Plus,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b17,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b18,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, +38346630346135363335366265656666,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +38426974446f20534e33302050726f2b,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +536f6e7920436f6d707574657220456e,8BitDo SN30 Pro Plus,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +66306331643531333230306437353936,8BitDo SN30 Pro Plus,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, 050000002028000009000000ffff3f00,8BitDo SNES30,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, 050000003512000020ab000000780f00,8BitDo SNES30,a:b21,b:b20,back:b30,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b26,rightshoulder:b27,start:b31,x:b24,y:b23,platform:Android, 33666663316164653937326237613331,8BitDo Zero,a:b0,b:b1,back:b15,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b2,y:b3,platform:Android, 38426974646f205a65726f2047616d65,8BitDo Zero,a:b0,b:b1,back:b15,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b2,y:b3,platform:Android, 05000000c82d000018900000ffff0f00,8BitDo Zero 2,a:b1,b:b0,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b3,y:b2,platform:Android, 05000000c82d000030320000ffff0f00,8BitDo Zero 2,a:b1,b:b0,back:b4,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b3,y:b2,platform:Android, -33663434393362303033616630346337,8BitDo Zero 2,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftx:a0,lefty:a1,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, -34656330626361666438323266633963,8BitDo Zero 2,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b20,start:b10,x:b19,y:b2,platform:Android, -63396666386564393334393236386630,8BitDo Zero 2,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,start:b6,x:b3,y:b2,platform:Android, -63633435623263373466343461646430,8BitDo Zero 2,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,start:b6,x:b2,y:b3,platform:Android, -32333634613735616163326165323731,Amazon Luna Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b2,y:b3,platform:Android, -4c696e757820342e31392e3137322077,Anbernic Gamepad,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a4,start:b6,x:b2,y:b3,platform:Android, +33663434393362303033616630346337,8BitDo Zero 2,a:b0,b:b1,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftx:a0,lefty:a1,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, +34656330626361666438323266633963,8BitDo Zero 2,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b4,leftx:a0,lefty:a1,rightshoulder:b20,start:b10,x:b19,y:b2,platform:Android, +63396666386564393334393236386630,8BitDo Zero 2,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,start:b6,x:b3,y:b2,platform:Android, +63633435623263373466343461646430,8BitDo Zero 2,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftx:a0,lefty:a1,rightshoulder:b10,start:b6,x:b2,y:b3,platform:Android, +32333634613735616163326165323731,Amazon Luna Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b2,y:b3,platform:Android, +4c696e757820342e31392e3137322077,Anbernic Gamepad,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a4,start:b6,x:b2,y:b3,platform:Android, 417374726f2063697479206d696e6920,Astro City Mini,a:b23,b:b22,back:b29,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,rightshoulder:b25,righttrigger:b26,start:b30,x:b24,y:b21,platform:Android, -35643263313264386134376362363435,Atari VCS Classic Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,start:b6,platform:Android, -32353831643566306563643065356239,Atari VCS Modern Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -32303165626138343962363666346165,Brook Mars PS4 Controller,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +35643263313264386134376362363435,Atari VCS Classic Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,start:b6,platform:Android, +32353831643566306563643065356239,Atari VCS Modern Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +4f64696e20436f6e74726f6c6c657200,AYN Odin,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b14,dpright:b13,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:+a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:+a4,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +32303165626138343962363666346165,Brook Mars PS4 Controller,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, 38383337343564366131323064613561,Brook Mars PS4 Controller,a:b1,b:b19,back:b17,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -34313430343161653665353737323365,Elecom JC-W01U,a:b23,b:b24,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b21,y:b22,platform:Android, -4875694a6961204a432d573031550000,Elecom JC-W01U,a:b23,b:b24,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b21,y:b22,platform:Android, -30363230653635633863366338623265,Evo VR,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftx:a0,lefty:a1,x:b2,y:b3,platform:Android, +34313430343161653665353737323365,Elecom JC-W01U,a:b23,b:b24,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b21,y:b22,platform:Android, +4875694a6961204a432d573031550000,Elecom JC-W01U,a:b23,b:b24,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b21,y:b22,platform:Android, +30363230653635633863366338623265,Evo VR,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftx:a0,lefty:a1,x:b2,y:b3,platform:Android, 05000000b404000011240000dfff3f00,Flydigi Vader 2,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,paddle1:b17,paddle2:b18,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 05000000bc20000000550000ffff3f00,GameSir G3w,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -34323662653333636330306631326233,Google Nexus,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -35383633353935396534393230616564,Google Stadia Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +34323662653333636330306631326233,Google Nexus,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +35383633353935396534393230616564,Google Stadia Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 05000000d6020000e5890000dfff3f00,GPD XD Plus,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a5,start:b6,x:b2,y:b3,platform:Android, 05000000d6020000e5890000dfff3f80,GPD XD Plus,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a3,rightx:a4,righty:a5,start:b6,x:b2,y:b3,platform:Android, -66633030656131663837396562323935,Hori Battle,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, -35623466343433653739346434636330,Hori Fighting Commander 3 Pro,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -484f524920434f2e2c4c54442e203130,Hori Fighting Commander 3 Pro,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b20,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b3,righttrigger:b9,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -484f524920434f2e2c4c544420205041,Hori Gem Pad 3,a:b1,b:b17,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b4,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b16,x:b0,y:b2,platform:Android, -65656436646661313232656661616130,Hori PC Engine Mini Controller,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,start:b18,platform:Android, -31303433326562636431653534636633,Hori Real Arcade Pro 3,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -32656664353964393561366362333636,Hori Switch Split Pad Pro,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, -30306539356238653637313730656134,HORIPAD Switch Pro Controller,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b19,y:b2,platform:Android, +66633030656131663837396562323935,Hori Battle,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, +35623466343433653739346434636330,Hori Fighting Commander 3 Pro,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +484f524920434f2e2c4c54442e203130,Hori Fighting Commander 3 Pro,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b20,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b3,righttrigger:b9,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +484f524920434f2e2c4c544420205041,Hori Gem Pad 3,a:b1,b:b17,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b4,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b16,x:b0,y:b2,platform:Android, +65656436646661313232656661616130,Hori PC Engine Mini Controller,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,start:b18,platform:Android, +31303433326562636431653534636633,Hori Real Arcade Pro 3,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +32656664353964393561366362333636,Hori Switch Split Pad Pro,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, +30306539356238653637313730656134,HORIPAD Switch Pro Controller,a:b0,b:b1,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b19,y:b2,platform:Android, 48797065726b696e2050616400000000,Hyperkin Admiral N64 Controller,+rightx:b6,+righty:b7,-rightx:b17,-righty:b5,a:b1,b:b0,leftshoulder:b3,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b20,start:b18,platform:Android, 62333331353131353034386136626636,Hyperkin Admiral N64 Controller,+rightx:b6,+righty:b7,-rightx:b17,-righty:b5,a:b1,b:b0,leftshoulder:b3,lefttrigger:b8,leftx:a0,lefty:a1,rightshoulder:b20,start:b18,platform:Android, -31306635363562663834633739396333,Hyperkin N64 Adapter,a:b1,b:b19,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a2,righty:a3,start:b18,platform:Android, -5368616e57616e202020202048797065,Hyperkin N64 Adapter,a:b1,b:b19,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a2,righty:a3,start:b18,platform:Android, +31306635363562663834633739396333,Hyperkin N64 Adapter,a:b1,b:b19,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a2,righty:a3,start:b18,platform:Android, +5368616e57616e202020202048797065,Hyperkin N64 Adapter,a:b1,b:b19,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a2,righty:a3,start:b18,platform:Android, 0500000083050000602000000ffe0000,iBuffalo SNES Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b15,rightshoulder:b16,start:b10,x:b2,y:b3,platform:Android, -5553422c322d6178697320382d627574,iBuffalo Super Famicom Controller,a:b1,b:b0,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b17,rightshoulder:b18,start:b10,x:b3,y:b2,platform:Android, +5553422c322d6178697320382d627574,iBuffalo Super Famicom Controller,a:b1,b:b0,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b17,rightshoulder:b18,start:b10,x:b3,y:b2,platform:Android, 64306137363261396266353433303531,InterAct GoPad,a:b24,b:b25,leftshoulder:b23,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,x:b21,y:b22,platform:Android, -532e542e442e20496e74657261637420,InterAct HammerHead FX,a:b23,b:b24,back:b30,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b26,leftstick:b22,lefttrigger:b28,leftx:a0,lefty:a1,rightshoulder:b27,rightstick:b25,righttrigger:b29,rightx:a2,righty:a3,start:b31,x:b20,y:b21,platform:Android, -65346535636333663931613264643164,Joy-Con,a:b21,b:b22,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b23,y:b24,platform:Android, -33346566643039343630376565326335,Joy-Con (L),a:b0,b:b1,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,rightshoulder:b20,start:b17,x:b19,y:b2,platform:Android, -35313531613435623366313835326238,Joy-Con (L),a:b0,b:b1,back:b7,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,rightshoulder:b20,start:b17,x:b19,y:b2,platform:Android, -4a6f792d436f6e20284c290000000000,Joy-Con (L),a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,rightshoulder:b20,start:b17,x:b19,y:b2,platform:Android, -38383665633039363066383334653465,Joy-Con (R),a:b0,b:b1,back:b5,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, -39363561613936303237333537383931,Joy-Con (R),a:b0,b:b1,back:b5,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, -4a6f792d436f6e202852290000000000,Joy-Con (R),a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, -39656136363638323036303865326464,JYS Aapter,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -63316564383539663166353034616434,JYS Adapter,a:b1,b:b3,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b0,y:b2,platform:Android, -64623163333561643339623235373232,Logitech F310,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -35623364393661626231343866613337,Logitech F710,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -4c6f6769746563682047616d65706164,Logitech F710,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -64396331333230326333313330336533,Logitech F710,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -39653365373864633935383236363438,Logitech G Cloud,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -416d617a6f6e2047616d6520436f6e74,Luna Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, -4c756e612047616d6570616400000000,Luna Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -30363066623539323534363639323363,Magic NS,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -31353762393935386662336365626334,Magic NS,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -39623565346366623931666633323530,Magic NS,a:b1,b:b3,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b0,y:b2,platform:Android, -6d6179666c617368206c696d69746564,Mayflash GameCube Adapter,a:b22,b:b21,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a5,righty:a2,start:b30,x:b23,y:b24,platform:Android, -436f6e74726f6c6c6572000000000000,Mayflash N64 Adapter,a:b1,b:b19,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a2,righty:a3,start:b18,platform:Android, -65666330633838383061313633326461,Mayflash N64 Adapter,a:b1,b:b19,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a2,righty:a3,start:b18,platform:Android, -37316565396364386635383230353365,Mayflash Saturn Adapter,a:b21,b:b22,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b26,lefttrigger:b28,rightshoulder:b27,righttrigger:b23,start:b30,x:b24,y:b25,platform:Android, -4875694a696120205553422047616d65,Mayflash Saturn Adapter,a:b21,b:b22,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b26,lefttrigger:b28,rightshoulder:b27,righttrigger:b23,start:b30,x:b24,y:b25,platform:Android, -535a4d792d706f776572204c54442043,Mayflash Wii Classic Adapter,a:b23,b:b22,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b31,leftshoulder:b27,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a2,righty:a3,start:b30,x:b24,y:b21,platform:Android, -30653962643666303631376438373532,Mayflash Wii DolphinBar,a:b23,b:b24,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b0,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b21,y:b22,platform:Android, -39346131396233376535393665363161,Mayflash Wii U Pro Adapter,a:b22,b:b23,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b25,leftstick:b31,lefttrigger:b27,rightshoulder:b26,rightstick:b0,righttrigger:b28,rightx:a0,righty:a1,start:b30,x:b21,y:b24,platform:Android, +532e542e442e20496e74657261637420,InterAct HammerHead FX,a:b23,b:b24,back:b30,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b26,leftstick:b22,lefttrigger:b28,leftx:a0,lefty:a1,rightshoulder:b27,rightstick:b25,righttrigger:b29,rightx:a2,righty:a3,start:b31,x:b20,y:b21,platform:Android, +65346535636333663931613264643164,Joy-Con,a:b21,b:b22,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b23,y:b24,platform:Android, +33346566643039343630376565326335,Joy-Con (L),a:b0,b:b1,back:b7,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,rightshoulder:b20,start:b17,x:b19,y:b2,platform:Android, +35313531613435623366313835326238,Joy-Con (L),a:b0,b:b1,back:b7,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,rightshoulder:b20,start:b17,x:b19,y:b2,platform:Android, +4a6f792d436f6e20284c290000000000,Joy-Con (L),a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,rightshoulder:b20,start:b17,x:b19,y:b2,platform:Android, +38383665633039363066383334653465,Joy-Con (R),a:b0,b:b1,back:b5,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, +39363561613936303237333537383931,Joy-Con (R),a:b0,b:b1,back:b5,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, +4a6f792d436f6e202852290000000000,Joy-Con (R),a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, +39656136363638323036303865326464,JYS Aapter,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +63316564383539663166353034616434,JYS Adapter,a:b1,b:b3,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b0,y:b2,platform:Android, +64623163333561643339623235373232,Logitech F310,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +35623364393661626231343866613337,Logitech F710,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +4c6f6769746563682047616d65706164,Logitech F710,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +64396331333230326333313330336533,Logitech F710,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +39653365373864633935383236363438,Logitech G Cloud,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +416d617a6f6e2047616d6520436f6e74,Luna Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, +4c756e612047616d6570616400000000,Luna Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +30363066623539323534363639323363,Magic NS,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +31353762393935386662336365626334,Magic NS,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +39623565346366623931666633323530,Magic NS,a:b1,b:b3,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b0,y:b2,platform:Android, +6d6179666c617368206c696d69746564,Mayflash GameCube Adapter,a:b22,b:b21,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a5,righty:a2,start:b30,x:b23,y:b24,platform:Android, +436f6e74726f6c6c6572000000000000,Mayflash N64 Adapter,a:b1,b:b19,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a2,righty:a3,start:b18,platform:Android, +65666330633838383061313633326461,Mayflash N64 Adapter,a:b1,b:b19,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a2,righty:a3,start:b18,platform:Android, +37316565396364386635383230353365,Mayflash Saturn Adapter,a:b21,b:b22,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b26,lefttrigger:b28,rightshoulder:b27,righttrigger:b23,start:b30,x:b24,y:b25,platform:Android, +4875694a696120205553422047616d65,Mayflash Saturn Adapter,a:b21,b:b22,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b26,lefttrigger:b28,rightshoulder:b27,righttrigger:b23,start:b30,x:b24,y:b25,platform:Android, +535a4d792d706f776572204c54442043,Mayflash Wii Classic Adapter,a:b23,b:b22,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b31,leftshoulder:b27,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a2,righty:a3,start:b30,x:b24,y:b21,platform:Android, +30653962643666303631376438373532,Mayflash Wii DolphinBar,a:b23,b:b24,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b0,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b21,y:b22,platform:Android, +39346131396233376535393665363161,Mayflash Wii U Pro Adapter,a:b22,b:b23,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b25,leftstick:b31,lefttrigger:b27,rightshoulder:b26,rightstick:b0,righttrigger:b28,rightx:a0,righty:a1,start:b30,x:b21,y:b24,platform:Android, 31323564663862633234646330373138,Mega Drive,a:b23,b:b22,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,rightshoulder:b25,righttrigger:b26,start:b30,x:b24,y:b21,platform:Android, -37333564393261653735306132613061,Mega Drive,a:b21,b:b22,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b26,lefttrigger:b28,rightshoulder:b27,righttrigger:b23,start:b30,x:b24,y:b25,platform:Android, -64363363336633363736393038313464,Mega Drive,a:b1,b:b0,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,start:b9,x:b2,y:b3,platform:Android, -33323763323132376537376266393366,Microsoft Dual Strike,a:b24,b:b23,back:b25,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b27,lefttrigger:b29,rightshoulder:b78,rightx:a0,righty:a1~,start:b26,x:b22,y:b21,platform:Android, +37333564393261653735306132613061,Mega Drive,a:b21,b:b22,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b26,lefttrigger:b28,rightshoulder:b27,righttrigger:b23,start:b30,x:b24,y:b25,platform:Android, +64363363336633363736393038313464,Mega Drive,a:b1,b:b0,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,start:b9,x:b2,y:b3,platform:Android, +33323763323132376537376266393366,Microsoft Dual Strike,a:b24,b:b23,back:b25,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b27,lefttrigger:b29,rightshoulder:b78,rightx:a0,righty:a1~,start:b26,x:b22,y:b21,platform:Android, 30306461613834333439303734316539,Microsoft SideWinder Pro,a:b0,b:b1,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b20,lefttrigger:b9,rightshoulder:b19,righttrigger:b10,start:b17,x:b2,y:b3,platform:Android, -32386235353630393033393135613831,Microsoft Xbox Series Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -4d4f42415041442050726f2d48440000,Mobapad Chitu HD,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -4d4f435554452d303533582d4d35312d,Mocute 053X,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -33343361376163623438613466616531,Mocute M053,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -39306635663061636563316166303966,Mocute M053,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +32386235353630393033393135613831,Microsoft Xbox Series Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +4d4f42415041442050726f2d48440000,Mobapad Chitu HD,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +4d4f435554452d303533582d4d35312d,Mocute 053X,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +33343361376163623438613466616531,Mocute M053,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +39306635663061636563316166303966,Mocute M053,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 7573622067616d657061642020202020,NEXT SNES Controller,a:b2,b:b1,back:b8,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b4,rightshoulder:b5,righttrigger:b6,start:b9,x:b3,y:b0,platform:Android, 050000007e05000009200000ffff0f00,Nintendo Switch Pro Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b16,x:b17,y:b2,platform:Android, 34323437396534643531326161633738,Nintendo Switch Pro Controller,a:b0,b:b1,back:b15,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,misc1:b5,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -50726f20436f6e74726f6c6c65720000,Nintendo Switch Pro Controller,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b2,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b10,rightx:a2,righty:a3,start:b18,y:b3,platform:Android, -36326533353166323965623661303933,NSO N64 Controller,+rightx:b17,+righty:b10,-rightx:b2,-righty:b19,a:b1,b:b0,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,misc1:b7,rightshoulder:b20,righttrigger:b15,start:b18,platform:Android, -4e363420436f6e74726f6c6c65720000,NSO N64 Controller,+rightx:b17,+righty:b10,-rightx:b2,-righty:b19,a:b1,b:b0,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,misc1:b7,rightshoulder:b20,righttrigger:b15,start:b18,platform:Android, -534e455320436f6e74726f6c6c657200,NSO SNES Controller,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, -64623863346133633561626136366634,NSO SNES Controller,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, +50726f20436f6e74726f6c6c65720000,Nintendo Switch Pro Controller,a:b0,b:b1,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b2,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:b10,rightx:a2,righty:a3,start:b18,y:b3,platform:Android, +36326533353166323965623661303933,NSO N64 Controller,+rightx:b17,+righty:b10,-rightx:b2,-righty:b19,a:b1,b:b0,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,misc1:b7,rightshoulder:b20,righttrigger:b15,start:b18,platform:Android, +4e363420436f6e74726f6c6c65720000,NSO N64 Controller,+rightx:b17,+righty:b10,-rightx:b2,-righty:b19,a:b1,b:b0,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,misc1:b7,rightshoulder:b20,righttrigger:b15,start:b18,platform:Android, +534e455320436f6e74726f6c6c657200,NSO SNES Controller,a:b0,b:b1,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, +64623863346133633561626136366634,NSO SNES Controller,a:b0,b:b1,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,rightshoulder:b20,start:b18,x:b19,y:b2,platform:Android, 050000005509000003720000cf7f3f00,NVIDIA Controller,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000005509000010720000ffff3f00,NVIDIA Controller,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000005509000014720000df7f3f00,NVIDIA Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a5,start:b6,x:b2,y:b3,platform:Android, 050000005509000014720000df7f3f80,NVIDIA Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a3,rightx:a4,righty:a5,start:b6,x:b2,y:b3,platform:Android, -37336435666338653565313731303834,NVIDIA Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -4e564944494120436f72706f72617469,NVIDIA Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -61363931656135336130663561616264,NVIDIA Controller,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +37336435666338653565313731303834,NVIDIA Controller,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +4e564944494120436f72706f72617469,NVIDIA Controller,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +61363931656135336130663561616264,NVIDIA Controller,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 39383335313438623439373538343266,OUYA Controller,a:b0,b:b2,dpdown:b18,dpleft:b15,dpright:b16,dpup:b17,leftshoulder:b3,leftstick:b9,lefttrigger:b5,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b10,righttrigger:b7,rightx:a3,righty:a4,x:b1,y:b19,platform:Android, 4f5559412047616d6520436f6e74726f,OUYA Controller,a:b0,b:b2,dpdown:b18,dpleft:b15,dpright:b6,dpup:b17,leftshoulder:b3,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b10,righttrigger:a5,rightx:a3,righty:a4,x:b1,y:b19,platform:Android, -506572666f726d616e63652044657369,PDP PS3 Rock Candy Controller,a:b1,b:b17,back:h0.2,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b4,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b16,x:b0,y:b2,platform:Android, +506572666f726d616e63652044657369,PDP PS3 Rock Candy Controller,a:b1,b:b17,back:h0.2,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b4,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b16,x:b0,y:b2,platform:Android, 62653335326261303663356263626339,PlayStation Classic Controller,a:b19,b:b1,back:b17,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,lefttrigger:b3,rightshoulder:b10,righttrigger:b20,start:b18,x:b2,y:b0,platform:Android, -536f6e7920496e746572616374697665,PlayStation Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,misc1:b8,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -576972656c65737320436f6e74726f6c,PlayStation Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -61653962353232366130326530363061,Pokken,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:b9,rightshoulder:b20,righttrigger:b10,start:b18,x:b0,y:b2,platform:Android, -32666633663735353234363064386132,PS2,a:b23,b:b22,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b27,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a3,righty:a2,start:b30,x:b24,y:b21,platform:Android, +536f6e7920496e746572616374697665,PlayStation Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,misc1:b8,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +576972656c65737320436f6e74726f6c,PlayStation Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +61653962353232366130326530363061,Pokken,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:b9,rightshoulder:b20,righttrigger:b10,start:b18,x:b0,y:b2,platform:Android, +32666633663735353234363064386132,PS2,a:b23,b:b22,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b27,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a3,righty:a2,start:b30,x:b24,y:b21,platform:Android, 050000004c05000068020000dfff3f00,PS3 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 536f6e7920504c415953544154494f4e,PS3 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -61363034663839376638653463633865,PS3 Controller,a:b0,b:b1,back:b15,dpdown:a14,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +61363034663839376638653463633865,PS3 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 66366539656564653432353139356536,PS3 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 66383132326164626636313737373037,PS3 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000004c050000c405000000783f00,PS4 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, @@ -1895,89 +1944,89 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 050000004c050000c4050000ffff3f00,PS4 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000004c050000cc090000fffe3f00,PS4 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000004c050000cc090000ffff3f00,PS4 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -30303839663330346632363232623138,PS4 Controller,a:b1,b:b17,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:a4,rightx:a2,righty:a5,start:b16,x:b0,y:b2,platform:Android, -31326235383662333266633463653332,PS4 Controller,a:b1,b:b16,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:a4,rightx:a2,righty:a5,start:b17,x:b0,y:b2,platform:Android, -31373231336561636235613666323035,PS4 Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -31663838336334393132303338353963,PS4 Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -34613139376634626133336530386430,PS4 Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -35643031303033326130316330353564,PS4 Controller,a:b1,b:b17,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:+a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:+a4,rightx:a2,righty:a5,start:b16,x:b0,y:b2,platform:Android, -37626233336235343937333961353732,PS4 Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -37626464343430636562316661643863,PS4 Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -38393161636261653636653532386639,PS4 Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -63313733393535663339656564343962,PS4 Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -63393662363836383439353064663939,PS4 Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -65366465656364636137653363376531,PS4 Controller,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -66613532303965383534396638613230,PS4 Controller,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a5,start:b18,x:b0,y:b2,platform:Android, +30303839663330346632363232623138,PS4 Controller,a:b1,b:b17,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:a4,rightx:a2,righty:a5,start:b16,x:b0,y:b2,platform:Android, +31326235383662333266633463653332,PS4 Controller,a:b1,b:b16,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:a4,rightx:a2,righty:a5,start:b17,x:b0,y:b2,platform:Android, +31373231336561636235613666323035,PS4 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +31663838336334393132303338353963,PS4 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +34613139376634626133336530386430,PS4 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +35643031303033326130316330353564,PS4 Controller,a:b1,b:b17,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:+a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:+a4,rightx:a2,righty:a5,start:b16,x:b0,y:b2,platform:Android, +37626233336235343937333961353732,PS4 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +37626464343430636562316661643863,PS4 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38393161636261653636653532386639,PS4 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +63313733393535663339656564343962,PS4 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +63393662363836383439353064663939,PS4 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +65366465656364636137653363376531,PS4 Controller,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +66613532303965383534396638613230,PS4 Controller,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a5,start:b18,x:b0,y:b2,platform:Android, 050000004c050000e60c0000fffe3f00,PS5 Controller,a:b1,b:b17,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:a3,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:a4,rightx:a2,righty:a5,start:b16,x:b0,y:b2,platform:Android, -050000004c050000e60c0000fffe3f80,PS5 Controller,a:b1,b:b17,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:a3,rightx:a4,righty:a5,start:b16,x:b0,y:b2,platform:Android, +050000004c050000e60c0000fffe3f80,PS5 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b4,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b6,righttrigger:a3,rightx:a4,righty:a5,start:b16,x:b2,y:b17,platform:Android, 050000004c050000e60c0000ffff3f00,PS5 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -32346465346533616263386539323932,PS5 Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -32633532643734376632656664383733,PS5 Controller,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a5,start:b18,x:b0,y:b2,platform:Android, -37363764353731323963323639666565,PS5 Controller,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a5,start:b18,x:b0,y:b2,platform:Android, -61303162353165316365336436343139,PS5 Controller,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,misc1:b8,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a5,start:b18,x:b0,y:b2,platform:Android, -64336263393933626535303339616332,Qanba 4RAF,a:b0,b:b1,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b20,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b3,righttrigger:b9,rightx:a2,righty:a3,start:b18,x:b19,y:b2,platform:Android, -36626666353861663864336130363137,Razer Junglecat,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +32346465346533616263386539323932,PS5 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +32633532643734376632656664383733,PS5 Controller,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a5,start:b18,x:b0,y:b2,platform:Android, +37363764353731323963323639666565,PS5 Controller,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a5,start:b18,x:b0,y:b2,platform:Android, +61303162353165316365336436343139,PS5 Controller,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,misc1:b8,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a5,start:b18,x:b0,y:b2,platform:Android, +64336263393933626535303339616332,Qanba 4RAF,a:b0,b:b1,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b20,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:b3,righttrigger:b9,rightx:a2,righty:a3,start:b18,x:b19,y:b2,platform:Android, +36626666353861663864336130363137,Razer Junglecat,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 05000000f8270000bf0b0000ffff3f00,Razer Kishi,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -62653861643333663663383332396665,Razer Kishi,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +62653861643333663663383332396665,Razer Kishi,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000003215000005070000ffff3f00,Razer Raiju Mobile,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000003215000007070000ffff3f00,Razer Raiju Mobile,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000003215000000090000bf7f3f00,Razer Serval,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b2,y:b3,platform:Android, -5a6869587520526574726f2042697420,Retro Bit Saturn Controller,a:b21,b:b22,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b25,lefttrigger:b26,rightshoulder:b27,righttrigger:b28,start:b30,x:b23,y:b24,platform:Android, +5a6869587520526574726f2042697420,Retro Bit Saturn Controller,a:b21,b:b22,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b25,lefttrigger:b26,rightshoulder:b27,righttrigger:b28,start:b30,x:b23,y:b24,platform:Android, 32417865732031314b6579732047616d,Retro Bit SNES Controller,a:b0,b:b1,back:b15,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b2,y:b3,platform:Android, 36313938306539326233393732613361,Retro Bit SNES Controller,a:b0,b:b1,back:b15,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b2,y:b3,platform:Android, -526574726f466c616720576972656420,Retro Controller,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b17,rightshoulder:b18,start:b10,x:b2,y:b3,platform:Android, -61343739353764363165343237303336,Retro Controller,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b17,lefttrigger:b18,leftx:a0,lefty:a1,start:b10,x:b2,y:b3,platform:Android, -526574726f696420506f636b65742043,Retroid Pocket,a:b1,b:b0,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b17,paddle2:b18,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, -582d426f7820436f6e74726f6c6c6572,Retroid Pocket,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b17,paddle2:b18,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -38653130373365613538333235303036,Retroid Pocket 2,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -64363363336633363736393038313463,Retrolink,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,start:b6,platform:Android, +526574726f466c616720576972656420,Retro Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b17,rightshoulder:b18,start:b10,x:b2,y:b3,platform:Android, +61343739353764363165343237303336,Retro Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b17,lefttrigger:b18,leftx:a0,lefty:a1,start:b10,x:b2,y:b3,platform:Android, +526574726f696420506f636b65742043,Retroid Pocket,a:b1,b:b0,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b17,paddle2:b18,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +582d426f7820436f6e74726f6c6c6572,Retroid Pocket,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,paddle1:b17,paddle2:b18,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +38653130373365613538333235303036,Retroid Pocket 2,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +64363363336633363736393038313463,Retrolink,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,start:b6,platform:Android, 37393234373533633333323633646531,RetroUSB N64 RetroPort,+rightx:b17,+righty:b15,-rightx:b18,-righty:b6,a:b10,b:b9,dpdown:b19,dpleft:b1,dpright:b0,dpup:b2,leftshoulder:b7,lefttrigger:b20,leftx:a0,lefty:a1,rightshoulder:b5,start:b3,platform:Android, 5365616c6965436f6d707574696e6720,RetroUSB N64 RetroPort,+rightx:b17,+righty:b15,-rightx:b18,-righty:b6,a:b10,b:b9,dpdown:b19,dpleft:b1,dpright:b0,dpup:b2,leftshoulder:b7,lefttrigger:b20,leftx:a0,lefty:a1,rightshoulder:b5,start:b3,platform:Android, 526574726f5553422e636f6d20534e45,RetroUSB SNES RetroPort,a:b1,b:b20,back:b19,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b2,x:b0,y:b3,platform:Android, 64643037633038386238303966376137,RetroUSB SNES RetroPort,a:b1,b:b20,back:b19,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,rightshoulder:b10,start:b2,x:b0,y:b3,platform:Android, -37656564346533643138636436356230,Rock Candy Switch Controller,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,misc1:b7,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, -33373336396634316434323337666361,RumblePad 2,a:b22,b:b23,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b21,y:b24,platform:Android, -36363537303435333566386638366333,Samsung EIGP20,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -53616d73756e672047616d6520506164,Samsung EIGP20,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +37656564346533643138636436356230,Rock Candy Switch Controller,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b3,leftstick:b15,lefttrigger:b9,leftx:a0,lefty:a1,misc1:b7,rightshoulder:b20,rightstick:b6,righttrigger:b10,rightx:a2,righty:a3,start:b18,x:b0,y:b2,platform:Android, +33373336396634316434323337666361,RumblePad 2,a:b22,b:b23,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b25,lefttrigger:b27,leftx:a0,lefty:a1,rightshoulder:b26,righttrigger:b28,rightx:a2,righty:a3,start:b30,x:b21,y:b24,platform:Android, +36363537303435333566386638366333,Samsung EIGP20,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +53616d73756e672047616d6520506164,Samsung EIGP20,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 66386565396238363534313863353065,Sanwa PlayOnline Mobile,a:b21,b:b22,back:b23,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,start:b24,platform:Android, -32383165316333383766336338373261,Saturn,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:a4,righttrigger:a5,x:b2,y:b3,platform:Android, +32383165316333383766336338373261,Saturn,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:a4,righttrigger:a5,x:b2,y:b3,platform:Android, 38613865396530353338373763623431,Saturn,a:b0,b:b1,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b9,lefttrigger:b10,rightshoulder:b20,righttrigger:b19,start:b17,x:b2,y:b3,platform:Android, -61316232336262373631343137633631,Saturn,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:a4,righttrigger:a5,x:b2,y:b3,platform:Android, -30353835333338613130373363646337,SG H510,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b17,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b18,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, -66386262366536653765333235343634,SG H510,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b2,y:b3,platform:Android, -66633132393363353531373465633064,SG H510,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftstick:b17,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b18,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, -62653761636366393366613135366338,SN30 PP,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, +61316232336262373631343137633631,Saturn,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,lefttrigger:b10,leftx:a0,lefty:a1,rightshoulder:a4,righttrigger:a5,x:b2,y:b3,platform:Android, +30353835333338613130373363646337,SG H510,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b17,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b18,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, +66386262366536653765333235343634,SG H510,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,x:b2,y:b3,platform:Android, +66633132393363353531373465633064,SG H510,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b17,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b20,rightstick:b18,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b19,y:b2,platform:Android, +62653761636366393366613135366338,SN30 PP,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b3,y:b2,platform:Android, 38376662666661636265313264613039,SNES,a:b0,b:b1,back:b9,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b3,rightshoulder:b20,start:b10,x:b19,y:b2,platform:Android, 5346432f555342205061640000000000,SNES Adapter,a:b0,b:b1,back:b9,dpdown:+a1,dpleft:-a0,dpright:+a0,dpup:-a1,leftshoulder:b3,rightshoulder:b20,start:b10,x:b19,y:b2,platform:Android, -5553422047616d657061642000000000,SNES Controller,a:b1,b:b0,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,rightshoulder:b10,start:b6,x:b3,y:b2,platform:Android, -63303964303462366136616266653561,Sony PSP,a:b21,b:b22,back:b27,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b25,leftx:a0,lefty:a1,rightshoulder:b26,start:b28,x:b23,y:b24,platform:Android, -63376637643462343766333462383235,Sony Vita,a:b1,b:b19,back:b17,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a3,righty:a4,start:b18,x:b0,y:b2,platform:Android, -476f6f676c65204c4c43205374616469,Stadia Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +5553422047616d657061642000000000,SNES Controller,a:b1,b:b0,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,rightshoulder:b10,start:b6,x:b3,y:b2,platform:Android, +63303964303462366136616266653561,Sony PSP,a:b21,b:b22,back:b27,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b25,leftx:a0,lefty:a1,rightshoulder:b26,start:b28,x:b23,y:b24,platform:Android, +63376637643462343766333462383235,Sony Vita,a:b1,b:b19,back:b17,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftx:a0,lefty:a1,rightshoulder:b20,rightx:a3,righty:a4,start:b18,x:b0,y:b2,platform:Android, +476f6f676c65204c4c43205374616469,Stadia Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 5374616469614e3848532d6532633400,Stadia Controller,a:b0,b:b1,back:b15,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 05000000de2800000511000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,platform:Android, 05000000de2800000611000001000000,Steam Controller,a:b0,b:b1,back:b6,guide:b8,leftshoulder:b4,leftstick:b9,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b5,righttrigger:a3,start:b7,x:b2,y:b3,platform:Android, -0500000011010000201400000f7e0f00,SteelSeries Nimbus,a:b0,b:b1,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,rightx:a2,righty:a3,x:b19,y:b2,platform:Android, +0500000011010000201400000f7e0f00,SteelSeries Nimbus,a:b0,b:b1,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,lefttrigger:b9,leftx:a0,lefty:a1,rightshoulder:b20,righttrigger:b10,rightx:a2,righty:a3,x:b19,y:b2,platform:Android, 35306436396437373135383665646464,SteelSeries Nimbus Plus,a:b0,b:b1,leftshoulder:b3,leftstick:b17,lefttrigger:b9,leftx:a0,rightshoulder:b20,rightstick:b18,righttrigger:b10,rightx:a2,x:b19,y:b2,platform:Android, 33313930373536613937326534303931,Taito Egret II Mini Control Panel,a:b25,b:b23,back:b27,guide:b30,leftx:a0,lefty:a1,rightshoulder:b21,righttrigger:b22,start:b28,x:b29,y:b24,platform:Android, -54475a20436f6e74726f6c6c65720000,TGZ Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -62363434353532386238336663643836,TGZ Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +54475a20436f6e74726f6c6c65720000,TGZ Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +62363434353532386238336663643836,TGZ Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:b17,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:b18,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 37323236633763666465316365313236,THEC64 Joystick,a:b21,b:b22,back:b27,leftshoulder:b25,leftx:a0,lefty:a1,rightshoulder:b26,start:b27,x:b23,y:b24,platform:Android, 38346162326232346533316164363336,THEGamepad,a:b23,b:b22,back:b27,leftshoulder:b25,leftx:a0,lefty:a1,rightshoulder:b26,start:b28,x:b24,y:b21,platform:Android, 050000004f0400000ed00000fffe3f00,ThrustMaster eSwap Pro Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -5477696e20555342204a6f7973746963,Twin Joystick,a:b22,b:b21,back:b28,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b26,leftstick:b30,lefttrigger:b24,leftx:a0,lefty:a1,rightshoulder:b27,rightstick:b31,righttrigger:b25,rightx:a3,righty:a2,start:b29,x:b23,y:b20,platform:Android, -30623739343039643830333266346439,Valve Steam Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,leftx:a0,lefty:a1,paddle1:b24,paddle2:b23,rightshoulder:b10,rightstick:b8,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -31643365666432386133346639383937,Valve Steam Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,leftx:a0,lefty:a1,paddle1:b24,paddle2:b23,rightshoulder:b10,rightstick:b8,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -30386438313564306161393537333663,Wii Classic Adapter,a:b23,b:b22,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b27,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a2,righty:a3,start:b30,x:b24,y:b21,platform:Android, -33333034646336346339646538643633,Wii Classic Adapter,a:b23,b:b22,back:b29,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b27,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a2,righty:a3,start:b30,x:b24,y:b21,platform:Android, +5477696e20555342204a6f7973746963,Twin Joystick,a:b22,b:b21,back:b28,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b26,leftstick:b30,lefttrigger:b24,leftx:a0,lefty:a1,rightshoulder:b27,rightstick:b31,righttrigger:b25,rightx:a3,righty:a2,start:b29,x:b23,y:b20,platform:Android, +30623739343039643830333266346439,Valve Steam Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,leftx:a0,lefty:a1,paddle1:b24,paddle2:b23,rightshoulder:b10,rightstick:b8,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +31643365666432386133346639383937,Valve Steam Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,leftx:a0,lefty:a1,paddle1:b24,paddle2:b23,rightshoulder:b10,rightstick:b8,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +30386438313564306161393537333663,Wii Classic Adapter,a:b23,b:b22,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b27,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a2,righty:a3,start:b30,x:b24,y:b21,platform:Android, +33333034646336346339646538643633,Wii Classic Adapter,a:b23,b:b22,back:b29,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b27,lefttrigger:b25,leftx:a0,lefty:a1,rightshoulder:b28,righttrigger:b26,rightx:a2,righty:a3,start:b30,x:b24,y:b21,platform:Android, 050000005e0400008e02000000783f00,Xbox 360 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 30396232393162346330326334636566,Xbox 360 Controller,a:b0,b:b1,back:b4,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 38313038323730383864666463383533,Xbox 360 Controller,a:b0,b:b1,back:b4,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -58626f782033363020576972656c6573,Xbox 360 Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +58626f782033363020576972656c6573,Xbox 360 Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 65353331386662343338643939643636,Xbox 360 Controller,a:b0,b:b1,back:b4,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 65613532386633373963616462363038,Xbox 360 Controller,a:b0,b:b1,back:b4,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -47656e6572696320582d426f78207061,Xbox Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -4d6963726f736f667420582d426f7820,Xbox Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -64633436313965656664373634323364,Xbox Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +47656e6572696320582d426f78207061,Xbox Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +4d6963726f736f667420582d426f7820,Xbox Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a4,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +64633436313965656664373634323364,Xbox Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b19,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000005e04000091020000ff073f00,Xbox One Controller,a:b0,b:b1,back:b4,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, 050000005e04000091020000ff073f80,Xbox One Controller,a:b0,b:b1,back:b4,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000005e040000e00200000ffe3f00,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b3,leftstick:b15,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b16,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b17,y:b2,platform:Android, @@ -1985,11 +2034,11 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 050000005e040000e0020000ffff3f00,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b4,leftshoulder:b3,leftstick:b8,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b7,righttrigger:a5,rightx:a3,righty:a4,start:b10,x:b17,y:b2,platform:Android, 050000005e040000e0020000ffff3f80,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b4,leftshoulder:b3,leftstick:b8,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b7,righttrigger:a5,rightx:a2,righty:a3,start:b10,x:b17,y:b2,platform:Android, 050000005e040000fd020000ffff3f00,Xbox One Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -33356661323266333733373865656366,Xbox One Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -34356136633366613530316338376136,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b10,leftshoulder:b3,leftstick:b15,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b16,righttrigger:a5,rightx:a3,righty:a4,x:b17,y:b2,platform:Android, -35623965373264386238353433656138,Xbox One Controller,a:b0,b:b1,back:b4,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +33356661323266333733373865656366,Xbox One Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +34356136633366613530316338376136,Xbox One Controller,a:b0,b:b1,back:b9,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b10,leftshoulder:b3,leftstick:b15,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b18,rightstick:b16,righttrigger:a5,rightx:a3,righty:a4,x:b17,y:b2,platform:Android, +35623965373264386238353433656138,Xbox One Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 36616131643361333337396261666433,Xbox One Controller,a:b0,b:b1,back:b15,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -58626f7820576972656c65737320436f,Xbox One Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +58626f7820576972656c65737320436f,Xbox One Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000005e040000000b000000783f00,Xbox One Elite 2 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, 050000005e040000000b000000783f80,Xbox One Elite 2 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000005e040000050b0000ffff3f00,Xbox One Elite 2 Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a5,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a6,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, @@ -1998,8 +2047,8 @@ xinput,XInput Controller,a:b0,b:b1,back:b6,dpdown:h0.4,dpleft:h0.8,dpright:h0.2, 050000005e040000fd020000ff7f3f00,Xbox One S Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000005e040000120b000000783f00,Xbox Series Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a2,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a3,righty:a4,start:b6,x:b2,y:b3,platform:Android, 050000005e040000120b000000783f80,Xbox Series Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -050000005e040000130b0000ffff3f00,Xbox Series Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, -65633038363832353634653836396239,Xbox Series Controller,a:b0,b:b1,back:b15,dpdown:h0.4,dpleft:h0.8,dpright:h0.2,dpup:h0.1,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +050000005e040000130b0000ffff3f00,Xbox Series Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, +65633038363832353634653836396239,Xbox Series Controller,a:b0,b:b1,back:b15,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,guide:b5,leftshoulder:b9,leftstick:b7,lefttrigger:a4,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a5,rightx:a2,righty:a3,start:b6,x:b2,y:b3,platform:Android, 050000001727000044310000ffff3f00,XiaoMi Controller,a:b0,b:b1,back:b4,dpdown:b12,dpleft:b13,dpright:b14,dpup:b11,leftshoulder:b9,leftstick:b7,lefttrigger:a7,leftx:a0,lefty:a1,rightshoulder:b10,rightstick:b8,righttrigger:a6,rightx:a2,righty:a5,start:b6,x:b2,y:b3,platform:Android, # iOS diff --git a/core/input/input.cpp b/core/input/input.cpp index 7e2227c729fa..5314e9f02d1c 100644 --- a/core/input/input.cpp +++ b/core/input/input.cpp @@ -236,7 +236,7 @@ void Input::get_argument_options(const StringName &p_function, int p_idx, Listpush_back(name.quote()); } } @@ -308,6 +308,26 @@ bool Input::is_anything_pressed() const { return false; } +bool Input::is_anything_pressed_except_mouse() const { + _THREAD_SAFE_METHOD_ + + if (disable_input) { + return false; + } + + if (!keys_pressed.is_empty() || !joy_buttons_pressed.is_empty()) { + return true; + } + + for (const KeyValue &E : action_states) { + if (E.value.cache.pressed) { + return true; + } + } + + return false; +} + bool Input::is_key_pressed(Key p_keycode) const { _THREAD_SAFE_METHOD_ diff --git a/core/input/input.h b/core/input/input.h index a189ae7d9ada..005ddcca4fdb 100644 --- a/core/input/input.h +++ b/core/input/input.h @@ -294,6 +294,7 @@ class Input : public Object { static Input *get_singleton(); bool is_anything_pressed() const; + bool is_anything_pressed_except_mouse() const; bool is_key_pressed(Key p_keycode) const; bool is_physical_key_pressed(Key p_keycode) const; bool is_key_label_pressed(Key p_keycode) const; diff --git a/core/input/input_event.cpp b/core/input/input_event.cpp index 4733aaf220ae..bd793ef6b8fc 100644 --- a/core/input/input_event.cpp +++ b/core/input/input_event.cpp @@ -35,6 +35,9 @@ #include "core/os/keyboard.h" #include "core/os/os.h" +const int InputEvent::DEVICE_ID_EMULATION = -1; +const int InputEvent::DEVICE_ID_INTERNAL = -2; + void InputEvent::set_device(int p_device) { device = p_device; emit_changed(); @@ -751,6 +754,8 @@ Ref InputEventMouseButton::xformed_by(const Transform2D &p_xform, co mb->set_factor(factor); mb->set_button_index(button_index); + mb->merge_meta_from(this); + return mb; } @@ -971,6 +976,8 @@ Ref InputEventMouseMotion::xformed_by(const Transform2D &p_xform, co mm->set_velocity(p_xform.basis_xform(get_velocity())); mm->set_screen_velocity(get_screen_velocity()); + mm->merge_meta_from(this); + return mm; } @@ -1363,6 +1370,8 @@ Ref InputEventScreenTouch::xformed_by(const Transform2D &p_xform, co st->set_canceled(canceled); st->set_double_tap(double_tap); + st->merge_meta_from(this); + return st; } @@ -1491,6 +1500,8 @@ Ref InputEventScreenDrag::xformed_by(const Transform2D &p_xform, con sd->set_velocity(p_xform.basis_xform(velocity)); sd->set_screen_velocity(get_screen_velocity()); + sd->merge_meta_from(this); + return sd; } @@ -1702,6 +1713,8 @@ Ref InputEventMagnifyGesture::xformed_by(const Transform2D &p_xform, ev->set_position(p_xform.xform(get_position() + p_local_ofs)); ev->set_factor(get_factor()); + ev->merge_meta_from(this); + return ev; } @@ -1742,6 +1755,8 @@ Ref InputEventPanGesture::xformed_by(const Transform2D &p_xform, con ev->set_position(p_xform.xform(get_position() + p_local_ofs)); ev->set_delta(get_delta()); + ev->merge_meta_from(this); + return ev; } diff --git a/core/input/input_event.h b/core/input/input_event.h index 80bca28fbf4d..19176f748e92 100644 --- a/core/input/input_event.h +++ b/core/input/input_event.h @@ -62,9 +62,8 @@ class InputEvent : public Resource { static void _bind_methods(); public: - inline static constexpr int DEVICE_ID_EMULATION = -1; - inline static constexpr int DEVICE_ID_INTERNAL = -2; - inline static constexpr int DEVICE_ID_ALL_DEVICES = -3; // Signify that a given Action can be triggered by any device. + static const int DEVICE_ID_EMULATION; + static const int DEVICE_ID_INTERNAL; void set_device(int p_device); int get_device() const; diff --git a/core/input/input_map.cpp b/core/input/input_map.cpp index 954df36f3e34..2c056c4f08a6 100644 --- a/core/input/input_map.cpp +++ b/core/input/input_map.cpp @@ -39,6 +39,8 @@ InputMap *InputMap::singleton = nullptr; +int InputMap::ALL_DEVICES = -1; + void InputMap::_bind_methods() { ClassDB::bind_method(D_METHOD("has_action", "action"), &InputMap::has_action); ClassDB::bind_method(D_METHOD("get_actions"), &InputMap::_get_actions); @@ -104,7 +106,7 @@ void InputMap::get_argument_options(const StringName &p_function, int p_idx, Lis continue; } - String name = pi.name.substr(pi.name.find("/") + 1, pi.name.length()); + String name = pi.name.substr(pi.name.find_char('/') + 1, pi.name.length()); r_options->push_back(name.quote()); } } @@ -161,7 +163,7 @@ List>::Element *InputMap::_find_event(Action &p_action, const Re int i = 0; for (List>::Element *E = p_action.inputs.front(); E; E = E->next()) { int device = E->get()->get_device(); - if (device == InputEvent::DEVICE_ID_ALL_DEVICES || device == p_event->get_device()) { + if (device == ALL_DEVICES || device == p_event->get_device()) { if (E->get()->action_match(p_event, p_exact_match, p_action.deadzone, r_pressed, r_strength, r_raw_strength)) { if (r_event_index) { *r_event_index = i; @@ -302,7 +304,7 @@ void InputMap::load_from_project_settings() { continue; } - String name = pi.name.substr(pi.name.find("/") + 1, pi.name.length()); + String name = pi.name.substr(pi.name.find_char('/') + 1, pi.name.length()); Dictionary action = GLOBAL_GET(pi.name); float deadzone = action.has("deadzone") ? (float)action["deadzone"] : DEFAULT_DEADZONE; @@ -517,12 +519,15 @@ const HashMap>> &InputMap::get_builtins() { default_builtin_cache.insert("ui_text_completion_query", inputs); inputs = List>(); - inputs.push_back(InputEventKey::create_reference(Key::ENTER)); - inputs.push_back(InputEventKey::create_reference(Key::KP_ENTER)); + inputs.push_back(InputEventKey::create_reference(KeyModifierMask::SHIFT | Key::TAB)); + inputs.push_back(InputEventKey::create_reference(KeyModifierMask::SHIFT | Key::ENTER)); + inputs.push_back(InputEventKey::create_reference(KeyModifierMask::SHIFT | Key::KP_ENTER)); default_builtin_cache.insert("ui_text_completion_accept", inputs); inputs = List>(); inputs.push_back(InputEventKey::create_reference(Key::TAB)); + inputs.push_back(InputEventKey::create_reference(Key::ENTER)); + inputs.push_back(InputEventKey::create_reference(Key::KP_ENTER)); default_builtin_cache.insert("ui_text_completion_replace", inputs); // Newlines @@ -532,7 +537,6 @@ const HashMap>> &InputMap::get_builtins() { default_builtin_cache.insert("ui_text_newline", inputs); inputs = List>(); - inputs.push_back(InputEventKey::create_reference(Key::ENTER | KeyModifierMask::CMD_OR_CTRL)); inputs.push_back(InputEventKey::create_reference(Key::KP_ENTER | KeyModifierMask::CMD_OR_CTRL)); default_builtin_cache.insert("ui_text_newline_blank", inputs); diff --git a/core/input/input_map.h b/core/input/input_map.h index 2b2a0253321c..0479d45c57b5 100644 --- a/core/input/input_map.h +++ b/core/input/input_map.h @@ -43,6 +43,11 @@ class InputMap : public Object { GDCLASS(InputMap, Object); public: + /** + * A special value used to signify that a given Action can be triggered by any device + */ + static int ALL_DEVICES; + struct Action { int id; float deadzone; diff --git a/core/io/dir_access.cpp b/core/io/dir_access.cpp index 5d2f65ca998f..ca92b3e1181c 100644 --- a/core/io/dir_access.cpp +++ b/core/io/dir_access.cpp @@ -32,8 +32,8 @@ #include "core/config/project_settings.h" #include "core/io/file_access.h" -#include "core/os/memory.h" #include "core/os/os.h" +#include "core/os/time.h" #include "core/templates/local_vector.h" thread_local Error DirAccess::last_dir_open_error = OK; @@ -155,9 +155,9 @@ Error DirAccess::make_dir_recursive(const String &p_dir) { } else if (full_dir.begins_with("user://")) { base = "user://"; } else if (full_dir.is_network_share_path()) { - int pos = full_dir.find("/", 2); + int pos = full_dir.find_char('/', 2); ERR_FAIL_COND_V(pos < 0, ERR_INVALID_PARAMETER); - pos = full_dir.find("/", pos + 1); + pos = full_dir.find_char('/', pos + 1); ERR_FAIL_COND_V(pos < 0, ERR_INVALID_PARAMETER); base = full_dir.substr(0, pos + 1); } else if (full_dir.begins_with("/")) { @@ -323,6 +323,80 @@ Ref DirAccess::create(AccessType p_access) { return da; } +Ref DirAccess::create_temp(const String &p_prefix, bool p_keep, Error *r_error) { + const String ERROR_COMMON_PREFIX = "Error while creating temporary directory"; + + if (!p_prefix.is_valid_filename()) { + *r_error = ERR_FILE_BAD_PATH; + ERR_FAIL_V_MSG(Ref(), vformat(R"(%s: "%s" is not a valid prefix.)", ERROR_COMMON_PREFIX, p_prefix)); + } + + Ref dir_access = DirAccess::open(OS::get_singleton()->get_temp_path()); + + uint32_t suffix_i = 0; + String path; + while (true) { + String datetime = Time::get_singleton()->get_datetime_string_from_system().replace("-", "").replace("T", "").replace(":", ""); + datetime += itos(Time::get_singleton()->get_ticks_usec()); + String suffix = datetime + (suffix_i > 0 ? itos(suffix_i) : ""); + path = (p_prefix.is_empty() ? "" : p_prefix + "-") + suffix; + if (!path.is_valid_filename()) { + *r_error = ERR_FILE_BAD_PATH; + return Ref(); + } + if (!DirAccess::exists(path)) { + break; + } + suffix_i += 1; + } + + Error err = dir_access->make_dir(path); + if (err != OK) { + *r_error = err; + ERR_FAIL_V_MSG(Ref(), vformat(R"(%s: "%s" couldn't create directory "%s".)", ERROR_COMMON_PREFIX, path)); + } + err = dir_access->change_dir(path); + if (err != OK) { + *r_error = err; + return Ref(); + } + + dir_access->_is_temp = true; + dir_access->_temp_keep_after_free = p_keep; + dir_access->_temp_path = dir_access->get_current_dir(); + + *r_error = OK; + return dir_access; +} + +Ref DirAccess::_create_temp(const String &p_prefix, bool p_keep) { + return create_temp(p_prefix, p_keep, &last_dir_open_error); +} + +void DirAccess::_delete_temp() { + if (!_is_temp || _temp_keep_after_free) { + return; + } + + if (!DirAccess::exists(_temp_path)) { + return; + } + + Error err; + { + Ref dir_access = DirAccess::open(_temp_path, &err); + if (err != OK) { + return; + } + err = dir_access->erase_contents_recursive(); + if (err != OK) { + return; + } + } + + DirAccess::remove_absolute(_temp_path); +} + Error DirAccess::get_open_error() { return last_dir_open_error; } @@ -555,6 +629,7 @@ bool DirAccess::is_case_sensitive(const String &p_path) const { void DirAccess::_bind_methods() { ClassDB::bind_static_method("DirAccess", D_METHOD("open", "path"), &DirAccess::_open); ClassDB::bind_static_method("DirAccess", D_METHOD("get_open_error"), &DirAccess::get_open_error); + ClassDB::bind_static_method("DirAccess", D_METHOD("create_temp", "prefix", "keep"), &DirAccess::_create_temp, DEFVAL(""), DEFVAL(false)); ClassDB::bind_method(D_METHOD("list_dir_begin"), &DirAccess::list_dir_begin, DEFVAL(false), DEFVAL(false)); ClassDB::bind_method(D_METHOD("get_next"), &DirAccess::_get_next); @@ -588,6 +663,8 @@ void DirAccess::_bind_methods() { ClassDB::bind_method(D_METHOD("read_link", "path"), &DirAccess::read_link); ClassDB::bind_method(D_METHOD("create_link", "source", "target"), &DirAccess::create_link); + ClassDB::bind_method(D_METHOD("is_bundle", "path"), &DirAccess::is_bundle); + ClassDB::bind_method(D_METHOD("set_include_navigational", "enable"), &DirAccess::set_include_navigational); ClassDB::bind_method(D_METHOD("get_include_navigational"), &DirAccess::get_include_navigational); ClassDB::bind_method(D_METHOD("set_include_hidden", "enable"), &DirAccess::set_include_hidden); @@ -598,3 +675,7 @@ void DirAccess::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::BOOL, "include_navigational"), "set_include_navigational", "get_include_navigational"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "include_hidden"), "set_include_hidden", "get_include_hidden"); } + +DirAccess::~DirAccess() { + _delete_temp(); +} diff --git a/core/io/dir_access.h b/core/io/dir_access.h index 2392944f7618..bdeb4324831d 100644 --- a/core/io/dir_access.h +++ b/core/io/dir_access.h @@ -61,6 +61,13 @@ class DirAccess : public RefCounted { bool include_navigational = false; bool include_hidden = false; + bool _is_temp = false; + bool _temp_keep_after_free = false; + String _temp_path; + void _delete_temp(); + + static Ref _create_temp(const String &p_prefix = "", bool p_keep = false); + protected: static void _bind_methods(); @@ -136,6 +143,7 @@ class DirAccess : public RefCounted { } static Ref open(const String &p_path, Error *r_error = nullptr); + static Ref create_temp(const String &p_prefix = "", bool p_keep = false, Error *r_error = nullptr); static int _get_drive_count(); static String get_drive_name(int p_idx); @@ -160,9 +168,11 @@ class DirAccess : public RefCounted { bool get_include_hidden() const; virtual bool is_case_sensitive(const String &p_path) const; + virtual bool is_bundle(const String &p_file) const { return false; } +public: DirAccess() {} - virtual ~DirAccess() {} + virtual ~DirAccess(); }; #endif // DIR_ACCESS_H diff --git a/core/io/file_access.compat.inc b/core/io/file_access.compat.inc index ed16050126e1..97c7849e6773 100644 --- a/core/io/file_access.compat.inc +++ b/core/io/file_access.compat.inc @@ -34,8 +34,79 @@ Ref FileAccess::_open_encrypted_bind_compat_98918(const String &p_pa return open_encrypted(p_path, p_mode_flags, p_key, Vector()); } +void FileAccess::store_8_bind_compat_78289(uint8_t p_dest) { + store_8(p_dest); +} + +void FileAccess::store_16_bind_compat_78289(uint16_t p_dest) { + store_16(p_dest); +} + +void FileAccess::store_32_bind_compat_78289(uint32_t p_dest) { + store_32(p_dest); +} + +void FileAccess::store_64_bind_compat_78289(uint64_t p_dest) { + store_64(p_dest); +} + +void FileAccess::store_buffer_bind_compat_78289(const Vector &p_buffer) { + store_buffer(p_buffer); +} + +void FileAccess::store_var_bind_compat_78289(const Variant &p_var, bool p_full_objects) { + store_var(p_var, p_full_objects); +} + +void FileAccess::store_half_bind_compat_78289(float p_dest) { + store_half(p_dest); +} + +void FileAccess::store_float_bind_compat_78289(float p_dest) { + store_float(p_dest); +} + +void FileAccess::store_double_bind_compat_78289(double p_dest) { + store_double(p_dest); +} + +void FileAccess::store_real_bind_compat_78289(real_t p_real) { + store_real(p_real); +} + +void FileAccess::store_string_bind_compat_78289(const String &p_string) { + store_string(p_string); +} + +void FileAccess::store_line_bind_compat_78289(const String &p_line) { + store_line(p_line); +} + +void FileAccess::store_csv_line_bind_compat_78289(const Vector &p_values, const String &p_delim) { + store_csv_line(p_values, p_delim); +} + +void FileAccess::store_pascal_string_bind_compat_78289(const String &p_string) { + store_pascal_string(p_string); +} + void FileAccess::_bind_compatibility_methods() { ClassDB::bind_compatibility_static_method("FileAccess", D_METHOD("open_encrypted", "path", "mode_flags", "key"), &FileAccess::_open_encrypted_bind_compat_98918); + + ClassDB::bind_compatibility_method(D_METHOD("store_8", "value"), &FileAccess::store_8_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_16", "value"), &FileAccess::store_16_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_32", "value"), &FileAccess::store_32_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_64", "value"), &FileAccess::store_64_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_half", "value"), &FileAccess::store_half_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_float", "value"), &FileAccess::store_float_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_double", "value"), &FileAccess::store_double_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_real", "value"), &FileAccess::store_real_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_buffer", "buffer"), &FileAccess::store_buffer_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_line", "line"), &FileAccess::store_line_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_csv_line", "values", "delim"), &FileAccess::store_csv_line_bind_compat_78289, DEFVAL(",")); + ClassDB::bind_compatibility_method(D_METHOD("store_string", "string"), &FileAccess::store_string_bind_compat_78289); + ClassDB::bind_compatibility_method(D_METHOD("store_var", "value", "full_objects"), &FileAccess::store_var_bind_compat_78289, DEFVAL(false)); + ClassDB::bind_compatibility_method(D_METHOD("store_pascal_string", "string"), &FileAccess::store_pascal_string_bind_compat_78289); } -#endif // DISABLE_DEPRECATED +#endif diff --git a/core/io/file_access.cpp b/core/io/file_access.cpp index dd826e626bac..c04ad1cf495f 100644 --- a/core/io/file_access.cpp +++ b/core/io/file_access.cpp @@ -38,6 +38,7 @@ #include "core/io/file_access_pack.h" #include "core/io/marshalls.h" #include "core/os/os.h" +#include "core/os/time.h" FileAccess::CreateFunc FileAccess::create_func[ACCESS_MAX] = {}; @@ -71,7 +72,7 @@ void FileAccess::_set_access_type(AccessType p_access) { Ref FileAccess::create_for_path(const String &p_path) { Ref ret; - if (p_path.begins_with("res://")) { + if (p_path.begins_with("res://") || p_path.begins_with("uid://")) { ret = create(ACCESS_RESOURCES); } else if (p_path.begins_with("user://")) { ret = create(ACCESS_USERDATA); @@ -84,6 +85,79 @@ Ref FileAccess::create_for_path(const String &p_path) { return ret; } +Ref FileAccess::create_temp(int p_mode_flags, const String &p_prefix, const String &p_extension, bool p_keep, Error *r_error) { + const String ERROR_COMMON_PREFIX = "Error while creating temporary file"; + + if (!p_prefix.is_valid_filename()) { + *r_error = ERR_FILE_BAD_PATH; + ERR_FAIL_V_MSG(Ref(), vformat(R"(%s: "%s" is not a valid prefix.)", ERROR_COMMON_PREFIX, p_prefix)); + } + + if (!p_extension.is_valid_filename()) { + *r_error = ERR_FILE_BAD_PATH; + ERR_FAIL_V_MSG(Ref(), vformat(R"(%s: "%s" is not a valid extension.)", ERROR_COMMON_PREFIX, p_extension)); + } + + const String TEMP_DIR = OS::get_singleton()->get_temp_path(); + String extension = p_extension.trim_prefix("."); + + uint32_t suffix_i = 0; + String path; + while (true) { + String datetime = Time::get_singleton()->get_datetime_string_from_system().replace("-", "").replace("T", "").replace(":", ""); + datetime += itos(Time::get_singleton()->get_ticks_usec()); + String suffix = datetime + (suffix_i > 0 ? itos(suffix_i) : ""); + path = TEMP_DIR.path_join((p_prefix.is_empty() ? "" : p_prefix + "-") + suffix + (extension.is_empty() ? "" : "." + extension)); + if (!DirAccess::exists(path)) { + break; + } + suffix_i += 1; + } + + Error err; + { + // Create file first with WRITE mode. + // Otherwise, it would fail to open with a READ mode. + Ref ret = FileAccess::open(path, FileAccess::ModeFlags::WRITE, &err); + if (err != OK) { + *r_error = err; + ERR_FAIL_V_MSG(Ref(), vformat(R"(%s: could not create "%s".)", ERROR_COMMON_PREFIX, path)); + } + ret->flush(); + } + + // Open then the temp file with the correct mode flag. + Ref ret = FileAccess::open(path, p_mode_flags, &err); + if (err != OK) { + *r_error = err; + ERR_FAIL_V_MSG(Ref(), vformat(R"(%s: could not open "%s".)", ERROR_COMMON_PREFIX, path)); + } + if (ret.is_valid()) { + ret->_is_temp_file = true; + ret->_temp_keep_after_use = p_keep; + ret->_temp_path = ret->get_path_absolute(); + } + + *r_error = OK; + return ret; +} + +Ref FileAccess::_create_temp(int p_mode_flags, const String &p_prefix, const String &p_extension, bool p_keep) { + return create_temp(p_mode_flags, p_prefix, p_extension, p_keep, &last_file_open_error); +} + +void FileAccess::_delete_temp() { + if (!_is_temp_file || _temp_keep_after_use) { + return; + } + + if (!FileAccess::exists(_temp_path)) { + return; + } + + DirAccess::remove_absolute(_temp_path); +} + Error FileAccess::reopen(const String &p_path, int p_mode_flags) { return open_internal(p_path, p_mode_flags); } @@ -183,13 +257,17 @@ FileAccess::AccessType FileAccess::get_access_type() const { } String FileAccess::fix_path(const String &p_path) const { - //helper used by file accesses that use a single filesystem + // Helper used by file accesses that use a single filesystem. String r_path = p_path.replace("\\", "/"); switch (_access_type) { case ACCESS_RESOURCES: { if (ProjectSettings::get_singleton()) { + if (r_path.begins_with("uid://")) { + r_path = ResourceUID::uid_to_path(r_path); + } + if (r_path.begins_with("res://")) { String resource_path = ProjectSettings::get_singleton()->get_resource_path(); if (!resource_path.is_empty()) { @@ -264,6 +342,10 @@ uint64_t FileAccess::get_64() const { return data; } +float FileAccess::get_half() const { + return Math::half_to_float(get_16()); +} + float FileAccess::get_float() const { MarshallFloat m; m.i = get_32(); @@ -483,56 +565,60 @@ String FileAccess::get_as_utf8_string(bool p_skip_cr) const { w[len] = 0; String s; - s.parse_utf8((const char *)w, -1, p_skip_cr); + s.parse_utf8((const char *)w, len, p_skip_cr); return s; } -void FileAccess::store_8(uint8_t p_dest) { - store_buffer(&p_dest, sizeof(uint8_t)); +bool FileAccess::store_8(uint8_t p_dest) { + return store_buffer(&p_dest, sizeof(uint8_t)); } -void FileAccess::store_16(uint16_t p_dest) { +bool FileAccess::store_16(uint16_t p_dest) { if (big_endian) { p_dest = BSWAP16(p_dest); } - store_buffer(reinterpret_cast(&p_dest), sizeof(uint16_t)); + return store_buffer(reinterpret_cast(&p_dest), sizeof(uint16_t)); } -void FileAccess::store_32(uint32_t p_dest) { +bool FileAccess::store_32(uint32_t p_dest) { if (big_endian) { p_dest = BSWAP32(p_dest); } - store_buffer(reinterpret_cast(&p_dest), sizeof(uint32_t)); + return store_buffer(reinterpret_cast(&p_dest), sizeof(uint32_t)); } -void FileAccess::store_64(uint64_t p_dest) { +bool FileAccess::store_64(uint64_t p_dest) { if (big_endian) { p_dest = BSWAP64(p_dest); } - store_buffer(reinterpret_cast(&p_dest), sizeof(uint64_t)); + return store_buffer(reinterpret_cast(&p_dest), sizeof(uint64_t)); } -void FileAccess::store_real(real_t p_real) { +bool FileAccess::store_real(real_t p_real) { if constexpr (sizeof(real_t) == 4) { - store_float(p_real); + return store_float(p_real); } else { - store_double(p_real); + return store_double(p_real); } } -void FileAccess::store_float(float p_dest) { +bool FileAccess::store_half(float p_dest) { + return store_16(Math::make_half_float(p_dest)); +} + +bool FileAccess::store_float(float p_dest) { MarshallFloat m; m.f = p_dest; - store_32(m.i); + return store_32(m.i); } -void FileAccess::store_double(double p_dest) { +bool FileAccess::store_double(double p_dest) { MarshallDouble m; m.d = p_dest; - store_64(m.l); + return store_64(m.l); } uint64_t FileAccess::get_modified_time(const String &p_file) { @@ -616,19 +702,18 @@ Error FileAccess::set_read_only_attribute(const String &p_file, bool p_ro) { return err; } -void FileAccess::store_string(const String &p_string) { +bool FileAccess::store_string(const String &p_string) { if (p_string.length() == 0) { - return; + return true; } CharString cs = p_string.utf8(); - store_buffer((uint8_t *)&cs[0], cs.length()); + return store_buffer((uint8_t *)&cs[0], cs.length()); } -void FileAccess::store_pascal_string(const String &p_string) { +bool FileAccess::store_pascal_string(const String &p_string) { CharString cs = p_string.utf8(); - store_32(cs.length()); - store_buffer((uint8_t *)&cs[0], cs.length()); + return store_32(cs.length()) && store_buffer((uint8_t *)&cs[0], cs.length()); } String FileAccess::get_pascal_string() { @@ -639,24 +724,23 @@ String FileAccess::get_pascal_string() { cs[sl] = 0; String ret; - ret.parse_utf8(cs.ptr()); + ret.parse_utf8(cs.ptr(), sl); return ret; } -void FileAccess::store_line(const String &p_line) { - store_string(p_line); - store_8('\n'); +bool FileAccess::store_line(const String &p_line) { + return store_string(p_line) && store_8('\n'); } -void FileAccess::store_csv_line(const Vector &p_values, const String &p_delim) { - ERR_FAIL_COND(p_delim.length() != 1); +bool FileAccess::store_csv_line(const Vector &p_values, const String &p_delim) { + ERR_FAIL_COND_V(p_delim.length() != 1, false); String line = ""; int size = p_values.size(); for (int i = 0; i < size; ++i) { String value = p_values[i]; - if (value.contains("\"") || value.contains(p_delim) || value.contains("\n")) { + if (value.contains_char('"') || value.contains(p_delim) || value.contains_char('\n')) { value = "\"" + value.replace("\"", "\"\"") + "\""; } if (i < size - 1) { @@ -666,30 +750,43 @@ void FileAccess::store_csv_line(const Vector &p_values, const String &p_ line += value; } - store_line(line); + return store_line(line); } -void FileAccess::store_buffer(const Vector &p_buffer) { +bool FileAccess::store_buffer(const Vector &p_buffer) { uint64_t len = p_buffer.size(); + if (len == 0) { + return true; + } + const uint8_t *r = p_buffer.ptr(); - store_buffer(r, len); + return store_buffer(r, len); +} + +bool FileAccess::store_buffer(const uint8_t *p_src, uint64_t p_length) { + ERR_FAIL_COND_V(!p_src && p_length > 0, false); + for (uint64_t i = 0; i < p_length; i++) { + if (unlikely(!store_8(p_src[i]))) { + return false; + } + } + return true; } -void FileAccess::store_var(const Variant &p_var, bool p_full_objects) { +bool FileAccess::store_var(const Variant &p_var, bool p_full_objects) { int len; Error err = encode_variant(p_var, nullptr, len, p_full_objects); - ERR_FAIL_COND_MSG(err != OK, "Error when trying to encode Variant."); + ERR_FAIL_COND_V_MSG(err != OK, false, "Error when trying to encode Variant."); Vector buff; buff.resize(len); uint8_t *w = buff.ptrw(); err = encode_variant(p_var, &w[0], len, p_full_objects); - ERR_FAIL_COND_MSG(err != OK, "Error when trying to encode Variant."); + ERR_FAIL_COND_V_MSG(err != OK, false, "Error when trying to encode Variant."); - store_32(len); - store_buffer(buff); + return store_32(len) && store_buffer(buff); } Vector FileAccess::get_file_as_bytes(const String &p_path, Error *r_error) { @@ -811,6 +908,7 @@ void FileAccess::_bind_methods() { ClassDB::bind_static_method("FileAccess", D_METHOD("open_encrypted_with_pass", "path", "mode_flags", "pass"), &FileAccess::open_encrypted_pass); ClassDB::bind_static_method("FileAccess", D_METHOD("open_compressed", "path", "mode_flags", "compression_mode"), &FileAccess::open_compressed, DEFVAL(0)); ClassDB::bind_static_method("FileAccess", D_METHOD("get_open_error"), &FileAccess::get_open_error); + ClassDB::bind_static_method("FileAccess", D_METHOD("create_temp", "mode_flags", "prefix", "extension", "keep"), &FileAccess::_create_temp, DEFVAL(""), DEFVAL(""), DEFVAL(false)); ClassDB::bind_static_method("FileAccess", D_METHOD("get_file_as_bytes", "path"), &FileAccess::_get_file_as_bytes); ClassDB::bind_static_method("FileAccess", D_METHOD("get_file_as_string", "path"), &FileAccess::_get_file_as_string); @@ -829,6 +927,7 @@ void FileAccess::_bind_methods() { ClassDB::bind_method(D_METHOD("get_16"), &FileAccess::get_16); ClassDB::bind_method(D_METHOD("get_32"), &FileAccess::get_32); ClassDB::bind_method(D_METHOD("get_64"), &FileAccess::get_64); + ClassDB::bind_method(D_METHOD("get_half"), &FileAccess::get_half); ClassDB::bind_method(D_METHOD("get_float"), &FileAccess::get_float); ClassDB::bind_method(D_METHOD("get_double"), &FileAccess::get_double); ClassDB::bind_method(D_METHOD("get_real"), &FileAccess::get_real); @@ -847,10 +946,11 @@ void FileAccess::_bind_methods() { ClassDB::bind_method(D_METHOD("store_16", "value"), &FileAccess::store_16); ClassDB::bind_method(D_METHOD("store_32", "value"), &FileAccess::store_32); ClassDB::bind_method(D_METHOD("store_64", "value"), &FileAccess::store_64); + ClassDB::bind_method(D_METHOD("store_half", "value"), &FileAccess::store_half); ClassDB::bind_method(D_METHOD("store_float", "value"), &FileAccess::store_float); ClassDB::bind_method(D_METHOD("store_double", "value"), &FileAccess::store_double); ClassDB::bind_method(D_METHOD("store_real", "value"), &FileAccess::store_real); - ClassDB::bind_method(D_METHOD("store_buffer", "buffer"), (void(FileAccess::*)(const Vector &)) & FileAccess::store_buffer); + ClassDB::bind_method(D_METHOD("store_buffer", "buffer"), (bool(FileAccess::*)(const Vector &)) & FileAccess::store_buffer); ClassDB::bind_method(D_METHOD("store_line", "line"), &FileAccess::store_line); ClassDB::bind_method(D_METHOD("store_csv_line", "values", "delim"), &FileAccess::store_csv_line, DEFVAL(",")); ClassDB::bind_method(D_METHOD("store_string", "string"), &FileAccess::store_string); @@ -898,3 +998,7 @@ void FileAccess::_bind_methods() { BIND_BITFIELD_FLAG(UNIX_SET_GROUP_ID); BIND_BITFIELD_FLAG(UNIX_RESTRICTED_DELETE); } + +FileAccess::~FileAccess() { + _delete_temp(); +} diff --git a/core/io/file_access.h b/core/io/file_access.h index 88c8110a513c..da3cf0e28a80 100644 --- a/core/io/file_access.h +++ b/core/io/file_access.h @@ -112,6 +112,21 @@ class FileAccess : public RefCounted { #ifndef DISABLE_DEPRECATED static Ref _open_encrypted_bind_compat_98918(const String &p_path, ModeFlags p_mode_flags, const Vector &p_key); + void store_8_bind_compat_78289(uint8_t p_dest); + void store_16_bind_compat_78289(uint16_t p_dest); + void store_32_bind_compat_78289(uint32_t p_dest); + void store_64_bind_compat_78289(uint64_t p_dest); + void store_buffer_bind_compat_78289(const Vector &p_buffer); + void store_var_bind_compat_78289(const Variant &p_var, bool p_full_objects = false); + void store_half_bind_compat_78289(float p_dest); + void store_float_bind_compat_78289(float p_dest); + void store_double_bind_compat_78289(double p_dest); + void store_real_bind_compat_78289(real_t p_real); + void store_string_bind_compat_78289(const String &p_string); + void store_line_bind_compat_78289(const String &p_line); + void store_csv_line_bind_compat_78289(const Vector &p_values, const String &p_delim = ","); + void store_pascal_string_bind_compat_78289(const String &p_string); + static void _bind_compatibility_methods(); #endif @@ -128,6 +143,13 @@ class FileAccess : public RefCounted { static Ref _open(const String &p_path, ModeFlags p_mode_flags); + bool _is_temp_file = false; + bool _temp_keep_after_use = false; + String _temp_path; + void _delete_temp(); + + static Ref _create_temp(int p_mode_flags, const String &p_prefix = "", const String &p_extension = "", bool p_keep = false); + public: static void set_file_close_fail_notify_callback(FileCloseFailNotify p_cbk) { close_fail_notify = p_cbk; } @@ -148,6 +170,7 @@ class FileAccess : public RefCounted { virtual uint32_t get_32() const; ///< get 32 bits uint virtual uint64_t get_64() const; ///< get 64 bits uint + virtual float get_half() const; virtual float get_float() const; virtual double get_double() const; virtual real_t get_real() const; @@ -163,6 +186,7 @@ class FileAccess : public RefCounted { virtual String get_as_utf8_string(bool p_skip_cr = false) const; /** + * Use this for files WRITTEN in _big_ endian machines (ie, amiga/mac) * It's not about the current CPU type but file formats. * This flag gets reset to `false` (little endian) on each open. @@ -174,26 +198,27 @@ class FileAccess : public RefCounted { virtual Error resize(int64_t p_length) = 0; virtual void flush() = 0; - virtual void store_8(uint8_t p_dest); ///< store a byte - virtual void store_16(uint16_t p_dest); ///< store 16 bits uint - virtual void store_32(uint32_t p_dest); ///< store 32 bits uint - virtual void store_64(uint64_t p_dest); ///< store 64 bits uint + virtual bool store_8(uint8_t p_dest); ///< store a byte + virtual bool store_16(uint16_t p_dest); ///< store 16 bits uint + virtual bool store_32(uint32_t p_dest); ///< store 32 bits uint + virtual bool store_64(uint64_t p_dest); ///< store 64 bits uint - virtual void store_float(float p_dest); - virtual void store_double(double p_dest); - virtual void store_real(real_t p_real); + virtual bool store_half(float p_dest); + virtual bool store_float(float p_dest); + virtual bool store_double(double p_dest); + virtual bool store_real(real_t p_real); - virtual void store_string(const String &p_string); - virtual void store_line(const String &p_line); - virtual void store_csv_line(const Vector &p_values, const String &p_delim = ","); + virtual bool store_string(const String &p_string); + virtual bool store_line(const String &p_line); + virtual bool store_csv_line(const Vector &p_values, const String &p_delim = ","); - virtual void store_pascal_string(const String &p_string); + virtual bool store_pascal_string(const String &p_string); virtual String get_pascal_string(); - virtual void store_buffer(const uint8_t *p_src, uint64_t p_length) = 0; ///< store an array of bytes, needs to be overwritten by children. - void store_buffer(const Vector &p_buffer); + virtual bool store_buffer(const uint8_t *p_src, uint64_t p_length) = 0; ///< store an array of bytes, needs to be overwritten by children. + bool store_buffer(const Vector &p_buffer); - void store_var(const Variant &p_var, bool p_full_objects = false); + bool store_var(const Variant &p_var, bool p_full_objects = false); virtual void close() = 0; @@ -204,6 +229,7 @@ class FileAccess : public RefCounted { static Ref create(AccessType p_access); /// Create a file access (for the current platform) this is the only portable way of accessing files. static Ref create_for_path(const String &p_path); static Ref open(const String &p_path, int p_mode_flags, Error *r_error = nullptr); /// Create a file access (for the current platform) this is the only portable way of accessing files. + static Ref create_temp(int p_mode_flags, const String &p_prefix = "", const String &p_extension = "", bool p_keep = false, Error *r_error = nullptr); static Ref open_encrypted(const String &p_path, ModeFlags p_mode_flags, const Vector &p_key, const Vector &p_iv = Vector()); static Ref open_encrypted_pass(const String &p_path, ModeFlags p_mode_flags, const String &p_pass); @@ -239,8 +265,9 @@ class FileAccess : public RefCounted { create_func[p_access] = _create_builtin; } +public: FileAccess() {} - virtual ~FileAccess() {} + virtual ~FileAccess(); }; VARIANT_ENUM_CAST(FileAccess::CompressionMode); diff --git a/core/io/file_access_compressed.cpp b/core/io/file_access_compressed.cpp index f7f2852e0aaa..53ef93d09b9e 100644 --- a/core/io/file_access_compressed.cpp +++ b/core/io/file_access_compressed.cpp @@ -40,18 +40,6 @@ void FileAccessCompressed::configure(const String &p_magic, Compression::Mode p_ block_size = p_block_size; } -#define WRITE_FIT(m_bytes) \ - { \ - if (write_pos + (m_bytes) > write_max) { \ - write_max = write_pos + (m_bytes); \ - } \ - if (write_max > write_buffer_size) { \ - write_buffer_size = next_power_of_2(write_max); \ - buffer.resize(write_buffer_size); \ - write_ptr = buffer.ptrw(); \ - } \ - } - Error FileAccessCompressed::open_after_magic(Ref p_base) { f = p_base; cmode = (Compression::Mode)f->get_32(); @@ -309,13 +297,23 @@ void FileAccessCompressed::flush() { // compressed files keep data in memory till close() } -void FileAccessCompressed::store_buffer(const uint8_t *p_src, uint64_t p_length) { - ERR_FAIL_COND_MSG(f.is_null(), "File must be opened before use."); - ERR_FAIL_COND_MSG(!writing, "File has not been opened in write mode."); +bool FileAccessCompressed::store_buffer(const uint8_t *p_src, uint64_t p_length) { + ERR_FAIL_COND_V_MSG(f.is_null(), false, "File must be opened before use."); + ERR_FAIL_COND_V_MSG(!writing, false, "File has not been opened in write mode."); + + if (write_pos + (p_length) > write_max) { + write_max = write_pos + (p_length); + } + if (write_max > write_buffer_size) { + write_buffer_size = next_power_of_2(write_max); + ERR_FAIL_COND_V(buffer.resize(write_buffer_size) != OK, false); + write_ptr = buffer.ptrw(); + } - WRITE_FIT(p_length); memcpy(write_ptr + write_pos, p_src, p_length); + write_pos += p_length; + return true; } bool FileAccessCompressed::file_exists(const String &p_name) { diff --git a/core/io/file_access_compressed.h b/core/io/file_access_compressed.h index ea9837dd0394..607a45fc0a06 100644 --- a/core/io/file_access_compressed.h +++ b/core/io/file_access_compressed.h @@ -89,7 +89,7 @@ class FileAccessCompressed : public FileAccess { virtual Error resize(int64_t p_length) override { return ERR_UNAVAILABLE; } virtual void flush() override; - virtual void store_buffer(const uint8_t *p_src, uint64_t p_length) override; + virtual bool store_buffer(const uint8_t *p_src, uint64_t p_length) override; virtual bool file_exists(const String &p_name) override; ///< return true if a file exists diff --git a/core/io/file_access_encrypted.cpp b/core/io/file_access_encrypted.cpp index ba26f2e07bc6..c899c860c6d6 100644 --- a/core/io/file_access_encrypted.cpp +++ b/core/io/file_access_encrypted.cpp @@ -228,16 +228,17 @@ Error FileAccessEncrypted::get_error() const { return eofed ? ERR_FILE_EOF : OK; } -void FileAccessEncrypted::store_buffer(const uint8_t *p_src, uint64_t p_length) { - ERR_FAIL_COND_MSG(!writing, "File has not been opened in write mode."); - ERR_FAIL_COND(!p_src && p_length > 0); +bool FileAccessEncrypted::store_buffer(const uint8_t *p_src, uint64_t p_length) { + ERR_FAIL_COND_V_MSG(!writing, false, "File has not been opened in write mode."); + ERR_FAIL_COND_V(!p_src && p_length > 0, false); if (pos + p_length >= get_length()) { - data.resize(pos + p_length); + ERR_FAIL_COND_V(data.resize(pos + p_length) != OK, false); } memcpy(data.ptrw() + pos, p_src, p_length); pos += p_length; + return true; } void FileAccessEncrypted::flush() { diff --git a/core/io/file_access_encrypted.h b/core/io/file_access_encrypted.h index 63a8cab145b1..a373fc0cb5d9 100644 --- a/core/io/file_access_encrypted.h +++ b/core/io/file_access_encrypted.h @@ -82,7 +82,7 @@ class FileAccessEncrypted : public FileAccess { virtual Error resize(int64_t p_length) override { return ERR_UNAVAILABLE; } virtual void flush() override; - virtual void store_buffer(const uint8_t *p_src, uint64_t p_length) override; ///< store an array of bytes + virtual bool store_buffer(const uint8_t *p_src, uint64_t p_length) override; ///< store an array of bytes virtual bool file_exists(const String &p_name) override; ///< return true if a file exists diff --git a/core/io/file_access_memory.cpp b/core/io/file_access_memory.cpp index 8d7401163272..b5ab18407b3e 100644 --- a/core/io/file_access_memory.cpp +++ b/core/io/file_access_memory.cpp @@ -147,16 +147,16 @@ void FileAccessMemory::flush() { ERR_FAIL_NULL(data); } -void FileAccessMemory::store_buffer(const uint8_t *p_src, uint64_t p_length) { - ERR_FAIL_COND(!p_src && p_length > 0); +bool FileAccessMemory::store_buffer(const uint8_t *p_src, uint64_t p_length) { + ERR_FAIL_COND_V(!p_src && p_length > 0, false); uint64_t left = length - pos; uint64_t write = MIN(p_length, left); - if (write < p_length) { - WARN_PRINT("Writing less data than requested"); - } - memcpy(&data[pos], p_src, write); pos += write; + + ERR_FAIL_COND_V_MSG(write < p_length, false, "Writing less data than requested."); + + return true; } diff --git a/core/io/file_access_memory.h b/core/io/file_access_memory.h index 39e1528d9782..6845cf7130d0 100644 --- a/core/io/file_access_memory.h +++ b/core/io/file_access_memory.h @@ -61,7 +61,7 @@ class FileAccessMemory : public FileAccess { virtual Error resize(int64_t p_length) override { return ERR_UNAVAILABLE; } virtual void flush() override; - virtual void store_buffer(const uint8_t *p_src, uint64_t p_length) override; ///< store an array of bytes + virtual bool store_buffer(const uint8_t *p_src, uint64_t p_length) override; ///< store an array of bytes virtual bool file_exists(const String &p_name) override; ///< return true if a file exists diff --git a/core/io/file_access_pack.cpp b/core/io/file_access_pack.cpp index b9af1bfb57b1..5ccd8a8fb758 100644 --- a/core/io/file_access_pack.cpp +++ b/core/io/file_access_pack.cpp @@ -71,7 +71,7 @@ void PackedData::add_path(const String &p_pkg_path, const String &p_path, uint64 // Search for directory. PackedDir *cd = root; - if (simplified_path.contains("/")) { // In a subdirectory. + if (simplified_path.contains_char('/')) { // In a subdirectory. Vector ds = simplified_path.get_base_dir().split("/"); for (int j = 0; j < ds.size(); j++) { @@ -104,7 +104,7 @@ void PackedData::remove_path(const String &p_path) { // Search for directory. PackedDir *cd = root; - if (simplified_path.contains("/")) { // In a subdirectory. + if (simplified_path.contains_char('/')) { // In a subdirectory. Vector ds = simplified_path.get_base_dir().split("/"); for (int j = 0; j < ds.size(); j++) { @@ -309,7 +309,7 @@ bool PackedSourcePCK::try_open_pack(const String &p_path, bool p_replace_files, cs[sl] = 0; String path; - path.parse_utf8(cs.ptr()); + path.parse_utf8(cs.ptr(), sl); uint64_t ofs = f->get_64(); uint64_t size = f->get_64(); @@ -417,8 +417,8 @@ void FileAccessPack::flush() { ERR_FAIL(); } -void FileAccessPack::store_buffer(const uint8_t *p_src, uint64_t p_length) { - ERR_FAIL(); +bool FileAccessPack::store_buffer(const uint8_t *p_src, uint64_t p_length) { + ERR_FAIL_V(false); } bool FileAccessPack::file_exists(const String &p_name) { @@ -590,8 +590,6 @@ String DirAccessPack::get_current_dir(bool p_include_drive) const { } bool DirAccessPack::file_exists(String p_file) { - p_file = fix_path(p_file); - PackedData::PackedDir *pd = _find_dir(p_file.get_base_dir()); if (!pd) { return false; @@ -600,8 +598,6 @@ bool DirAccessPack::file_exists(String p_file) { } bool DirAccessPack::dir_exists(String p_dir) { - p_dir = fix_path(p_dir); - return _find_dir(p_dir) != nullptr; } diff --git a/core/io/file_access_pack.h b/core/io/file_access_pack.h index b957a43de275..701197116496 100644 --- a/core/io/file_access_pack.h +++ b/core/io/file_access_pack.h @@ -184,7 +184,7 @@ class FileAccessPack : public FileAccess { virtual Error resize(int64_t p_length) override { return ERR_UNAVAILABLE; } virtual void flush() override; - virtual void store_buffer(const uint8_t *p_src, uint64_t p_length) override; + virtual bool store_buffer(const uint8_t *p_src, uint64_t p_length) override; virtual bool file_exists(const String &p_name) override; diff --git a/core/io/file_access_zip.cpp b/core/io/file_access_zip.cpp index 41907d1a3fc8..047b27cf8239 100644 --- a/core/io/file_access_zip.cpp +++ b/core/io/file_access_zip.cpp @@ -322,8 +322,8 @@ void FileAccessZip::flush() { ERR_FAIL(); } -void FileAccessZip::store_buffer(const uint8_t *p_src, uint64_t p_length) { - ERR_FAIL(); +bool FileAccessZip::store_buffer(const uint8_t *p_src, uint64_t p_length) { + ERR_FAIL_V(false); } bool FileAccessZip::file_exists(const String &p_name) { diff --git a/core/io/file_access_zip.h b/core/io/file_access_zip.h index 1e11e050dfb9..57f1d203588a 100644 --- a/core/io/file_access_zip.h +++ b/core/io/file_access_zip.h @@ -101,7 +101,7 @@ class FileAccessZip : public FileAccess { virtual Error resize(int64_t p_length) override { return ERR_UNAVAILABLE; } virtual void flush() override; - virtual void store_buffer(const uint8_t *p_src, uint64_t p_length) override; + virtual bool store_buffer(const uint8_t *p_src, uint64_t p_length) override; virtual bool file_exists(const String &p_name) override; ///< return true if a file exists diff --git a/core/io/http_client.cpp b/core/io/http_client.cpp index 9e426c49080d..b7a324e710e5 100644 --- a/core/io/http_client.cpp +++ b/core/io/http_client.cpp @@ -101,7 +101,7 @@ Error HTTPClient::verify_headers(const Vector &p_headers) { for (int i = 0; i < p_headers.size(); i++) { String sanitized = p_headers[i].strip_edges(); ERR_FAIL_COND_V_MSG(sanitized.is_empty(), ERR_INVALID_PARAMETER, vformat("Invalid HTTP header at index %d: empty.", i)); - ERR_FAIL_COND_V_MSG(sanitized.find(":") < 1, ERR_INVALID_PARAMETER, + ERR_FAIL_COND_V_MSG(sanitized.find_char(':') < 1, ERR_INVALID_PARAMETER, vformat("Invalid HTTP header at index %d: String must contain header-value pair, delimited by ':', but was: '%s'.", i, p_headers[i])); } @@ -113,7 +113,7 @@ Dictionary HTTPClient::_get_response_headers_as_dictionary() { get_response_headers(&rh); Dictionary ret; for (const String &s : rh) { - int sp = s.find(":"); + int sp = s.find_char(':'); if (sp == -1) { continue; } diff --git a/core/io/http_client_tcp.cpp b/core/io/http_client_tcp.cpp index 237ba30a8067..3147b59c6017 100644 --- a/core/io/http_client_tcp.cpp +++ b/core/io/http_client_tcp.cpp @@ -484,7 +484,7 @@ Error HTTPClientTCP::poll() { // End of response, parse. response_str.push_back(0); String response; - response.parse_utf8((const char *)response_str.ptr()); + response.parse_utf8((const char *)response_str.ptr(), response_str.size()); Vector responses = response.split("\n"); body_size = -1; chunked = false; @@ -508,11 +508,11 @@ Error HTTPClientTCP::poll() { continue; } if (s.begins_with("content-length:")) { - body_size = s.substr(s.find(":") + 1, s.length()).strip_edges().to_int(); + body_size = s.substr(s.find_char(':') + 1, s.length()).strip_edges().to_int(); body_left = body_size; } else if (s.begins_with("transfer-encoding:")) { - String encoding = header.substr(header.find(":") + 1, header.length()).strip_edges(); + String encoding = header.substr(header.find_char(':') + 1, header.length()).strip_edges(); if (encoding == "chunked") { chunked = true; } diff --git a/core/io/image.cpp b/core/io/image.cpp index fa4484bb63b4..b35c98b1eba1 100644 --- a/core/io/image.cpp +++ b/core/io/image.cpp @@ -572,21 +572,18 @@ static bool _are_formats_compatible(Image::Format p_format0, Image::Format p_for void Image::convert(Format p_new_format) { ERR_FAIL_INDEX_MSG(p_new_format, FORMAT_MAX, vformat("The Image format specified (%d) is out of range. See Image's Format enum.", p_new_format)); - if (data.size() == 0) { - return; - } - if (p_new_format == format) { + if (data.size() == 0 || p_new_format == format) { return; } + ERR_FAIL_COND_MSG(Image::is_format_compressed(format) || Image::is_format_compressed(p_new_format), + "Cannot convert to (or from) compressed formats. Use compress() and decompress() instead."); + // Includes the main image. const int mipmap_count = get_mipmap_count() + 1; - if (Image::is_format_compressed(format) || Image::is_format_compressed(p_new_format)) { - ERR_FAIL_MSG("Cannot convert to <-> from compressed formats. Use compress() and decompress() instead."); - - } else if (!_are_formats_compatible(format, p_new_format)) { + if (!_are_formats_compatible(format, p_new_format)) { // Use put/set pixel which is slower but works with non-byte formats. Image new_img(width, height, mipmaps, p_new_format); @@ -612,9 +609,10 @@ void Image::convert(Format p_new_format) { return; } + // Convert the formats in an optimized way by removing/adding color channels if necessary. Image new_img(width, height, mipmaps, p_new_format); - int conversion_type = format | p_new_format << 8; + const int conversion_type = format | p_new_format << 8; for (int mip = 0; mip < mipmap_count; mip++) { int64_t mip_offset = 0; @@ -1801,7 +1799,7 @@ template static void _generate_po2_mipmap(const Component *p_src, Component *p_dst, uint32_t p_width, uint32_t p_height) { - //fast power of 2 mipmap generation + // Fast power of 2 mipmap generation. uint32_t dst_w = MAX(p_width >> 1, 1u); uint32_t dst_h = MAX(p_height >> 1, 1u); @@ -1831,99 +1829,116 @@ static void _generate_po2_mipmap(const Component *p_src, Component *p_dst, uint3 } } -void Image::shrink_x2() { - ERR_FAIL_COND(data.is_empty()); +void Image::_generate_mipmap_from_format(Image::Format p_format, const uint8_t *p_src, uint8_t *p_dst, uint32_t p_width, uint32_t p_height, bool p_renormalize) { + const float *src_float = reinterpret_cast(p_src); + float *dst_float = reinterpret_cast(p_dst); - if (mipmaps) { - //just use the lower mipmap as base and copy all - Vector new_img; + const uint16_t *src_u16 = reinterpret_cast(p_src); + uint16_t *dst_u16 = reinterpret_cast(p_dst); - int ofs = get_mipmap_offset(1); + const uint32_t *src_u32 = reinterpret_cast(p_src); + uint32_t *dst_u32 = reinterpret_cast(p_dst); - int new_size = data.size() - ofs; - new_img.resize(new_size); - ERR_FAIL_COND(new_img.is_empty()); + switch (p_format) { + case Image::FORMAT_L8: + case Image::FORMAT_R8: + _generate_po2_mipmap(p_src, p_dst, p_width, p_height); + break; + case Image::FORMAT_LA8: + _generate_po2_mipmap(p_src, p_dst, p_width, p_height); + break; + case Image::FORMAT_RG8: + _generate_po2_mipmap(p_src, p_dst, p_width, p_height); + break; + case Image::FORMAT_RGB8: { + if (p_renormalize) { + _generate_po2_mipmap(p_src, p_dst, p_width, p_height); + } else { + _generate_po2_mipmap(p_src, p_dst, p_width, p_height); + } + } break; + case Image::FORMAT_RGBA8: { + if (p_renormalize) { + _generate_po2_mipmap(p_src, p_dst, p_width, p_height); + } else { + _generate_po2_mipmap(p_src, p_dst, p_width, p_height); + } + } break; + case Image::FORMAT_RF: + _generate_po2_mipmap(src_float, dst_float, p_width, p_height); + break; + case Image::FORMAT_RGF: + _generate_po2_mipmap(src_float, dst_float, p_width, p_height); + break; + case Image::FORMAT_RGBF: { + if (p_renormalize) { + _generate_po2_mipmap(src_float, dst_float, p_width, p_height); + } else { + _generate_po2_mipmap(src_float, dst_float, p_width, p_height); + } + } break; + case Image::FORMAT_RGBAF: { + if (p_renormalize) { + _generate_po2_mipmap(src_float, dst_float, p_width, p_height); + } else { + _generate_po2_mipmap(src_float, dst_float, p_width, p_height); + } + } break; + case Image::FORMAT_RH: + _generate_po2_mipmap(src_u16, dst_u16, p_width, p_height); + break; + case Image::FORMAT_RGH: + _generate_po2_mipmap(src_u16, dst_u16, p_width, p_height); + break; + case Image::FORMAT_RGBH: { + if (p_renormalize) { + _generate_po2_mipmap(src_u16, dst_u16, p_width, p_height); + } else { + _generate_po2_mipmap(src_u16, dst_u16, p_width, p_height); + } + } break; + case Image::FORMAT_RGBAH: { + if (p_renormalize) { + _generate_po2_mipmap(src_u16, dst_u16, p_width, p_height); + } else { + _generate_po2_mipmap(src_u16, dst_u16, p_width, p_height); + } + } break; + case Image::FORMAT_RGBE9995: + _generate_po2_mipmap(src_u32, dst_u32, p_width, p_height); + break; - { - uint8_t *w = new_img.ptrw(); - const uint8_t *r = data.ptr(); + default: + return; + } +} - memcpy(w, &r[ofs], new_size); - } +void Image::shrink_x2() { + ERR_FAIL_COND(data.is_empty()); + Vector new_data; - width = MAX(width / 2, 1); - height = MAX(height / 2, 1); - data = new_img; + if (mipmaps) { + // Just use the lower mipmap as base and copy all. + int64_t ofs = get_mipmap_offset(1); + int64_t new_size = data.size() - ofs; - } else { - Vector new_img; + new_data.resize(new_size); + ERR_FAIL_COND(new_data.is_empty()); + memcpy(new_data.ptrw(), data.ptr() + ofs, new_size); + } else { + // Generate a mipmap and replace the original. ERR_FAIL_COND(!_can_modify(format)); - int ps = get_format_pixel_size(format); - new_img.resize((width / 2) * (height / 2) * ps); - ERR_FAIL_COND(new_img.is_empty()); - ERR_FAIL_COND(data.is_empty()); - - { - uint8_t *w = new_img.ptrw(); - const uint8_t *r = data.ptr(); - - switch (format) { - case FORMAT_L8: - case FORMAT_R8: - _generate_po2_mipmap(r, w, width, height); - break; - case FORMAT_LA8: - _generate_po2_mipmap(r, w, width, height); - break; - case FORMAT_RG8: - _generate_po2_mipmap(r, w, width, height); - break; - case FORMAT_RGB8: - _generate_po2_mipmap(r, w, width, height); - break; - case FORMAT_RGBA8: - _generate_po2_mipmap(r, w, width, height); - break; - case FORMAT_RF: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; - case FORMAT_RGF: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; - case FORMAT_RGBF: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; - case FORMAT_RGBAF: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; + new_data.resize((width / 2) * (height / 2) * get_format_pixel_size(format)); + ERR_FAIL_COND(data.is_empty() || new_data.is_empty()); - case FORMAT_RH: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; - case FORMAT_RGH: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; - case FORMAT_RGBH: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; - case FORMAT_RGBAH: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; - - case FORMAT_RGBE9995: - _generate_po2_mipmap(reinterpret_cast(r), reinterpret_cast(w), width, height); - break; - default: { - } - } - } - - width /= 2; - height /= 2; - data = new_img; + _generate_mipmap_from_format(format, data.ptr(), new_data.ptrw(), width, height, false); } + + width = MAX(width / 2, 1); + height = MAX(height / 2, 1); + data = new_data; } void Image::normalize() { @@ -1951,107 +1966,25 @@ void Image::normalize() { Error Image::generate_mipmaps(bool p_renormalize) { ERR_FAIL_COND_V_MSG(!_can_modify(format), ERR_UNAVAILABLE, "Cannot generate mipmaps in compressed or custom image formats."); - ERR_FAIL_COND_V_MSG(format == FORMAT_RGBA4444, ERR_UNAVAILABLE, "Cannot generate mipmaps from RGBA4444 format."); - ERR_FAIL_COND_V_MSG(width == 0 || height == 0, ERR_UNCONFIGURED, "Cannot generate mipmaps with width or height equal to 0."); - int mmcount; - - int size = _get_dst_image_size(width, height, format, mmcount); + int gen_mipmap_count; + int64_t size = _get_dst_image_size(width, height, format, gen_mipmap_count); data.resize(size); - uint8_t *wp = data.ptrw(); int prev_ofs = 0; int prev_h = height; int prev_w = width; - for (int i = 1; i <= mmcount; i++) { + for (int i = 1; i <= gen_mipmap_count; i++) { int64_t ofs; int w, h; _get_mipmap_offset_and_size(i, ofs, w, h); - switch (format) { - case FORMAT_L8: - case FORMAT_R8: - _generate_po2_mipmap(&wp[prev_ofs], &wp[ofs], prev_w, prev_h); - break; - case FORMAT_LA8: - case FORMAT_RG8: - _generate_po2_mipmap(&wp[prev_ofs], &wp[ofs], prev_w, prev_h); - break; - case FORMAT_RGB8: - if (p_renormalize) { - _generate_po2_mipmap(&wp[prev_ofs], &wp[ofs], prev_w, prev_h); - } else { - _generate_po2_mipmap(&wp[prev_ofs], &wp[ofs], prev_w, prev_h); - } - - break; - case FORMAT_RGBA8: - if (p_renormalize) { - _generate_po2_mipmap(&wp[prev_ofs], &wp[ofs], prev_w, prev_h); - } else { - _generate_po2_mipmap(&wp[prev_ofs], &wp[ofs], prev_w, prev_h); - } - break; - case FORMAT_RF: - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - break; - case FORMAT_RGF: - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - break; - case FORMAT_RGBF: - if (p_renormalize) { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } else { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } - - break; - case FORMAT_RGBAF: - if (p_renormalize) { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } else { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } - - break; - case FORMAT_RH: - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - break; - case FORMAT_RGH: - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - break; - case FORMAT_RGBH: - if (p_renormalize) { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } else { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } - - break; - case FORMAT_RGBAH: - if (p_renormalize) { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } else { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } - - break; - case FORMAT_RGBE9995: - if (p_renormalize) { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } else { - _generate_po2_mipmap(reinterpret_cast(&wp[prev_ofs]), reinterpret_cast(&wp[ofs]), prev_w, prev_h); - } - - break; - default: { - } - } + _generate_mipmap_from_format(format, wp + prev_ofs, wp + ofs, prev_w, prev_h, p_renormalize); prev_ofs = ofs; prev_w = w; @@ -2080,8 +2013,7 @@ Error Image::generate_mipmap_roughness(RoughnessChannel p_roughness_channel, con normal_sat_vec.resize(normal_w * normal_h * 3); double *normal_sat = normal_sat_vec.ptr(); - //create summed area table - + // Create summed area table. for (int y = 0; y < normal_h; y++) { double line_sum[3] = { 0, 0, 0 }; for (int x = 0; x < normal_w; x++) { @@ -2110,15 +2042,6 @@ Error Image::generate_mipmap_roughness(RoughnessChannel p_roughness_channel, con } } -#if 0 - { - Vector3 beg(normal_sat_vec[0], normal_sat_vec[1], normal_sat_vec[2]); - Vector3 end(normal_sat_vec[normal_sat_vec.size() - 3], normal_sat_vec[normal_sat_vec.size() - 2], normal_sat_vec[normal_sat_vec.size() - 1]); - Vector3 avg = (end - beg) / (normal_w * normal_h); - print_line("average: " + avg); - } -#endif - int mmcount; _get_dst_image_size(width, height, format, mmcount); @@ -2520,8 +2443,7 @@ void Image::initialize_data(const char **p_xpm) { } bool Image::is_invisible() const { - if (format == FORMAT_L8 || - format == FORMAT_RGB8 || format == FORMAT_RG8) { + if (format == FORMAT_L8 || format == FORMAT_RGB8 || format == FORMAT_RG8) { return false; } @@ -2617,7 +2539,7 @@ Error Image::load(const String &p_path) { WARN_PRINT(vformat("Loaded resource as image file, this will not work on export: '%s'. Instead, import the image file as an Image resource and load it normally as a resource.", path)); } #endif - return ImageLoader::load_image(ResourceUID::ensure_path(p_path), this); + return ImageLoader::load_image(path, this); } Ref Image::load_from_file(const String &p_path) { @@ -2784,12 +2706,9 @@ Error Image::compress_from_channels(CompressMode p_mode, UsedChannels p_channels return OK; } } - } break; - case COMPRESS_S3TC: { - // BC3 is unsupported currently. - if ((p_channels == USED_CHANNELS_R || p_channels == USED_CHANNELS_RGB || p_channels == USED_CHANNELS_L) && _image_compress_bc_rd_func) { + if (_image_compress_bc_rd_func) { Error result = _image_compress_bc_rd_func(this, p_channels); // If the image was compressed successfully, we return here. If not, we fall back to the default compression scheme. @@ -2797,7 +2716,6 @@ Error Image::compress_from_channels(CompressMode p_mode, UsedChannels p_channels return OK; } } - } break; default: { @@ -3314,6 +3232,7 @@ Color Image::_get_color_at_ofs(const uint8_t *ptr, uint32_t ofs) const { case FORMAT_RGBE9995: { return Color::from_rgbe9995(((uint32_t *)ptr)[ofs]); } + default: { ERR_FAIL_V_MSG(Color(), "Can't get_pixel() on compressed image, sorry."); } @@ -3346,7 +3265,6 @@ void Image::_set_color_at_ofs(uint8_t *ptr, uint32_t ofs, const Color &p_color) ptr[ofs * 4 + 1] = uint8_t(CLAMP(p_color.g * 255.0, 0, 255)); ptr[ofs * 4 + 2] = uint8_t(CLAMP(p_color.b * 255.0, 0, 255)); ptr[ofs * 4 + 3] = uint8_t(CLAMP(p_color.a * 255.0, 0, 255)); - } break; case FORMAT_RGBA4444: { uint16_t rgba = 0; @@ -3357,7 +3275,6 @@ void Image::_set_color_at_ofs(uint8_t *ptr, uint32_t ofs, const Color &p_color) rgba |= uint16_t(CLAMP(p_color.a * 15.0, 0, 15)); ((uint16_t *)ptr)[ofs] = rgba; - } break; case FORMAT_RGB565: { uint16_t rgba = 0; @@ -3367,7 +3284,6 @@ void Image::_set_color_at_ofs(uint8_t *ptr, uint32_t ofs, const Color &p_color) rgba |= uint16_t(CLAMP(p_color.b * 31.0, 0, 31)) << 11; ((uint16_t *)ptr)[ofs] = rgba; - } break; case FORMAT_RF: { ((float *)ptr)[ofs] = p_color.r; @@ -3407,8 +3323,8 @@ void Image::_set_color_at_ofs(uint8_t *ptr, uint32_t ofs, const Color &p_color) } break; case FORMAT_RGBE9995: { ((uint32_t *)ptr)[ofs] = p_color.to_rgbe9995(); - } break; + default: { ERR_FAIL_MSG("Can't set_pixel() on compressed image, sorry."); } @@ -3476,30 +3392,50 @@ void Image::adjust_bcs(float p_brightness, float p_contrast, float p_saturation) Image::UsedChannels Image::detect_used_channels(CompressSource p_source) const { ERR_FAIL_COND_V(data.is_empty(), USED_CHANNELS_RGBA); ERR_FAIL_COND_V(is_compressed(), USED_CHANNELS_RGBA); + + if (p_source == COMPRESS_SOURCE_NORMAL) { + return USED_CHANNELS_RG; // Normal maps only use RG channels. + } + + if (format == FORMAT_L8) { + return USED_CHANNELS_L; // Grayscale only cannot have any channel less. + } else if (format == FORMAT_R8 || format == FORMAT_RH || format == FORMAT_RF) { + return USED_CHANNELS_R; // Red only cannot have any channel less. + } + + const bool supports_alpha = format == FORMAT_RGBA8 || format == FORMAT_RGBA4444 || format == FORMAT_RGBAH || format == FORMAT_RGBAF; bool r = false, g = false, b = false, a = false, c = false; const uint8_t *data_ptr = data.ptr(); - - uint32_t data_total = width * height; + const uint32_t data_total = width * height; for (uint32_t i = 0; i < data_total; i++) { Color col = _get_color_at_ofs(data_ptr, i); - if (col.r > 0.001) { + if (!r && col.r > 0.001) { r = true; } - if (col.g > 0.001) { + if (!g && col.g > 0.001) { g = true; } - if (col.b > 0.001) { + if (!b && col.b > 0.001) { b = true; } - if (col.a < 0.999) { + if (!a && col.a < 0.999) { a = true; } if (col.r != col.b || col.r != col.g || col.b != col.g) { - c = true; + c = true; // The image is not grayscale. + } + + if (r && g && b && c) { + // All channels are used, no need to continue. + if (!supports_alpha) { + break; + } else if (a) { + break; + } } } @@ -3520,13 +3456,7 @@ Image::UsedChannels Image::detect_used_channels(CompressSource p_source) const { } if (p_source == COMPRESS_SOURCE_SRGB && (used_channels == USED_CHANNELS_R || used_channels == USED_CHANNELS_RG)) { - //R and RG do not support SRGB - used_channels = USED_CHANNELS_RGB; - } - - if (p_source == COMPRESS_SOURCE_NORMAL) { - //use RG channels only for normal - used_channels = USED_CHANNELS_RG; + used_channels = USED_CHANNELS_RGB; // R and RG do not support SRGB. } return used_channels; @@ -3731,14 +3661,6 @@ void Image::_bind_methods() { BIND_ENUM_CONSTANT(ASTC_FORMAT_8x8); } -void Image::set_compress_bc_func(void (*p_compress_func)(Image *, UsedChannels)) { - _image_compress_bc_func = p_compress_func; -} - -void Image::set_compress_bptc_func(void (*p_compress_func)(Image *, UsedChannels)) { - _image_compress_bptc_func = p_compress_func; -} - void Image::normal_map_to_xy() { convert(Image::FORMAT_RGBA8); diff --git a/core/io/image.h b/core/io/image.h index 3149314ad888..f6504c7be887 100644 --- a/core/io/image.h +++ b/core/io/image.h @@ -266,6 +266,8 @@ class Image : public Resource { Error _load_from_buffer(const Vector &p_array, ImageMemLoadFunc p_loader); + _FORCE_INLINE_ void _generate_mipmap_from_format(Image::Format p_format, const uint8_t *p_src, uint8_t *p_dst, uint32_t p_width, uint32_t p_height, bool p_renormalize = false); + static void average_4_uint8(uint8_t &p_out, const uint8_t &p_a, const uint8_t &p_b, const uint8_t &p_c, const uint8_t &p_d); static void average_4_float(float &p_out, const float &p_a, const float &p_b, const float &p_c, const float &p_d); static void average_4_half(uint16_t &p_out, const uint16_t &p_a, const uint16_t &p_b, const uint16_t &p_c, const uint16_t &p_d); @@ -393,8 +395,6 @@ class Image : public Resource { Rect2i get_used_rect() const; Ref get_region(const Rect2i &p_area) const; - static void set_compress_bc_func(void (*p_compress_func)(Image *, UsedChannels)); - static void set_compress_bptc_func(void (*p_compress_func)(Image *, UsedChannels)); static String get_format_name(Format p_format); Error load_png_from_buffer(const Vector &p_array); diff --git a/core/io/image_loader.cpp b/core/io/image_loader.cpp index 50c4704aa3f4..58e63a4cc997 100644 --- a/core/io/image_loader.cpp +++ b/core/io/image_loader.cpp @@ -82,15 +82,16 @@ void ImageFormatLoaderExtension::_bind_methods() { Error ImageLoader::load_image(const String &p_file, Ref p_image, Ref p_custom, BitField p_flags, float p_scale) { ERR_FAIL_COND_V_MSG(p_image.is_null(), ERR_INVALID_PARAMETER, "Can't load an image: invalid Image object."); + const String file = ResourceUID::ensure_path(p_file); Ref f = p_custom; if (f.is_null()) { Error err; - f = FileAccess::open(p_file, FileAccess::READ, &err); - ERR_FAIL_COND_V_MSG(f.is_null(), err, vformat("Error opening file '%s'.", p_file)); + f = FileAccess::open(file, FileAccess::READ, &err); + ERR_FAIL_COND_V_MSG(f.is_null(), err, vformat("Error opening file '%s'.", file)); } - String extension = p_file.get_extension(); + String extension = file.get_extension(); for (int i = 0; i < loader.size(); i++) { if (!loader[i]->recognize(extension)) { @@ -98,7 +99,7 @@ Error ImageLoader::load_image(const String &p_file, Ref p_image, Refload_image(p_image, f, p_flags, p_scale); if (err != OK) { - ERR_PRINT(vformat("Error loading image: '%s'.", p_file)); + ERR_PRINT(vformat("Error loading image: '%s'.", file)); } if (err != ERR_FILE_UNRECOGNIZED) { diff --git a/core/io/ip_address.cpp b/core/io/ip_address.cpp index a93876a2b510..9342792493f3 100644 --- a/core/io/ip_address.cpp +++ b/core/io/ip_address.cpp @@ -202,7 +202,7 @@ IPAddress::IPAddress(const String &p_string) { // Wildcard (not a valid IP) wildcard = true; - } else if (p_string.contains(":")) { + } else if (p_string.contains_char(':')) { // IPv6 _parse_ipv6(p_string); valid = true; diff --git a/core/io/json.cpp b/core/io/json.cpp index e73677be9ca8..b6b1a88479bd 100644 --- a/core/io/json.cpp +++ b/core/io/json.cpp @@ -31,7 +31,8 @@ #include "json.h" #include "core/config/engine.h" -#include "core/string/print_string.h" +#include "core/object/script_language.h" +#include "core/variant/container_type_validate.h" const char *JSON::tk_name[TK_MAX] = { "'{'", @@ -563,18 +564,18 @@ String JSON::get_parsed_text() const { } String JSON::stringify(const Variant &p_var, const String &p_indent, bool p_sort_keys, bool p_full_precision) { - Ref jason; - jason.instantiate(); + Ref json; + json.instantiate(); HashSet markers; - return jason->_stringify(p_var, p_indent, 0, p_sort_keys, markers, p_full_precision); + return json->_stringify(p_var, p_indent, 0, p_sort_keys, markers, p_full_precision); } Variant JSON::parse_string(const String &p_json_string) { - Ref jason; - jason.instantiate(); - Error error = jason->parse(p_json_string); - ERR_FAIL_COND_V_MSG(error != Error::OK, Variant(), vformat("Parse JSON failed. Error at line %d: %s", jason->get_error_line(), jason->get_error_message())); - return jason->get_data(); + Ref json; + json.instantiate(); + Error error = json->parse(p_json_string); + ERR_FAIL_COND_V_MSG(error != Error::OK, Variant(), vformat("Parse JSON failed. Error at line %d: %s", json->get_error_line(), json->get_error_message())); + return json->get_data(); } void JSON::_bind_methods() { @@ -588,756 +589,1015 @@ void JSON::_bind_methods() { ClassDB::bind_method(D_METHOD("get_error_line"), &JSON::get_error_line); ClassDB::bind_method(D_METHOD("get_error_message"), &JSON::get_error_message); - ClassDB::bind_static_method("JSON", D_METHOD("to_native", "json", "allow_classes", "allow_scripts"), &JSON::to_native, DEFVAL(false), DEFVAL(false)); - ClassDB::bind_static_method("JSON", D_METHOD("from_native", "variant", "allow_classes", "allow_scripts"), &JSON::from_native, DEFVAL(false), DEFVAL(false)); + ClassDB::bind_static_method("JSON", D_METHOD("from_native", "variant", "full_objects"), &JSON::from_native, DEFVAL(false)); + ClassDB::bind_static_method("JSON", D_METHOD("to_native", "json", "allow_objects"), &JSON::to_native, DEFVAL(false)); ADD_PROPERTY(PropertyInfo(Variant::NIL, "data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_NIL_IS_VARIANT), "set_data", "get_data"); // Ensures that it can be serialized as binary. } -#define GDTYPE "__gdtype" -#define VALUES "values" -#define PASS_ARG p_allow_classes, p_allow_scripts +#define TYPE "type" +#define ELEM_TYPE "elem_type" +#define KEY_TYPE "key_type" +#define VALUE_TYPE "value_type" +#define ARGS "args" +#define PROPS "props" + +static bool _encode_container_type(Dictionary &r_dict, const String &p_key, const ContainerType &p_type, bool p_full_objects) { + if (p_type.builtin_type != Variant::NIL) { + if (p_type.script.is_valid()) { + ERR_FAIL_COND_V(!p_full_objects, false); + const String path = p_type.script->get_path(); + ERR_FAIL_COND_V_MSG(path.is_empty() || !path.begins_with("res://"), false, "Failed to encode a path to a custom script for a container type."); + r_dict[p_key] = path; + } else if (p_type.class_name != StringName()) { + ERR_FAIL_COND_V(!p_full_objects, false); + r_dict[p_key] = String(p_type.class_name); + } else { + // No need to check `p_full_objects` since `class_name` should be non-empty for `builtin_type == Variant::OBJECT`. + r_dict[p_key] = Variant::get_type_name(p_type.builtin_type); + } + } + return true; +} + +Variant JSON::_from_native(const Variant &p_variant, bool p_full_objects, int p_depth) { +#define RETURN_ARGS \ + Dictionary ret; \ + ret[TYPE] = Variant::get_type_name(p_variant.get_type()); \ + ret[ARGS] = args; \ + return ret -Variant JSON::from_native(const Variant &p_variant, bool p_allow_classes, bool p_allow_scripts) { switch (p_variant.get_type()) { - case Variant::NIL: { - Dictionary nil; - nil[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return nil; - } break; + case Variant::NIL: case Variant::BOOL: { return p_variant; } break; + case Variant::INT: { - return p_variant; + return "i:" + String(p_variant); } break; case Variant::FLOAT: { - return p_variant; + return "f:" + String(p_variant); } break; case Variant::STRING: { - return p_variant; + return "s:" + String(p_variant); + } break; + case Variant::STRING_NAME: { + return "sn:" + String(p_variant); + } break; + case Variant::NODE_PATH: { + return "np:" + String(p_variant); + } break; + + case Variant::RID: + case Variant::CALLABLE: + case Variant::SIGNAL: { + Dictionary ret; + ret[TYPE] = Variant::get_type_name(p_variant.get_type()); + return ret; } break; + case Variant::VECTOR2: { - Dictionary d; - Vector2 v = p_variant; - Array values; - values.push_back(v.x); - values.push_back(v.y); - d[VALUES] = values; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Vector2 v = p_variant; + + Array args; + args.push_back(v.x); + args.push_back(v.y); + + RETURN_ARGS; } break; case Variant::VECTOR2I: { - Dictionary d; - Vector2i v = p_variant; - Array values; - values.push_back(v.x); - values.push_back(v.y); - d[VALUES] = values; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Vector2i v = p_variant; + + Array args; + args.push_back(v.x); + args.push_back(v.y); + + RETURN_ARGS; } break; case Variant::RECT2: { - Dictionary d; - Rect2 r = p_variant; - d["position"] = from_native(r.position); - d["size"] = from_native(r.size); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Rect2 r = p_variant; + + Array args; + args.push_back(r.position.x); + args.push_back(r.position.y); + args.push_back(r.size.width); + args.push_back(r.size.height); + + RETURN_ARGS; } break; case Variant::RECT2I: { - Dictionary d; - Rect2i r = p_variant; - d["position"] = from_native(r.position); - d["size"] = from_native(r.size); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Rect2i r = p_variant; + + Array args; + args.push_back(r.position.x); + args.push_back(r.position.y); + args.push_back(r.size.width); + args.push_back(r.size.height); + + RETURN_ARGS; } break; case Variant::VECTOR3: { - Dictionary d; - Vector3 v = p_variant; - Array values; - values.push_back(v.x); - values.push_back(v.y); - values.push_back(v.z); - d[VALUES] = values; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Vector3 v = p_variant; + + Array args; + args.push_back(v.x); + args.push_back(v.y); + args.push_back(v.z); + + RETURN_ARGS; } break; case Variant::VECTOR3I: { - Dictionary d; - Vector3i v = p_variant; - Array values; - values.push_back(v.x); - values.push_back(v.y); - values.push_back(v.z); - d[VALUES] = values; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Vector3i v = p_variant; + + Array args; + args.push_back(v.x); + args.push_back(v.y); + args.push_back(v.z); + + RETURN_ARGS; } break; case Variant::TRANSFORM2D: { - Dictionary d; - Transform2D t = p_variant; - d["x"] = from_native(t[0]); - d["y"] = from_native(t[1]); - d["origin"] = from_native(t[2]); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Transform2D t = p_variant; + + Array args; + args.push_back(t[0].x); + args.push_back(t[0].y); + args.push_back(t[1].x); + args.push_back(t[1].y); + args.push_back(t[2].x); + args.push_back(t[2].y); + + RETURN_ARGS; } break; case Variant::VECTOR4: { - Dictionary d; - Vector4 v = p_variant; - Array values; - values.push_back(v.x); - values.push_back(v.y); - values.push_back(v.z); - values.push_back(v.w); - d[VALUES] = values; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Vector4 v = p_variant; + + Array args; + args.push_back(v.x); + args.push_back(v.y); + args.push_back(v.z); + args.push_back(v.w); + + RETURN_ARGS; } break; case Variant::VECTOR4I: { - Dictionary d; - Vector4i v = p_variant; - Array values; - values.push_back(v.x); - values.push_back(v.y); - values.push_back(v.z); - values.push_back(v.w); - d[VALUES] = values; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Vector4i v = p_variant; + + Array args; + args.push_back(v.x); + args.push_back(v.y); + args.push_back(v.z); + args.push_back(v.w); + + RETURN_ARGS; } break; case Variant::PLANE: { - Dictionary d; - Plane p = p_variant; - d["normal"] = from_native(p.normal); - d["d"] = p.d; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Plane p = p_variant; + + Array args; + args.push_back(p.normal.x); + args.push_back(p.normal.y); + args.push_back(p.normal.z); + args.push_back(p.d); + + RETURN_ARGS; } break; case Variant::QUATERNION: { - Dictionary d; - Quaternion q = p_variant; - Array values; - values.push_back(q.x); - values.push_back(q.y); - values.push_back(q.z); - values.push_back(q.w); - d[VALUES] = values; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Quaternion q = p_variant; + + Array args; + args.push_back(q.x); + args.push_back(q.y); + args.push_back(q.z); + args.push_back(q.w); + + RETURN_ARGS; } break; case Variant::AABB: { - Dictionary d; - AABB aabb = p_variant; - d["position"] = from_native(aabb.position); - d["size"] = from_native(aabb.size); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const AABB aabb = p_variant; + + Array args; + args.push_back(aabb.position.x); + args.push_back(aabb.position.y); + args.push_back(aabb.position.z); + args.push_back(aabb.size.x); + args.push_back(aabb.size.y); + args.push_back(aabb.size.z); + + RETURN_ARGS; } break; case Variant::BASIS: { - Dictionary d; - Basis t = p_variant; - d["x"] = from_native(t.get_column(0)); - d["y"] = from_native(t.get_column(1)); - d["z"] = from_native(t.get_column(2)); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Basis b = p_variant; + + Array args; + args.push_back(b.get_column(0).x); + args.push_back(b.get_column(0).y); + args.push_back(b.get_column(0).z); + args.push_back(b.get_column(1).x); + args.push_back(b.get_column(1).y); + args.push_back(b.get_column(1).z); + args.push_back(b.get_column(2).x); + args.push_back(b.get_column(2).y); + args.push_back(b.get_column(2).z); + + RETURN_ARGS; } break; case Variant::TRANSFORM3D: { - Dictionary d; - Transform3D t = p_variant; - d["basis"] = from_native(t.basis); - d["origin"] = from_native(t.origin); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Transform3D t = p_variant; + + Array args; + args.push_back(t.basis.get_column(0).x); + args.push_back(t.basis.get_column(0).y); + args.push_back(t.basis.get_column(0).z); + args.push_back(t.basis.get_column(1).x); + args.push_back(t.basis.get_column(1).y); + args.push_back(t.basis.get_column(1).z); + args.push_back(t.basis.get_column(2).x); + args.push_back(t.basis.get_column(2).y); + args.push_back(t.basis.get_column(2).z); + args.push_back(t.origin.x); + args.push_back(t.origin.y); + args.push_back(t.origin.z); + + RETURN_ARGS; } break; case Variant::PROJECTION: { - Dictionary d; - Projection t = p_variant; - d["x"] = from_native(t[0]); - d["y"] = from_native(t[1]); - d["z"] = from_native(t[2]); - d["w"] = from_native(t[3]); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Projection p = p_variant; + + Array args; + args.push_back(p[0].x); + args.push_back(p[0].y); + args.push_back(p[0].z); + args.push_back(p[0].w); + args.push_back(p[1].x); + args.push_back(p[1].y); + args.push_back(p[1].z); + args.push_back(p[1].w); + args.push_back(p[2].x); + args.push_back(p[2].y); + args.push_back(p[2].z); + args.push_back(p[2].w); + args.push_back(p[3].x); + args.push_back(p[3].y); + args.push_back(p[3].z); + args.push_back(p[3].w); + + RETURN_ARGS; } break; case Variant::COLOR: { - Dictionary d; - Color c = p_variant; - Array values; - values.push_back(c.r); - values.push_back(c.g); - values.push_back(c.b); - values.push_back(c.a); - d[VALUES] = values; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; - } break; - case Variant::STRING_NAME: { - Dictionary d; - d["name"] = String(p_variant); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; - } break; - case Variant::NODE_PATH: { - Dictionary d; - d["path"] = String(p_variant); - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; - } break; - case Variant::RID: { - Dictionary d; - d[GDTYPE] = Variant::get_type_name(p_variant.get_type()); - return d; + const Color c = p_variant; + + Array args; + args.push_back(c.r); + args.push_back(c.g); + args.push_back(c.b); + args.push_back(c.a); + + RETURN_ARGS; } break; + case Variant::OBJECT: { - Object *obj = p_variant.get_validated_object(); + ERR_FAIL_COND_V(!p_full_objects, Variant()); - if (p_allow_classes && obj) { - Dictionary d; - List property_list; - obj->get_property_list(&property_list); - - d["type"] = obj->get_class(); - Dictionary p; - for (const PropertyInfo &P : property_list) { - if (P.usage & PROPERTY_USAGE_STORAGE) { - if (P.name == "script" && !p_allow_scripts) { - continue; - } - p[P.name] = from_native(obj->get(P.name), PASS_ARG); + ERR_FAIL_COND_V_MSG(p_depth > Variant::MAX_RECURSION_DEPTH, Variant(), "Variant is too deep. Bailing."); + + const Object *obj = p_variant.get_validated_object(); + if (obj == nullptr) { + return Variant(); + } + + ERR_FAIL_COND_V(!ClassDB::can_instantiate(obj->get_class()), Variant()); + + List prop_list; + obj->get_property_list(&prop_list); + + Array props; + for (const PropertyInfo &pi : prop_list) { + if (!(pi.usage & PROPERTY_USAGE_STORAGE)) { + continue; + } + + Variant value; + if (pi.name == CoreStringName(script)) { + const Ref + + + + + +
+ +)"; +} + +#endif // JPH_PROFILE_ENABLED + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.h b/thirdparty/jolt_physics/Jolt/Core/Profiler.h new file mode 100644 index 000000000000..878865313ba2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.h @@ -0,0 +1,301 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#include +#include +#include + +#if defined(JPH_EXTERNAL_PROFILE) + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_SHARED_LIBRARY +/// Functions called when a profiler measurement starts or stops, need to be overridden by the user. +using ProfileStartMeasurementFunction = void (*)(const char *inName, uint32 inColor, uint8 *ioUserData); +using ProfileEndMeasurementFunction = void (*)(uint8 *ioUserData); + +JPH_EXPORT extern ProfileStartMeasurementFunction ProfileStartMeasurement; +JPH_EXPORT extern ProfileEndMeasurementFunction ProfileEndMeasurement; +#endif // JPH_SHARED_LIBRARY + +/// Create this class on the stack to start sampling timing information of a particular scope. +/// +/// For statically linked builds, this is left unimplemented intentionally. Needs to be implemented by the user of the library. +/// On construction a measurement should start, on destruction it should be stopped. +/// For dynamically linked builds, the user should override the ProfileStartMeasurement and ProfileEndMeasurement functions. +class alignas(16) ExternalProfileMeasurement : public NonCopyable +{ +public: + /// Constructor +#ifdef JPH_SHARED_LIBRARY + JPH_INLINE ExternalProfileMeasurement(const char *inName, uint32 inColor = 0) { ProfileStartMeasurement(inName, inColor, mUserData); } + JPH_INLINE ~ExternalProfileMeasurement() { ProfileEndMeasurement(mUserData); } +#else + ExternalProfileMeasurement(const char *inName, uint32 inColor = 0); + ~ExternalProfileMeasurement(); +#endif + +private: + uint8 mUserData[64]; +}; + +JPH_NAMESPACE_END + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to do the actual profiling +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +// Dummy implementations +#define JPH_PROFILE_START(name) +#define JPH_PROFILE_END() +#define JPH_PROFILE_THREAD_START(name) +#define JPH_PROFILE_THREAD_END() +#define JPH_PROFILE_NEXTFRAME() +#define JPH_PROFILE_DUMP(...) + +// Scope profiling measurement +#define JPH_PROFILE_TAG2(line) profile##line +#define JPH_PROFILE_TAG(line) JPH_PROFILE_TAG2(line) + +/// Macro to collect profiling information. +/// +/// Usage: +/// +/// { +/// JPH_PROFILE("Operation"); +/// do operation; +/// } +/// +#define JPH_PROFILE(...) ExternalProfileMeasurement JPH_PROFILE_TAG(__LINE__)(__VA_ARGS__) + +// Scope profiling for function +#define JPH_PROFILE_FUNCTION() JPH_PROFILE(JPH_FUNCTION_NAME) + +JPH_SUPPRESS_WARNING_POP + +#elif defined(JPH_PROFILE_ENABLED) + +JPH_NAMESPACE_BEGIN + +class ProfileSample; +class ProfileThread; + +/// Singleton class for managing profiling information +class JPH_EXPORT Profiler : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Profiler() { UpdateReferenceTime(); } + + /// Increments the frame counter to provide statistics per frame + void NextFrame(); + + /// Dump profiling statistics at the start of the next frame + /// @param inTag If not empty, this overrides the auto incrementing number in the filename of the dump file + void Dump(const string_view &inTag = string_view()); + + /// Add a thread to be instrumented + void AddThread(ProfileThread *inThread); + + /// Remove a thread from being instrumented + void RemoveThread(ProfileThread *inThread); + + /// Singleton instance + static Profiler * sInstance; + +private: + /// Helper class to freeze ProfileSamples per thread while processing them + struct ThreadSamples + { + String mThreadName; + ProfileSample * mSamplesBegin; + ProfileSample * mSamplesEnd; + }; + + /// Helper class to aggregate ProfileSamples + class Aggregator + { + public: + /// Constructor + Aggregator(const char *inName) : mName(inName) { } + + /// Accumulate results for a measurement + void AccumulateMeasurement(uint64 inCyclesInCallWithChildren) + { + mCallCounter++; + mTotalCyclesInCallWithChildren += inCyclesInCallWithChildren; + mMinCyclesInCallWithChildren = min(inCyclesInCallWithChildren, mMinCyclesInCallWithChildren); + mMaxCyclesInCallWithChildren = max(inCyclesInCallWithChildren, mMaxCyclesInCallWithChildren); + } + + /// Sort descending by total cycles + bool operator < (const Aggregator &inRHS) const + { + return mTotalCyclesInCallWithChildren > inRHS.mTotalCyclesInCallWithChildren; + } + + /// Identification + const char * mName; ///< User defined name of this item + + /// Statistics + uint32 mCallCounter = 0; ///< Number of times AccumulateMeasurement was called + uint64 mTotalCyclesInCallWithChildren = 0; ///< Total amount of cycles spent in this scope + uint64 mMinCyclesInCallWithChildren = 0xffffffffffffffffUL; ///< Minimum amount of cycles spent per call + uint64 mMaxCyclesInCallWithChildren = 0; ///< Maximum amount of cycles spent per call + }; + + using Threads = Array; + using Aggregators = Array; + using KeyToAggregator = UnorderedMap; + + /// Helper function to aggregate profile sample data + static void sAggregate(int inDepth, uint32 inColor, ProfileSample *&ioSample, const ProfileSample *inEnd, Aggregators &ioAggregators, KeyToAggregator &ioKeyToAggregator); + + /// We measure the amount of ticks per second, this function resets the reference time point + void UpdateReferenceTime(); + + /// Get the amount of ticks per second, note that this number will never be fully accurate as the amount of ticks per second may vary with CPU load, so this number is only to be used to give an indication of time for profiling purposes + uint64 GetProcessorTicksPerSecond() const; + + /// Dump profiling statistics + void DumpInternal(); + void DumpChart(const char *inTag, const Threads &inThreads, const KeyToAggregator &inKeyToAggregators, const Aggregators &inAggregators); + + std::mutex mLock; ///< Lock that protects mThreads + uint64 mReferenceTick; ///< Tick count at the start of the frame + std::chrono::high_resolution_clock::time_point mReferenceTime; ///< Time at the start of the frame + Array mThreads; ///< List of all active threads + bool mDump = false; ///< When true, the samples are dumped next frame + String mDumpTag; ///< When not empty, this overrides the auto incrementing number of the dump filename +}; + +// Class that contains the information of a single scoped measurement +class alignas(16) JPH_EXPORT_GCC_BUG_WORKAROUND ProfileSample : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + const char * mName; ///< User defined name of this item + uint32 mColor; ///< Color to use for this sample + uint8 mDepth; ///< Calculated depth + uint8 mUnused[3]; + uint64 mStartCycle; ///< Cycle counter at start of measurement + uint64 mEndCycle; ///< Cycle counter at end of measurement +}; + +/// Collects all samples of a single thread +class ProfileThread : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + inline ProfileThread(const string_view &inThreadName); + inline ~ProfileThread(); + + static const uint cMaxSamples = 65536; + + String mThreadName; ///< Name of the thread that we're collecting information for + ProfileSample mSamples[cMaxSamples]; ///< Buffer of samples + uint mCurrentSample = 0; ///< Next position to write a sample to + +#ifdef JPH_SHARED_LIBRARY + JPH_EXPORT static void sSetInstance(ProfileThread *inInstance); + JPH_EXPORT static ProfileThread *sGetInstance(); +#else + static inline void sSetInstance(ProfileThread *inInstance) { sInstance = inInstance; } + static inline ProfileThread *sGetInstance() { return sInstance; } + +private: + static thread_local ProfileThread *sInstance; +#endif +}; + +/// Create this class on the stack to start sampling timing information of a particular scope +class JPH_EXPORT ProfileMeasurement : public NonCopyable +{ +public: + /// Constructor + inline ProfileMeasurement(const char *inName, uint32 inColor = 0); + inline ~ProfileMeasurement(); + +private: + ProfileSample * mSample; + ProfileSample mTemp; + + static bool sOutOfSamplesReported; +}; + +JPH_NAMESPACE_END + +#include "Profiler.inl" + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to do the actual profiling +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +/// Start instrumenting program +#define JPH_PROFILE_START(name) do { Profiler::sInstance = new Profiler; JPH_PROFILE_THREAD_START(name); } while (false) + +/// End instrumenting program +#define JPH_PROFILE_END() do { JPH_PROFILE_THREAD_END(); delete Profiler::sInstance; Profiler::sInstance = nullptr; } while (false) + +/// Start instrumenting a thread +#define JPH_PROFILE_THREAD_START(name) do { if (Profiler::sInstance) ProfileThread::sSetInstance(new ProfileThread(name)); } while (false) + +/// End instrumenting a thread +#define JPH_PROFILE_THREAD_END() do { delete ProfileThread::sGetInstance(); ProfileThread::sSetInstance(nullptr); } while (false) + +/// Scope profiling measurement +#define JPH_PROFILE_TAG2(line) profile##line +#define JPH_PROFILE_TAG(line) JPH_PROFILE_TAG2(line) +#define JPH_PROFILE(...) ProfileMeasurement JPH_PROFILE_TAG(__LINE__)(__VA_ARGS__) + +/// Scope profiling for function +#define JPH_PROFILE_FUNCTION() JPH_PROFILE(JPH_FUNCTION_NAME) + +/// Update frame counter +#define JPH_PROFILE_NEXTFRAME() Profiler::sInstance->NextFrame() + +/// Dump profiling info +#define JPH_PROFILE_DUMP(...) Profiler::sInstance->Dump(__VA_ARGS__) + +JPH_SUPPRESS_WARNING_POP + +#else + +////////////////////////////////////////////////////////////////////////////////////////// +// Dummy profiling instructions +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +#define JPH_PROFILE_START(name) +#define JPH_PROFILE_END() +#define JPH_PROFILE_THREAD_START(name) +#define JPH_PROFILE_THREAD_END() +#define JPH_PROFILE(...) +#define JPH_PROFILE_FUNCTION() +#define JPH_PROFILE_NEXTFRAME() +#define JPH_PROFILE_DUMP(...) + +JPH_SUPPRESS_WARNING_POP + +#endif diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.inl b/thirdparty/jolt_physics/Jolt/Core/Profiler.inl new file mode 100644 index 000000000000..912ba8383379 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.inl @@ -0,0 +1,90 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// ProfileThread +////////////////////////////////////////////////////////////////////////////////////////// + +ProfileThread::ProfileThread(const string_view &inThreadName) : + mThreadName(inThreadName) +{ + Profiler::sInstance->AddThread(this); +} + +ProfileThread::~ProfileThread() +{ + Profiler::sInstance->RemoveThread(this); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// ProfileMeasurement +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_TSAN_NO_SANITIZE // TSAN reports a race on sOutOfSamplesReported, however the worst case is that we report the out of samples message multiple times +ProfileMeasurement::ProfileMeasurement(const char *inName, uint32 inColor) +{ + ProfileThread *current_thread = ProfileThread::sGetInstance(); + if (current_thread == nullptr) + { + // Thread not instrumented + mSample = nullptr; + } + else if (current_thread->mCurrentSample < ProfileThread::cMaxSamples) + { + // Get pointer to write data to + mSample = ¤t_thread->mSamples[current_thread->mCurrentSample++]; + + // Start constructing sample (will end up on stack) + mTemp.mName = inName; + mTemp.mColor = inColor; + + // Collect start sample last + mTemp.mStartCycle = GetProcessorTickCount(); + } + else + { + // Out of samples + if (!sOutOfSamplesReported) + { + sOutOfSamplesReported = true; + Trace("ProfileMeasurement: Too many samples, some data will be lost!"); + } + mSample = nullptr; + } +} + +ProfileMeasurement::~ProfileMeasurement() +{ + if (mSample != nullptr) + { + // Finalize sample + mTemp.mEndCycle = GetProcessorTickCount(); + + // Write it to the memory buffer bypassing the cache + static_assert(sizeof(ProfileSample) == 32, "Assume 32 bytes"); + static_assert(alignof(ProfileSample) == 16, "Assume 16 byte alignment"); + #if defined(JPH_USE_SSE) + const __m128i *src = reinterpret_cast(&mTemp); + __m128i *dst = reinterpret_cast<__m128i *>(mSample); + __m128i val = _mm_loadu_si128(src); + _mm_stream_si128(dst, val); + val = _mm_loadu_si128(src + 1); + _mm_stream_si128(dst + 1, val); + #elif defined(JPH_USE_NEON) + const int *src = reinterpret_cast(&mTemp); + int *dst = reinterpret_cast(mSample); + int32x4_t val = vld1q_s32(src); + vst1q_s32(dst, val); + val = vld1q_s32(src + 4); + vst1q_s32(dst + 4, val); + #else + memcpy(mSample, &mTemp, sizeof(ProfileSample)); + #endif + mSample = nullptr; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/QuickSort.h b/thirdparty/jolt_physics/Jolt/Core/QuickSort.h new file mode 100644 index 000000000000..bd12a3b339db --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/QuickSort.h @@ -0,0 +1,137 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper function for QuickSort, will move the pivot element to inMiddle. +template +inline void QuickSortMedianOfThree(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare) +{ + // This should be guaranteed because we switch over to insertion sort when there's 32 or less elements + JPH_ASSERT(inFirst != inMiddle && inMiddle != inLast); + + if (inCompare(*inMiddle, *inFirst)) + std::swap(*inFirst, *inMiddle); + + if (inCompare(*inLast, *inFirst)) + std::swap(*inFirst, *inLast); + + if (inCompare(*inLast, *inMiddle)) + std::swap(*inMiddle, *inLast); +} + +/// Helper function for QuickSort using the Ninther method, will move the pivot element to inMiddle. +template +inline void QuickSortNinther(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare) +{ + // Divide the range in 8 equal parts (this means there are 9 points) + auto diff = (inLast - inFirst) >> 3; + auto two_diff = diff << 1; + + // Median of first 3 points + Iterator mid1 = inFirst + diff; + QuickSortMedianOfThree(inFirst, mid1, inFirst + two_diff, inCompare); + + // Median of second 3 points + QuickSortMedianOfThree(inMiddle - diff, inMiddle, inMiddle + diff, inCompare); + + // Median of third 3 points + Iterator mid3 = inLast - diff; + QuickSortMedianOfThree(inLast - two_diff, mid3, inLast, inCompare); + + // Determine the median of the 3 medians + QuickSortMedianOfThree(mid1, inMiddle, mid3, inCompare); +} + +/// Implementation of the quick sort algorithm. The STL version implementation is not consistent across platforms. +template +inline void QuickSort(Iterator inBegin, Iterator inEnd, Compare inCompare) +{ + // Implementation based on https://en.wikipedia.org/wiki/Quicksort using Hoare's partition scheme + + // Loop so that we only need to do 1 recursive call instead of 2. + for (;;) + { + // If there's less than 2 elements we're done + auto num_elements = inEnd - inBegin; + if (num_elements < 2) + return; + + // Fall back to insertion sort if there are too few elements + if (num_elements <= 32) + { + InsertionSort(inBegin, inEnd, inCompare); + return; + } + + // Determine pivot + Iterator pivot_iterator = inBegin + ((num_elements - 1) >> 1); + QuickSortNinther(inBegin, pivot_iterator, inEnd - 1, inCompare); + auto pivot = *pivot_iterator; + + // Left and right iterators + Iterator i = inBegin; + Iterator j = inEnd; + + for (;;) + { + // Find the first element that is bigger than the pivot + while (inCompare(*i, pivot)) + i++; + + // Find the last element that is smaller than the pivot + do + --j; + while (inCompare(pivot, *j)); + + // If the two iterators crossed, we're done + if (i >= j) + break; + + // Swap the elements + std::swap(*i, *j); + + // Note that the first while loop in this function should + // have been do i++ while (...) but since we cannot decrement + // the iterator from inBegin we left that out, so we need to do + // it here. + ++i; + } + + // Include the middle element on the left side + j++; + + // Check which partition is smaller + if (j - inBegin < inEnd - j) + { + // Left side is smaller, recurse to left first + QuickSort(inBegin, j, inCompare); + + // Loop again with the right side to avoid a call + inBegin = j; + } + else + { + // Right side is smaller, recurse to right first + QuickSort(j, inEnd, inCompare); + + // Loop again with the left side to avoid a call + inEnd = j; + } + } +} + +/// Implementation of quick sort algorithm without comparator. +template +inline void QuickSort(Iterator inBegin, Iterator inEnd) +{ + std::less<> compare; + QuickSort(inBegin, inEnd, compare); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp b/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp new file mode 100644 index 000000000000..91235745adc1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp @@ -0,0 +1,149 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// RTTI +////////////////////////////////////////////////////////////////////////////////////////// + +RTTI::RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject) : + mName(inName), + mSize(inSize), + mCreate(inCreateObject), + mDestruct(inDestructObject) +{ + JPH_ASSERT(inDestructObject != nullptr, "Object cannot be destructed"); +} + +RTTI::RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject, pCreateRTTIFunction inCreateRTTI) : + mName(inName), + mSize(inSize), + mCreate(inCreateObject), + mDestruct(inDestructObject) +{ + JPH_ASSERT(inDestructObject != nullptr, "Object cannot be destructed"); + + inCreateRTTI(*this); +} + +int RTTI::GetBaseClassCount() const +{ + return (int)mBaseClasses.size(); +} + +const RTTI *RTTI::GetBaseClass(int inIdx) const +{ + return mBaseClasses[inIdx].mRTTI; +} + +uint32 RTTI::GetHash() const +{ + // Perform diffusion step to get from 64 to 32 bits (see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function) + uint64 hash = HashString(mName); + return (uint32)(hash ^ (hash >> 32)); +} + +void *RTTI::CreateObject() const +{ + return IsAbstract()? nullptr : mCreate(); +} + +void RTTI::DestructObject(void *inObject) const +{ + mDestruct(inObject); +} + +void RTTI::AddBaseClass(const RTTI *inRTTI, int inOffset) +{ + JPH_ASSERT(inOffset >= 0 && inOffset < mSize, "Base class not contained in derived class"); + + // Add base class + BaseClass base; + base.mRTTI = inRTTI; + base.mOffset = inOffset; + mBaseClasses.push_back(base); + +#ifdef JPH_OBJECT_STREAM + // Add attributes of base class + for (const SerializableAttribute &a : inRTTI->mAttributes) + mAttributes.push_back(SerializableAttribute(a, inOffset)); +#endif // JPH_OBJECT_STREAM +} + +bool RTTI::operator == (const RTTI &inRHS) const +{ + // Compare addresses + if (this == &inRHS) + return true; + + // Check that the names differ (if that is the case we probably have two instances + // of the same attribute info across the program, probably the second is in a DLL) + JPH_ASSERT(strcmp(mName, inRHS.mName) != 0); + return false; +} + +bool RTTI::IsKindOf(const RTTI *inRTTI) const +{ + // Check if this is the same type + if (this == inRTTI) + return true; + + // Check all base classes + for (const BaseClass &b : mBaseClasses) + if (b.mRTTI->IsKindOf(inRTTI)) + return true; + + return false; +} + +const void *RTTI::CastTo(const void *inObject, const RTTI *inRTTI) const +{ + JPH_ASSERT(inObject != nullptr); + + // Check if this is the same type + if (this == inRTTI) + return inObject; + + // Check all base classes + for (const BaseClass &b : mBaseClasses) + { + // Cast the pointer to the base class + const void *casted = (const void *)(((const uint8 *)inObject) + b.mOffset); + + // Test base class + const void *rv = b.mRTTI->CastTo(casted, inRTTI); + if (rv != nullptr) + return rv; + } + + // Not possible to cast + return nullptr; +} + +#ifdef JPH_OBJECT_STREAM + +void RTTI::AddAttribute(const SerializableAttribute &inAttribute) +{ + mAttributes.push_back(inAttribute); +} + +int RTTI::GetAttributeCount() const +{ + return (int)mAttributes.size(); +} + +const SerializableAttribute &RTTI::GetAttribute(int inIdx) const +{ + return mAttributes[inIdx]; +} + +#endif // JPH_OBJECT_STREAM + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/RTTI.h b/thirdparty/jolt_physics/Jolt/Core/RTTI.h new file mode 100644 index 000000000000..90f358cbd580 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/RTTI.h @@ -0,0 +1,436 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// RTTI +////////////////////////////////////////////////////////////////////////////////////////// + +/// Light weight runtime type information system. This way we don't need to turn +/// on the default RTTI system of the compiler (introducing a possible overhead for every +/// class) +/// +/// Notes: +/// - An extra virtual member function is added. This adds 8 bytes to the size of +/// an instance of the class (unless you are already using virtual functions). +/// +/// To use RTTI on a specific class use: +/// +/// Header file: +/// +/// class Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL_BASE(Foo) +/// } +/// +/// class Bar : public Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL(Bar) +/// }; +/// +/// Implementation file: +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(Foo) +/// { +/// } +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL(Bar) +/// { +/// JPH_ADD_BASE_CLASS(Bar, Foo) // Multiple inheritance is allowed, just do JPH_ADD_BASE_CLASS for every base class +/// } +/// +/// For abstract classes use: +/// +/// Header file: +/// +/// class Foo +/// { +/// JPH_DECLARE_RTTI_ABSTRACT_BASE(Foo) +/// +/// public: +/// virtual void AbstractFunction() = 0; +/// } +/// +/// class Bar : public Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL(Bar) +/// +/// public: +/// virtual void AbstractFunction() { } // Function is now implemented so this class is no longer abstract +/// }; +/// +/// Implementation file: +/// +/// JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(Foo) +/// { +/// } +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL(Bar) +/// { +/// JPH_ADD_BASE_CLASS(Bar, Foo) +/// } +/// +/// Example of usage in a program: +/// +/// Foo *foo_ptr = new Foo; +/// Foo *bar_ptr = new Bar; +/// +/// IsType(foo_ptr, RTTI(Bar)) returns false +/// IsType(bar_ptr, RTTI(Bar)) returns true +/// +/// IsKindOf(foo_ptr, RTTI(Bar)) returns false +/// IsKindOf(bar_ptr, RTTI(Foo)) returns true +/// IsKindOf(bar_ptr, RTTI(Bar)) returns true +/// +/// StaticCast(foo_ptr) asserts and returns foo_ptr casted to Bar * +/// StaticCast(bar_ptr) returns bar_ptr casted to Bar * +/// +/// DynamicCast(foo_ptr) returns nullptr +/// DynamicCast(bar_ptr) returns bar_ptr casted to Bar * +/// +/// Other feature of DynamicCast: +/// +/// class A { int data[5]; }; +/// class B { int data[7]; }; +/// class C : public A, public B { int data[9]; }; +/// +/// C *c = new C; +/// A *a = c; +/// +/// Note that: +/// +/// B *b = (B *)a; +/// +/// generates an invalid pointer, +/// +/// B *b = StaticCast(a); +/// +/// doesn't compile, and +/// +/// B *b = DynamicCast(a); +/// +/// does the correct cast +class JPH_EXPORT RTTI +{ +public: + /// Function to create an object + using pCreateObjectFunction = void *(*)(); + + /// Function to destroy an object + using pDestructObjectFunction = void (*)(void *inObject); + + /// Function to initialize the runtime type info structure + using pCreateRTTIFunction = void (*)(RTTI &inRTTI); + + /// Constructor + RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject); + RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject, pCreateRTTIFunction inCreateRTTI); + + // Properties + inline const char * GetName() const { return mName; } + void SetName(const char *inName) { mName = inName; } + inline int GetSize() const { return mSize; } + bool IsAbstract() const { return mCreate == nullptr || mDestruct == nullptr; } + int GetBaseClassCount() const; + const RTTI * GetBaseClass(int inIdx) const; + uint32 GetHash() const; + + /// Create an object of this type (returns nullptr if the object is abstract) + void * CreateObject() const; + + /// Destruct object of this type (does nothing if the object is abstract) + void DestructObject(void *inObject) const; + + /// Add base class + void AddBaseClass(const RTTI *inRTTI, int inOffset); + + /// Equality operators + bool operator == (const RTTI &inRHS) const; + bool operator != (const RTTI &inRHS) const { return !(*this == inRHS); } + + /// Test if this class is derived from class of type inRTTI + bool IsKindOf(const RTTI *inRTTI) const; + + /// Cast inObject of this type to object of type inRTTI, returns nullptr if the cast is unsuccessful + const void * CastTo(const void *inObject, const RTTI *inRTTI) const; + +#ifdef JPH_OBJECT_STREAM + /// Attribute access + void AddAttribute(const SerializableAttribute &inAttribute); + int GetAttributeCount() const; + const SerializableAttribute & GetAttribute(int inIdx) const; +#endif // JPH_OBJECT_STREAM + +protected: + /// Base class information + struct BaseClass + { + const RTTI * mRTTI; + int mOffset; + }; + + const char * mName; ///< Class name + int mSize; ///< Class size + StaticArray mBaseClasses; ///< Names of base classes + pCreateObjectFunction mCreate; ///< Pointer to a function that will create a new instance of this class + pDestructObjectFunction mDestruct; ///< Pointer to a function that will destruct an object of this class +#ifdef JPH_OBJECT_STREAM + StaticArray mAttributes; ///< All attributes of this class +#endif // JPH_OBJECT_STREAM +}; + +////////////////////////////////////////////////////////////////////////////////////////// +// Add run time type info to types that don't have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_RTTI_NON_VIRTUAL +#define JPH_DECLARE_RTTI_NON_VIRTUAL(linkage, class_name) \ +public: \ + JPH_OVERRIDE_NEW_DELETE \ + friend linkage RTTI * GetRTTIOfType(class_name *); \ + friend inline const RTTI * GetRTTI([[maybe_unused]] const class_name *inObject) { return GetRTTIOfType(static_cast(nullptr)); }\ + static void sCreateRTTI(RTTI &inRTTI); \ + +// JPH_IMPLEMENT_RTTI_NON_VIRTUAL +#define JPH_IMPLEMENT_RTTI_NON_VIRTUAL(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but when you cannot insert the declaration in the class +// itself, for example for templates and third party classes +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_RTTI_OUTSIDE_CLASS +#define JPH_DECLARE_RTTI_OUTSIDE_CLASS(linkage, class_name) \ + linkage RTTI * GetRTTIOfType(class_name *); \ + inline const RTTI * GetRTTI(const class_name *inObject) { return GetRTTIOfType((class_name *)nullptr); }\ + void CreateRTTI##class_name(RTTI &inRTTI); \ + +// JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS +#define JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti((const char *)#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &CreateRTTI##class_name); \ + return &rtti; \ + } \ + void CreateRTTI##class_name(RTTI &inRTTI) + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but for classes that have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_DECLARE_RTTI_HELPER(linkage, class_name, modifier) \ +public: \ + JPH_OVERRIDE_NEW_DELETE \ + friend linkage RTTI * GetRTTIOfType(class_name *); \ + friend inline const RTTI * GetRTTI(const class_name *inObject) { return inObject->GetRTTI(); } \ + virtual const RTTI * GetRTTI() const modifier; \ + virtual const void * CastTo(const RTTI *inRTTI) const modifier; \ + static void sCreateRTTI(RTTI &inRTTI); \ + +// JPH_DECLARE_RTTI_VIRTUAL - for derived classes with RTTI +#define JPH_DECLARE_RTTI_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, override) + +// JPH_IMPLEMENT_RTTI_VIRTUAL +#define JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + const RTTI * class_name::GetRTTI() const \ + { \ + return JPH_RTTI(class_name); \ + } \ + const void * class_name::CastTo(const RTTI *inRTTI) const \ + { \ + return JPH_RTTI(class_name)->CastTo((const void *)this, inRTTI); \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +// JPH_DECLARE_RTTI_VIRTUAL_BASE - for concrete base class that has RTTI +#define JPH_DECLARE_RTTI_VIRTUAL_BASE(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, ) + +// JPH_IMPLEMENT_RTTI_VIRTUAL_BASE +#define JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) + +// JPH_DECLARE_RTTI_ABSTRACT - for derived abstract class that have RTTI +#define JPH_DECLARE_RTTI_ABSTRACT(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, override) + +// JPH_IMPLEMENT_RTTI_ABSTRACT +#define JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), nullptr, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + const RTTI * class_name::GetRTTI() const \ + { \ + return JPH_RTTI(class_name); \ + } \ + const void * class_name::CastTo(const RTTI *inRTTI) const \ + { \ + return JPH_RTTI(class_name)->CastTo((const void *)this, inRTTI); \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +// JPH_DECLARE_RTTI_ABSTRACT_BASE - for abstract base class that has RTTI +#define JPH_DECLARE_RTTI_ABSTRACT_BASE(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, ) + +// JPH_IMPLEMENT_RTTI_ABSTRACT_BASE +#define JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) + +////////////////////////////////////////////////////////////////////////////////////////// +// Declare an RTTI class for registering with the factory +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_DECLARE_RTTI_FOR_FACTORY(linkage, class_name) \ + linkage RTTI * GetRTTIOfType(class class_name *); + +#define JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(linkage, name_space, class_name) \ + namespace name_space { \ + class class_name; \ + linkage RTTI * GetRTTIOfType(class class_name *); \ + } + +////////////////////////////////////////////////////////////////////////////////////////// +// Find the RTTI of a class +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_RTTI(class_name) GetRTTIOfType(static_cast(nullptr)) + +////////////////////////////////////////////////////////////////////////////////////////// +// Macro to rename a class, useful for embedded classes: +// +// class A { class B { }; } +// +// Now use JPH_RENAME_CLASS(B, A::B) to avoid conflicts with other classes named B +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_RENAME_CLASS +#define JPH_RENAME_CLASS(class_name, new_name) \ + inRTTI.SetName(#new_name); + +////////////////////////////////////////////////////////////////////////////////////////// +// Macro to add base classes +////////////////////////////////////////////////////////////////////////////////////////// + +/// Define very dirty macro to get the offset of a baseclass into a class +#define JPH_BASE_CLASS_OFFSET(inClass, inBaseClass) ((int(uint64((inBaseClass *)((inClass *)0x10000))))-0x10000) + +// JPH_ADD_BASE_CLASS +#define JPH_ADD_BASE_CLASS(class_name, base_class_name) \ + inRTTI.AddBaseClass(JPH_RTTI(base_class_name), JPH_BASE_CLASS_OFFSET(class_name, base_class_name)); + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros and templates to identify a class +////////////////////////////////////////////////////////////////////////////////////////// + +/// Check if inObject is of DstType +template +inline bool IsType(const Type *inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +template +inline bool IsType(const RefConst &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +template +inline bool IsType(const Ref &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +/// Check if inObject is or is derived from DstType +template +inline bool IsKindOf(const Type *inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +template +inline bool IsKindOf(const RefConst &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +template +inline bool IsKindOf(const Ref &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +/// Cast inObject to DstType, asserts on failure +template || std::is_base_of_v, bool> = true> +inline const DstType *StaticCast(const SrcType *inObject) +{ + return static_cast(inObject); +} + +template || std::is_base_of_v, bool> = true> +inline DstType *StaticCast(SrcType *inObject) +{ + return static_cast(inObject); +} + +template || std::is_base_of_v, bool> = true> +inline const DstType *StaticCast(const RefConst &inObject) +{ + return static_cast(inObject.GetPtr()); +} + +template || std::is_base_of_v, bool> = true> +inline DstType *StaticCast(const Ref &inObject) +{ + return static_cast(inObject.GetPtr()); +} + +/// Cast inObject to DstType, returns nullptr on failure +template +inline const DstType *DynamicCast(const SrcType *inObject) +{ + return inObject != nullptr? reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType))) : nullptr; +} + +template +inline DstType *DynamicCast(SrcType *inObject) +{ + return inObject != nullptr? const_cast(reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType)))) : nullptr; +} + +template +inline const DstType *DynamicCast(const RefConst &inObject) +{ + return inObject != nullptr? reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType))) : nullptr; +} + +template +inline DstType *DynamicCast(const Ref &inObject) +{ + return inObject != nullptr? const_cast(reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType)))) : nullptr; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Reference.h b/thirdparty/jolt_physics/Jolt/Core/Reference.h new file mode 100644 index 000000000000..8a2de696865d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Reference.h @@ -0,0 +1,244 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +// Forward declares +template class Ref; +template class RefConst; + +/// Simple class to facilitate reference counting / releasing +/// Derive your class from RefTarget and you can reference it by using Ref or RefConst +/// +/// Reference counting classes keep an integer which indicates how many references +/// to the object are active. Reference counting objects are derived from RefTarget +/// and staT & their life with a reference count of zero. They can then be assigned +/// to equivalents of pointers (Ref) which will increase the reference count immediately. +/// If the destructor of Ref is called or another object is assigned to the reference +/// counting pointer it will decrease the reference count of the object again. If this +/// reference count becomes zero, the object is destroyed. +/// +/// This provides a very powerful mechanism to prevent memory leaks, but also gives +/// some responsibility to the programmer. The most notable point is that you cannot +/// have one object reference another and have the other reference the first one +/// back, because this way the reference count of both objects will never become +/// lower than 1, resulting in a memory leak. By carefully designing your classes +/// (and particularly identifying who owns who in the class hierarchy) you can avoid +/// these problems. +template +class RefTarget +{ +public: + /// Constructor + inline RefTarget() = default; + inline RefTarget(const RefTarget &) { /* Do not copy refcount */ } + inline ~RefTarget() { JPH_IF_ENABLE_ASSERTS(uint32 value = mRefCount.load(memory_order_relaxed);) JPH_ASSERT(value == 0 || value == cEmbedded); } ///< assert no one is referencing us + + /// Mark this class as embedded, this means the type can be used in a compound or constructed on the stack. + /// The Release function will never destruct the object, it is assumed the destructor will be called by whoever allocated + /// the object and at that point in time it is checked that no references are left to the structure. + inline void SetEmbedded() const { JPH_IF_ENABLE_ASSERTS(uint32 old = ) mRefCount.fetch_add(cEmbedded, memory_order_relaxed); JPH_ASSERT(old < cEmbedded); } + + /// Assignment operator + inline RefTarget & operator = (const RefTarget &) { /* Don't copy refcount */ return *this; } + + /// Get current refcount of this object + uint32 GetRefCount() const { return mRefCount.load(memory_order_relaxed); } + + /// Add or release a reference to this object + inline void AddRef() const + { + // Adding a reference can use relaxed memory ordering + mRefCount.fetch_add(1, memory_order_relaxed); + } + + inline void Release() const + { + #ifndef JPH_TSAN_ENABLED + // Releasing a reference must use release semantics... + if (mRefCount.fetch_sub(1, memory_order_release) == 1) + { + // ... so that we can use acquire to ensure that we see any updates from other threads that released a ref before deleting the object + atomic_thread_fence(memory_order_acquire); + delete static_cast(this); + } + #else + // But under TSAN, we cannot use atomic_thread_fence, so we use an acq_rel operation unconditionally instead + if (mRefCount.fetch_sub(1, memory_order_acq_rel) == 1) + delete static_cast(this); + #endif + } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + static int sInternalGetRefCountOffset() { return offsetof(T, mRefCount); } + +protected: + static constexpr uint32 cEmbedded = 0x0ebedded; ///< A large value that gets added to the refcount to mark the object as embedded + + mutable atomic mRefCount = 0; ///< Current reference count +}; + +/// Pure virtual version of RefTarget +class JPH_EXPORT RefTargetVirtual +{ +public: + /// Virtual destructor + virtual ~RefTargetVirtual() = default; + + /// Virtual add reference + virtual void AddRef() = 0; + + /// Virtual release reference + virtual void Release() = 0; +}; + +/// Class for automatic referencing, this is the equivalent of a pointer to type T +/// if you assign a value to this class it will increment the reference count by one +/// of this object, and if you assign something else it will decrease the reference +/// count of the first object again. If it reaches a reference count of zero it will +/// be deleted +template +class Ref +{ +public: + /// Constructor + inline Ref() : mPtr(nullptr) { } + inline Ref(T *inRHS) : mPtr(inRHS) { AddRef(); } + inline Ref(const Ref &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline Ref(Ref &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline ~Ref() { Release(); } + + /// Assignment operators + inline Ref & operator = (T *inRHS) { if (mPtr != inRHS) { Release(); mPtr = inRHS; AddRef(); } return *this; } + inline Ref & operator = (const Ref &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline Ref & operator = (Ref &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + + /// Casting operators + inline operator T *() const { return mPtr; } + + /// Access like a normal pointer + inline T * operator -> () const { return mPtr; } + inline T & operator * () const { return *mPtr; } + + /// Comparison + inline bool operator == (const T * inRHS) const { return mPtr == inRHS; } + inline bool operator == (const Ref &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const T * inRHS) const { return mPtr != inRHS; } + inline bool operator != (const Ref &inRHS) const { return mPtr != inRHS.mPtr; } + + /// Get pointer + inline T * GetPtr() const { return mPtr; } + + /// Get hash for this object + uint64 GetHash() const + { + return Hash { } (mPtr); + } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + void ** InternalGetPointer() { return reinterpret_cast(&mPtr); } + +private: + template friend class RefConst; + + /// Use "variable = nullptr;" to release an object, do not call these functions + inline void AddRef() { if (mPtr != nullptr) mPtr->AddRef(); } + inline void Release() { if (mPtr != nullptr) mPtr->Release(); } + + T * mPtr; ///< Pointer to object that we are reference counting +}; + +/// Class for automatic referencing, this is the equivalent of a CONST pointer to type T +/// if you assign a value to this class it will increment the reference count by one +/// of this object, and if you assign something else it will decrease the reference +/// count of the first object again. If it reaches a reference count of zero it will +/// be deleted +template +class RefConst +{ +public: + /// Constructor + inline RefConst() : mPtr(nullptr) { } + inline RefConst(const T * inRHS) : mPtr(inRHS) { AddRef(); } + inline RefConst(const RefConst &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline RefConst(RefConst &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline RefConst(const Ref &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline RefConst(Ref &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline ~RefConst() { Release(); } + + /// Assignment operators + inline RefConst & operator = (const T * inRHS) { if (mPtr != inRHS) { Release(); mPtr = inRHS; AddRef(); } return *this; } + inline RefConst & operator = (const RefConst &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline RefConst & operator = (RefConst &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + inline RefConst & operator = (const Ref &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline RefConst & operator = (Ref &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + + /// Casting operators + inline operator const T * () const { return mPtr; } + + /// Access like a normal pointer + inline const T * operator -> () const { return mPtr; } + inline const T & operator * () const { return *mPtr; } + + /// Comparison + inline bool operator == (const T * inRHS) const { return mPtr == inRHS; } + inline bool operator == (const RefConst &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator == (const Ref &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const T * inRHS) const { return mPtr != inRHS; } + inline bool operator != (const RefConst &inRHS) const { return mPtr != inRHS.mPtr; } + inline bool operator != (const Ref &inRHS) const { return mPtr != inRHS.mPtr; } + + /// Get pointer + inline const T * GetPtr() const { return mPtr; } + + /// Get hash for this object + uint64 GetHash() const + { + return Hash { } (mPtr); + } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + void ** InternalGetPointer() { return const_cast(reinterpret_cast(&mPtr)); } + +private: + /// Use "variable = nullptr;" to release an object, do not call these functions + inline void AddRef() { if (mPtr != nullptr) mPtr->AddRef(); } + inline void Release() { if (mPtr != nullptr) mPtr->Release(); } + + const T * mPtr; ///< Pointer to object that we are reference counting +}; + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") + +namespace std +{ + /// Declare std::hash for Ref + template + struct hash> + { + size_t operator () (const JPH::Ref &inRHS) const + { + return size_t(inRHS.GetHash()); + } + }; + + /// Declare std::hash for RefConst + template + struct hash> + { + size_t operator () (const JPH::RefConst &inRHS) const + { + return size_t(inRHS.GetHash()); + } + }; +} + +JPH_SUPPRESS_WARNING_POP diff --git a/thirdparty/jolt_physics/Jolt/Core/Result.h b/thirdparty/jolt_physics/Jolt/Core/Result.h new file mode 100644 index 000000000000..ad2eab164bdf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Result.h @@ -0,0 +1,174 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Helper class that either contains a valid result or an error +template +class Result +{ +public: + /// Default constructor + Result() { } + + /// Copy constructor + Result(const Result &inRHS) : + mState(inRHS.mState) + { + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (inRHS.mResult); + break; + + case EState::Error: + ::new (&mError) String(inRHS.mError); + break; + + case EState::Invalid: + break; + } + } + + /// Move constructor + Result(Result &&inRHS) noexcept : + mState(inRHS.mState) + { + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (std::move(inRHS.mResult)); + break; + + case EState::Error: + ::new (&mError) String(std::move(inRHS.mError)); + break; + + case EState::Invalid: + break; + } + + // Don't reset the state of inRHS, the destructors still need to be called after a move operation + } + + /// Destructor + ~Result() { Clear(); } + + /// Copy assignment + Result & operator = (const Result &inRHS) + { + Clear(); + + mState = inRHS.mState; + + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (inRHS.mResult); + break; + + case EState::Error: + ::new (&mError) String(inRHS.mError); + break; + + case EState::Invalid: + break; + } + + return *this; + } + + /// Move assignment + Result & operator = (Result &&inRHS) noexcept + { + Clear(); + + mState = inRHS.mState; + + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (std::move(inRHS.mResult)); + break; + + case EState::Error: + ::new (&mError) String(std::move(inRHS.mError)); + break; + + case EState::Invalid: + break; + } + + // Don't reset the state of inRHS, the destructors still need to be called after a move operation + + return *this; + } + + /// Clear result or error + void Clear() + { + switch (mState) + { + case EState::Valid: + mResult.~Type(); + break; + + case EState::Error: + mError.~String(); + break; + + case EState::Invalid: + break; + } + + mState = EState::Invalid; + } + + /// Checks if the result is still uninitialized + bool IsEmpty() const { return mState == EState::Invalid; } + + /// Checks if the result is valid + bool IsValid() const { return mState == EState::Valid; } + + /// Get the result value + const Type & Get() const { JPH_ASSERT(IsValid()); return mResult; } + + /// Set the result value + void Set(const Type &inResult) { Clear(); ::new (&mResult) Type(inResult); mState = EState::Valid; } + + /// Set the result value (move value) + void Set(Type &&inResult) { Clear(); ::new (&mResult) Type(std::move(inResult)); mState = EState::Valid; } + + /// Check if we had an error + bool HasError() const { return mState == EState::Error; } + + /// Get the error value + const String & GetError() const { JPH_ASSERT(HasError()); return mError; } + + /// Set an error value + void SetError(const char *inError) { Clear(); ::new (&mError) String(inError); mState = EState::Error; } + void SetError(const string_view &inError) { Clear(); ::new (&mError) String(inError); mState = EState::Error; } + void SetError(String &&inError) { Clear(); ::new (&mError) String(std::move(inError)); mState = EState::Error; } + +private: + union + { + Type mResult; ///< The actual result object + String mError; ///< The error description if the result failed + }; + + /// State of the result + enum class EState : uint8 + { + Invalid, + Valid, + Error + }; + + EState mState = EState::Invalid; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h new file mode 100644 index 000000000000..60e3ad4873dc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// STL allocator that takes care that memory is aligned to N bytes +template +class STLAlignedAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is stateless + using is_always_equal = std::true_type; + + /// Allocator supports moving + using propagate_on_container_move_assignment = std::true_type; + + /// Constructor + inline STLAlignedAllocator() = default; + + /// Constructor from other allocator + template + inline explicit STLAlignedAllocator(const STLAlignedAllocator &) { } + + /// Allocate memory + inline pointer allocate(size_type inN) + { + return (pointer)AlignedAllocate(inN * sizeof(value_type), N); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type) + { + AlignedFree(inPointer); + } + + /// Allocators are stateless so assumed to be equal + inline bool operator == (const STLAlignedAllocator &) const + { + return true; + } + + inline bool operator != (const STLAlignedAllocator &) const + { + return false; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLAlignedAllocator; + }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h new file mode 100644 index 000000000000..f9573279ad61 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h @@ -0,0 +1,127 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Default implementation of AllocatorHasReallocate which tells if an allocator has a reallocate function +template struct AllocatorHasReallocate { static constexpr bool sValue = false; }; + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + +/// STL allocator that forwards to our allocation functions +template +class STLAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is stateless + using is_always_equal = std::true_type; + + /// Allocator supports moving + using propagate_on_container_move_assignment = std::true_type; + + /// Constructor + inline STLAllocator() = default; + + /// Constructor from other allocator + template + inline STLAllocator(const STLAllocator &) { } + + /// If this allocator needs to fall back to aligned allocations because the type requires it + static constexpr bool needs_aligned_allocate = alignof(T) > (JPH_CPU_ADDRESS_BITS == 32? 8 : 16); + + /// Allocate memory + inline pointer allocate(size_type inN) + { + if constexpr (needs_aligned_allocate) + return pointer(AlignedAllocate(inN * sizeof(value_type), alignof(T))); + else + return pointer(Allocate(inN * sizeof(value_type))); + } + + /// Should we expose a reallocate function? + static constexpr bool has_reallocate = std::is_trivially_copyable() && !needs_aligned_allocate; + + /// Reallocate memory + template > + inline pointer reallocate(pointer inOldPointer, size_type inOldSize, size_type inNewSize) + { + JPH_ASSERT(inNewSize > 0); // Reallocating to zero size is implementation dependent, so we don't allow it + return pointer(Reallocate(inOldPointer, inOldSize * sizeof(value_type), inNewSize * sizeof(value_type))); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type) + { + if constexpr (needs_aligned_allocate) + AlignedFree(inPointer); + else + Free(inPointer); + } + + /// Allocators are stateless so assumed to be equal + inline bool operator == (const STLAllocator &) const + { + return true; + } + + inline bool operator != (const STLAllocator &) const + { + return false; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLAllocator; + }; +}; + +/// The STLAllocator implements the reallocate function if the alignment of the class is smaller or equal to the default alignment for the platform +template struct AllocatorHasReallocate> { static constexpr bool sValue = STLAllocator::has_reallocate; }; + +#else + +template using STLAllocator = std::allocator; + +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + +// Declare STL containers that use our allocator +using String = std::basic_string, STLAllocator>; +using IStringStream = std::basic_istringstream, STLAllocator>; + +JPH_NAMESPACE_END + +#if (!defined(JPH_PLATFORM_WINDOWS) || defined(JPH_COMPILER_MINGW)) && !defined(JPH_DISABLE_CUSTOM_ALLOCATOR) + +namespace std +{ + /// Declare std::hash for String, for some reason on Linux based platforms template deduction takes the wrong variant + template <> + struct hash + { + inline size_t operator () (const JPH::String &inRHS) const + { + return hash { } (inRHS); + } + }; +} + +#endif // (!JPH_PLATFORM_WINDOWS || JPH_COMPILER_MINGW) && !JPH_DISABLE_CUSTOM_ALLOCATOR diff --git a/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h new file mode 100644 index 000000000000..cf7c39d45550 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// STL allocator that wraps around TempAllocator +template +class STLTempAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is not stateless (depends on the temp allocator) + using is_always_equal = std::false_type; + + /// Constructor + inline STLTempAllocator(TempAllocator &inAllocator) : mAllocator(inAllocator) { } + + /// Constructor from other allocator + template + inline explicit STLTempAllocator(const STLTempAllocator &inRHS) : mAllocator(inRHS.GetAllocator()) { } + + /// Allocate memory + inline pointer allocate(size_type inN) + { + return pointer(mAllocator.Allocate(uint(inN * sizeof(value_type)))); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type inN) + { + mAllocator.Free(inPointer, uint(inN * sizeof(value_type))); + } + + /// Allocators are not-stateless, assume if allocator address matches that the allocators are the same + inline bool operator == (const STLTempAllocator &inRHS) const + { + return &mAllocator == &inRHS.mAllocator; + } + + inline bool operator != (const STLTempAllocator &inRHS) const + { + return &mAllocator != &inRHS.mAllocator; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLTempAllocator; + }; + + /// Get our temp allocator + TempAllocator & GetAllocator() const + { + return mAllocator; + } + +private: + TempAllocator & mAllocator; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h b/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h new file mode 100644 index 000000000000..613838421319 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that calls a function when it goes out of scope +template +class ScopeExit : public NonCopyable +{ +public: + /// Constructor specifies the exit function + JPH_INLINE explicit ScopeExit(F &&inFunction) : mFunction(std::move(inFunction)) { } + + /// Destructor calls the exit function + JPH_INLINE ~ScopeExit() { if (!mInvoked) mFunction(); } + + /// Call the exit function now instead of when going out of scope + JPH_INLINE void Invoke() + { + if (!mInvoked) + { + mFunction(); + mInvoked = true; + } + } + + /// No longer call the exit function when going out of scope + JPH_INLINE void Release() + { + mInvoked = true; + } + +private: + F mFunction; + bool mInvoked = false; +}; + +#define JPH_SCOPE_EXIT_TAG2(line) scope_exit##line +#define JPH_SCOPE_EXIT_TAG(line) JPH_SCOPE_EXIT_TAG2(line) + +/// Usage: JPH_SCOPE_EXIT([]{ code to call on scope exit }); +#define JPH_SCOPE_EXIT(...) ScopeExit JPH_SCOPE_EXIT_TAG(__LINE__)(__VA_ARGS__) + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp b/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp new file mode 100644 index 000000000000..27bb591bce52 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp @@ -0,0 +1,82 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_PLATFORM_WINDOWS + JPH_SUPPRESS_WARNING_PUSH + JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception. + #ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN + #endif +#ifndef JPH_COMPILER_MINGW + #include +#else + #include +#endif + + JPH_SUPPRESS_WARNING_POP +#endif + +JPH_NAMESPACE_BEGIN + +Semaphore::Semaphore() +{ +#ifdef JPH_PLATFORM_WINDOWS + mSemaphore = CreateSemaphore(nullptr, 0, INT_MAX, nullptr); +#endif +} + +Semaphore::~Semaphore() +{ +#ifdef JPH_PLATFORM_WINDOWS + CloseHandle(mSemaphore); +#endif +} + +void Semaphore::Release(uint inNumber) +{ + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_PLATFORM_WINDOWS + int old_value = mCount.fetch_add(inNumber); + if (old_value < 0) + { + int new_value = old_value + (int)inNumber; + int num_to_release = min(new_value, 0) - old_value; + ::ReleaseSemaphore(mSemaphore, num_to_release, nullptr); + } +#else + std::lock_guard lock(mLock); + mCount.fetch_add(inNumber, std::memory_order_relaxed); + if (inNumber > 1) + mWaitVariable.notify_all(); + else + mWaitVariable.notify_one(); +#endif +} + +void Semaphore::Acquire(uint inNumber) +{ + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_PLATFORM_WINDOWS + int old_value = mCount.fetch_sub(inNumber); + int new_value = old_value - (int)inNumber; + if (new_value < 0) + { + int num_to_acquire = min(old_value, 0) - new_value; + for (int i = 0; i < num_to_acquire; ++i) + WaitForSingleObject(mSemaphore, INFINITE); + } +#else + std::unique_lock lock(mLock); + mCount.fetch_sub(inNumber, std::memory_order_relaxed); + mWaitVariable.wait(lock, [this]() { return mCount >= 0; }); +#endif +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/Semaphore.h b/thirdparty/jolt_physics/Jolt/Core/Semaphore.h new file mode 100644 index 000000000000..091d7d5b0aec --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/Semaphore.h @@ -0,0 +1,51 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::atomic; +using std::mutex; +using std::condition_variable; + +/// Implements a semaphore +/// When we switch to C++20 we can use counting_semaphore to unify this +class JPH_EXPORT Semaphore +{ +public: + /// Constructor + Semaphore(); + ~Semaphore(); + + /// Release the semaphore, signaling the thread waiting on the barrier that there may be work + void Release(uint inNumber = 1); + + /// Acquire the semaphore inNumber times + void Acquire(uint inNumber = 1); + + /// Get the current value of the semaphore + inline int GetValue() const { return mCount.load(std::memory_order_relaxed); } + +private: +#ifdef JPH_PLATFORM_WINDOWS + // On windows we use a semaphore object since it is more efficient than a lock and a condition variable + alignas(JPH_CACHE_LINE_SIZE) atomic mCount { 0 }; ///< We increment mCount for every release, to acquire we decrement the count. If the count is negative we know that we are waiting on the actual semaphore. + void * mSemaphore; ///< The semaphore is an expensive construct so we only acquire/release it if we know that we need to wait/have waiting threads +#else + // Other platforms: Emulate a semaphore using a mutex, condition variable and count + mutex mLock; + condition_variable mWaitVariable; + atomic mCount { 0 }; +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StaticArray.h b/thirdparty/jolt_physics/Jolt/Core/StaticArray.h new file mode 100644 index 000000000000..fddaaa37a2c2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StaticArray.h @@ -0,0 +1,329 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple variable length array backed by a fixed size buffer +template +class [[nodiscard]] StaticArray +{ +public: + using value_type = T; + + using size_type = uint; + + static constexpr uint Capacity = N; + + /// Default constructor + StaticArray() = default; + + /// Constructor from initializer list + explicit StaticArray(std::initializer_list inList) + { + JPH_ASSERT(inList.size() <= N); + for (const T &v : inList) + ::new (reinterpret_cast(&mElements[mSize++])) T(v); + } + + /// Copy constructor + StaticArray(const StaticArray &inRHS) + { + while (mSize < inRHS.mSize) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + /// Destruct all elements + ~StaticArray() + { + if constexpr (!std::is_trivially_destructible()) + for (T *e = reinterpret_cast(mElements), *end = e + mSize; e < end; ++e) + e->~T(); + } + + /// Destruct all elements and set length to zero + void clear() + { + if constexpr (!std::is_trivially_destructible()) + for (T *e = reinterpret_cast(mElements), *end = e + mSize; e < end; ++e) + e->~T(); + mSize = 0; + } + + /// Add element to the back of the array + void push_back(const T &inElement) + { + JPH_ASSERT(mSize < N); + ::new (&mElements[mSize++]) T(inElement); + } + + /// Construct element at the back of the array + template + void emplace_back(A &&... inElement) + { + JPH_ASSERT(mSize < N); + ::new (&mElements[mSize++]) T(std::forward(inElement)...); + } + + /// Remove element from the back of the array + void pop_back() + { + JPH_ASSERT(mSize > 0); + reinterpret_cast(mElements[--mSize]).~T(); + } + + /// Returns true if there are no elements in the array + bool empty() const + { + return mSize == 0; + } + + /// Returns amount of elements in the array + size_type size() const + { + return mSize; + } + + /// Returns maximum amount of elements the array can hold + size_type capacity() const + { + return N; + } + + /// Resize array to new length + void resize(size_type inNewSize) + { + JPH_ASSERT(inNewSize <= N); + if constexpr (!std::is_trivially_constructible()) + for (T *element = reinterpret_cast(mElements) + mSize, *element_end = reinterpret_cast(mElements) + inNewSize; element < element_end; ++element) + ::new (element) T; + if constexpr (!std::is_trivially_destructible()) + for (T *element = reinterpret_cast(mElements) + inNewSize, *element_end = reinterpret_cast(mElements) + mSize; element < element_end; ++element) + element->~T(); + mSize = inNewSize; + } + + using const_iterator = const T *; + + /// Iterators + const_iterator begin() const + { + return reinterpret_cast(mElements); + } + + const_iterator end() const + { + return reinterpret_cast(mElements + mSize); + } + + using iterator = T *; + + iterator begin() + { + return reinterpret_cast(mElements); + } + + iterator end() + { + return reinterpret_cast(mElements + mSize); + } + + const T * data() const + { + return reinterpret_cast(mElements); + } + + T * data() + { + return reinterpret_cast(mElements); + } + + /// Access element + T & operator [] (size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + const T & operator [] (size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + /// Access element + T & at(size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + const T & at(size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + /// First element in the array + const T & front() const + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[0]); + } + + T & front() + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[0]); + } + + /// Last element in the array + const T & back() const + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[mSize - 1]); + } + + T & back() + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[mSize - 1]); + } + + /// Remove one element from the array + void erase(const_iterator inIter) + { + size_type p = size_type(inIter - begin()); + JPH_ASSERT(p < mSize); + reinterpret_cast(mElements[p]).~T(); + if (p + 1 < mSize) + memmove(mElements + p, mElements + p + 1, (mSize - p - 1) * sizeof(T)); + --mSize; + } + + /// Remove multiple element from the array + void erase(const_iterator inBegin, const_iterator inEnd) + { + size_type p = size_type(inBegin - begin()); + size_type n = size_type(inEnd - inBegin); + JPH_ASSERT(inEnd <= end()); + for (size_type i = 0; i < n; ++i) + reinterpret_cast(mElements[p + i]).~T(); + if (p + n < mSize) + memmove(mElements + p, mElements + p + n, (mSize - p - n) * sizeof(T)); + mSize -= n; + } + + /// Assignment operator + StaticArray & operator = (const StaticArray &inRHS) + { + size_type rhs_size = inRHS.size(); + + if (static_cast(this) != static_cast(&inRHS)) + { + clear(); + + while (mSize < rhs_size) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + return *this; + } + + /// Assignment operator with static array of different max length + template + StaticArray & operator = (const StaticArray &inRHS) + { + size_type rhs_size = inRHS.size(); + JPH_ASSERT(rhs_size <= N); + + if (static_cast(this) != static_cast(&inRHS)) + { + clear(); + + while (mSize < rhs_size) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + return *this; + } + + /// Comparing arrays + bool operator == (const StaticArray &inRHS) const + { + if (mSize != inRHS.mSize) + return false; + for (size_type i = 0; i < mSize; ++i) + if (!(reinterpret_cast(mElements[i]) == reinterpret_cast(inRHS.mElements[i]))) + return false; + return true; + } + + bool operator != (const StaticArray &inRHS) const + { + if (mSize != inRHS.mSize) + return true; + for (size_type i = 0; i < mSize; ++i) + if (reinterpret_cast(mElements[i]) != reinterpret_cast(inRHS.mElements[i])) + return true; + return false; + } + + /// Get hash for this array + uint64 GetHash() const + { + // Hash length first + uint64 ret = Hash { } (uint32(size())); + + // Then hash elements + for (const T *element = reinterpret_cast(mElements), *element_end = reinterpret_cast(mElements) + mSize; element < element_end; ++element) + HashCombine(ret, *element); + + return ret; + } + +protected: + struct alignas(T) Storage + { + uint8 mData[sizeof(T)]; + }; + + static_assert(sizeof(T) == sizeof(Storage), "Mismatch in size"); + static_assert(alignof(T) == alignof(Storage), "Mismatch in alignment"); + + size_type mSize = 0; + Storage mElements[N]; +}; + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") + +namespace std +{ + /// Declare std::hash for StaticArray + template + struct hash> + { + size_t operator () (const JPH::StaticArray &inRHS) const + { + return std::size_t(inRHS.GetHash()); + } + }; +} + +JPH_SUPPRESS_WARNING_POP diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamIn.h b/thirdparty/jolt_physics/Jolt/Core/StreamIn.h new file mode 100644 index 000000000000..4de299e6d1cf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StreamIn.h @@ -0,0 +1,119 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple binary input stream +class JPH_EXPORT StreamIn : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~StreamIn() = default; + + /// Read a string of bytes from the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) = 0; + + /// Returns true when an attempt has been made to read past the end of the file + virtual bool IsEOF() const = 0; + + /// Returns true if there was an IO failure + virtual bool IsFailed() const = 0; + + /// Read a primitive (e.g. float, int, etc.) from the binary stream + template , bool> = true> + void Read(T &outT) + { + ReadBytes(&outT, sizeof(outT)); + } + + /// Read a vector of primitives from the binary stream + template , bool> = true> + void Read(Array &outT) + { + uint32 len = uint32(outT.size()); // Initialize to previous array size, this is used for validation in the StateRecorder class + Read(len); + if (!IsEOF() && !IsFailed()) + { + outT.resize(len); + if constexpr (std::is_same_v || std::is_same_v || std::is_same_v) + { + // These types have unused components that we don't want to read + for (typename Array::size_type i = 0; i < len; ++i) + Read(outT[i]); + } + else + { + // Read all elements at once + ReadBytes(outT.data(), len * sizeof(T)); + } + } + else + outT.clear(); + } + + /// Read a string from the binary stream (reads the number of characters and then the characters) + template + void Read(std::basic_string &outString) + { + uint32 len = 0; + Read(len); + if (!IsEOF() && !IsFailed()) + { + outString.resize(len); + ReadBytes(outString.data(), len * sizeof(Type)); + } + else + outString.clear(); + } + + /// Read a vector of primitives from the binary stream using a custom function to read the elements + template + void Read(Array &outT, const F &inReadElement) + { + uint32 len = uint32(outT.size()); // Initialize to previous array size, this is used for validation in the StateRecorder class + Read(len); + if (!IsEOF() && !IsFailed()) + { + outT.resize(len); + for (typename Array::size_type i = 0; i < len; ++i) + inReadElement(*this, outT[i]); + } + else + outT.clear(); + } + + /// Read a Vec3 (don't read W) + void Read(Vec3 &outVec) + { + ReadBytes(&outVec, 3 * sizeof(float)); + outVec = Vec3::sFixW(outVec.mValue); + } + + /// Read a DVec3 (don't read W) + void Read(DVec3 &outVec) + { + ReadBytes(&outVec, 3 * sizeof(double)); + outVec = DVec3::sFixW(outVec.mValue); + } + + /// Read a DMat44 (don't read W component of translation) + void Read(DMat44 &outVec) + { + Vec4 x, y, z; + Read(x); + Read(y); + Read(z); + + DVec3 t; + Read(t); + + outVec = DMat44(x, y, z, t); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamOut.h b/thirdparty/jolt_physics/Jolt/Core/StreamOut.h new file mode 100644 index 000000000000..566f8ec8da32 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StreamOut.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple binary output stream +class JPH_EXPORT StreamOut : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~StreamOut() = default; + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) = 0; + + /// Returns true if there was an IO failure + virtual bool IsFailed() const = 0; + + /// Write a primitive (e.g. float, int, etc.) to the binary stream + template , bool> = true> + void Write(const T &inT) + { + WriteBytes(&inT, sizeof(inT)); + } + + /// Write a vector of primitives to the binary stream + template , bool> = true> + void Write(const Array &inT) + { + uint32 len = uint32(inT.size()); + Write(len); + if (!IsFailed()) + { + if constexpr (std::is_same_v || std::is_same_v || std::is_same_v) + { + // These types have unused components that we don't want to write + for (typename Array::size_type i = 0; i < len; ++i) + Write(inT[i]); + } + else + { + // Write all elements at once + WriteBytes(inT.data(), len * sizeof(T)); + } + } + } + + /// Write a string to the binary stream (writes the number of characters and then the characters) + template + void Write(const std::basic_string &inString) + { + uint32 len = uint32(inString.size()); + Write(len); + if (!IsFailed()) + WriteBytes(inString.data(), len * sizeof(Type)); + } + + /// Write a vector of primitives to the binary stream using a custom write function + template + void Write(const Array &inT, const F &inWriteElement) + { + uint32 len = uint32(inT.size()); + Write(len); + if (!IsFailed()) + for (typename Array::size_type i = 0; i < len; ++i) + inWriteElement(inT[i], *this); + } + + /// Write a Vec3 (don't write W) + void Write(const Vec3 &inVec) + { + WriteBytes(&inVec, 3 * sizeof(float)); + } + + /// Write a DVec3 (don't write W) + void Write(const DVec3 &inVec) + { + WriteBytes(&inVec, 3 * sizeof(double)); + } + + /// Write a DMat44 (don't write W component of translation) + void Write(const DMat44 &inVec) + { + Write(inVec.GetColumn4(0)); + Write(inVec.GetColumn4(1)); + Write(inVec.GetColumn4(2)); + + Write(inVec.GetTranslation()); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h b/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h new file mode 100644 index 000000000000..1feb232f350c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h @@ -0,0 +1,168 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +namespace StreamUtils { + +template +using ObjectToIDMap = UnorderedMap; + +template +using IDToObjectMap = Array>; + +// Restore a single object by reading the hash of the type, constructing it and then calling the restore function +template +Result> RestoreObject(StreamIn &inStream, void (Type::*inRestoreBinaryStateFunction)(StreamIn &)) +{ + Result> result; + + // Read the hash of the type + uint32 hash; + inStream.Read(hash); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read type hash"); + return result; + } + + // Get the RTTI for the type + const RTTI *rtti = Factory::sInstance->Find(hash); + if (rtti == nullptr) + { + result.SetError("Failed to create instance of type"); + return result; + } + + // Construct and read the data of the type + Ref object = reinterpret_cast(rtti->CreateObject()); + (object->*inRestoreBinaryStateFunction)(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to restore object"); + return result; + } + + result.Set(object); + return result; +} + +/// Save an object reference to a stream. Uses a map to map objects to IDs which is also used to prevent writing duplicates. +template +void SaveObjectReference(StreamOut &inStream, const Type *inObject, ObjectToIDMap *ioObjectToIDMap) +{ + if (ioObjectToIDMap == nullptr || inObject == nullptr) + { + // Write null ID + inStream.Write(~uint32(0)); + } + else + { + typename ObjectToIDMap::const_iterator id = ioObjectToIDMap->find(inObject); + if (id != ioObjectToIDMap->end()) + { + // Existing object, write ID + inStream.Write(id->second); + } + else + { + // New object, write the ID + uint32 new_id = uint32(ioObjectToIDMap->size()); + (*ioObjectToIDMap)[inObject] = new_id; + inStream.Write(new_id); + + // Write the object + inObject->SaveBinaryState(inStream); + } + } +} + +/// Restore an object reference from stream. +template +Result> RestoreObjectReference(StreamIn &inStream, IDToObjectMap &ioIDToObjectMap) +{ + Result> result; + + // Read id + uint32 id = ~uint32(0); + inStream.Read(id); + + // Check null + if (id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if it already exists + if (id >= ioIDToObjectMap.size()) + { + // New object, restore it + result = Type::sRestoreFromBinaryState(inStream); + if (result.HasError()) + return result; + JPH_ASSERT(id == ioIDToObjectMap.size()); + ioIDToObjectMap.push_back(result.Get()); + } + else + { + // Existing object filter + result.Set(ioIDToObjectMap[id].GetPtr()); + } + + return result; +} + +// Save an array of objects to a stream. +template +void SaveObjectArray(StreamOut &inStream, const ArrayType &inArray, ObjectToIDMap *ioObjectToIDMap) +{ + uint32 len = uint32(inArray.size()); + inStream.Write(len); + for (const ValueType *value: inArray) + SaveObjectReference(inStream, value, ioObjectToIDMap); +} + +// Restore an array of objects from a stream. +template +Result RestoreObjectArray(StreamIn &inStream, IDToObjectMap &ioIDToObjectMap) +{ + Result result; + + uint32 len; + inStream.Read(len); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read stream"); + return result; + } + + ArrayType values; + values.reserve(len); + for (size_t i = 0; i < len; ++i) + { + Result value = RestoreObjectReference(inStream, ioIDToObjectMap); + if (value.HasError()) + { + result.SetError(value.GetError()); + return result; + } + values.push_back(std::move(value.Get())); + } + + result.Set(values); + return result; +} + +} // StreamUtils + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h b/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h new file mode 100644 index 000000000000..66a36b0571ec --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h @@ -0,0 +1,53 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +/// Wrapper around std::ostream +class StreamOutWrapper : public StreamOut +{ +public: + /// Constructor + StreamOutWrapper(ostream &ioWrapped) : mWrapped(ioWrapped) { } + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) override { mWrapped.write((const char *)inData, inNumBytes); } + + /// Returns true if there was an IO failure + virtual bool IsFailed() const override { return mWrapped.fail(); } + +private: + ostream & mWrapped; +}; + +/// Wrapper around std::istream +class StreamInWrapper : public StreamIn +{ +public: + /// Constructor + StreamInWrapper(istream &ioWrapped) : mWrapped(ioWrapped) { } + + /// Write a string of bytes to the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) override { mWrapped.read((char *)outData, inNumBytes); } + + /// Returns true when an attempt has been made to read past the end of the file + virtual bool IsEOF() const override { return mWrapped.eof(); } + + /// Returns true if there was an IO failure + virtual bool IsFailed() const override { return mWrapped.fail(); } + +private: + istream & mWrapped; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h b/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h new file mode 100644 index 000000000000..1cf97fa67831 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h @@ -0,0 +1,63 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// A strided pointer behaves exactly like a normal pointer except that the +/// elements that the pointer points to can be part of a larger structure. +/// The stride gives the number of bytes from one element to the next. +template +class JPH_EXPORT StridedPtr +{ +public: + using value_type = T; + + /// Constructors + StridedPtr() = default; + StridedPtr(const StridedPtr &inRHS) = default; + StridedPtr(T *inPtr, int inStride = sizeof(T)) : mPtr(const_cast(reinterpret_cast(inPtr))), mStride(inStride) { } + + /// Assignment + inline StridedPtr & operator = (const StridedPtr &inRHS) = default; + + /// Incrementing / decrementing + inline StridedPtr & operator ++ () { mPtr += mStride; return *this; } + inline StridedPtr & operator -- () { mPtr -= mStride; return *this; } + inline StridedPtr operator ++ (int) { StridedPtr old_ptr(*this); mPtr += mStride; return old_ptr; } + inline StridedPtr operator -- (int) { StridedPtr old_ptr(*this); mPtr -= mStride; return old_ptr; } + inline StridedPtr operator + (int inOffset) const { StridedPtr new_ptr(*this); new_ptr.mPtr += inOffset * mStride; return new_ptr; } + inline StridedPtr operator - (int inOffset) const { StridedPtr new_ptr(*this); new_ptr.mPtr -= inOffset * mStride; return new_ptr; } + inline void operator += (int inOffset) { mPtr += inOffset * mStride; } + inline void operator -= (int inOffset) { mPtr -= inOffset * mStride; } + + /// Distance between two pointers in elements + inline int operator - (const StridedPtr &inRHS) const { JPH_ASSERT(inRHS.mStride == mStride); return (mPtr - inRHS.mPtr) / mStride; } + + /// Comparison operators + inline bool operator == (const StridedPtr &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const StridedPtr &inRHS) const { return mPtr != inRHS.mPtr; } + inline bool operator <= (const StridedPtr &inRHS) const { return mPtr <= inRHS.mPtr; } + inline bool operator >= (const StridedPtr &inRHS) const { return mPtr >= inRHS.mPtr; } + inline bool operator < (const StridedPtr &inRHS) const { return mPtr < inRHS.mPtr; } + inline bool operator > (const StridedPtr &inRHS) const { return mPtr > inRHS.mPtr; } + + /// Access value + inline T & operator * () const { return *reinterpret_cast(mPtr); } + inline T * operator -> () const { return reinterpret_cast(mPtr); } + inline T & operator [] (int inOffset) const { uint8 *ptr = mPtr + inOffset * mStride; return *reinterpret_cast(ptr); } + + /// Explicit conversion + inline T * GetPtr() const { return reinterpret_cast(mPtr); } + + /// Get stride in bytes + inline int GetStride() const { return mStride; } + +private: + uint8 * mPtr = nullptr; /// Pointer to element + int mStride = 0; /// Stride (number of bytes) between elements +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp b/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp new file mode 100644 index 000000000000..6eae982cc6bf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +String StringFormat(const char *inFMT, ...) +{ + char buffer[1024]; + + // Format the string + va_list list; + va_start(list, inFMT); + vsnprintf(buffer, sizeof(buffer), inFMT, list); + va_end(list); + + return String(buffer); +} + +void StringReplace(String &ioString, const string_view &inSearch, const string_view &inReplace) +{ + size_t index = 0; + for (;;) + { + index = ioString.find(inSearch, index); + if (index == String::npos) + break; + + ioString.replace(index, inSearch.size(), inReplace); + + index += inReplace.size(); + } +} + +void StringToVector(const string_view &inString, Array &outVector, const string_view &inDelimiter, bool inClearVector) +{ + JPH_ASSERT(inDelimiter.size() > 0); + + // Ensure vector empty + if (inClearVector) + outVector.clear(); + + // No string? no elements + if (inString.empty()) + return; + + // Start with initial string + String s(inString); + + // Add to vector while we have a delimiter + size_t i; + while (!s.empty() && (i = s.find(inDelimiter)) != String::npos) + { + outVector.push_back(s.substr(0, i)); + s.erase(0, i + inDelimiter.length()); + } + + // Add final element + outVector.push_back(s); +} + +void VectorToString(const Array &inVector, String &outString, const string_view &inDelimiter) +{ + // Ensure string empty + outString.clear(); + + for (const String &s : inVector) + { + // Add delimiter if not first element + if (!outString.empty()) + outString.append(inDelimiter); + + // Add element + outString.append(s); + } +} + +String ToLower(const string_view &inString) +{ + String out; + out.reserve(inString.length()); + for (char c : inString) + out.push_back((char)tolower(c)); + return out; +} + +const char *NibbleToBinary(uint32 inNibble) +{ + static const char *nibbles[] = { "0000", "0001", "0010", "0011", "0100", "0101", "0110", "0111", "1000", "1001", "1010", "1011", "1100", "1101", "1110", "1111" }; + return nibbles[inNibble & 0xf]; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/StringTools.h b/thirdparty/jolt_physics/Jolt/Core/StringTools.h new file mode 100644 index 000000000000..378278b15d99 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/StringTools.h @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Create a formatted text string for debugging purposes. +/// Note that this function has an internal buffer of 1024 characters, so long strings will be trimmed. +JPH_EXPORT String StringFormat(const char *inFMT, ...); + +/// Convert type to string +template +String ConvertToString(const T &inValue) +{ + using OStringStream = std::basic_ostringstream, STLAllocator>; + OStringStream oss; + oss << inValue; + return oss.str(); +} + +/// Replace substring with other string +JPH_EXPORT void StringReplace(String &ioString, const string_view &inSearch, const string_view &inReplace); + +/// Convert a delimited string to an array of strings +JPH_EXPORT void StringToVector(const string_view &inString, Array &outVector, const string_view &inDelimiter = ",", bool inClearVector = true); + +/// Convert an array strings to a delimited string +JPH_EXPORT void VectorToString(const Array &inVector, String &outString, const string_view &inDelimiter = ","); + +/// Convert a string to lower case +JPH_EXPORT String ToLower(const string_view &inString); + +/// Converts the lower 4 bits of inNibble to a string that represents the number in binary format +JPH_EXPORT const char *NibbleToBinary(uint32 inNibble); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h b/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h new file mode 100644 index 000000000000..99586bde646f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h @@ -0,0 +1,188 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Allocator for temporary allocations. +/// This allocator works as a stack: The blocks must always be freed in the reverse order as they are allocated. +/// Note that allocations and frees can take place from different threads, but the order is guaranteed though +/// job dependencies, so it is not needed to use any form of locking. +class JPH_EXPORT TempAllocator : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + virtual ~TempAllocator() = default; + + /// Allocates inSize bytes of memory, returned memory address must be JPH_RVECTOR_ALIGNMENT byte aligned + virtual void * Allocate(uint inSize) = 0; + + /// Frees inSize bytes of memory located at inAddress + virtual void Free(void *inAddress, uint inSize) = 0; +}; + +/// Default implementation of the temp allocator that allocates a large block through malloc upfront +class JPH_EXPORT TempAllocatorImpl final : public TempAllocator +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs the allocator with a maximum allocatable size of inSize + explicit TempAllocatorImpl(uint inSize) : + mBase(static_cast(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT))), + mSize(inSize) + { + } + + /// Destructor, frees the block + virtual ~TempAllocatorImpl() override + { + JPH_ASSERT(mTop == 0); + AlignedFree(mBase); + } + + // See: TempAllocator + virtual void * Allocate(uint inSize) override + { + if (inSize == 0) + { + return nullptr; + } + else + { + uint new_top = mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT); + if (new_top > mSize) + { + Trace("TempAllocator: Out of memory"); + std::abort(); + } + void *address = mBase + mTop; + mTop = new_top; + return address; + } + } + + // See: TempAllocator + virtual void Free(void *inAddress, uint inSize) override + { + if (inAddress == nullptr) + { + JPH_ASSERT(inSize == 0); + } + else + { + mTop -= AlignUp(inSize, JPH_RVECTOR_ALIGNMENT); + if (mBase + mTop != inAddress) + { + Trace("TempAllocator: Freeing in the wrong order"); + std::abort(); + } + } + } + + /// Check if no allocations have been made + bool IsEmpty() const + { + return mTop == 0; + } + + /// Get the total size of the fixed buffer + uint GetSize() const + { + return mSize; + } + + /// Get current usage in bytes of the buffer + uint GetUsage() const + { + return mTop; + } + + /// Check if an allocation of inSize can be made in this fixed buffer allocator + bool CanAllocate(uint inSize) const + { + return mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT) <= mSize; + } + + /// Check if memory block at inAddress is owned by this allocator + bool OwnsMemory(const void *inAddress) const + { + return inAddress >= mBase && inAddress < mBase + mSize; + } + +private: + uint8 * mBase; ///< Base address of the memory block + uint mSize; ///< Size of the memory block + uint mTop = 0; ///< End of currently allocated area +}; + +/// Implementation of the TempAllocator that just falls back to malloc/free +/// Note: This can be quite slow when running in the debugger as large memory blocks need to be initialized with 0xcd +class JPH_EXPORT TempAllocatorMalloc final : public TempAllocator +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // See: TempAllocator + virtual void * Allocate(uint inSize) override + { + return inSize > 0? AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT) : nullptr; + } + + // See: TempAllocator + virtual void Free(void *inAddress, [[maybe_unused]] uint inSize) override + { + if (inAddress != nullptr) + AlignedFree(inAddress); + } +}; + +/// Implementation of the TempAllocator that tries to allocate from a large preallocated block, but falls back to malloc when it is exhausted +class JPH_EXPORT TempAllocatorImplWithMallocFallback final : public TempAllocator +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs the allocator with an initial fixed block if inSize + explicit TempAllocatorImplWithMallocFallback(uint inSize) : + mAllocator(inSize) + { + } + + // See: TempAllocator + virtual void * Allocate(uint inSize) override + { + if (mAllocator.CanAllocate(inSize)) + return mAllocator.Allocate(inSize); + else + return mFallbackAllocator.Allocate(inSize); + } + + // See: TempAllocator + virtual void Free(void *inAddress, uint inSize) override + { + if (inAddress == nullptr) + { + JPH_ASSERT(inSize == 0); + } + else + { + if (mAllocator.OwnsMemory(inAddress)) + mAllocator.Free(inAddress, inSize); + else + mFallbackAllocator.Free(inAddress, inSize); + } + } + +private: + TempAllocatorImpl mAllocator; + TempAllocatorMalloc mFallbackAllocator; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp b/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp new file mode 100644 index 000000000000..d08140091e92 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#if defined(JPH_PLATFORM_WINDOWS) + JPH_SUPPRESS_WARNING_PUSH + JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception. + #ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN + #endif +#ifndef JPH_COMPILER_MINGW + #include +#else + #include +#endif + JPH_SUPPRESS_WARNING_POP +#endif + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_PLATFORM_WINDOWS_UWP) || (defined(JPH_PLATFORM_WINDOWS) && defined(JPH_CPU_ARM)) + +uint64 GetProcessorTickCount() +{ + LARGE_INTEGER count; + QueryPerformanceCounter(&count); + return uint64(count.QuadPart); +} + +#endif // JPH_PLATFORM_WINDOWS_UWP || (JPH_PLATFORM_WINDOWS && JPH_CPU_ARM) + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.h b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h new file mode 100644 index 000000000000..2b5410e3d9f9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Include for __rdtsc +#if defined(JPH_PLATFORM_WINDOWS) + #include +#elif defined(JPH_CPU_X86) && defined(JPH_COMPILER_GCC) + #include +#elif defined(JPH_CPU_E2K) + #include +#endif + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_PLATFORM_WINDOWS_UWP) || (defined(JPH_PLATFORM_WINDOWS) && defined(JPH_CPU_ARM)) + +/// Functionality to get the processors cycle counter +uint64 GetProcessorTickCount(); // Not inline to avoid having to include Windows.h + +#else + +/// Functionality to get the processors cycle counter +JPH_INLINE uint64 GetProcessorTickCount() +{ +#if defined(JPH_PLATFORM_BLUE) + return JPH_PLATFORM_BLUE_GET_TICKS(); +#elif defined(JPH_CPU_X86) + return __rdtsc(); +#elif defined(JPH_CPU_E2K) + return __rdtsc(); +#elif defined(JPH_CPU_ARM) && defined(JPH_USE_NEON) + uint64 val; + asm volatile("mrs %0, cntvct_el0" : "=r" (val)); + return val; +#elif defined(JPH_CPU_ARM) + return 0; // Not supported +#elif defined(JPH_CPU_WASM) + return 0; // Not supported +#else + #error Undefined +#endif +} + +#endif // JPH_PLATFORM_WINDOWS_UWP || (JPH_PLATFORM_WINDOWS && JPH_CPU_ARM) + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h b/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h new file mode 100644 index 000000000000..f876e2849e20 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Internal helper class to provide context for UnorderedMap +template +class UnorderedMapDetail +{ +public: + /// Get key from key value pair + static const Key & sGetKey(const std::pair &inKeyValue) + { + return inKeyValue.first; + } +}; + +/// Hash Map class +/// @tparam Key Key type +/// @tparam Value Value type +/// @tparam Hash Hash function (note should be 64-bits) +/// @tparam KeyEqual Equality comparison function +template , class KeyEqual = std::equal_to> +class UnorderedMap : public HashTable, UnorderedMapDetail, Hash, KeyEqual> +{ + using Base = HashTable, UnorderedMapDetail, Hash, KeyEqual>; + +public: + using size_type = typename Base::size_type; + using iterator = typename Base::iterator; + using const_iterator = typename Base::const_iterator; + using value_type = typename Base::value_type; + + Value & operator [] (const Key &inKey) + { + size_type index; + bool inserted = this->InsertKey(inKey, index); + value_type &key_value = this->GetElement(index); + if (inserted) + ::new (&key_value) value_type(inKey, Value()); + return key_value.second; + } + + template + std::pair try_emplace(const Key &inKey, Args &&...inArgs) + { + size_type index; + bool inserted = this->InsertKey(inKey, index); + if (inserted) + ::new (&this->GetElement(index)) value_type(std::piecewise_construct, std::forward_as_tuple(inKey), std::forward_as_tuple(std::forward(inArgs)...)); + return std::make_pair(iterator(this, index), inserted); + } + + template + std::pair try_emplace(Key &&inKey, Args &&...inArgs) + { + size_type index; + bool inserted = this->InsertKey(inKey, index); + if (inserted) + ::new (&this->GetElement(index)) value_type(std::piecewise_construct, std::forward_as_tuple(std::move(inKey)), std::forward_as_tuple(std::forward(inArgs)...)); + return std::make_pair(iterator(this, index), inserted); + } + + /// Const version of find + using Base::find; + + /// Non-const version of find + iterator find(const Key &inKey) + { + const_iterator it = Base::find(inKey); + return iterator(this, it.mIndex); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h b/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h new file mode 100644 index 000000000000..a3202554435e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Internal helper class to provide context for UnorderedSet +template +class UnorderedSetDetail +{ +public: + /// The key is the key, just return it + static const Key & sGetKey(const Key &inKey) + { + return inKey; + } +}; + +/// Hash Set class +/// @tparam Key Key type +/// @tparam Hash Hash function (note should be 64-bits) +/// @tparam KeyEqual Equality comparison function +template , class KeyEqual = std::equal_to> +class UnorderedSet : public HashTable, Hash, KeyEqual> +{ +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/AABox.h b/thirdparty/jolt_physics/Jolt/Geometry/AABox.h new file mode 100644 index 000000000000..65e353aff919 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/AABox.h @@ -0,0 +1,312 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Axis aligned box +class [[nodiscard]] AABox +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + AABox() : mMin(Vec3::sReplicate(FLT_MAX)), mMax(Vec3::sReplicate(-FLT_MAX)) { } + AABox(Vec3Arg inMin, Vec3Arg inMax) : mMin(inMin), mMax(inMax) { } + AABox(DVec3Arg inMin, DVec3Arg inMax) : mMin(inMin.ToVec3RoundDown()), mMax(inMax.ToVec3RoundUp()) { } + AABox(Vec3Arg inCenter, float inRadius) : mMin(inCenter - Vec3::sReplicate(inRadius)), mMax(inCenter + Vec3::sReplicate(inRadius)) { } + + /// Create box from 2 points + static AABox sFromTwoPoints(Vec3Arg inP1, Vec3Arg inP2) { return AABox(Vec3::sMin(inP1, inP2), Vec3::sMax(inP1, inP2)); } + + /// Create box from indexed triangle + static AABox sFromTriangle(const VertexList &inVertices, const IndexedTriangle &inTriangle) + { + AABox box = sFromTwoPoints(Vec3(inVertices[inTriangle.mIdx[0]]), Vec3(inVertices[inTriangle.mIdx[1]])); + box.Encapsulate(Vec3(inVertices[inTriangle.mIdx[2]])); + return box; + } + + /// Get bounding box of size 2 * FLT_MAX + static AABox sBiggest() + { + return AABox(Vec3::sReplicate(-FLT_MAX), Vec3::sReplicate(FLT_MAX)); + } + + /// Comparison operators + bool operator == (const AABox &inRHS) const { return mMin == inRHS.mMin && mMax == inRHS.mMax; } + bool operator != (const AABox &inRHS) const { return mMin != inRHS.mMin || mMax != inRHS.mMax; } + + /// Reset the bounding box to an empty bounding box + void SetEmpty() + { + mMin = Vec3::sReplicate(FLT_MAX); + mMax = Vec3::sReplicate(-FLT_MAX); + } + + /// Check if the bounding box is valid (max >= min) + bool IsValid() const + { + return mMin.GetX() <= mMax.GetX() && mMin.GetY() <= mMax.GetY() && mMin.GetZ() <= mMax.GetZ(); + } + + /// Encapsulate point in bounding box + void Encapsulate(Vec3Arg inPos) + { + mMin = Vec3::sMin(mMin, inPos); + mMax = Vec3::sMax(mMax, inPos); + } + + /// Encapsulate bounding box in bounding box + void Encapsulate(const AABox &inRHS) + { + mMin = Vec3::sMin(mMin, inRHS.mMin); + mMax = Vec3::sMax(mMax, inRHS.mMax); + } + + /// Encapsulate triangle in bounding box + void Encapsulate(const Triangle &inRHS) + { + Vec3 v = Vec3::sLoadFloat3Unsafe(inRHS.mV[0]); + Encapsulate(v); + v = Vec3::sLoadFloat3Unsafe(inRHS.mV[1]); + Encapsulate(v); + v = Vec3::sLoadFloat3Unsafe(inRHS.mV[2]); + Encapsulate(v); + } + + /// Encapsulate triangle in bounding box + void Encapsulate(const VertexList &inVertices, const IndexedTriangle &inTriangle) + { + for (uint32 idx : inTriangle.mIdx) + Encapsulate(Vec3(inVertices[idx])); + } + + /// Intersect this bounding box with inOther, returns the intersection + AABox Intersect(const AABox &inOther) const + { + return AABox(Vec3::sMax(mMin, inOther.mMin), Vec3::sMin(mMax, inOther.mMax)); + } + + /// Make sure that each edge of the bounding box has a minimal length + void EnsureMinimalEdgeLength(float inMinEdgeLength) + { + Vec3 min_length = Vec3::sReplicate(inMinEdgeLength); + mMax = Vec3::sSelect(mMax, mMin + min_length, Vec3::sLess(mMax - mMin, min_length)); + } + + /// Widen the box on both sides by inVector + void ExpandBy(Vec3Arg inVector) + { + mMin -= inVector; + mMax += inVector; + } + + /// Get center of bounding box + Vec3 GetCenter() const + { + return 0.5f * (mMin + mMax); + } + + /// Get extent of bounding box (half of the size) + Vec3 GetExtent() const + { + return 0.5f * (mMax - mMin); + } + + /// Get size of bounding box + Vec3 GetSize() const + { + return mMax - mMin; + } + + /// Get surface area of bounding box + float GetSurfaceArea() const + { + Vec3 extent = mMax - mMin; + return 2.0f * (extent.GetX() * extent.GetY() + extent.GetX() * extent.GetZ() + extent.GetY() * extent.GetZ()); + } + + /// Get volume of bounding box + float GetVolume() const + { + Vec3 extent = mMax - mMin; + return extent.GetX() * extent.GetY() * extent.GetZ(); + } + + /// Check if this box contains another box + bool Contains(const AABox &inOther) const + { + return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther.mMin), Vec3::sGreaterOrEqual(mMax, inOther.mMax)).TestAllXYZTrue(); + } + + /// Check if this box contains a point + bool Contains(Vec3Arg inOther) const + { + return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther), Vec3::sGreaterOrEqual(mMax, inOther)).TestAllXYZTrue(); + } + + /// Check if this box contains a point + bool Contains(DVec3Arg inOther) const + { + return Contains(Vec3(inOther)); + } + + /// Check if this box overlaps with another box + bool Overlaps(const AABox &inOther) const + { + return !UVec4::sOr(Vec3::sGreater(mMin, inOther.mMax), Vec3::sLess(mMax, inOther.mMin)).TestAnyXYZTrue(); + } + + /// Check if this box overlaps with a plane + bool Overlaps(const Plane &inPlane) const + { + Vec3 normal = inPlane.GetNormal(); + float dist_normal = inPlane.SignedDistance(GetSupport(normal)); + float dist_min_normal = inPlane.SignedDistance(GetSupport(-normal)); + return dist_normal * dist_min_normal <= 0.0f; // If both support points are on the same side of the plane we don't overlap + } + + /// Translate bounding box + void Translate(Vec3Arg inTranslation) + { + mMin += inTranslation; + mMax += inTranslation; + } + + /// Translate bounding box + void Translate(DVec3Arg inTranslation) + { + mMin = (DVec3(mMin) + inTranslation).ToVec3RoundDown(); + mMax = (DVec3(mMax) + inTranslation).ToVec3RoundUp(); + } + + /// Transform bounding box + AABox Transformed(Mat44Arg inMatrix) const + { + // Start with the translation of the matrix + Vec3 new_min, new_max; + new_min = new_max = inMatrix.GetTranslation(); + + // Now find the extreme points by considering the product of the min and max with each column of inMatrix + for (int c = 0; c < 3; ++c) + { + Vec3 col = inMatrix.GetColumn3(c); + + Vec3 a = col * mMin[c]; + Vec3 b = col * mMax[c]; + + new_min += Vec3::sMin(a, b); + new_max += Vec3::sMax(a, b); + } + + // Return the new bounding box + return AABox(new_min, new_max); + } + + /// Transform bounding box + AABox Transformed(DMat44Arg inMatrix) const + { + AABox transformed = Transformed(inMatrix.GetRotation()); + transformed.Translate(inMatrix.GetTranslation()); + return transformed; + } + + /// Scale this bounding box, can handle non-uniform and negative scaling + AABox Scaled(Vec3Arg inScale) const + { + return AABox::sFromTwoPoints(mMin * inScale, mMax * inScale); + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return Vec3::sSelect(mMax, mMin, Vec3::sLess(inDirection, Vec3::sZero())); + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + outVertices.resize(4); + + int axis = inDirection.Abs().GetHighestComponentIndex(); + if (inDirection[axis] < 0.0f) + { + switch (axis) + { + case 0: + outVertices[0] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + break; + + case 1: + outVertices[0] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + break; + + case 2: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + break; + } + } + else + { + switch (axis) + { + case 0: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + break; + + case 1: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + break; + + case 2: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + break; + } + } + } + + /// Get the closest point on or in this box to inPoint + Vec3 GetClosestPoint(Vec3Arg inPoint) const + { + return Vec3::sMin(Vec3::sMax(inPoint, mMin), mMax); + } + + /// Get the squared distance between inPoint and this box (will be 0 if in Point is inside the box) + inline float GetSqDistanceTo(Vec3Arg inPoint) const + { + return (GetClosestPoint(inPoint) - inPoint).LengthSq(); + } + + /// Bounding box min and max + Vec3 mMin; + Vec3 mMax; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h b/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h new file mode 100644 index 000000000000..4465d4dabe5a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h @@ -0,0 +1,224 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions that process 4 axis aligned boxes at the same time using SIMD +/// Test if 4 bounding boxes overlap with 1 bounding box, splat 1 box +JPH_INLINE UVec4 AABox4VsBox(const AABox &inBox1, Vec4Arg inBox2MinX, Vec4Arg inBox2MinY, Vec4Arg inBox2MinZ, Vec4Arg inBox2MaxX, Vec4Arg inBox2MaxY, Vec4Arg inBox2MaxZ) +{ + // Splat values of box 1 + Vec4 box1_minx = inBox1.mMin.SplatX(); + Vec4 box1_miny = inBox1.mMin.SplatY(); + Vec4 box1_minz = inBox1.mMin.SplatZ(); + Vec4 box1_maxx = inBox1.mMax.SplatX(); + Vec4 box1_maxy = inBox1.mMax.SplatY(); + Vec4 box1_maxz = inBox1.mMax.SplatZ(); + + // Test separation over each axis + UVec4 nooverlapx = UVec4::sOr(Vec4::sGreater(box1_minx, inBox2MaxX), Vec4::sGreater(inBox2MinX, box1_maxx)); + UVec4 nooverlapy = UVec4::sOr(Vec4::sGreater(box1_miny, inBox2MaxY), Vec4::sGreater(inBox2MinY, box1_maxy)); + UVec4 nooverlapz = UVec4::sOr(Vec4::sGreater(box1_minz, inBox2MaxZ), Vec4::sGreater(inBox2MinZ, box1_maxz)); + + // Return overlap + return UVec4::sNot(UVec4::sOr(UVec4::sOr(nooverlapx, nooverlapy), nooverlapz)); +} + +/// Scale 4 axis aligned boxes +JPH_INLINE void AABox4Scale(Vec3Arg inScale, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, Vec4 &outBoundsMinX, Vec4 &outBoundsMinY, Vec4 &outBoundsMinZ, Vec4 &outBoundsMaxX, Vec4 &outBoundsMaxY, Vec4 &outBoundsMaxZ) +{ + Vec4 scale_x = inScale.SplatX(); + Vec4 scaled_min_x = scale_x * inBoxMinX; + Vec4 scaled_max_x = scale_x * inBoxMaxX; + outBoundsMinX = Vec4::sMin(scaled_min_x, scaled_max_x); // Negative scale can flip min and max + outBoundsMaxX = Vec4::sMax(scaled_min_x, scaled_max_x); + + Vec4 scale_y = inScale.SplatY(); + Vec4 scaled_min_y = scale_y * inBoxMinY; + Vec4 scaled_max_y = scale_y * inBoxMaxY; + outBoundsMinY = Vec4::sMin(scaled_min_y, scaled_max_y); + outBoundsMaxY = Vec4::sMax(scaled_min_y, scaled_max_y); + + Vec4 scale_z = inScale.SplatZ(); + Vec4 scaled_min_z = scale_z * inBoxMinZ; + Vec4 scaled_max_z = scale_z * inBoxMaxZ; + outBoundsMinZ = Vec4::sMin(scaled_min_z, scaled_max_z); + outBoundsMaxZ = Vec4::sMax(scaled_min_z, scaled_max_z); +} + +/// Enlarge 4 bounding boxes with extent (add to both sides) +JPH_INLINE void AABox4EnlargeWithExtent(Vec3Arg inExtent, Vec4 &ioBoundsMinX, Vec4 &ioBoundsMinY, Vec4 &ioBoundsMinZ, Vec4 &ioBoundsMaxX, Vec4 &ioBoundsMaxY, Vec4 &ioBoundsMaxZ) +{ + Vec4 extent_x = inExtent.SplatX(); + ioBoundsMinX -= extent_x; + ioBoundsMaxX += extent_x; + + Vec4 extent_y = inExtent.SplatY(); + ioBoundsMinY -= extent_y; + ioBoundsMaxY += extent_y; + + Vec4 extent_z = inExtent.SplatZ(); + ioBoundsMinZ -= extent_z; + ioBoundsMaxZ += extent_z; +} + +/// Test if 4 bounding boxes overlap with a point +JPH_INLINE UVec4 AABox4VsPoint(Vec3Arg inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Splat point to 4 component vectors + Vec4 point_x = Vec4(inPoint).SplatX(); + Vec4 point_y = Vec4(inPoint).SplatY(); + Vec4 point_z = Vec4(inPoint).SplatZ(); + + // Test if point overlaps with box + UVec4 overlapx = UVec4::sAnd(Vec4::sGreaterOrEqual(point_x, inBoxMinX), Vec4::sLessOrEqual(point_x, inBoxMaxX)); + UVec4 overlapy = UVec4::sAnd(Vec4::sGreaterOrEqual(point_y, inBoxMinY), Vec4::sLessOrEqual(point_y, inBoxMaxY)); + UVec4 overlapz = UVec4::sAnd(Vec4::sGreaterOrEqual(point_z, inBoxMinZ), Vec4::sLessOrEqual(point_z, inBoxMaxZ)); + + // Test if all are overlapping + return UVec4::sAnd(UVec4::sAnd(overlapx, overlapy), overlapz); +} + +/// Test if 4 bounding boxes overlap with an oriented box +JPH_INLINE UVec4 AABox4VsBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f) +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box) + + // Compute translation vector t (the translation of B in the space of A) + Vec4 t[3] { + inOrientation.GetTranslation().SplatX() - 0.5f * (inBoxMinX + inBoxMaxX), + inOrientation.GetTranslation().SplatY() - 0.5f * (inBoxMinY + inBoxMaxY), + inOrientation.GetTranslation().SplatZ() - 0.5f * (inBoxMinZ + inBoxMaxZ) }; + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { inOrientation.GetAxisX().Abs() + epsilon, inOrientation.GetAxisY().Abs() + epsilon, inOrientation.GetAxisZ().Abs() + epsilon }; + + // Half extents for a + Vec4 a_half_extents[3] { + 0.5f * (inBoxMaxX - inBoxMinX), + 0.5f * (inBoxMaxY - inBoxMinY), + 0.5f * (inBoxMaxZ - inBoxMinZ) }; + + // Half extents of b + Vec4 b_half_extents_x = inHalfExtents.SplatX(); + Vec4 b_half_extents_y = inHalfExtents.SplatY(); + Vec4 b_half_extents_z = inHalfExtents.SplatZ(); + + // Each component corresponds to 1 overlapping OBB vs ABB + UVec4 overlaps = UVec4(0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff); + + // Test axes L = A0, L = A1, L = A2 + Vec4 ra, rb; + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[i]; + rb = b_half_extents_x * abs_r[0][i] + b_half_extents_y * abs_r[1][i] + b_half_extents_z * abs_r[2][i]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual(t[i].Abs(), ra + rb)); + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[0] * abs_r[i][0] + a_half_extents[1] * abs_r[i][1] + a_half_extents[2] * abs_r[i][2]; + rb = Vec4::sReplicate(inHalfExtents[i]); + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(0, i) + t[1] * inOrientation(1, i) + t[2] * inOrientation(2, i)).Abs(), ra + rb)); + } + + // Test axis L = A0 x B0 + ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1]; + rb = b_half_extents_y * abs_r[2][0] + b_half_extents_z * abs_r[1][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 0) - t[1] * inOrientation(2, 0)).Abs(), ra + rb)); + + // Test axis L = A0 x B1 + ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1]; + rb = b_half_extents_x * abs_r[2][0] + b_half_extents_z * abs_r[0][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 1) - t[1] * inOrientation(2, 1)).Abs(), ra + rb)); + + // Test axis L = A0 x B2 + ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1]; + rb = b_half_extents_x * abs_r[1][0] + b_half_extents_y * abs_r[0][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 2) - t[1] * inOrientation(2, 2)).Abs(), ra + rb)); + + // Test axis L = A1 x B0 + ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0]; + rb = b_half_extents_y * abs_r[2][1] + b_half_extents_z * abs_r[1][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 0) - t[2] * inOrientation(0, 0)).Abs(), ra + rb)); + + // Test axis L = A1 x B1 + ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0]; + rb = b_half_extents_x * abs_r[2][1] + b_half_extents_z * abs_r[0][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 1) - t[2] * inOrientation(0, 1)).Abs(), ra + rb)); + + // Test axis L = A1 x B2 + ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0]; + rb = b_half_extents_x * abs_r[1][1] + b_half_extents_y * abs_r[0][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 2) - t[2] * inOrientation(0, 2)).Abs(), ra + rb)); + + // Test axis L = A2 x B0 + ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0]; + rb = b_half_extents_y * abs_r[2][2] + b_half_extents_z * abs_r[1][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 0) - t[0] * inOrientation(1, 0)).Abs(), ra + rb)); + + // Test axis L = A2 x B1 + ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0]; + rb = b_half_extents_x * abs_r[2][2] + b_half_extents_z * abs_r[0][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 1) - t[0] * inOrientation(1, 1)).Abs(), ra + rb)); + + // Test axis L = A2 x B2 + ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0]; + rb = b_half_extents_x * abs_r[1][2] + b_half_extents_y * abs_r[0][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 2) - t[0] * inOrientation(1, 2)).Abs(), ra + rb)); + + // Return if the OBB vs AABBs are intersecting + return overlaps; +} + +/// Convenience function that tests 4 AABoxes vs OrientedBox +JPH_INLINE UVec4 AABox4VsBox(const OrientedBox &inBox, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f) +{ + return AABox4VsBox(inBox.mOrientation, inBox.mHalfExtents, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ, inEpsilon); +} + +/// Get the squared distance between 4 AABoxes and a point +JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec4Arg inPointX, Vec4Arg inPointY, Vec4Arg inPointZ, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Get closest point on box + Vec4 closest_x = Vec4::sMin(Vec4::sMax(inPointX, inBoxMinX), inBoxMaxX); + Vec4 closest_y = Vec4::sMin(Vec4::sMax(inPointY, inBoxMinY), inBoxMaxY); + Vec4 closest_z = Vec4::sMin(Vec4::sMax(inPointZ, inBoxMinZ), inBoxMaxZ); + + // Return the squared distance between the box and point + return Square(closest_x - inPointX) + Square(closest_y - inPointY) + Square(closest_z - inPointZ); +} + +/// Get the squared distance between 4 AABoxes and a point +JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec3 inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + return AABox4DistanceSqToPoint(inPoint.SplatX(), inPoint.SplatY(), inPoint.SplatZ(), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); +} + +/// Test 4 AABoxes vs a sphere +JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Test the distance from the center of the sphere to the box is smaller than the radius + Vec4 distance_sq = AABox4DistanceSqToPoint(inCenterX, inCenterY, inCenterZ, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); + return Vec4::sLessOrEqual(distance_sq, inRadiusSq); +} + +/// Test 4 AABoxes vs a sphere +JPH_INLINE UVec4 AABox4VsSphere(Vec3Arg inCenter, float inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + return AABox4VsSphere(inCenter.SplatX(), inCenter.SplatY(), inCenter.SplatZ(), Vec4::sReplicate(inRadiusSq), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h b/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h new file mode 100644 index 000000000000..7301d8e6f0a5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h @@ -0,0 +1,200 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Clip inPolygonToClip against the positive halfspace of plane defined by inPlaneOrigin and inPlaneNormal. +/// inPlaneNormal does not need to be normalized. +template +void ClipPolyVsPlane(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inPlaneOrigin, Vec3Arg inPlaneNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + JPH_ASSERT(outClippedPolygon.empty()); + + // Determine state of last point + Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1]; + float prev_num = (inPlaneOrigin - e1).Dot(inPlaneNormal); + bool prev_inside = prev_num < 0.0f; + + // Loop through all vertices + for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j) + { + // Check if second point is inside + Vec3Arg e2 = inPolygonToClip[j]; + float num = (inPlaneOrigin - e2).Dot(inPlaneNormal); + bool cur_inside = num < 0.0f; + + // In -> Out or Out -> In: Add point on clipping plane + if (cur_inside != prev_inside) + { + // Solve: (X - inPlaneOrigin) . inPlaneNormal = 0 and X = e1 + t * (e2 - e1) for X + Vec3 e12 = e2 - e1; + float denom = e12.Dot(inPlaneNormal); + if (denom != 0.0f) + outClippedPolygon.push_back(e1 + (prev_num / denom) * e12); + else + cur_inside = prev_inside; // Edge is parallel to plane, treat point as if it were on the same side as the last point + } + + // Point inside, add it + if (cur_inside) + outClippedPolygon.push_back(e2); + + // Update previous state + prev_num = num; + prev_inside = cur_inside; + e1 = e2; + } +} + +/// Clip polygon versus polygon. +/// Both polygons are assumed to be in counter clockwise order. +/// @param inClippingPolygonNormal is used to create planes of all edges in inClippingPolygon against which inPolygonToClip is clipped, inClippingPolygonNormal does not need to be normalized +/// @param inClippingPolygon is the polygon which inClippedPolygon is clipped against +/// @param inPolygonToClip is the polygon that is clipped +/// @param outClippedPolygon will contain clipped polygon when function returns +template +void ClipPolyVsPoly(const VERTEX_ARRAY &inPolygonToClip, const VERTEX_ARRAY &inClippingPolygon, Vec3Arg inClippingPolygonNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + JPH_ASSERT(inClippingPolygon.size() >= 3); + + VERTEX_ARRAY tmp_vertices[2]; + int tmp_vertices_idx = 0; + + for (typename VERTEX_ARRAY::size_type i = 0; i < inClippingPolygon.size(); ++i) + { + // Get edge to clip against + Vec3 clip_e1 = inClippingPolygon[i]; + Vec3 clip_e2 = inClippingPolygon[(i + 1) % inClippingPolygon.size()]; + Vec3 clip_normal = inClippingPolygonNormal.Cross(clip_e2 - clip_e1); // Pointing inward to the clipping polygon + + // Get source and target polygon + const VERTEX_ARRAY &src_polygon = (i == 0)? inPolygonToClip : tmp_vertices[tmp_vertices_idx]; + tmp_vertices_idx ^= 1; + VERTEX_ARRAY &tgt_polygon = (i == inClippingPolygon.size() - 1)? outClippedPolygon : tmp_vertices[tmp_vertices_idx]; + tgt_polygon.clear(); + + // Clip against the edge + ClipPolyVsPlane(src_polygon, clip_e1, clip_normal, tgt_polygon); + + // Break out if no polygon left + if (tgt_polygon.size() < 3) + { + outClippedPolygon.clear(); + break; + } + } +} + +/// Clip inPolygonToClip against an edge, the edge is projected on inPolygonToClip using inClippingEdgeNormal. +/// The positive half space (the side on the edge in the direction of inClippingEdgeNormal) is cut away. +template +void ClipPolyVsEdge(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inEdgeVertex1, Vec3Arg inEdgeVertex2, Vec3Arg inClippingEdgeNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 3); + JPH_ASSERT(outClippedPolygon.empty()); + + // Get normal that is perpendicular to the edge and the clipping edge normal + Vec3 edge = inEdgeVertex2 - inEdgeVertex1; + Vec3 edge_normal = inClippingEdgeNormal.Cross(edge); + + // Project vertices of edge on inPolygonToClip + Vec3 polygon_normal = (inPolygonToClip[2] - inPolygonToClip[0]).Cross(inPolygonToClip[1] - inPolygonToClip[0]); + float polygon_normal_len_sq = polygon_normal.LengthSq(); + Vec3 v1 = inEdgeVertex1 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex1) * polygon_normal / polygon_normal_len_sq; + Vec3 v2 = inEdgeVertex2 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex2) * polygon_normal / polygon_normal_len_sq; + Vec3 v12 = v2 - v1; + float v12_len_sq = v12.LengthSq(); + + // Determine state of last point + Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1]; + float prev_num = (inEdgeVertex1 - e1).Dot(edge_normal); + bool prev_inside = prev_num < 0.0f; + + // Loop through all vertices + for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j) + { + // Check if second point is inside + Vec3 e2 = inPolygonToClip[j]; + float num = (inEdgeVertex1 - e2).Dot(edge_normal); + bool cur_inside = num < 0.0f; + + // In -> Out or Out -> In: Add point on clipping plane + if (cur_inside != prev_inside) + { + // Solve: (inEdgeVertex1 - X) . edge_normal = 0 and X = e1 + t * (e2 - e1) for X + Vec3 e12 = e2 - e1; + float denom = e12.Dot(edge_normal); + Vec3 clipped_point = denom != 0.0f? e1 + (prev_num / denom) * e12 : e1; + + // Project point on line segment v1, v2 so see if it falls outside if the edge + float projection = (clipped_point - v1).Dot(v12); + if (projection < 0.0f) + outClippedPolygon.push_back(v1); + else if (projection > v12_len_sq) + outClippedPolygon.push_back(v2); + else + outClippedPolygon.push_back(clipped_point); + } + + // Update previous state + prev_num = num; + prev_inside = cur_inside; + e1 = e2; + } +} + +/// Clip polygon vs axis aligned box, inPolygonToClip is assume to be in counter clockwise order. +/// Output will be stored in outClippedPolygon. Everything inside inAABox will be kept. +template +void ClipPolyVsAABox(const VERTEX_ARRAY &inPolygonToClip, const AABox &inAABox, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + + VERTEX_ARRAY tmp_vertices[2]; + int tmp_vertices_idx = 0; + + for (int coord = 0; coord < 3; ++coord) + for (int side = 0; side < 2; ++side) + { + // Get plane to clip against + Vec3 origin = Vec3::sZero(), normal = Vec3::sZero(); + if (side == 0) + { + normal.SetComponent(coord, 1.0f); + origin.SetComponent(coord, inAABox.mMin[coord]); + } + else + { + normal.SetComponent(coord, -1.0f); + origin.SetComponent(coord, inAABox.mMax[coord]); + } + + // Get source and target polygon + const VERTEX_ARRAY &src_polygon = tmp_vertices_idx == 0? inPolygonToClip : tmp_vertices[tmp_vertices_idx & 1]; + tmp_vertices_idx++; + VERTEX_ARRAY &tgt_polygon = tmp_vertices_idx == 6? outClippedPolygon : tmp_vertices[tmp_vertices_idx & 1]; + tgt_polygon.clear(); + + // Clip against the edge + ClipPolyVsPlane(src_polygon, origin, normal, tgt_polygon); + + // Break out if no polygon left + if (tgt_polygon.size() < 3) + { + outClippedPolygon.clear(); + return; + } + + // Flip normal + normal = -normal; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h b/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h new file mode 100644 index 000000000000..a437763f5744 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h @@ -0,0 +1,498 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +// Turn off fused multiply add instruction because it makes the equations of the form a * b - c * d inaccurate below +JPH_PRECISE_MATH_ON + +/// Helper utils to find the closest point to a line segment, triangle or tetrahedron +namespace ClosestPoint +{ + /// Compute barycentric coordinates of closest point to origin for infinite line defined by (inA, inB) + /// Point can then be computed as inA * outU + inB * outV + /// Returns false if the points inA, inB do not form a line (are at the same point) + inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, float &outU, float &outV) + { + Vec3 ab = inB - inA; + float denominator = ab.LengthSq(); + if (denominator < Square(FLT_EPSILON)) + { + // Degenerate line segment, fallback to points + if (inA.LengthSq() < inB.LengthSq()) + { + // A closest + outU = 1.0f; + outV = 0.0f; + } + else + { + // B closest + outU = 0.0f; + outV = 1.0f; + } + return false; + } + else + { + outV = -inA.Dot(ab) / denominator; + outU = 1.0f - outV; + } + return true; + } + + /// Compute barycentric coordinates of closest point to origin for plane defined by (inA, inB, inC) + /// Point can then be computed as inA * outU + inB * outV + inC * outW + /// Returns false if the points inA, inB, inC do not form a plane (are on the same line or at the same point) + inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, float &outU, float &outV, float &outW) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Barycentric Coordinates) + // With p = 0 + // Adjusted to always include the shortest edge of the triangle in the calculation to improve numerical accuracy + + // First calculate the three edges + Vec3 v0 = inB - inA; + Vec3 v1 = inC - inA; + Vec3 v2 = inC - inB; + + // Make sure that the shortest edge is included in the calculation to keep the products a * b - c * d as small as possible to preserve accuracy + float d00 = v0.LengthSq(); + float d11 = v1.LengthSq(); + float d22 = v2.LengthSq(); + if (d00 <= d22) + { + // Use v0 and v1 to calculate barycentric coordinates + float d01 = v0.Dot(v1); + + // Denominator must be positive: + // |v0|^2 * |v1|^2 - (v0 . v1)^2 = |v0|^2 * |v1|^2 * (1 - cos(angle)^2) >= 0 + float denominator = d00 * d11 - d01 * d01; + if (denominator < 1.0e-12f) + { + // Degenerate triangle, return coordinates along longest edge + if (d00 > d11) + { + GetBaryCentricCoordinates(inA, inB, outU, outV); + outW = 0.0f; + } + else + { + GetBaryCentricCoordinates(inA, inC, outU, outW); + outV = 0.0f; + } + return false; + } + else + { + float a0 = inA.Dot(v0); + float a1 = inA.Dot(v1); + outV = (d01 * a1 - d11 * a0) / denominator; + outW = (d01 * a0 - d00 * a1) / denominator; + outU = 1.0f - outV - outW; + } + } + else + { + // Use v1 and v2 to calculate barycentric coordinates + float d12 = v1.Dot(v2); + + float denominator = d11 * d22 - d12 * d12; + if (denominator < 1.0e-12f) + { + // Degenerate triangle, return coordinates along longest edge + if (d11 > d22) + { + GetBaryCentricCoordinates(inA, inC, outU, outW); + outV = 0.0f; + } + else + { + GetBaryCentricCoordinates(inB, inC, outV, outW); + outU = 0.0f; + } + return false; + } + else + { + float c1 = inC.Dot(v1); + float c2 = inC.Dot(v2); + outU = (d22 * c1 - d12 * c2) / denominator; + outV = (d11 * c2 - d12 * c1) / denominator; + outW = 1.0f - outU - outV; + } + } + return true; + } + + /// Get the closest point to the origin of line (inA, inB) + /// outSet describes which features are closest: 1 = a, 2 = b, 3 = line segment ab + inline Vec3 GetClosestPointOnLine(Vec3Arg inA, Vec3Arg inB, uint32 &outSet) + { + float u, v; + GetBaryCentricCoordinates(inA, inB, u, v); + if (v <= 0.0f) + { + // inA is closest point + outSet = 0b0001; + return inA; + } + else if (u <= 0.0f) + { + // inB is closest point + outSet = 0b0010; + return inB; + } + else + { + // Closest point lies on line inA inB + outSet = 0b0011; + return u * inA + v * inB; + } + } + + /// Get the closest point to the origin of triangle (inA, inB, inC) + /// outSet describes which features are closest: 1 = a, 2 = b, 4 = c, 5 = line segment ac, 7 = triangle interior etc. + /// If MustIncludeC is true, the function assumes that C is part of the closest feature (vertex, edge, face) and does less work, if the assumption is not true then a closest point to the other features is returned. + template + inline Vec3 GetClosestPointOnTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, uint32 &outSet) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Triangle to Point) + // With p = 0 + + // The most accurate normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter and if bc is shorter than ac then we swap a with c to a is always on the shortest edge + UVec4 swap_ac; + { + Vec3 ac = inC - inA; + Vec3 bc = inC - inB; + swap_ac = Vec4::sLess(bc.DotV4(bc), ac.DotV4(ac)); + } + Vec3 a = Vec3::sSelect(inA, inC, swap_ac); + Vec3 c = Vec3::sSelect(inC, inA, swap_ac); + + // Calculate normal + Vec3 ab = inB - a; + Vec3 ac = c - a; + Vec3 n = ab.Cross(ac); + float n_len_sq = n.LengthSq(); + + // Check degenerate + if (n_len_sq < 1.0e-10f) // Square(FLT_EPSILON) was too small and caused numerical problems, see test case TestCollideParallelTriangleVsCapsule + { + // Degenerate, fallback to vertices and edges + + // Start with vertex C being the closest + uint32 closest_set = 0b0100; + Vec3 closest_point = inC; + float best_dist_sq = inC.LengthSq(); + + // If the closest point must include C then A or B cannot be closest + // Note that we test vertices first because we want to prefer a closest vertex over a closest edge (this results in an outSet with fewer bits set) + if constexpr (!MustIncludeC) + { + // Try vertex A + float a_len_sq = inA.LengthSq(); + if (a_len_sq < best_dist_sq) + { + closest_set = 0b0001; + closest_point = inA; + best_dist_sq = a_len_sq; + } + + // Try vertex B + float b_len_sq = inB.LengthSq(); + if (b_len_sq < best_dist_sq) + { + closest_set = 0b0010; + closest_point = inB; + best_dist_sq = b_len_sq; + } + } + + // Edge AC + float ac_len_sq = ac.LengthSq(); + if (ac_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-a.Dot(ac) / ac_len_sq, 0.0f, 1.0f); + Vec3 q = a + v * ac; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0101; + closest_point = q; + best_dist_sq = dist_sq; + } + } + + // Edge BC + Vec3 bc = inC - inB; + float bc_len_sq = bc.LengthSq(); + if (bc_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-inB.Dot(bc) / bc_len_sq, 0.0f, 1.0f); + Vec3 q = inB + v * bc; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0110; + closest_point = q; + best_dist_sq = dist_sq; + } + } + + // If the closest point must include C then AB cannot be closest + if constexpr (!MustIncludeC) + { + // Edge AB + ab = inB - inA; + float ab_len_sq = ab.LengthSq(); + if (ab_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-inA.Dot(ab) / ab_len_sq, 0.0f, 1.0f); + Vec3 q = inA + v * ab; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0011; + closest_point = q; + best_dist_sq = dist_sq; + } + } + } + + outSet = closest_set; + return closest_point; + } + + // Check if P in vertex region outside A + Vec3 ap = -a; + float d1 = ab.Dot(ap); + float d2 = ac.Dot(ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + outSet = swap_ac.GetX()? 0b0100 : 0b0001; + return a; // barycentric coordinates (1,0,0) + } + + // Check if P in vertex region outside B + Vec3 bp = -inB; + float d3 = ab.Dot(bp); + float d4 = ac.Dot(bp); + if (d3 >= 0.0f && d4 <= d3) + { + outSet = 0b0010; + return inB; // barycentric coordinates (0,1,0) + } + + // Check if P in edge region of AB, if so return projection of P onto AB + if (d1 * d4 <= d3 * d2 && d1 >= 0.0f && d3 <= 0.0f) + { + float v = d1 / (d1 - d3); + outSet = swap_ac.GetX()? 0b0110 : 0b0011; + return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + Vec3 cp = -c; + float d5 = ab.Dot(cp); + float d6 = ac.Dot(cp); + if (d6 >= 0.0f && d5 <= d6) + { + outSet = swap_ac.GetX()? 0b0001 : 0b0100; + return c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + if (d5 * d2 <= d1 * d6 && d2 >= 0.0f && d6 <= 0.0f) + { + float w = d2 / (d2 - d6); + outSet = 0b0101; + return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float d4_d3 = d4 - d3; + float d5_d6 = d5 - d6; + if (d3 * d6 <= d5 * d4 && d4_d3 >= 0.0f && d5_d6 >= 0.0f) + { + float w = d4_d3 / (d4_d3 + d5_d6); + outSet = swap_ac.GetX()? 0b0011 : 0b0110; + return inB + w * (c - inB); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. + // Here we deviate from Christer Ericson's article to improve accuracy. + // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal| + // Closest point to origin is then: distance . normal / |normal| + // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates + // and then calculating the closest point based on those coordinates. + outSet = 0b0111; + return n * (a + inB + c).Dot(n) / (3.0f * n_len_sq); + } + + /// Check if the origin is outside the plane of triangle (inA, inB, inC). inD specifies the front side of the plane. + inline bool OriginOutsideOfPlane(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point) + // With p = 0 + + // Test if point p and d lie on opposite sides of plane through abc + Vec3 n = (inB - inA).Cross(inC - inA); + float signp = inA.Dot(n); // [AP AB AC] + float signd = (inD - inA).Dot(n); // [AD AB AC] + + // Points on opposite sides if expression signs are the same + // Note that we left out the minus sign in signp so we need to check > 0 instead of < 0 as in Christer's book + // We compare against a small negative value to allow for a little bit of slop in the calculations + return signp * signd > -FLT_EPSILON; + } + + /// Returns for each of the planes of the tetrahedron if the origin is inside it + /// Roughly equivalent to: + /// [OriginOutsideOfPlane(inA, inB, inC, inD), + /// OriginOutsideOfPlane(inA, inC, inD, inB), + /// OriginOutsideOfPlane(inA, inD, inB, inC), + /// OriginOutsideOfPlane(inB, inD, inC, inA)] + inline UVec4 OriginOutsideOfTetrahedronPlanes(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD) + { + Vec3 ab = inB - inA; + Vec3 ac = inC - inA; + Vec3 ad = inD - inA; + Vec3 bd = inD - inB; + Vec3 bc = inC - inB; + + Vec3 ab_cross_ac = ab.Cross(ac); + Vec3 ac_cross_ad = ac.Cross(ad); + Vec3 ad_cross_ab = ad.Cross(ab); + Vec3 bd_cross_bc = bd.Cross(bc); + + // For each plane get the side on which the origin is + float signp0 = inA.Dot(ab_cross_ac); // ABC + float signp1 = inA.Dot(ac_cross_ad); // ACD + float signp2 = inA.Dot(ad_cross_ab); // ADB + float signp3 = inB.Dot(bd_cross_bc); // BDC + Vec4 signp(signp0, signp1, signp2, signp3); + + // For each plane get the side that is outside (determined by the 4th point) + float signd0 = ad.Dot(ab_cross_ac); // D + float signd1 = ab.Dot(ac_cross_ad); // B + float signd2 = ac.Dot(ad_cross_ab); // C + float signd3 = -ab.Dot(bd_cross_bc); // A + Vec4 signd(signd0, signd1, signd2, signd3); + + // The winding of all triangles has been chosen so that signd should have the + // same sign for all components. If this is not the case the tetrahedron + // is degenerate and we return that the origin is in front of all sides + int sign_bits = signd.GetSignBits(); + switch (sign_bits) + { + case 0: + // All positive + return Vec4::sGreaterOrEqual(signp, Vec4::sReplicate(-FLT_EPSILON)); + + case 0xf: + // All negative + return Vec4::sLessOrEqual(signp, Vec4::sReplicate(FLT_EPSILON)); + + default: + // Mixed signs, degenerate tetrahedron + return UVec4::sReplicate(0xffffffff); + } + } + + /// Get the closest point between tetrahedron (inA, inB, inC, inD) to the origin + /// outSet specifies which feature was closest, 1 = a, 2 = b, 4 = c, 8 = d. Edges have 2 bits set, triangles 3 and if the point is in the interior 4 bits are set. + /// If MustIncludeD is true, the function assumes that D is part of the closest feature (vertex, edge, face, tetrahedron) and does less work, if the assumption is not true then a closest point to the other features is returned. + template + inline Vec3 GetClosestPointOnTetrahedron(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD, uint32 &outSet) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point) + // With p = 0 + + // Start out assuming point inside all halfspaces, so closest to itself + uint32 closest_set = 0b1111; + Vec3 closest_point = Vec3::sZero(); + float best_dist_sq = FLT_MAX; + + // Determine for each of the faces of the tetrahedron if the origin is in front of the plane + UVec4 origin_out_of_planes = OriginOutsideOfTetrahedronPlanes(inA, inB, inC, inD); + + // If point outside face abc then compute closest point on abc + if (origin_out_of_planes.GetX()) // OriginOutsideOfPlane(inA, inB, inC, inD) + { + if constexpr (MustIncludeD) + { + // If the closest point must include D then ABC cannot be closest but the closest point + // cannot be an interior point either so we return A as closest point + closest_set = 0b0001; + closest_point = inA; + } + else + { + // Test the face normally + closest_point = GetClosestPointOnTriangle(inA, inB, inC, closest_set); + } + best_dist_sq = closest_point.LengthSq(); + } + + // Repeat test for face acd + if (origin_out_of_planes.GetY()) // OriginOutsideOfPlane(inA, inC, inD, inB) + { + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inA, inC, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_dist_sq = dist_sq; + closest_point = q; + closest_set = (set & 0b0001) + ((set & 0b0110) << 1); + } + } + + // Repeat test for face adb + if (origin_out_of_planes.GetZ()) // OriginOutsideOfPlane(inA, inD, inB, inC) + { + // Keep original vertex order, it doesn't matter if the triangle is facing inward or outward + // and it improves consistency for GJK which will always add a new vertex D and keep the closest + // feature from the previous iteration in ABC + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inA, inB, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_dist_sq = dist_sq; + closest_point = q; + closest_set = (set & 0b0011) + ((set & 0b0100) << 1); + } + } + + // Repeat test for face bdc + if (origin_out_of_planes.GetW()) // OriginOutsideOfPlane(inB, inD, inC, inA) + { + // Keep original vertex order, it doesn't matter if the triangle is facing inward or outward + // and it improves consistency for GJK which will always add a new vertex D and keep the closest + // feature from the previous iteration in ABC + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inB, inC, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_point = q; + closest_set = set << 1; + } + } + + outSet = closest_set; + return closest_point; + } +}; + +JPH_PRECISE_MATH_OFF + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp new file mode 100644 index 000000000000..e15c0912ffd2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp @@ -0,0 +1,1467 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include + +#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END +#endif // JPH_CONVEX_BUILDER_DUMP_SHAPE + +#ifdef JPH_CONVEX_BUILDER_DEBUG + #include +#endif + +JPH_NAMESPACE_BEGIN + +ConvexHullBuilder::Face::~Face() +{ + // Free all edges + Edge *e = mFirstEdge; + if (e != nullptr) + { + do + { + Edge *next = e->mNextEdge; + delete e; + e = next; + } while (e != mFirstEdge); + } +} + +void ConvexHullBuilder::Face::CalculateNormalAndCentroid(const Vec3 *inPositions) +{ + // Get point that we use to construct a triangle fan + Edge *e = mFirstEdge; + Vec3 y0 = inPositions[e->mStartIdx]; + + // Get the 2nd point + e = e->mNextEdge; + Vec3 y1 = inPositions[e->mStartIdx]; + + // Start accumulating the centroid + mCentroid = y0 + y1; + int n = 2; + + // Start accumulating the normal + mNormal = Vec3::sZero(); + + // Loop over remaining edges accumulating normals in a triangle fan fashion + for (e = e->mNextEdge; e != mFirstEdge; e = e->mNextEdge) + { + // Get the 3rd point + Vec3 y2 = inPositions[e->mStartIdx]; + + // Calculate edges (counter clockwise) + Vec3 e0 = y1 - y0; + Vec3 e1 = y2 - y1; + Vec3 e2 = y0 - y2; + + // The best normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the others must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter: e1 or e2 + UVec4 e1_shorter_than_e2 = Vec4::sLess(e1.DotV4(e1), e2.DotV4(e2)); + + // We calculate both normals and then select the one that had the shortest edge for our normal (this avoids branching) + Vec3 normal_e01 = e0.Cross(e1); + Vec3 normal_e02 = e2.Cross(e0); + mNormal += Vec3::sSelect(normal_e02, normal_e01, e1_shorter_than_e2); + + // Accumulate centroid + mCentroid += y2; + n++; + + // Update y1 for next triangle + y1 = y2; + } + + // Finalize centroid + mCentroid /= float(n); +} + +void ConvexHullBuilder::Face::Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) +{ + JPH_ASSERT(mFirstEdge == nullptr); + JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2); + + // Create 3 edges + Edge *e0 = new Edge(this, inIdx0); + Edge *e1 = new Edge(this, inIdx1); + Edge *e2 = new Edge(this, inIdx2); + + // Link edges + e0->mNextEdge = e1; + e1->mNextEdge = e2; + e2->mNextEdge = e0; + mFirstEdge = e0; + + CalculateNormalAndCentroid(inPositions); +} + +ConvexHullBuilder::ConvexHullBuilder(const Positions &inPositions) : + mPositions(inPositions) +{ +#ifdef JPH_CONVEX_BUILDER_DEBUG + mIteration = 0; + + // Center the drawing of the first hull around the origin and calculate the delta offset between states + mOffset = RVec3::sZero(); + if (mPositions.empty()) + { + // No hull will be generated + mDelta = Vec3::sZero(); + } + else + { + Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX); + for (Vec3 v : mPositions) + { + minv = Vec3::sMin(minv, v); + maxv = Vec3::sMax(maxv, v); + mOffset -= v; + } + mOffset /= Real(mPositions.size()); + mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0); + mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there + } +#endif +} + +void ConvexHullBuilder::FreeFaces() +{ + for (Face *f : mFaces) + delete f; + mFaces.clear(); +} + +void ConvexHullBuilder::GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const +{ + outFace = nullptr; + outDistSq = 0.0f; + + for (Face *f : inFaces) + if (!f->mRemoved) + { + // Determine distance to face + float dot = f->mNormal.Dot(inPoint - f->mCentroid); + if (dot > 0.0f) + { + float dist_sq = dot * dot / f->mNormal.LengthSq(); + if (dist_sq > outDistSq) + { + outFace = f; + outDistSq = dist_sq; + } + } + } +} + +float ConvexHullBuilder::GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const +{ + bool all_inside = true; + float edge_dist_sq = FLT_MAX; + + // Test if it is inside the edges of the polygon + Edge *edge = inFace->mFirstEdge; + Vec3 p1 = mPositions[edge->GetPreviousEdge()->mStartIdx]; + do + { + Vec3 p2 = mPositions[edge->mStartIdx]; + if ((p2 - p1).Cross(inPoint - p1).Dot(inFace->mNormal) < 0.0f) + { + // It is outside + all_inside = false; + + // Measure distance to this edge + uint32 s; + edge_dist_sq = min(edge_dist_sq, ClosestPoint::GetClosestPointOnLine(p1 - inPoint, p2 - inPoint, s).LengthSq()); + } + p1 = p2; + edge = edge->mNextEdge; + } while (edge != inFace->mFirstEdge); + + return all_inside? 0.0f : edge_dist_sq; +} + +bool ConvexHullBuilder::AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq) +{ + Vec3 point = mPositions[inPositionIdx]; + + // Find the face for which the point is furthest away + Face *best_face; + float best_dist_sq; + GetFaceForPoint(point, inFaces, best_face, best_dist_sq); + + if (best_face != nullptr) + { + // Check if this point is within the tolerance margin to the plane + if (best_dist_sq <= inToleranceSq) + { + // Check distance to edges + float dist_to_edge_sq = GetDistanceToEdgeSq(point, best_face); + if (dist_to_edge_sq > inToleranceSq) + { + // Point is outside of the face and too far away to discard + mCoplanarList.push_back({ inPositionIdx, dist_to_edge_sq }); + } + } + else + { + // This point is in front of the face, add it to the conflict list + if (best_dist_sq > best_face->mFurthestPointDistanceSq) + { + // This point is further away than any others, update the distance and add point as last point + best_face->mFurthestPointDistanceSq = best_dist_sq; + best_face->mConflictList.push_back(inPositionIdx); + } + else + { + // Not the furthest point, add it as the before last point + best_face->mConflictList.insert(best_face->mConflictList.begin() + best_face->mConflictList.size() - 1, inPositionIdx); + } + + return true; + } + } + + return false; +} + +float ConvexHullBuilder::DetermineCoplanarDistance() const +{ + // Formula as per: Implementing Quickhull - Dirk Gregorius. + Vec3 vmax = Vec3::sZero(); + for (Vec3 v : mPositions) + vmax = Vec3::sMax(vmax, v.Abs()); + return 3.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY() + vmax.GetZ()); +} + +int ConvexHullBuilder::GetNumVerticesUsed() const +{ + UnorderedSet used_verts; + used_verts.reserve(UnorderedSet::size_type(mPositions.size())); + for (Face *f : mFaces) + { + Edge *e = f->mFirstEdge; + do + { + used_verts.insert(e->mStartIdx); + e = e->mNextEdge; + } while (e != f->mFirstEdge); + } + return (int)used_verts.size(); +} + +bool ConvexHullBuilder::ContainsFace(const Array &inIndices) const +{ + for (Face *f : mFaces) + { + Edge *e = f->mFirstEdge; + Array::const_iterator index = std::find(inIndices.begin(), inIndices.end(), e->mStartIdx); + if (index != inIndices.end()) + { + size_t matches = 0; + + do + { + // Check if index matches + if (*index != e->mStartIdx) + break; + + // Increment number of matches + matches++; + + // Next index in list of inIndices + index++; + if (index == inIndices.end()) + index = inIndices.begin(); + + // Next edge + e = e->mNextEdge; + } while (e != f->mFirstEdge); + + if (matches == inIndices.size()) + return true; + } + } + + return false; +} + +ConvexHullBuilder::EResult ConvexHullBuilder::Initialize(int inMaxVertices, float inTolerance, const char *&outError) +{ + // Free the faces possibly left over from an earlier hull + FreeFaces(); + + // Test that we have at least 3 points + if (mPositions.size() < 3) + { + outError = "Need at least 3 points to make a hull"; + return EResult::TooFewPoints; + } + + // Determine a suitable tolerance for detecting that points are coplanar + float coplanar_tolerance_sq = Square(DetermineCoplanarDistance()); + + // Increase desired tolerance if accuracy doesn't allow it + float tolerance_sq = max(coplanar_tolerance_sq, Square(inTolerance)); + + // Find point furthest from the origin + int idx1 = -1; + float max_dist_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + { + float dist_sq = mPositions[i].LengthSq(); + if (dist_sq > max_dist_sq) + { + max_dist_sq = dist_sq; + idx1 = i; + } + } + JPH_ASSERT(idx1 >= 0); + + // Find point that is furthest away from this point + int idx2 = -1; + max_dist_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1) + { + float dist_sq = (mPositions[i] - mPositions[idx1]).LengthSq(); + if (dist_sq > max_dist_sq) + { + max_dist_sq = dist_sq; + idx2 = i; + } + } + JPH_ASSERT(idx2 >= 0); + + // Find point that forms the biggest triangle + int idx3 = -1; + float best_triangle_area_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1 && i != idx2) + { + float triangle_area_sq = (mPositions[idx1] - mPositions[i]).Cross(mPositions[idx2] - mPositions[i]).LengthSq(); + if (triangle_area_sq > best_triangle_area_sq) + { + best_triangle_area_sq = triangle_area_sq; + idx3 = i; + } + } + JPH_ASSERT(idx3 >= 0); + if (best_triangle_area_sq < cMinTriangleAreaSq) + { + outError = "Could not find a suitable initial triangle because its area was too small"; + return EResult::Degenerate; + } + + // Check if we have only 3 vertices + if (mPositions.size() == 3) + { + // Create two triangles (back to back) + Face *t1 = CreateTriangle(idx1, idx2, idx3); + Face *t2 = CreateTriangle(idx1, idx3, idx2); + + // Link faces edges + sLinkFace(t1->mFirstEdge, t2->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t2->mFirstEdge); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state + DrawState(); +#endif + + return EResult::Success; + } + + // Find point that forms the biggest tetrahedron + Vec3 initial_plane_normal = (mPositions[idx2] - mPositions[idx1]).Cross(mPositions[idx3] - mPositions[idx1]).Normalized(); + Vec3 initial_plane_centroid = (mPositions[idx1] + mPositions[idx2] + mPositions[idx3]) / 3.0f; + int idx4 = -1; + float max_dist = 0.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1 && i != idx2 && i != idx3) + { + float dist = (mPositions[i] - initial_plane_centroid).Dot(initial_plane_normal); + if (abs(dist) > abs(max_dist)) + { + max_dist = dist; + idx4 = i; + } + } + + // Check if the hull is coplanar + if (Square(max_dist) <= 25.0f * coplanar_tolerance_sq) + { + // First project all points in 2D space + Vec3 base1 = initial_plane_normal.GetNormalizedPerpendicular(); + Vec3 base2 = initial_plane_normal.Cross(base1); + Array positions_2d; + positions_2d.reserve(mPositions.size()); + for (Vec3 v : mPositions) + positions_2d.emplace_back(base1.Dot(v), base2.Dot(v), 0.0f); + + // Build hull + Array edges_2d; + ConvexHullBuilder2D builder_2d(positions_2d); + ConvexHullBuilder2D::EResult result = builder_2d.Initialize(idx1, idx2, idx3, inMaxVertices, inTolerance, edges_2d); + + // Create faces (back to back) + Face *f1 = CreateFace(); + Face *f2 = CreateFace(); + + // Create edges for face 1 + Array edges_f1; + edges_f1.reserve(edges_2d.size()); + for (int start_idx : edges_2d) + { + Edge *edge = new Edge(f1, start_idx); + if (edges_f1.empty()) + f1->mFirstEdge = edge; + else + edges_f1.back()->mNextEdge = edge; + edges_f1.push_back(edge); + } + edges_f1.back()->mNextEdge = f1->mFirstEdge; + + // Create edges for face 2 + Array edges_f2; + edges_f2.reserve(edges_2d.size()); + for (int i = (int)edges_2d.size() - 1; i >= 0; --i) + { + Edge *edge = new Edge(f2, edges_2d[i]); + if (edges_f2.empty()) + f2->mFirstEdge = edge; + else + edges_f2.back()->mNextEdge = edge; + edges_f2.push_back(edge); + } + edges_f2.back()->mNextEdge = f2->mFirstEdge; + + // Link edges + for (size_t i = 0; i < edges_2d.size(); ++i) + sLinkFace(edges_f1[i], edges_f2[(2 * edges_2d.size() - 2 - i) % edges_2d.size()]); + + // Calculate the plane for both faces + f1->CalculateNormalAndCentroid(mPositions.data()); + f2->mNormal = -f1->mNormal; + f2->mCentroid = f1->mCentroid; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state + DrawState(); +#endif + + return result == ConvexHullBuilder2D::EResult::MaxVerticesReached? EResult::MaxVerticesReached : EResult::Success; + } + + // Ensure the planes are facing outwards + if (max_dist < 0.0f) + std::swap(idx2, idx3); + + // Create tetrahedron + Face *t1 = CreateTriangle(idx1, idx2, idx4); + Face *t2 = CreateTriangle(idx2, idx3, idx4); + Face *t3 = CreateTriangle(idx3, idx1, idx4); + Face *t4 = CreateTriangle(idx1, idx3, idx2); + + // Link face edges + sLinkFace(t1->mFirstEdge, t4->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t3->mFirstEdge->mNextEdge); + sLinkFace(t2->mFirstEdge, t4->mFirstEdge->mNextEdge); + sLinkFace(t2->mFirstEdge->mNextEdge, t3->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t3->mFirstEdge, t4->mFirstEdge); + + // Build the initial conflict lists + Faces faces { t1, t2, t3, t4 }; + for (int idx = 0; idx < (int)mPositions.size(); ++idx) + if (idx != idx1 && idx != idx2 && idx != idx3 && idx != idx4) + AssignPointToFace(idx, faces, tolerance_sq); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state including conflict list + DrawState(true); + + // Increment iteration counter + ++mIteration; +#endif + + // Overestimate of the actual amount of vertices we use, for limiting the amount of vertices in the hull + int num_vertices_used = 4; + + // Loop through the remainder of the points and add them + for (;;) + { + // Find the face with the furthest point on it + Face *face_with_furthest_point = nullptr; + float furthest_dist_sq = 0.0f; + for (Face *f : mFaces) + if (f->mFurthestPointDistanceSq > furthest_dist_sq) + { + furthest_dist_sq = f->mFurthestPointDistanceSq; + face_with_furthest_point = f; + } + + int furthest_point_idx; + if (face_with_furthest_point != nullptr) + { + // Take the furthest point + furthest_point_idx = face_with_furthest_point->mConflictList.back(); + face_with_furthest_point->mConflictList.pop_back(); + } + else if (!mCoplanarList.empty()) + { + // Try to assign points to faces (this also recalculates the distance to the hull for the coplanar vertices) + CoplanarList coplanar; + mCoplanarList.swap(coplanar); + bool added = false; + for (const Coplanar &c : coplanar) + added |= AssignPointToFace(c.mPositionIdx, mFaces, tolerance_sq); + + // If we were able to assign a point, loop again to pick it up + if (added) + continue; + + // If the coplanar list is empty, there are no points left and we're done + if (mCoplanarList.empty()) + break; + + do + { + // Find the vertex that is furthest from the hull + CoplanarList::size_type best_idx = 0; + float best_dist_sq = mCoplanarList.front().mDistanceSq; + for (CoplanarList::size_type idx = 1; idx < mCoplanarList.size(); ++idx) + { + const Coplanar &c = mCoplanarList[idx]; + if (c.mDistanceSq > best_dist_sq) + { + best_idx = idx; + best_dist_sq = c.mDistanceSq; + } + } + + // Swap it to the end + std::swap(mCoplanarList[best_idx], mCoplanarList.back()); + + // Remove it + furthest_point_idx = mCoplanarList.back().mPositionIdx; + mCoplanarList.pop_back(); + + // Find the face for which the point is furthest away + GetFaceForPoint(mPositions[furthest_point_idx], mFaces, face_with_furthest_point, best_dist_sq); + } while (!mCoplanarList.empty() && face_with_furthest_point == nullptr); + + if (face_with_furthest_point == nullptr) + break; + } + else + { + // If there are no more vertices, we're done + break; + } + + // Check if we have a limit on the max vertices that we should produce + if (num_vertices_used >= inMaxVertices) + { + // Count the actual amount of used vertices (we did not take the removal of any vertices into account) + num_vertices_used = GetNumVerticesUsed(); + + // Check if there are too many + if (num_vertices_used >= inMaxVertices) + return EResult::MaxVerticesReached; + } + + // We're about to add another vertex + ++num_vertices_used; + + // Add the point to the hull + Faces new_faces; + AddPoint(face_with_furthest_point, furthest_point_idx, coplanar_tolerance_sq, new_faces); + + // Redistribute points on conflict lists belonging to removed faces + for (const Face *face : mFaces) + if (face->mRemoved) + for (int idx : face->mConflictList) + AssignPointToFace(idx, new_faces, tolerance_sq); + + // Permanently delete faces that we removed in AddPoint() + GarbageCollectFaces(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw state at the end of this step including conflict list + DrawState(true); + + // Increment iteration counter + ++mIteration; +#endif + } + + // Check if we are left with a hull. It is possible that hull building fails if the points are nearly coplanar. + if (mFaces.size() < 2) + { + outError = "Too few faces in hull"; + return EResult::TooFewFaces; + } + + return EResult::Success; +} + +void ConvexHullBuilder::AddPoint(Face *inFacingFace, int inIdx, float inCoplanarToleranceSq, Faces &outNewFaces) +{ + // Get position + Vec3 pos = mPositions[inIdx]; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw point to be added + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + pos), Color::sYellow, 0.1f); + DebugRenderer::sInstance->DrawText3D(cDrawScale * (mOffset + pos), ConvertToString(inIdx), Color::sWhite); +#endif + +#ifdef JPH_ENABLE_ASSERTS + // Check if structure is intact + ValidateFaces(); +#endif + + // Find edge of convex hull of faces that are not facing the new vertex + FullEdges edges; + FindEdge(inFacingFace, pos, edges); + JPH_ASSERT(edges.size() >= 3); + + // Create new faces + outNewFaces.reserve(edges.size()); + for (const FullEdge &e : edges) + { + JPH_ASSERT(e.mStartIdx != e.mEndIdx); + Face *f = CreateTriangle(e.mStartIdx, e.mEndIdx, inIdx); + outNewFaces.push_back(f); + } + + // Link edges + for (Faces::size_type i = 0; i < outNewFaces.size(); ++i) + { + sLinkFace(outNewFaces[i]->mFirstEdge, edges[i].mNeighbourEdge); + sLinkFace(outNewFaces[i]->mFirstEdge->mNextEdge, outNewFaces[(i + 1) % outNewFaces.size()]->mFirstEdge->mNextEdge->mNextEdge); + } + + // Loop on faces that were modified until nothing needs to be checked anymore + Faces affected_faces = outNewFaces; + while (!affected_faces.empty()) + { + // Take the next face + Face *face = affected_faces.back(); + affected_faces.pop_back(); + + if (!face->mRemoved) + { + // Merge with neighbour if this is a degenerate face + MergeDegenerateFace(face, affected_faces); + + // Merge with coplanar neighbours (or when the neighbour forms a concave edge) + if (!face->mRemoved) + MergeCoplanarOrConcaveFaces(face, inCoplanarToleranceSq, affected_faces); + } + } + +#ifdef JPH_ENABLE_ASSERTS + // Check if structure is intact + ValidateFaces(); +#endif +} + +void ConvexHullBuilder::GarbageCollectFaces() +{ + for (int i = (int)mFaces.size() - 1; i >= 0; --i) + { + Face *f = mFaces[i]; + if (f->mRemoved) + { + FreeFace(f); + mFaces.erase(mFaces.begin() + i); + } + } +} + +ConvexHullBuilder::Face *ConvexHullBuilder::CreateFace() +{ + // Call provider to create face + Face *f = new Face(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Remember iteration counter + f->mIteration = mIteration; +#endif + + // Add to list + mFaces.push_back(f); + return f; +} + +ConvexHullBuilder::Face *ConvexHullBuilder::CreateTriangle(int inIdx1, int inIdx2, int inIdx3) +{ + Face *f = CreateFace(); + f->Initialize(inIdx1, inIdx2, inIdx3, mPositions.data()); + return f; +} + +void ConvexHullBuilder::FreeFace(Face *inFace) +{ + JPH_ASSERT(inFace->mRemoved); + +#ifdef JPH_ENABLE_ASSERTS + // Make sure that this face is not connected + Edge *e = inFace->mFirstEdge; + if (e != nullptr) + do + { + JPH_ASSERT(e->mNeighbourEdge == nullptr); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +#endif + + // Free the face + delete inFace; +} + +void ConvexHullBuilder::sLinkFace(Edge *inEdge1, Edge *inEdge2) +{ + // Check not connected yet + JPH_ASSERT(inEdge1->mNeighbourEdge == nullptr); + JPH_ASSERT(inEdge2->mNeighbourEdge == nullptr); + JPH_ASSERT(inEdge1->mFace != inEdge2->mFace); + + // Check vertices match + JPH_ASSERT(inEdge1->mStartIdx == inEdge2->mNextEdge->mStartIdx); + JPH_ASSERT(inEdge2->mStartIdx == inEdge1->mNextEdge->mStartIdx); + + // Link up + inEdge1->mNeighbourEdge = inEdge2; + inEdge2->mNeighbourEdge = inEdge1; +} + +void ConvexHullBuilder::sUnlinkFace(Face *inFace) +{ + // Unlink from neighbours + Edge *e = inFace->mFirstEdge; + do + { + if (e->mNeighbourEdge != nullptr) + { + // Validate that neighbour points to us + JPH_ASSERT(e->mNeighbourEdge->mNeighbourEdge == e); + + // Unlink + e->mNeighbourEdge->mNeighbourEdge = nullptr; + e->mNeighbourEdge = nullptr; + } + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const +{ + // Assert that we were given an empty array + JPH_ASSERT(outEdges.empty()); + + // Should start with a facing face + JPH_ASSERT(inFacingFace->IsFacing(inVertex)); + + // Flag as removed + inFacingFace->mRemoved = true; + + // Instead of recursing, we build our own stack with the information we need + struct StackEntry + { + Edge * mFirstEdge; + Edge * mCurrentEdge; + }; + constexpr int cMaxEdgeLength = 128; + StackEntry stack[cMaxEdgeLength]; + int cur_stack_pos = 0; + + static_assert(alignof(Edge) >= 2, "Need lowest bit to indicate to tell if we completed the loop"); + + // Start with the face / edge provided + stack[0].mFirstEdge = inFacingFace->mFirstEdge; + stack[0].mCurrentEdge = reinterpret_cast(reinterpret_cast(inFacingFace->mFirstEdge) | 1); // Set lowest bit of pointer to make it different from the first edge + + for (;;) + { + StackEntry &cur_entry = stack[cur_stack_pos]; + + // Next edge + Edge *raw_e = cur_entry.mCurrentEdge; + Edge *e = reinterpret_cast(reinterpret_cast(raw_e) & ~uintptr_t(1)); // Remove the lowest bit which was used to indicate that this is the first edge we're testing + cur_entry.mCurrentEdge = e->mNextEdge; + + // If we're back at the first edge we've completed the face and we're done + if (raw_e == cur_entry.mFirstEdge) + { + // This face needs to be removed, unlink it now, caller will free + sUnlinkFace(e->mFace); + + // Pop from stack + if (--cur_stack_pos < 0) + break; + } + else + { + // Visit neighbour face + Edge *ne = e->mNeighbourEdge; + if (ne != nullptr) + { + Face *n = ne->mFace; + if (!n->mRemoved) + { + // Check if vertex is on the front side of this face + if (n->IsFacing(inVertex)) + { + // Vertex on front, this face needs to be removed + n->mRemoved = true; + + // Add element to the stack of elements to visit + cur_stack_pos++; + JPH_ASSERT(cur_stack_pos < cMaxEdgeLength); + StackEntry &new_entry = stack[cur_stack_pos]; + new_entry.mFirstEdge = ne; + new_entry.mCurrentEdge = ne->mNextEdge; // We don't need to test this edge again since we came from it + } + else + { + // Vertex behind, keep edge + FullEdge full; + full.mNeighbourEdge = ne; + full.mStartIdx = e->mStartIdx; + full.mEndIdx = ne->mStartIdx; + outEdges.push_back(full); + } + } + } + } + } + + // Assert that we have a fully connected loop +#ifdef JPH_ENABLE_ASSERTS + for (int i = 0; i < (int)outEdges.size(); ++i) + JPH_ASSERT(outEdges[i].mEndIdx == outEdges[(i + 1) % outEdges.size()].mStartIdx); +#endif + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw edge of facing faces + for (int i = 0; i < (int)outEdges.size(); ++i) + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]), cDrawScale * (mOffset + mPositions[outEdges[i].mEndIdx]), Color::sWhite, 0.01f); + DrawState(); +#endif +} + +void ConvexHullBuilder::MergeFaces(Edge *inEdge) +{ + // Get the face + Face *face = inEdge->mFace; + + // Find the previous and next edge + Edge *next_edge = inEdge->mNextEdge; + Edge *prev_edge = inEdge->GetPreviousEdge(); + + // Get the other face + Edge *other_edge = inEdge->mNeighbourEdge; + Face *other_face = other_edge->mFace; + + // Check if attempting to merge with self + JPH_ASSERT(face != other_face); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(face, Color::sGreen); + DrawWireFace(other_face, Color::sRed); + DrawState(); +#endif + + // Loop over the edges of the other face and make them belong to inFace + Edge *edge = other_edge->mNextEdge; + prev_edge->mNextEdge = edge; + for (;;) + { + edge->mFace = face; + if (edge->mNextEdge == other_edge) + { + // Terminate when we are back at other_edge + edge->mNextEdge = next_edge; + break; + } + edge = edge->mNextEdge; + } + + // If the first edge happens to be inEdge we need to fix it because this edge is no longer part of the face. + // Note that we replace it with the first edge of the merged face so that if the MergeFace function is called + // from a loop that loops around the face that it will still terminate after visiting all edges once. + if (face->mFirstEdge == inEdge) + face->mFirstEdge = prev_edge->mNextEdge; + + // Free the edges + delete inEdge; + delete other_edge; + + // Mark the other face as removed + other_face->mFirstEdge = nullptr; + other_face->mRemoved = true; + + // Recalculate plane + face->CalculateNormalAndCentroid(mPositions.data()); + + // Merge conflict lists + if (face->mFurthestPointDistanceSq > other_face->mFurthestPointDistanceSq) + { + // This face has a point that's further away, make sure it remains the last one as we add the other points to this faces list + face->mConflictList.insert(face->mConflictList.end() - 1, other_face->mConflictList.begin(), other_face->mConflictList.end()); + } + else + { + // The other face has a point that's furthest away, add that list at the end. + face->mConflictList.insert(face->mConflictList.end(), other_face->mConflictList.begin(), other_face->mConflictList.end()); + face->mFurthestPointDistanceSq = other_face->mFurthestPointDistanceSq; + } + other_face->mConflictList.clear(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(face, Color::sWhite); + DrawState(); +#endif +} + +void ConvexHullBuilder::MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces) +{ + // Check area of face + if (inFace->mNormal.LengthSq() < cMinTriangleAreaSq) + { + // Find longest edge, since this face is a sliver this should keep the face convex + float max_length_sq = 0.0f; + Edge *longest_edge = nullptr; + Edge *e = inFace->mFirstEdge; + Vec3 p1 = mPositions[e->mStartIdx]; + do + { + Edge *next = e->mNextEdge; + Vec3 p2 = mPositions[next->mStartIdx]; + float length_sq = (p2 - p1).LengthSq(); + if (length_sq >= max_length_sq) + { + max_length_sq = length_sq; + longest_edge = e; + } + p1 = p2; + e = next; + } while (e != inFace->mFirstEdge); + + // Merge with face on longest edge + MergeFaces(longest_edge); + + // Remove any invalid edges + RemoveInvalidEdges(inFace, ioAffectedFaces); + } +} + +void ConvexHullBuilder::MergeCoplanarOrConcaveFaces(Face *inFace, float inCoplanarToleranceSq, Faces &ioAffectedFaces) +{ + bool merged = false; + + Edge *edge = inFace->mFirstEdge; + do + { + // Store next edge since this edge can be removed + Edge *next_edge = edge->mNextEdge; + + // Test if centroid of one face is above plane of the other face by inCoplanarToleranceSq. + // If so we need to merge other face into inFace. + const Face *other_face = edge->mNeighbourEdge->mFace; + Vec3 delta_centroid = other_face->mCentroid - inFace->mCentroid; + float dist_other_face_centroid = inFace->mNormal.Dot(delta_centroid); + float signed_dist_other_face_centroid_sq = abs(dist_other_face_centroid) * dist_other_face_centroid; + float dist_face_centroid = -other_face->mNormal.Dot(delta_centroid); + float signed_dist_face_centroid_sq = abs(dist_face_centroid) * dist_face_centroid; + float face_normal_len_sq = inFace->mNormal.LengthSq(); + float other_face_normal_len_sq = other_face->mNormal.LengthSq(); + if ((signed_dist_other_face_centroid_sq > -inCoplanarToleranceSq * face_normal_len_sq + || signed_dist_face_centroid_sq > -inCoplanarToleranceSq * other_face_normal_len_sq) + && inFace->mNormal.Dot(other_face->mNormal) > 0.0f) // Never merge faces that are back to back + { + MergeFaces(edge); + merged = true; + } + + edge = next_edge; + } while (edge != inFace->mFirstEdge); + + if (merged) + RemoveInvalidEdges(inFace, ioAffectedFaces); +} + +void ConvexHullBuilder::sMarkAffected(Face *inFace, Faces &ioAffectedFaces) +{ + if (std::find(ioAffectedFaces.begin(), ioAffectedFaces.end(), inFace) == ioAffectedFaces.end()) + ioAffectedFaces.push_back(inFace); +} + +void ConvexHullBuilder::RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces) +{ + // This marks that the plane needs to be recalculated (we delay this until the end of the + // function since we don't use the plane and we want to avoid calculating it multiple times) + bool recalculate_plane = false; + + // We keep going through this loop until no more edges were removed + bool removed; + do + { + removed = false; + + // Loop over all edges in this face + Edge *edge = inFace->mFirstEdge; + Face *neighbour_face = edge->mNeighbourEdge->mFace; + do + { + Edge *next_edge = edge->mNextEdge; + Face *next_neighbour_face = next_edge->mNeighbourEdge->mFace; + + if (neighbour_face == inFace) + { + // We only remove 1 edge at a time, check if this edge's next edge is our neighbour. + // If this check fails, we will continue to scan along the edge until we find an edge where this is the case. + if (edge->mNeighbourEdge == next_edge) + { + // This edge leads back to the starting point, this means the edge is interior and needs to be removed +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sBlue); + DrawState(); +#endif + + // Remove edge + Edge *prev_edge = edge->GetPreviousEdge(); + prev_edge->mNextEdge = next_edge->mNextEdge; + if (inFace->mFirstEdge == edge || inFace->mFirstEdge == next_edge) + inFace->mFirstEdge = prev_edge; + delete edge; + delete next_edge; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sGreen); + DrawState(); +#endif + + // Check if inFace now has only 2 edges left + if (RemoveTwoEdgeFace(inFace, ioAffectedFaces)) + return; // Bail if face no longer exists + + // Restart the loop + recalculate_plane = true; + removed = true; + break; + } + } + else if (neighbour_face == next_neighbour_face) + { + // There are two edges that connect to the same face, we will remove the second one +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sYellow); + DrawWireFace(neighbour_face, Color::sRed); + DrawState(); +#endif + + // First merge the neighbours edges + Edge *neighbour_edge = next_edge->mNeighbourEdge; + Edge *next_neighbour_edge = neighbour_edge->mNextEdge; + if (neighbour_face->mFirstEdge == next_neighbour_edge) + neighbour_face->mFirstEdge = neighbour_edge; + neighbour_edge->mNextEdge = next_neighbour_edge->mNextEdge; + neighbour_edge->mNeighbourEdge = edge; + delete next_neighbour_edge; + + // Then merge my own edges + if (inFace->mFirstEdge == next_edge) + inFace->mFirstEdge = edge; + edge->mNextEdge = next_edge->mNextEdge; + edge->mNeighbourEdge = neighbour_edge; + delete next_edge; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sYellow); + DrawWireFace(neighbour_face, Color::sGreen); + DrawState(); +#endif + + // Check if neighbour has only 2 edges left + if (!RemoveTwoEdgeFace(neighbour_face, ioAffectedFaces)) + { + // No, we need to recalculate its plane + neighbour_face->CalculateNormalAndCentroid(mPositions.data()); + + // Mark neighbour face as affected + sMarkAffected(neighbour_face, ioAffectedFaces); + } + + // Check if inFace now has only 2 edges left + if (RemoveTwoEdgeFace(inFace, ioAffectedFaces)) + return; // Bail if face no longer exists + + // Restart loop + recalculate_plane = true; + removed = true; + break; + } + + // This edge is ok, go to the next edge + edge = next_edge; + neighbour_face = next_neighbour_face; + + } while (edge != inFace->mFirstEdge); + } while (removed); + + // Recalculate plane? + if (recalculate_plane) + inFace->CalculateNormalAndCentroid(mPositions.data()); +} + +bool ConvexHullBuilder::RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const +{ + // Check if this face contains only 2 edges + Edge *edge = inFace->mFirstEdge; + Edge *next_edge = edge->mNextEdge; + JPH_ASSERT(edge != next_edge); // 1 edge faces should not exist + if (next_edge->mNextEdge == edge) + { +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sRed); + DrawState(); +#endif + + // Schedule both neighbours for re-checking + Edge *neighbour_edge = edge->mNeighbourEdge; + Face *neighbour_face = neighbour_edge->mFace; + Edge *next_neighbour_edge = next_edge->mNeighbourEdge; + Face *next_neighbour_face = next_neighbour_edge->mFace; + sMarkAffected(neighbour_face, ioAffectedFaces); + sMarkAffected(next_neighbour_face, ioAffectedFaces); + + // Link my neighbours to each other + neighbour_edge->mNeighbourEdge = next_neighbour_edge; + next_neighbour_edge->mNeighbourEdge = neighbour_edge; + + // Unlink my edges + edge->mNeighbourEdge = nullptr; + next_edge->mNeighbourEdge = nullptr; + + // Mark this face as removed + inFace->mRemoved = true; + + return true; + } + + return false; +} + +#ifdef JPH_ENABLE_ASSERTS + +void ConvexHullBuilder::DumpFace(const Face *inFace) const +{ + Trace("f:0x%p", inFace); + + const Edge *e = inFace->mFirstEdge; + do + { + Trace("\te:0x%p { i:%d e:0x%p f:0x%p }", e, e->mStartIdx, e->mNeighbourEdge, e->mNeighbourEdge->mFace); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::DumpFaces() const +{ + Trace("Dump Faces:"); + + for (const Face *f : mFaces) + if (!f->mRemoved) + DumpFace(f); +} + +void ConvexHullBuilder::ValidateFace(const Face *inFace) const +{ + if (inFace->mRemoved) + { + const Edge *e = inFace->mFirstEdge; + if (e != nullptr) + do + { + JPH_ASSERT(e->mNeighbourEdge == nullptr); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); + } + else + { + int edge_count = 0; + + const Edge *e = inFace->mFirstEdge; + do + { + // Count edge + ++edge_count; + + // Validate that adjacent faces are all different + if (mFaces.size() > 2) + for (const Edge *other_edge = e->mNextEdge; other_edge != inFace->mFirstEdge; other_edge = other_edge->mNextEdge) + JPH_ASSERT(e->mNeighbourEdge->mFace != other_edge->mNeighbourEdge->mFace); + + // Assert that the face is correct + JPH_ASSERT(e->mFace == inFace); + + // Assert that we have a neighbour + const Edge *nb_edge = e->mNeighbourEdge; + JPH_ASSERT(nb_edge != nullptr); + if (nb_edge != nullptr) + { + // Assert that our neighbours edge points to us + JPH_ASSERT(nb_edge->mNeighbourEdge == e); + + // Assert that it belongs to a different face + JPH_ASSERT(nb_edge->mFace != inFace); + + // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex + JPH_ASSERT(nb_edge->mNextEdge->mStartIdx == e->mStartIdx); + + // Assert that my next edge points to the same vertex as my neighbours vertex + JPH_ASSERT(e->mNextEdge->mStartIdx == nb_edge->mStartIdx); + } + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); + + // Assert that we have 3 or more edges + JPH_ASSERT(edge_count >= 3); + } +} + +void ConvexHullBuilder::ValidateFaces() const +{ + for (const Face *f : mFaces) + ValidateFace(f); +} + +#endif // JPH_ENABLE_ASSERTS + +void ConvexHullBuilder::GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const +{ + // Fourth point is the average of all face centroids + Vec3 v4 = Vec3::sZero(); + for (const Face *f : mFaces) + v4 += f->mCentroid; + v4 /= float(mFaces.size()); + + // Calculate mass and center of mass of this convex hull by summing all tetrahedrons + outVolume = 0.0f; + outCenterOfMass = Vec3::sZero(); + for (const Face *f : mFaces) + { + // Get the first vertex that we'll use to create a triangle fan + Edge *e = f->mFirstEdge; + Vec3 v1 = mPositions[e->mStartIdx]; + + // Get the second vertex + e = e->mNextEdge; + Vec3 v2 = mPositions[e->mStartIdx]; + + for (e = e->mNextEdge; e != f->mFirstEdge; e = e->mNextEdge) + { + // Fetch the last point of the triangle + Vec3 v3 = mPositions[e->mStartIdx]; + + // Calculate center of mass and mass of this tetrahedron, + // see: https://en.wikipedia.org/wiki/Tetrahedron#Volume + float volume_tetrahedron = (v1 - v4).Dot((v2 - v4).Cross(v3 - v4)); // Needs to be divided by 6, postpone this until the end of the loop + Vec3 center_of_mass_tetrahedron = v1 + v2 + v3 + v4; // Needs to be divided by 4, postpone this until the end of the loop + + // Accumulate results + outVolume += volume_tetrahedron; + outCenterOfMass += volume_tetrahedron * center_of_mass_tetrahedron; + + // Update v2 for next triangle + v2 = v3; + } + } + + // Calculate center of mass, fall back to average point in case there is no volume (everything is on a plane in this case) + if (outVolume > FLT_EPSILON) + outCenterOfMass /= 4.0f * outVolume; + else + outCenterOfMass = v4; + + outVolume /= 6.0f; +} + +void ConvexHullBuilder::DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const +{ + outCoplanarDistance = DetermineCoplanarDistance(); + + // This measures the distance from a polygon to the furthest point outside of the hull + float max_error = 0.0f; + Face *max_error_face = nullptr; + int max_error_point = -1; + + for (int i = 0; i < (int)mPositions.size(); ++i) + { + Vec3 v = mPositions[i]; + + // This measures the closest edge from all faces to point v + // Note that we take the min of all faces since there may be multiple near coplanar faces so if we were to test this per face + // we may find that a point is outside of a polygon and mark it as an error, while it is actually inside a nearly coplanar + // polygon. + float min_edge_dist_sq = FLT_MAX; + Face *min_edge_dist_face = nullptr; + + for (Face *f : mFaces) + { + // Check if point is on or in front of plane + float normal_len = f->mNormal.Length(); + JPH_ASSERT(normal_len > 0.0f); + float plane_dist = f->mNormal.Dot(v - f->mCentroid) / normal_len; + if (plane_dist > -outCoplanarDistance) + { + // Check distance to the edges of this face + float edge_dist_sq = GetDistanceToEdgeSq(v, f); + if (edge_dist_sq < min_edge_dist_sq) + { + min_edge_dist_sq = edge_dist_sq; + min_edge_dist_face = f; + } + + // If the point is inside the polygon and the point is in front of the plane, measure the distance + if (edge_dist_sq == 0.0f && plane_dist > max_error) + { + max_error = plane_dist; + max_error_face = f; + max_error_point = i; + } + } + } + + // If the minimum distance to an edge is further than our current max error, we use that as max error + float min_edge_dist = sqrt(min_edge_dist_sq); + if (min_edge_dist_face != nullptr && min_edge_dist > max_error) + { + max_error = min_edge_dist; + max_error_face = min_edge_dist_face; + max_error_point = i; + } + } + + outFaceWithMaxError = max_error_face; + outMaxError = max_error; + outMaxErrorPositionIdx = max_error_point; +} + +#ifdef JPH_CONVEX_BUILDER_DEBUG + +void ConvexHullBuilder::DrawState(bool inDrawConflictList) const +{ + // Draw origin + DebugRenderer::sInstance->DrawMarker(cDrawScale * mOffset, Color::sRed, 0.2f); + + int face_idx = 0; + + // Draw faces + for (const Face *f : mFaces) + if (!f->mRemoved) + { + Color iteration_color = Color::sGetDistinctColor(f->mIteration); + Color face_color = Color::sGetDistinctColor(face_idx++); + + // First point + const Edge *e = f->mFirstEdge; + RVec3 p1 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + // Second point + e = e->mNextEdge; + RVec3 p2 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + // First line + DebugRenderer::sInstance->DrawLine(p1, p2, Color::sGrey); + + do + { + // Third point + e = e->mNextEdge; + RVec3 p3 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, iteration_color); + + DebugRenderer::sInstance->DrawLine(p2, p3, Color::sGrey); + + p2 = p3; + } + while (e != f->mFirstEdge); + + // Draw normal + RVec3 centroid = cDrawScale * (mOffset + f->mCentroid); + DebugRenderer::sInstance->DrawArrow(centroid, centroid + f->mNormal.NormalizedOr(Vec3::sZero()), face_color, 0.01f); + + // Draw conflict list + if (inDrawConflictList) + for (int idx : f->mConflictList) + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), face_color, 0.05f); + } + + // Offset to the right + mOffset += mDelta; +} + +void ConvexHullBuilder::DrawWireFace(const Face *inFace, ColorArg inColor) const +{ + const Edge *e = inFace->mFirstEdge; + RVec3 prev = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + do + { + const Edge *next = e->mNextEdge; + RVec3 cur = cDrawScale * (mOffset + mPositions[next->mStartIdx]); + DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f); + DebugRenderer::sInstance->DrawText3D(prev, ConvertToString(e->mStartIdx), inColor); + e = next; + prev = cur; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::DrawEdge(const Edge *inEdge, ColorArg inColor) const +{ + RVec3 p1 = cDrawScale * (mOffset + mPositions[inEdge->mStartIdx]); + RVec3 p2 = cDrawScale * (mOffset + mPositions[inEdge->mNextEdge->mStartIdx]); + DebugRenderer::sInstance->DrawArrow(p1, p2, inColor, 0.01f); +} + +#endif // JPH_CONVEX_BUILDER_DEBUG + +#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE + +void ConvexHullBuilder::DumpShape() const +{ + static atomic sShapeNo = 1; + int shape_no = sShapeNo++; + + std::ofstream f; + f.open(StringFormat("dumped_shape%d.cpp", shape_no).c_str(), std::ofstream::out | std::ofstream::trunc); + if (!f.is_open()) + return; + + f << "{\n"; + for (Vec3 v : mPositions) + f << StringFormat("\tVec3(%.9gf, %.9gf, %.9gf),\n", (double)v.GetX(), (double)v.GetY(), (double)v.GetZ()); + f << "},\n"; +} + +#endif // JPH_CONVEX_BUILDER_DUMP_SHAPE + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h new file mode 100644 index 000000000000..db1ef358d160 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h @@ -0,0 +1,276 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +//#define JPH_CONVEX_BUILDER_DEBUG +//#define JPH_CONVEX_BUILDER_DUMP_SHAPE + +#ifdef JPH_CONVEX_BUILDER_DEBUG + #include +#endif + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder that tries to create hulls as accurately as possible. Used for offline processing. +class JPH_EXPORT ConvexHullBuilder : public NonCopyable +{ +public: + // Forward declare + class Face; + + /// Class that holds the information of an edge + class Edge : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Edge(Face *inFace, int inStartIdx) : mFace(inFace), mStartIdx(inStartIdx) { } + + /// Get the previous edge + inline Edge * GetPreviousEdge() + { + Edge *prev_edge = this; + while (prev_edge->mNextEdge != this) + prev_edge = prev_edge->mNextEdge; + return prev_edge; + } + + Face * mFace; ///< Face that this edge belongs to + Edge * mNextEdge = nullptr; ///< Next edge of this face + Edge * mNeighbourEdge = nullptr; ///< Edge that this edge is connected to + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + }; + + using ConflictList = Array; + + /// Class that holds the information of one face + class Face : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + ~Face(); + + /// Initialize a face with three indices + void Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions); + + /// Calculates the centroid and normal for this face + void CalculateNormalAndCentroid(const Vec3 *inPositions); + + /// Check if face inFace is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(inPosition - mCentroid) > 0.0f; + } + + Vec3 mNormal; ///< Normal of this face, length is 2 times area of face + Vec3 mCentroid; ///< Center of the face + ConflictList mConflictList; ///< Positions associated with this edge (that are closest to this edge). The last position in the list is the point that is furthest away from the face. + Edge * mFirstEdge = nullptr; ///< First edge of this face + float mFurthestPointDistanceSq = 0.0f; ///< Squared distance of furthest point from the conflict list to the face + bool mRemoved = false; ///< Flag that indicates that face has been removed (face will be freed later) +#ifdef JPH_CONVEX_BUILDER_DEBUG + int mIteration; ///< Iteration that this face was created +#endif + }; + + // Typedefs + using Positions = Array; + using Faces = Array; + + /// Constructor + explicit ConvexHullBuilder(const Positions &inPositions); + + /// Destructor + ~ConvexHullBuilder() { FreeFaces(); } + + /// Result enum that indicates how the hull got created + enum class EResult + { + Success, ///< Hull building finished successfully + MaxVerticesReached, ///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached + TooFewPoints, ///< Too few points to create a hull + TooFewFaces, ///< Too few faces in the created hull (signifies precision errors during building) + Degenerate, ///< Degenerate hull detected + }; + + /// Takes all positions as provided by the constructor and use them to build a hull + /// Any points that are closer to the hull than inTolerance will be discarded + /// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit. + /// @param inTolerance Max distance that a point is allowed to be outside of the hull + /// @param outError Error message when building fails + /// @return Status code that reports if the hull was created or not + EResult Initialize(int inMaxVertices, float inTolerance, const char *&outError); + + /// Returns the amount of vertices that are currently used by the hull + int GetNumVerticesUsed() const; + + /// Returns true if the hull contains a polygon with inIndices (counter clockwise indices in mPositions) + bool ContainsFace(const Array &inIndices) const; + + /// Calculate the center of mass and the volume of the current convex hull + void GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const; + + /// Determines the point that is furthest outside of the hull and reports how far it is outside of the hull (which indicates a failure during hull building) + /// @param outFaceWithMaxError The face that caused the error + /// @param outMaxError The maximum distance of a point to the hull + /// @param outMaxErrorPositionIdx The index of the point that had this distance + /// @param outCoplanarDistance Points that are less than this distance from the hull are considered on the hull. This should be used as a lowerbound for the allowed error. + void DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const; + + /// Access to the created faces. Memory is owned by the convex hull builder. + const Faces & GetFaces() const { return mFaces; } + +private: + /// Minimal square area of a triangle (used for merging and checking if a triangle is degenerate) + static constexpr float cMinTriangleAreaSq = 1.0e-12f; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + + /// Class that holds an edge including start and end index + class FullEdge + { + public: + Edge * mNeighbourEdge; ///< Edge that this edge is connected to + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + int mEndIdx; ///< Vertex index in mPosition that indicates the end vertex of this edge + }; + + // Private typedefs + using FullEdges = Array; + + // Determine a suitable tolerance for detecting that points are coplanar + float DetermineCoplanarDistance() const; + + /// Find the face for which inPoint is furthest to the front + /// @param inPoint Point to test + /// @param inFaces List of faces to test + /// @param outFace Returns the best face + /// @param outDistSq Returns the squared distance how much inPoint is in front of the plane of the face + void GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const; + + /// @brief Calculates the distance between inPoint and inFace + /// @param inFace Face to test + /// @param inPoint Point to test + /// @return If the projection of the point on the plane is interior to the face 0, otherwise the squared distance to the closest edge + float GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const; + + /// Assigns a position to one of the supplied faces based on which face is closest. + /// @param inPositionIdx Index of the position to add + /// @param inFaces List of faces to consider + /// @param inToleranceSq Tolerance of the hull, if the point is closer to the face than this, we ignore it + /// @return True if point was assigned, false if it was discarded or added to the coplanar list + bool AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq); + + /// Add a new point to the convex hull + void AddPoint(Face *inFacingFace, int inIdx, float inToleranceSq, Faces &outNewFaces); + + /// Remove all faces that have been marked 'removed' from mFaces list + void GarbageCollectFaces(); + + /// Create a new face + Face * CreateFace(); + + /// Create a new triangle + Face * CreateTriangle(int inIdx1, int inIdx2, int inIdx3); + + /// Delete a face (checking that it is not connected to any other faces) + void FreeFace(Face *inFace); + + /// Release all faces and edges + void FreeFaces(); + + /// Link face edge to other face edge + static void sLinkFace(Edge *inEdge1, Edge *inEdge2); + + /// Unlink this face from all of its neighbours + static void sUnlinkFace(Face *inFace); + + /// Given one face that faces inVertex, find the edges of the faces that are not facing inVertex. + /// Will flag all those faces for removal. + void FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const; + + /// Merges the two faces that share inEdge into the face inEdge->mFace + void MergeFaces(Edge *inEdge); + + /// Merges inFace with a neighbour if it is degenerate (a sliver) + void MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces); + + /// Merges any coplanar as well as neighbours that form a non-convex edge into inFace. + /// Faces are considered coplanar if the distance^2 of the other face's centroid is smaller than inToleranceSq. + void MergeCoplanarOrConcaveFaces(Face *inFace, float inToleranceSq, Faces &ioAffectedFaces); + + /// Mark face as affected if it is not already in the list + static void sMarkAffected(Face *inFace, Faces &ioAffectedFaces); + + /// Removes all invalid edges. + /// 1. Merges inFace with faces that share two edges with it since this means inFace or the other face cannot be convex or the edge is colinear. + /// 2. Removes edges that are interior to inFace (that have inFace on both sides) + /// Any faces that need to be checked for validity will be added to ioAffectedFaces. + void RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces); + + /// Removes inFace if it consists of only 2 edges, linking its neighbouring faces together + /// Any faces that need to be checked for validity will be added to ioAffectedFaces. + /// @return True if face was removed. + bool RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const; + +#ifdef JPH_ENABLE_ASSERTS + /// Dumps the text representation of a face to the TTY + void DumpFace(const Face *inFace) const; + + /// Dumps the text representation of all faces to the TTY + void DumpFaces() const; + + /// Check consistency of 1 face + void ValidateFace(const Face *inFace) const; + + /// Check consistency of all faces + void ValidateFaces() const; +#endif + +#ifdef JPH_CONVEX_BUILDER_DEBUG + /// Draw state of algorithm + void DrawState(bool inDrawConflictList = false) const; + + /// Draw a face for debugging purposes + void DrawWireFace(const Face *inFace, ColorArg inColor) const; + + /// Draw an edge for debugging purposes + void DrawEdge(const Edge *inEdge, ColorArg inColor) const; +#endif + +#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE + void DumpShape() const; +#endif + + const Positions & mPositions; ///< List of positions (some of them are part of the hull) + Faces mFaces; ///< List of faces that are part of the hull (if !mRemoved) + + struct Coplanar + { + int mPositionIdx; ///< Index in mPositions + float mDistanceSq; ///< Distance to the edge of closest face (should be > 0) + }; + using CoplanarList = Array; + + CoplanarList mCoplanarList; ///< List of positions that are coplanar to a face but outside of the face, these are added to the hull at the end + +#ifdef JPH_CONVEX_BUILDER_DEBUG + int mIteration; ///< Number of iterations we've had so far (for debug purposes) + mutable RVec3 mOffset; ///< Offset to use for state drawing + Vec3 mDelta; ///< Delta offset between next states +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp new file mode 100644 index 000000000000..c821f92eef77 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp @@ -0,0 +1,335 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + #include +#endif + +JPH_NAMESPACE_BEGIN + +void ConvexHullBuilder2D::Edge::CalculateNormalAndCenter(const Vec3 *inPositions) +{ + Vec3 p1 = inPositions[mStartIdx]; + Vec3 p2 = inPositions[mNextEdge->mStartIdx]; + + // Center of edge + mCenter = 0.5f * (p1 + p2); + + // Create outward pointing normal. + // We have two choices for the normal (which satisfies normal . edge = 0): + // normal1 = (-edge.y, edge.x, 0) + // normal2 = (edge.y, -edge.x, 0) + // We want (normal x edge).z > 0 so that the normal points out of the polygon. Only normal2 satisfies this condition. + Vec3 edge = p2 - p1; + mNormal = Vec3(edge.GetY(), -edge.GetX(), 0); +} + +ConvexHullBuilder2D::ConvexHullBuilder2D(const Positions &inPositions) : + mPositions(inPositions) +{ +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + // Center the drawing of the first hull around the origin and calculate the delta offset between states + mOffset = RVec3::sZero(); + if (mPositions.empty()) + { + // No hull will be generated + mDelta = Vec3::sZero(); + } + else + { + Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX); + for (Vec3 v : mPositions) + { + minv = Vec3::sMin(minv, v); + maxv = Vec3::sMax(maxv, v); + mOffset -= v; + } + mOffset /= Real(mPositions.size()); + mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0); + mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there + } +#endif +} + +ConvexHullBuilder2D::~ConvexHullBuilder2D() +{ + FreeEdges(); +} + +void ConvexHullBuilder2D::FreeEdges() +{ + if (mFirstEdge == nullptr) + return; + + Edge *edge = mFirstEdge; + do + { + Edge *next = edge->mNextEdge; + delete edge; + edge = next; + } while (edge != mFirstEdge); + + mFirstEdge = nullptr; + mNumEdges = 0; +} + +#ifdef JPH_ENABLE_ASSERTS + +void ConvexHullBuilder2D::ValidateEdges() const +{ + if (mFirstEdge == nullptr) + { + JPH_ASSERT(mNumEdges == 0); + return; + } + + int count = 0; + + Edge *edge = mFirstEdge; + do + { + // Validate connectivity + JPH_ASSERT(edge->mNextEdge->mPrevEdge == edge); + JPH_ASSERT(edge->mPrevEdge->mNextEdge == edge); + + ++count; + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + // Validate that count matches + JPH_ASSERT(count == mNumEdges); +} + +#endif // JPH_ENABLE_ASSERTS + +void ConvexHullBuilder2D::AssignPointToEdge(int inPositionIdx, const Array &inEdges) const +{ + Vec3 point = mPositions[inPositionIdx]; + + Edge *best_edge = nullptr; + float best_dist_sq = 0.0f; + + // Test against all edges + for (Edge *edge : inEdges) + { + // Determine distance to edge + float dot = edge->mNormal.Dot(point - edge->mCenter); + if (dot > 0.0f) + { + float dist_sq = dot * dot / edge->mNormal.LengthSq(); + if (dist_sq > best_dist_sq) + { + best_edge = edge; + best_dist_sq = dist_sq; + } + } + } + + // If this point is in front of the edge, add it to the conflict list + if (best_edge != nullptr) + { + if (best_dist_sq > best_edge->mFurthestPointDistanceSq) + { + // This point is further away than any others, update the distance and add point as last point + best_edge->mFurthestPointDistanceSq = best_dist_sq; + best_edge->mConflictList.push_back(inPositionIdx); + } + else + { + // Not the furthest point, add it as the before last point + best_edge->mConflictList.insert(best_edge->mConflictList.begin() + best_edge->mConflictList.size() - 1, inPositionIdx); + } + } +} + +ConvexHullBuilder2D::EResult ConvexHullBuilder2D::Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges) +{ + // Clear any leftovers + FreeEdges(); + outEdges.clear(); + + // Reset flag + EResult result = EResult::Success; + + // Determine a suitable tolerance for detecting that points are colinear + // Formula as per: Implementing Quickhull - Dirk Gregorius. + Vec3 vmax = Vec3::sZero(); + for (Vec3 v : mPositions) + vmax = Vec3::sMax(vmax, v.Abs()); + float colinear_tolerance_sq = Square(2.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY())); + + // Increase desired tolerance if accuracy doesn't allow it + float tolerance_sq = max(colinear_tolerance_sq, Square(inTolerance)); + + // Start with the initial indices in counter clockwise order + float z = (mPositions[inIdx2] - mPositions[inIdx1]).Cross(mPositions[inIdx3] - mPositions[inIdx1]).GetZ(); + if (z < 0.0f) + std::swap(inIdx1, inIdx2); + + // Create and link edges + Edge *e1 = new Edge(inIdx1); + Edge *e2 = new Edge(inIdx2); + Edge *e3 = new Edge(inIdx3); + e1->mNextEdge = e2; + e1->mPrevEdge = e3; + e2->mNextEdge = e3; + e2->mPrevEdge = e1; + e3->mNextEdge = e1; + e3->mPrevEdge = e2; + mFirstEdge = e1; + mNumEdges = 3; + + // Build the initial conflict lists + Array edges { e1, e2, e3 }; + for (Edge *edge : edges) + edge->CalculateNormalAndCenter(mPositions.data()); + for (int idx = 0; idx < (int)mPositions.size(); ++idx) + if (idx != inIdx1 && idx != inIdx2 && idx != inIdx3) + AssignPointToEdge(idx, edges); + + JPH_IF_ENABLE_ASSERTS(ValidateEdges();) +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + DrawState(); +#endif + + // Add the remaining points to the hull + for (;;) + { + // Check if we've reached the max amount of vertices that are allowed + if (mNumEdges >= inMaxVertices) + { + result = EResult::MaxVerticesReached; + break; + } + + // Find the edge with the furthest point on it + Edge *edge_with_furthest_point = nullptr; + float furthest_dist_sq = 0.0f; + Edge *edge = mFirstEdge; + do + { + if (edge->mFurthestPointDistanceSq > furthest_dist_sq) + { + furthest_dist_sq = edge->mFurthestPointDistanceSq; + edge_with_furthest_point = edge; + } + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + // If there is none closer than our tolerance value, we're done + if (edge_with_furthest_point == nullptr || furthest_dist_sq < tolerance_sq) + break; + + // Take the furthest point + int furthest_point_idx = edge_with_furthest_point->mConflictList.back(); + edge_with_furthest_point->mConflictList.pop_back(); + Vec3 furthest_point = mPositions[furthest_point_idx]; + + // Find the horizon of edges that need to be removed + Edge *first_edge = edge_with_furthest_point; + do + { + Edge *prev = first_edge->mPrevEdge; + if (!prev->IsFacing(furthest_point)) + break; + first_edge = prev; + } while (first_edge != edge_with_furthest_point); + + Edge *last_edge = edge_with_furthest_point; + do + { + Edge *next = last_edge->mNextEdge; + if (!next->IsFacing(furthest_point)) + break; + last_edge = next; + } while (last_edge != edge_with_furthest_point); + + // Create new edges + e1 = new Edge(first_edge->mStartIdx); + e2 = new Edge(furthest_point_idx); + e1->mNextEdge = e2; + e1->mPrevEdge = first_edge->mPrevEdge; + e2->mPrevEdge = e1; + e2->mNextEdge = last_edge->mNextEdge; + e1->mPrevEdge->mNextEdge = e1; + e2->mNextEdge->mPrevEdge = e2; + mFirstEdge = e1; // We could delete mFirstEdge so just update it to the newly created edge + mNumEdges += 2; + + // Calculate normals + Array new_edges { e1, e2 }; + for (Edge *new_edge : new_edges) + new_edge->CalculateNormalAndCenter(mPositions.data()); + + // Delete the old edges + for (;;) + { + Edge *next = first_edge->mNextEdge; + + // Redistribute points in conflict list + for (int idx : first_edge->mConflictList) + AssignPointToEdge(idx, new_edges); + + // Delete the old edge + delete first_edge; + --mNumEdges; + + if (first_edge == last_edge) + break; + first_edge = next; + } + + JPH_IF_ENABLE_ASSERTS(ValidateEdges();) + #ifdef JPH_CONVEX_BUILDER_2D_DEBUG + DrawState(); + #endif + } + + // Convert the edge list to a list of indices + outEdges.reserve(mNumEdges); + Edge *edge = mFirstEdge; + do + { + outEdges.push_back(edge->mStartIdx); + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + return result; +} + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + +void ConvexHullBuilder2D::DrawState() +{ + int color_idx = 0; + + const Edge *edge = mFirstEdge; + do + { + const Edge *next = edge->mNextEdge; + + // Get unique color per edge + Color color = Color::sGetDistinctColor(color_idx++); + + // Draw edge and normal + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[edge->mStartIdx]), cDrawScale * (mOffset + mPositions[next->mStartIdx]), color, 0.1f); + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + edge->mCenter), cDrawScale * (mOffset + edge->mCenter) + edge->mNormal.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f); + + // Draw points that belong to this edge in the same color + for (int idx : edge->mConflictList) + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), color, 0.05f); + + edge = next; + } while (edge != mFirstEdge); + + mOffset += mDelta; +} + +#endif + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h new file mode 100644 index 000000000000..ff06a3403e0b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h @@ -0,0 +1,105 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +//#define JPH_CONVEX_BUILDER_2D_DEBUG + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder that tries to create 2D hulls as accurately as possible. Used for offline processing. +class JPH_EXPORT ConvexHullBuilder2D : public NonCopyable +{ +public: + using Positions = Array; + using Edges = Array; + + /// Constructor + /// @param inPositions Positions used to make the hull. Uses X and Y component of Vec3 only! + explicit ConvexHullBuilder2D(const Positions &inPositions); + + /// Destructor + ~ConvexHullBuilder2D(); + + /// Result enum that indicates how the hull got created + enum class EResult + { + Success, ///< Hull building finished successfully + MaxVerticesReached, ///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached + }; + + /// Takes all positions as provided by the constructor and use them to build a hull + /// Any points that are closer to the hull than inTolerance will be discarded + /// @param inIdx1 , inIdx2 , inIdx3 The indices to use as initial hull (in any order) + /// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit. + /// @param inTolerance Max distance that a point is allowed to be outside of the hull + /// @param outEdges On success this will contain the list of indices that form the hull (counter clockwise) + /// @return Status code that reports if the hull was created or not + EResult Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges); + +private: +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + + class Edge; + + /// Frees all edges + void FreeEdges(); + + /// Assigns a position to one of the supplied edges based on which edge is closest. + /// @param inPositionIdx Index of the position to add + /// @param inEdges List of edges to consider + void AssignPointToEdge(int inPositionIdx, const Array &inEdges) const; + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + /// Draw state of algorithm + void DrawState(); +#endif + +#ifdef JPH_ENABLE_ASSERTS + /// Validate that the edge structure is intact + void ValidateEdges() const; +#endif + + using ConflictList = Array; + + /// Linked list of edges + class Edge + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Edge(int inStartIdx) : mStartIdx(inStartIdx) { } + + /// Calculate the center of the edge and the edge normal + void CalculateNormalAndCenter(const Vec3 *inPositions); + + /// Check if this edge is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const { return mNormal.Dot(inPosition - mCenter) > 0.0f; } + + Vec3 mNormal; ///< Normal of the edge (not normalized) + Vec3 mCenter; ///< Center of the edge + ConflictList mConflictList; ///< Positions associated with this edge (that are closest to this edge). Last entry is the one furthest away from the edge, remainder is unsorted. + Edge * mPrevEdge = nullptr; ///< Previous edge in circular list + Edge * mNextEdge = nullptr; ///< Next edge in circular list + int mStartIdx; ///< Position index of start of this edge + float mFurthestPointDistanceSq = 0.0f; ///< Squared distance of furthest point from the conflict list to the edge + }; + + const Positions & mPositions; ///< List of positions (some of them are part of the hull) + Edge * mFirstEdge = nullptr; ///< First edge of the hull + int mNumEdges = 0; ///< Number of edges in hull + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + RVec3 mOffset; ///< Offset to use for state drawing + Vec3 mDelta; ///< Delta offset between next states +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h new file mode 100644 index 000000000000..3ba2c935db27 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h @@ -0,0 +1,188 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions to get the support point for a convex object +/// Structure that transforms a convex object (supports only uniform scaling) +template +struct TransformedConvexObject +{ + /// Create transformed convex object. + TransformedConvexObject(Mat44Arg inTransform, const ConvexObject &inObject) : + mTransform(inTransform), + mObject(inObject) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return mTransform * mObject.GetSupport(mTransform.Multiply3x3Transposed(inDirection)); + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + mObject.GetSupportingFace(mTransform.Multiply3x3Transposed(inDirection), outVertices); + + for (Vec3 &v : outVertices) + v = mTransform * v; + } + + Mat44 mTransform; + const ConvexObject & mObject; +}; + +/// Structure that adds a convex radius +template +struct AddConvexRadius +{ + AddConvexRadius(const ConvexObject &inObject, float inRadius) : + mObject(inObject), + mRadius(inRadius) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + float length = inDirection.Length(); + return length > 0.0f ? mObject.GetSupport(inDirection) + (mRadius / length) * inDirection : mObject.GetSupport(inDirection); + } + + const ConvexObject & mObject; + float mRadius; +}; + +/// Structure that performs a Minkowski difference A - B +template +struct MinkowskiDifference +{ + MinkowskiDifference(const ConvexObjectA &inObjectA, const ConvexObjectB &inObjectB) : + mObjectA(inObjectA), + mObjectB(inObjectB) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return mObjectA.GetSupport(inDirection) - mObjectB.GetSupport(-inDirection); + } + + const ConvexObjectA & mObjectA; + const ConvexObjectB & mObjectB; +}; + +/// Class that wraps a point so that it can be used with convex collision detection +struct PointConvexSupport +{ + /// Calculate the support vector for this convex shape. + Vec3 GetSupport([[maybe_unused]] Vec3Arg inDirection) const + { + return mPoint; + } + + Vec3 mPoint; +}; + +/// Class that wraps a triangle so that it can used with convex collision detection +struct TriangleConvexSupport +{ + /// Constructor + TriangleConvexSupport(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : + mV1(inV1), + mV2(inV2), + mV3(inV3) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + // Project vertices on inDirection + float d1 = mV1.Dot(inDirection); + float d2 = mV2.Dot(inDirection); + float d3 = mV3.Dot(inDirection); + + // Return vertex with biggest projection + if (d1 > d2) + { + if (d1 > d3) + return mV1; + else + return mV3; + } + else + { + if (d2 > d3) + return mV2; + else + return mV3; + } + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + outVertices.push_back(mV1); + outVertices.push_back(mV2); + outVertices.push_back(mV3); + } + + /// The three vertices of the triangle + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; +}; + +/// Class that wraps a polygon so that it can used with convex collision detection +template +struct PolygonConvexSupport +{ + /// Constructor + explicit PolygonConvexSupport(const VERTEX_ARRAY &inVertices) : + mVertices(inVertices) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + Vec3 support_point = mVertices[0]; + float best_dot = mVertices[0].Dot(inDirection); + + for (typename VERTEX_ARRAY::const_iterator v = mVertices.begin() + 1; v < mVertices.end(); ++v) + { + float dot = v->Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + support_point = *v; + } + } + + return support_point; + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY_ARG &outVertices) const + { + for (Vec3 v : mVertices) + outVertices.push_back(v); + } + + /// The vertices of the polygon + const VERTEX_ARRAY & mVertices; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h b/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h new file mode 100644 index 000000000000..15d61b068bd0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h @@ -0,0 +1,845 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Define to validate the integrity of the hull structure +//#define JPH_EPA_CONVEX_BUILDER_VALIDATE + +// Define to draw the building of the hull for debugging purposes +//#define JPH_EPA_CONVEX_BUILDER_DRAW + +#include +#include + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + #include + #include +#endif + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder specifically made for the EPA penetration depth calculation. It trades accuracy for speed and will simply abort of the hull forms defects due to numerical precision problems. +class EPAConvexHullBuilder : public NonCopyable +{ +private: +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + +public: + // Due to the Euler characteristic (https://en.wikipedia.org/wiki/Euler_characteristic) we know that Vertices - Edges + Faces = 2 + // In our case we only have triangles and they are always fully connected, so each edge is shared exactly between 2 faces: Edges = Faces * 3 / 2 + // Substituting: Vertices = Faces / 2 + 2 which is approximately Faces / 2. + static constexpr int cMaxTriangles = 256; ///< Max triangles in hull + static constexpr int cMaxPoints = cMaxTriangles / 2; ///< Max number of points in hull + + // Constants + static constexpr int cMaxEdgeLength = 128; ///< Max number of edges in FindEdge + static constexpr float cMinTriangleArea = 1.0e-10f; ///< Minimum area of a triangle before, if smaller than this it will not be added to the priority queue + static constexpr float cBarycentricEpsilon = 1.0e-3f; ///< Epsilon value used to determine if a point is in the interior of a triangle + + // Forward declare + class Triangle; + + /// Class that holds the information of an edge + class Edge + { + public: + /// Information about neighbouring triangle + Triangle * mNeighbourTriangle; ///< Triangle that neighbours this triangle + int mNeighbourEdge; ///< Index in mEdge that specifies edge that this Edge is connected to + + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + }; + + using Edges = StaticArray; + using NewTriangles = StaticArray; + + /// Class that holds the information of one triangle + class Triangle : public NonCopyable + { + public: + /// Constructor + inline Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions); + + /// Check if triangle is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(inPosition - mCentroid) > 0.0f; + } + + /// Check if triangle is facing the origin + inline bool IsFacingOrigin() const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(mCentroid) < 0.0f; + } + + /// Get the next edge of edge inIndex + inline const Edge & GetNextEdge(int inIndex) const + { + return mEdge[(inIndex + 1) % 3]; + } + + Edge mEdge[3]; ///< 3 edges of this triangle + Vec3 mNormal; ///< Normal of this triangle, length is 2 times area of triangle + Vec3 mCentroid; ///< Center of the triangle + float mClosestLenSq = FLT_MAX; ///< Closest distance^2 from origin to triangle + float mLambda[2]; ///< Barycentric coordinates of closest point to origin on triangle + bool mLambdaRelativeTo0; ///< How to calculate the closest point, true: y0 + l0 * (y1 - y0) + l1 * (y2 - y0), false: y1 + l0 * (y0 - y1) + l1 * (y2 - y1) + bool mClosestPointInterior = false; ///< Flag that indicates that the closest point from this triangle to the origin is an interior point + bool mRemoved = false; ///< Flag that indicates that triangle has been removed + bool mInQueue = false; ///< Flag that indicates that this triangle was placed in the sorted heap (stays true after it is popped because the triangle is freed by the main EPA algorithm loop) +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + int mIteration; ///< Iteration that this triangle was created +#endif + }; + + /// Factory that creates triangles in a fixed size buffer + class TriangleFactory : public NonCopyable + { + private: + /// Struct that stores both a triangle or a next pointer in case the triangle is unused + union alignas(Triangle) Block + { + uint8 mTriangle[sizeof(Triangle)]; + Block * mNextFree; + }; + + /// Storage for triangle data + Block mTriangles[cMaxTriangles]; ///< Storage for triangles + Block * mNextFree = nullptr; ///< List of free triangles + int mHighWatermark = 0; ///< High water mark for used triangles (if mNextFree == nullptr we can take one from here) + + public: + /// Return all triangles to the free pool + void Clear() + { + mNextFree = nullptr; + mHighWatermark = 0; + } + + /// Allocate a new triangle with 3 indexes + Triangle * CreateTriangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) + { + Triangle *t; + if (mNextFree != nullptr) + { + // Entry available from the free list + t = reinterpret_cast(&mNextFree->mTriangle); + mNextFree = mNextFree->mNextFree; + } + else + { + // Allocate from never used before triangle store + if (mHighWatermark >= cMaxTriangles) + return nullptr; // Buffer full + t = reinterpret_cast(&mTriangles[mHighWatermark].mTriangle); + ++mHighWatermark; + } + + // Call constructor + new (t) Triangle(inIdx0, inIdx1, inIdx2, inPositions); + + return t; + } + + /// Free a triangle + void FreeTriangle(Triangle *inT) + { + // Destruct triangle + inT->~Triangle(); +#ifdef JPH_DEBUG + memset(inT, 0xcd, sizeof(Triangle)); +#endif + + // Add triangle to the free list + Block *tu = reinterpret_cast(inT); + tu->mNextFree = mNextFree; + mNextFree = tu; + } + }; + + // Typedefs + using PointsBase = StaticArray; + using Triangles = StaticArray; + + /// Specialized points list that allows direct access to the size + class Points : public PointsBase + { + public: + size_type & GetSizeRef() + { + return mSize; + } + }; + + /// Specialized triangles list that keeps them sorted on closest distance to origin + class TriangleQueue : public Triangles + { + public: + /// Function to sort triangles on closest distance to origin + static bool sTriangleSorter(const Triangle *inT1, const Triangle *inT2) + { + return inT1->mClosestLenSq > inT2->mClosestLenSq; + } + + /// Add triangle to the list + void push_back(Triangle *inT) + { + // Add to base + Triangles::push_back(inT); + + // Mark in queue + inT->mInQueue = true; + + // Resort heap + BinaryHeapPush(begin(), end(), sTriangleSorter); + } + + /// Peek the next closest triangle without removing it + Triangle * PeekClosest() + { + return front(); + } + + /// Get next closest triangle + Triangle * PopClosest() + { + // Move closest to end + BinaryHeapPop(begin(), end(), sTriangleSorter); + + // Remove last triangle + Triangle *t = back(); + pop_back(); + return t; + } + }; + + /// Constructor + explicit EPAConvexHullBuilder(const Points &inPositions) : + mPositions(inPositions) + { +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + mIteration = 0; + mOffset = RVec3::sZero(); +#endif + } + + /// Initialize the hull with 3 points + void Initialize(int inIdx1, int inIdx2, int inIdx3) + { + // Release triangles + mFactory.Clear(); + + // Create triangles (back to back) + Triangle *t1 = CreateTriangle(inIdx1, inIdx2, inIdx3); + Triangle *t2 = CreateTriangle(inIdx1, inIdx3, inIdx2); + + // Link triangles edges + sLinkTriangle(t1, 0, t2, 2); + sLinkTriangle(t1, 1, t2, 1); + sLinkTriangle(t1, 2, t2, 0); + + // Always add both triangles to the priority queue + mTriangleQueue.push_back(t1); + mTriangleQueue.push_back(t2); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw current state + DrawState(); + + // Increment iteration counter + ++mIteration; +#endif + } + + /// Check if there's another triangle to process from the queue + bool HasNextTriangle() const + { + return !mTriangleQueue.empty(); + } + + /// Access to the next closest triangle to the origin (won't remove it from the queue). + Triangle * PeekClosestTriangleInQueue() + { + return mTriangleQueue.PeekClosest(); + } + + /// Access to the next closest triangle to the origin and remove it from the queue. + Triangle * PopClosestTriangleFromQueue() + { + return mTriangleQueue.PopClosest(); + } + + /// Find the triangle on which inPosition is the furthest to the front + /// Note this function works as long as all points added have been added with AddPoint(..., FLT_MAX). + Triangle * FindFacingTriangle(Vec3Arg inPosition, float &outBestDistSq) + { + Triangle *best = nullptr; + float best_dist_sq = 0.0f; + + for (Triangle *t : mTriangleQueue) + if (!t->mRemoved) + { + float dot = t->mNormal.Dot(inPosition - t->mCentroid); + if (dot > 0.0f) + { + float dist_sq = dot * dot / t->mNormal.LengthSq(); + if (dist_sq > best_dist_sq) + { + best = t; + best_dist_sq = dist_sq; + } + } + } + + outBestDistSq = best_dist_sq; + return best; + } + + /// Add a new point to the convex hull + bool AddPoint(Triangle *inFacingTriangle, int inIdx, float inClosestDistSq, NewTriangles &outTriangles) + { + // Get position + Vec3 pos = mPositions[inIdx]; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw new support point + DrawMarker(pos, Color::sYellow, 1.0f); +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + // Check if structure is intact + ValidateTriangles(); +#endif + + // Find edge of convex hull of triangles that are not facing the new vertex w + Edges edges; + if (!FindEdge(inFacingTriangle, pos, edges)) + return false; + + // Create new triangles + int num_edges = edges.size(); + for (int i = 0; i < num_edges; ++i) + { + // Create new triangle + Triangle *nt = CreateTriangle(edges[i].mStartIdx, edges[(i + 1) % num_edges].mStartIdx, inIdx); + if (nt == nullptr) + return false; + outTriangles.push_back(nt); + + // Check if we need to put this triangle in the priority queue + if ((nt->mClosestPointInterior && nt->mClosestLenSq < inClosestDistSq) // For the main algorithm + || nt->mClosestLenSq < 0.0f) // For when the origin is not inside the hull yet + mTriangleQueue.push_back(nt); + } + + // Link edges + for (int i = 0; i < num_edges; ++i) + { + sLinkTriangle(outTriangles[i], 0, edges[i].mNeighbourTriangle, edges[i].mNeighbourEdge); + sLinkTriangle(outTriangles[i], 1, outTriangles[(i + 1) % num_edges], 2); + } + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + // Check if structure is intact + ValidateTriangles(); +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw state of the hull + DrawState(); + + // Increment iteration counter + ++mIteration; +#endif + + return true; + } + + /// Free a triangle + void FreeTriangle(Triangle *inT) + { +#ifdef JPH_ENABLE_ASSERTS + // Make sure that this triangle is not connected + JPH_ASSERT(inT->mRemoved); + for (const Edge &e : inT->mEdge) + JPH_ASSERT(e.mNeighbourTriangle == nullptr); +#endif + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + // Remove from list of all triangles + Triangles::iterator i = std::find(mTriangles.begin(), mTriangles.end(), inT); + JPH_ASSERT(i != mTriangles.end()); + mTriangles.erase(i); +#endif + + mFactory.FreeTriangle(inT); + } + +private: + /// Create a new triangle + Triangle * CreateTriangle(int inIdx1, int inIdx2, int inIdx3) + { + // Call provider to create triangle + Triangle *t = mFactory.CreateTriangle(inIdx1, inIdx2, inIdx3, mPositions.data()); + if (t == nullptr) + return nullptr; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Remember iteration counter + t->mIteration = mIteration; +#endif + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + // Add to list of triangles for debugging purposes + mTriangles.push_back(t); +#endif + + return t; + } + + /// Link triangle edge to other triangle edge + static void sLinkTriangle(Triangle *inT1, int inEdge1, Triangle *inT2, int inEdge2) + { + JPH_ASSERT(inEdge1 >= 0 && inEdge1 < 3); + JPH_ASSERT(inEdge2 >= 0 && inEdge2 < 3); + Edge &e1 = inT1->mEdge[inEdge1]; + Edge &e2 = inT2->mEdge[inEdge2]; + + // Check not connected yet + JPH_ASSERT(e1.mNeighbourTriangle == nullptr); + JPH_ASSERT(e2.mNeighbourTriangle == nullptr); + + // Check vertices match + JPH_ASSERT(e1.mStartIdx == inT2->GetNextEdge(inEdge2).mStartIdx); + JPH_ASSERT(e2.mStartIdx == inT1->GetNextEdge(inEdge1).mStartIdx); + + // Link up + e1.mNeighbourTriangle = inT2; + e1.mNeighbourEdge = inEdge2; + e2.mNeighbourTriangle = inT1; + e2.mNeighbourEdge = inEdge1; + } + + /// Unlink this triangle + void UnlinkTriangle(Triangle *inT) + { + // Unlink from neighbours + for (int i = 0; i < 3; ++i) + { + Edge &edge = inT->mEdge[i]; + if (edge.mNeighbourTriangle != nullptr) + { + Edge &neighbour_edge = edge.mNeighbourTriangle->mEdge[edge.mNeighbourEdge]; + + // Validate that neighbour points to us + JPH_ASSERT(neighbour_edge.mNeighbourTriangle == inT); + JPH_ASSERT(neighbour_edge.mNeighbourEdge == i); + + // Unlink + neighbour_edge.mNeighbourTriangle = nullptr; + edge.mNeighbourTriangle = nullptr; + } + } + + // If this triangle is not in the priority queue, we can delete it now + if (!inT->mInQueue) + FreeTriangle(inT); + } + + /// Given one triangle that faces inVertex, find the edges of the triangles that are not facing inVertex. + /// Will flag all those triangles for removal. + bool FindEdge(Triangle *inFacingTriangle, Vec3Arg inVertex, Edges &outEdges) + { + // Assert that we were given an empty array + JPH_ASSERT(outEdges.empty()); + + // Should start with a facing triangle + JPH_ASSERT(inFacingTriangle->IsFacing(inVertex)); + + // Flag as removed + inFacingTriangle->mRemoved = true; + + // Instead of recursing, we build our own stack with the information we need + struct StackEntry + { + Triangle * mTriangle; + int mEdge; + int mIter; + }; + StackEntry stack[cMaxEdgeLength]; + int cur_stack_pos = 0; + + // Start with the triangle / edge provided + stack[0].mTriangle = inFacingTriangle; + stack[0].mEdge = 0; + stack[0].mIter = -1; // Start with edge 0 (is incremented below before use) + + // Next index that we expect to find, if we don't then there are 'islands' + int next_expected_start_idx = -1; + + for (;;) + { + StackEntry &cur_entry = stack[cur_stack_pos]; + + // Next iteration + if (++cur_entry.mIter >= 3) + { + // This triangle needs to be removed, unlink it now + UnlinkTriangle(cur_entry.mTriangle); + + // Pop from stack + if (--cur_stack_pos < 0) + break; + } + else + { + // Visit neighbour + Edge &e = cur_entry.mTriangle->mEdge[(cur_entry.mEdge + cur_entry.mIter) % 3]; + Triangle *n = e.mNeighbourTriangle; + if (n != nullptr && !n->mRemoved) + { + // Check if vertex is on the front side of this triangle + if (n->IsFacing(inVertex)) + { + // Vertex on front, this triangle needs to be removed + n->mRemoved = true; + + // Add element to the stack of elements to visit + cur_stack_pos++; + JPH_ASSERT(cur_stack_pos < cMaxEdgeLength); + StackEntry &new_entry = stack[cur_stack_pos]; + new_entry.mTriangle = n; + new_entry.mEdge = e.mNeighbourEdge; + new_entry.mIter = 0; // Is incremented before use, we don't need to test this edge again since we came from it + } + else + { + // Detect if edge doesn't connect to previous edge, if this happens we have found and 'island' which means + // the newly added point is so close to the triangles of the hull that we classified some (nearly) coplanar + // triangles as before and some behind the point. At this point we just abort adding the point because + // we've reached numerical precision. + // Note that we do not need to test if the first and last edge connect, since when there are islands + // there should be at least 2 disconnects. + if (e.mStartIdx != next_expected_start_idx && next_expected_start_idx != -1) + return false; + + // Next expected index is the start index of our neighbour's edge + next_expected_start_idx = n->mEdge[e.mNeighbourEdge].mStartIdx; + + // Vertex behind, keep edge + outEdges.push_back(e); + } + } + } + } + + // Assert that we have a fully connected loop + JPH_ASSERT(outEdges.empty() || outEdges[0].mStartIdx == next_expected_start_idx); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw edge of facing triangles + for (int i = 0; i < (int)outEdges.size(); ++i) + { + RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]); + DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mOffset + mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx]), Color::sYellow, 0.01f); + DebugRenderer::sInstance->DrawText3D(edge_start, ConvertToString(outEdges[i].mStartIdx), Color::sWhite); + } + + // Draw the state with the facing triangles removed + DrawState(); +#endif + + // When we start with two triangles facing away from each other and adding a point that is on the plane, + // sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list. + // In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration) + return outEdges.size() >= 3; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + /// Check consistency of 1 triangle + void ValidateTriangle(const Triangle *inT) const + { + if (inT->mRemoved) + { + // Validate that removed triangles are not connected to anything + for (const Edge &my_edge : inT->mEdge) + JPH_ASSERT(my_edge.mNeighbourTriangle == nullptr); + } + else + { + for (int i = 0; i < 3; ++i) + { + const Edge &my_edge = inT->mEdge[i]; + + // Assert that we have a neighbour + const Triangle *nb = my_edge.mNeighbourTriangle; + JPH_ASSERT(nb != nullptr); + + if (nb != nullptr) + { + // Assert that our neighbours edge points to us + const Edge &nb_edge = nb->mEdge[my_edge.mNeighbourEdge]; + JPH_ASSERT(nb_edge.mNeighbourTriangle == inT); + JPH_ASSERT(nb_edge.mNeighbourEdge == i); + + // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex + const Edge &nb_next_edge = nb->GetNextEdge(my_edge.mNeighbourEdge); + JPH_ASSERT(nb_next_edge.mStartIdx == my_edge.mStartIdx); + + // Assert that my next edge points to the same vertex as my neighbours vertex + const Edge &my_next_edge = inT->GetNextEdge(i); + JPH_ASSERT(my_next_edge.mStartIdx == nb_edge.mStartIdx); + } + } + } + } + + /// Check consistency of all triangles + void ValidateTriangles() const + { + for (const Triangle *t : mTriangles) + ValidateTriangle(t); + } +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW +public: + /// Draw state of algorithm + void DrawState() + { + // Draw origin + DebugRenderer::sInstance->DrawCoordinateSystem(RMat44::sTranslation(cDrawScale * mOffset), 1.0f); + + // Draw triangles + for (const Triangle *t : mTriangles) + if (!t->mRemoved) + { + // Calculate the triangle vertices + RVec3 p1 = cDrawScale * (mOffset + mPositions[t->mEdge[0].mStartIdx]); + RVec3 p2 = cDrawScale * (mOffset + mPositions[t->mEdge[1].mStartIdx]); + RVec3 p3 = cDrawScale * (mOffset + mPositions[t->mEdge[2].mStartIdx]); + + // Draw triangle + DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, Color::sGetDistinctColor(t->mIteration)); + DebugRenderer::sInstance->DrawWireTriangle(p1, p2, p3, Color::sGrey); + + // Draw normal + RVec3 centroid = cDrawScale * (mOffset + t->mCentroid); + float len = t->mNormal.Length(); + if (len > 0.0f) + DebugRenderer::sInstance->DrawArrow(centroid, centroid + t->mNormal / len, Color::sDarkGreen, 0.01f); + } + + // Determine max position + float min_x = FLT_MAX; + float max_x = -FLT_MAX; + for (Vec3 p : mPositions) + { + min_x = min(min_x, p.GetX()); + max_x = max(max_x, p.GetX()); + } + + // Offset to the right + mOffset += Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f); + } + + /// Draw a label to indicate the next stage in the algorithm + void DrawLabel(const string_view &inText) + { + DebugRenderer::sInstance->DrawText3D(cDrawScale * mOffset, inText, Color::sWhite, 0.1f * cDrawScale); + + mOffset += Vec3(5.0f, 0.0f, 0.0f); + } + + /// Draw geometry for debugging purposes + void DrawGeometry(const DebugRenderer::GeometryRef &inGeometry, ColorArg inColor) + { + RMat44 origin = RMat44::sScale(Vec3::sReplicate(cDrawScale)) * RMat44::sTranslation(mOffset); + DebugRenderer::sInstance->DrawGeometry(origin, inGeometry->mBounds.Transformed(origin), inGeometry->mBounds.GetExtent().LengthSq(), inColor, inGeometry); + + mOffset += Vec3(inGeometry->mBounds.GetSize().GetX(), 0, 0); + } + + /// Draw a triangle for debugging purposes + void DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor) + { + RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]); + for (const Edge &edge : inTriangle.mEdge) + { + RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]); + DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f); + prev = cur; + } + } + + /// Draw a marker for debugging purposes + void DrawMarker(Vec3Arg inPosition, ColorArg inColor, float inSize) + { + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + inPosition), inColor, inSize); + } + + /// Draw an arrow for debugging purposes + void DrawArrow(Vec3Arg inFrom, Vec3Arg inTo, ColorArg inColor, float inArrowSize) + { + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + inFrom), cDrawScale * (mOffset + inTo), inColor, inArrowSize); + } +#endif + +private: + TriangleFactory mFactory; ///< Factory to create new triangles and remove old ones + const Points & mPositions; ///< List of positions (some of them are part of the hull) + TriangleQueue mTriangleQueue; ///< List of triangles that are part of the hull that still need to be checked (if !mRemoved) + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + Triangles mTriangles; ///< The list of all triangles in this hull (for debug purposes) +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + int mIteration; ///< Number of iterations we've had so far (for debug purposes) + RVec3 mOffset; ///< Offset to use for state drawing +#endif +}; + +// The determinant that is calculated in the Triangle constructor is really sensitive +// to numerical round off, disable the fmadd instructions to maintain precision. +JPH_PRECISE_MATH_ON + +EPAConvexHullBuilder::Triangle::Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) +{ + // Fill in indexes + JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2); + mEdge[0].mStartIdx = inIdx0; + mEdge[1].mStartIdx = inIdx1; + mEdge[2].mStartIdx = inIdx2; + + // Clear links + mEdge[0].mNeighbourTriangle = nullptr; + mEdge[1].mNeighbourTriangle = nullptr; + mEdge[2].mNeighbourTriangle = nullptr; + + // Get vertex positions + Vec3 y0 = inPositions[inIdx0]; + Vec3 y1 = inPositions[inIdx1]; + Vec3 y2 = inPositions[inIdx2]; + + // Calculate centroid + mCentroid = (y0 + y1 + y2) / 3.0f; + + // Calculate edges + Vec3 y10 = y1 - y0; + Vec3 y20 = y2 - y0; + Vec3 y21 = y2 - y1; + + // The most accurate normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter. + float y20_dot_y20 = y20.Dot(y20); + float y21_dot_y21 = y21.Dot(y21); + if (y20_dot_y20 < y21_dot_y21) + { + // We select the edges y10 and y20 + mNormal = y10.Cross(y20); + + // Check if triangle is degenerate + float normal_len_sq = mNormal.LengthSq(); + if (normal_len_sq > cMinTriangleArea) + { + // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal| + // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates and then calculating the closest + // point based on those coordinates. Note that we preserve the sign of the distance to check on which side the origin is. + float c_dot_n = mCentroid.Dot(mNormal); + mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq; + + // Calculate closest point to origin using barycentric coordinates: + // + // v = y0 + l0 * (y1 - y0) + l1 * (y2 - y0) + // v . (y1 - y0) = 0 + // v . (y2 - y0) = 0 + // + // Written in matrix form: + // + // | y10.y10 y20.y10 | | l0 | = | -y0.y10 | + // | y10.y20 y20.y20 | | l1 | | -y0.y20 | + // + // (y10 = y1 - y0 etc.) + // + // Cramers rule to invert matrix: + float y10_dot_y10 = y10.LengthSq(); + float y10_dot_y20 = y10.Dot(y20); + float determinant = y10_dot_y10 * y20_dot_y20 - y10_dot_y20 * y10_dot_y20; + if (determinant > 0.0f) // If determinant == 0 then the system is linearly dependent and the triangle is degenerate, since y10.10 * y20.y20 > y10.y20^2 it should also be > 0 + { + float y0_dot_y10 = y0.Dot(y10); + float y0_dot_y20 = y0.Dot(y20); + float l0 = (y10_dot_y20 * y0_dot_y20 - y20_dot_y20 * y0_dot_y10) / determinant; + float l1 = (y10_dot_y20 * y0_dot_y10 - y10_dot_y10 * y0_dot_y20) / determinant; + mLambda[0] = l0; + mLambda[1] = l1; + mLambdaRelativeTo0 = true; + + // Check if closest point is interior to the triangle. For a convex hull which contains the origin each face must contain the origin, but because + // our faces are triangles, we can have multiple coplanar triangles and only 1 will have the origin as an interior point. We want to use this triangle + // to calculate the contact points because it gives the most accurate results, so we will only add these triangles to the priority queue. + if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon) + mClosestPointInterior = true; + } + } + } + else + { + // We select the edges y10 and y21 + mNormal = y10.Cross(y21); + + // Check if triangle is degenerate + float normal_len_sq = mNormal.LengthSq(); + if (normal_len_sq > cMinTriangleArea) + { + // Again calculate distance between triangle and origin + float c_dot_n = mCentroid.Dot(mNormal); + mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq; + + // Calculate closest point to origin using barycentric coordinates but this time using y1 as the reference vertex + // + // v = y1 + l0 * (y0 - y1) + l1 * (y2 - y1) + // v . (y0 - y1) = 0 + // v . (y2 - y1) = 0 + // + // Written in matrix form: + // + // | y10.y10 -y21.y10 | | l0 | = | y1.y10 | + // | -y10.y21 y21.y21 | | l1 | | -y1.y21 | + // + // Cramers rule to invert matrix: + float y10_dot_y10 = y10.LengthSq(); + float y10_dot_y21 = y10.Dot(y21); + float determinant = y10_dot_y10 * y21_dot_y21 - y10_dot_y21 * y10_dot_y21; + if (determinant > 0.0f) + { + float y1_dot_y10 = y1.Dot(y10); + float y1_dot_y21 = y1.Dot(y21); + float l0 = (y21_dot_y21 * y1_dot_y10 - y10_dot_y21 * y1_dot_y21) / determinant; + float l1 = (y10_dot_y21 * y1_dot_y10 - y10_dot_y10 * y1_dot_y21) / determinant; + mLambda[0] = l0; + mLambda[1] = l1; + mLambdaRelativeTo0 = false; + + // Again check if the closest point is inside the triangle + if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon) + mClosestPointInterior = true; + } + } + } +} + +JPH_PRECISE_MATH_OFF + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h new file mode 100644 index 000000000000..ffa0c39127a4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h @@ -0,0 +1,559 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +//#define JPH_EPA_PENETRATION_DEPTH_DEBUG + +JPH_NAMESPACE_BEGIN + +/// Implementation of Expanding Polytope Algorithm as described in: +/// +/// Proximity Queries and Penetration Depth Computation on 3D Game Objects - Gino van den Bergen +/// +/// The implementation of this algorithm does not completely follow the article, instead of splitting +/// triangles at each edge as in fig. 7 in the article, we build a convex hull (removing any triangles that +/// are facing the new point, thereby avoiding the problem of getting really oblong triangles as mentioned in +/// the article). +/// +/// The algorithm roughly works like: +/// +/// - Start with a simplex of the Minkowski sum (difference) of two objects that was calculated by GJK +/// - This simplex should contain the origin (or else GJK would have reported: no collision) +/// - In cases where the simplex consists of 1 - 3 points, find some extra support points (of the Minkowski sum) to get to at least 4 points +/// - Convert this into a convex hull with non-zero volume (which includes the origin) +/// - A: Calculate the closest point to the origin for all triangles of the hull and take the closest one +/// - Calculate a new support point (of the Minkowski sum) in this direction and add this point to the convex hull +/// - This will remove all faces that are facing the new point and will create new triangles to fill up the hole +/// - Loop to A until no closer point found +/// - The closest point indicates the position / direction of least penetration +class EPAPenetrationDepth +{ +private: + // Typedefs + static constexpr int cMaxPoints = EPAConvexHullBuilder::cMaxPoints; + static constexpr int cMaxPointsToIncludeOriginInHull = 32; + static_assert(cMaxPointsToIncludeOriginInHull < cMaxPoints); + + using Triangle = EPAConvexHullBuilder::Triangle; + using Points = EPAConvexHullBuilder::Points; + + /// The GJK algorithm, used to start the EPA algorithm + GJKClosestPoint mGJK; + +#ifdef JPH_ENABLE_ASSERTS + /// Tolerance as passed to the GJK algorithm, used for asserting. + float mGJKTolerance = 0.0f; +#endif // JPH_ENABLE_ASSERTS + + /// A list of support points for the EPA algorithm + class SupportPoints + { + public: + /// List of support points + Points mY; + Vec3 mP[cMaxPoints]; + Vec3 mQ[cMaxPoints]; + + /// Calculate and add new support point to the list of points + template + Vec3 Add(const A &inA, const B &inB, Vec3Arg inDirection, int &outIndex) + { + // Get support point of the minkowski sum A - B + Vec3 p = inA.GetSupport(inDirection); + Vec3 q = inB.GetSupport(-inDirection); + Vec3 w = p - q; + + // Store new point + outIndex = mY.size(); + mY.push_back(w); + mP[outIndex] = p; + mQ[outIndex] = q; + + return w; + } + }; + +public: + /// Return code for GetPenetrationDepthStepGJK + enum class EStatus + { + NotColliding, ///< Returned if the objects don't collide, in this case outPointA/outPointB are invalid + Colliding, ///< Returned if the objects penetrate + Indeterminate ///< Returned if the objects penetrate further than the convex radius. In this case you need to call GetPenetrationDepthStepEPA to get the actual penetration depth. + }; + + /// Calculates penetration depth between two objects, first step of two (the GJK step) + /// + /// @param inAExcludingConvexRadius Object A without convex radius. + /// @param inBExcludingConvexRadius Object B without convex radius. + /// @param inConvexRadiusA Convex radius for A. + /// @param inConvexRadiusB Convex radius for B. + /// @param ioV Pass in previously returned value or (1, 0, 0). On return this value is changed to direction to move B out of collision along the shortest path (magnitude is meaningless). + /// @param inTolerance Minimal distance before A and B are considered colliding. + /// @param outPointA Position on A that has the least amount of penetration. + /// @param outPointB Position on B that has the least amount of penetration. + /// Use |outPointB - outPointA| to get the distance of penetration. + template + EStatus GetPenetrationDepthStepGJK(const AE &inAExcludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, float inConvexRadiusB, float inTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) + { + JPH_PROFILE_FUNCTION(); + + JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inTolerance;) + + // Don't supply a zero ioV, we only want to get points on the hull of the Minkowsky sum and not internal points. + // + // Note that if the assert below triggers, it is very likely that you have a MeshShape that contains a degenerate triangle (e.g. a sliver). + // Go up a couple of levels in the call stack to see if we're indeed testing a triangle and if it is degenerate. + // If this is the case then fix the triangles you supply to the MeshShape. + JPH_ASSERT(!ioV.IsNearZero()); + + // Get closest points + float combined_radius = inConvexRadiusA + inConvexRadiusB; + float combined_radius_sq = combined_radius * combined_radius; + float closest_points_dist_sq = mGJK.GetClosestPoints(inAExcludingConvexRadius, inBExcludingConvexRadius, inTolerance, combined_radius_sq, ioV, outPointA, outPointB); + if (closest_points_dist_sq > combined_radius_sq) + { + // No collision + return EStatus::NotColliding; + } + if (closest_points_dist_sq > 0.0f) + { + // Collision within convex radius, adjust points for convex radius + float v_len = sqrt(closest_points_dist_sq); // GetClosestPoints function returns |ioV|^2 when return value < FLT_MAX + outPointA += ioV * (inConvexRadiusA / v_len); + outPointB -= ioV * (inConvexRadiusB / v_len); + return EStatus::Colliding; + } + + return EStatus::Indeterminate; + } + + /// Calculates penetration depth between two objects, second step (the EPA step) + /// + /// @param inAIncludingConvexRadius Object A with convex radius + /// @param inBIncludingConvexRadius Object B with convex radius + /// @param inTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON. + /// @param outV Direction to move B out of collision along the shortest path (magnitude is meaningless) + /// @param outPointA Position on A that has the least amount of penetration + /// @param outPointB Position on B that has the least amount of penetration + /// Use |outPointB - outPointA| to get the distance of penetration + /// + /// @return False if the objects don't collide, in this case outPointA/outPointB are invalid. + /// True if the objects penetrate + template + bool GetPenetrationDepthStepEPA(const AI &inAIncludingConvexRadius, const BI &inBIncludingConvexRadius, float inTolerance, Vec3 &outV, Vec3 &outPointA, Vec3 &outPointB) + { + JPH_PROFILE_FUNCTION(); + + // Check that the tolerance makes sense (smaller value than this will just result in needless iterations) + JPH_ASSERT(inTolerance >= FLT_EPSILON); + + // Fetch the simplex from GJK algorithm + SupportPoints support_points; + mGJK.GetClosestPointsSimplex(support_points.mY.data(), support_points.mP, support_points.mQ, support_points.mY.GetSizeRef()); + + // Fill up the amount of support points to 4 + switch (support_points.mY.size()) + { + case 1: + { + // 1 vertex, which must be at the origin, which is useless for our purpose + JPH_ASSERT(support_points.mY[0].IsNearZero(Square(mGJKTolerance))); + support_points.mY.pop_back(); + + // Add support points in 4 directions to form a tetrahedron around the origin + int p1, p2, p3, p4; + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, 1, 0), p1); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(-1, -1, -1), p2); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(1, -1, -1), p3); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, -1, 1), p4); + JPH_ASSERT(p1 == 0); + JPH_ASSERT(p2 == 1); + JPH_ASSERT(p3 == 2); + JPH_ASSERT(p4 == 3); + break; + } + + case 2: + { + // Two vertices, create 3 extra by taking perpendicular axis and rotating it around in 120 degree increments + Vec3 axis = (support_points.mY[1] - support_points.mY[0]).Normalized(); + Mat44 rotation = Mat44::sRotation(axis, DegreesToRadians(120.0f)); + Vec3 dir1 = axis.GetNormalizedPerpendicular(); + Vec3 dir2 = rotation * dir1; + Vec3 dir3 = rotation * dir2; + int p1, p2, p3; + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir1, p1); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir2, p2); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir3, p3); + JPH_ASSERT(p1 == 2); + JPH_ASSERT(p2 == 3); + JPH_ASSERT(p3 == 4); + break; + } + + case 3: + case 4: + // We already have enough points + break; + } + + // Create hull out of the initial points + JPH_ASSERT(support_points.mY.size() >= 3); + EPAConvexHullBuilder hull(support_points.mY); +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Build initial hull"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Init: num_points = %u", (uint)support_points.mY.size()); +#endif + hull.Initialize(0, 1, 2); + for (typename Points::size_type i = 3; i < support_points.mY.size(); ++i) + { + float dist_sq; + Triangle *t = hull.FindFacingTriangle(support_points.mY[i], dist_sq); + if (t != nullptr) + { + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!hull.AddPoint(t, i, FLT_MAX, new_triangles)) + { + // We can't recover from a failure to add a point to the hull because the old triangles have been unlinked already. + // Assume no collision. This can happen if the shapes touch in 1 point (or plane) in which case the hull is degenerate. + return false; + } + } + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Complete hull"); + + // Generate the hull of the Minkowski difference for visualization + MinkowskiDifference diff(inAIncludingConvexRadius, inBIncludingConvexRadius); + DebugRenderer::GeometryRef geometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); }); + hull.DrawGeometry(geometry, Color::sYellow); + + hull.DrawLabel("Ensure origin in hull"); +#endif + + // Loop until we are sure that the origin is inside the hull + for (;;) + { + // Get the next closest triangle + Triangle *t = hull.PeekClosestTriangleInQueue(); + + // Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap) + if (t->mRemoved) + { + hull.PopClosestTriangleFromQueue(); + + // If we run out of triangles, we couldn't include the origin in the hull so there must be very little penetration and we report no collision. + if (!hull.HasNextTriangle()) + return false; + + hull.FreeTriangle(t); + continue; + } + + // If the closest to the triangle is zero or positive, the origin is in the hull and we can proceed to the main algorithm + if (t->mClosestLenSq >= 0.0f) + break; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Next iteration"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("EncapsulateOrigin: verts = (%d, %d, %d), closest_dist_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)", + t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx, + t->mClosestLenSq, + t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(), + t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ()); +#endif + + // Remove the triangle from the queue before we start adding new ones (which may result in a new closest triangle at the front of the queue) + hull.PopClosestTriangleFromQueue(); + + // Add a support point to get the origin inside the hull + int new_index; + Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw the point that we're adding + hull.DrawMarker(w, Color::sRed, 1.0f); + hull.DrawWireTriangle(*t, Color::sRed); + hull.DrawState(); +#endif + + // Add the point to the hull, if we fail we terminate and report no collision + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!t->IsFacing(w) || !hull.AddPoint(t, new_index, FLT_MAX, new_triangles)) + return false; + + // The triangle is facing the support point "w" and can now be safely removed + JPH_ASSERT(t->mRemoved); + hull.FreeTriangle(t); + + // If we run out of triangles or points, we couldn't include the origin in the hull so there must be very little penetration and we report no collision. + if (!hull.HasNextTriangle() || support_points.mY.size() >= cMaxPointsToIncludeOriginInHull) + return false; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Main algorithm"); +#endif + + // Current closest distance to origin + float closest_dist_sq = FLT_MAX; + + // Remember last good triangle + Triangle *last = nullptr; + + // If we want to flip the penetration depth + bool flip_v_sign = false; + + // Loop until closest point found + do + { + // Get closest triangle to the origin + Triangle *t = hull.PopClosestTriangleFromQueue(); + + // Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap) + if (t->mRemoved) + { + hull.FreeTriangle(t); + continue; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Next iteration"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("FindClosest: verts = (%d, %d, %d), closest_len_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)", + t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx, + t->mClosestLenSq, + t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(), + t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ()); +#endif + // Check if next triangle is further away than closest point, we've found the closest point + if (t->mClosestLenSq >= closest_dist_sq) + break; + + // Replace last good with this triangle + if (last != nullptr) + hull.FreeTriangle(last); + last = t; + + // Add support point in direction of normal of the plane + // Note that the article uses the closest point between the origin and plane, but this always has the exact same direction as the normal (if the origin is behind the plane) + // and this way we do less calculations and lose less precision + int new_index; + Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index); + + // Project w onto the triangle normal + float dot = t->mNormal.Dot(w); + + // Check if we just found a separating axis. This can happen if the shape shrunk by convex radius and then expanded by + // convex radius is bigger then the original shape due to inaccuracies in the shrinking process. + if (dot < 0.0f) + return false; + + // Get the distance squared (along normal) to the support point + float dist_sq = Square(dot) / t->mNormal.LengthSq(); + +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("FindClosest: w = (%g, %g, %g), dot = %g, dist_sq = %g", + w.GetX(), w.GetY(), w.GetZ(), + dot, dist_sq); +#endif +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw the point that we're adding + hull.DrawMarker(w, Color::sPurple, 1.0f); + hull.DrawWireTriangle(*t, Color::sPurple); + hull.DrawState(); +#endif + + // If the error became small enough, we've converged + if (dist_sq - t->mClosestLenSq < t->mClosestLenSq * inTolerance) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Converged"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // Keep track of the minimum distance + closest_dist_sq = min(closest_dist_sq, dist_sq); + + // If the triangle thinks this point is not front facing, we've reached numerical precision and we're done + if (!t->IsFacing(w)) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Not facing triangle"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // Add point to hull + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!hull.AddPoint(t, new_index, closest_dist_sq, new_triangles)) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Could not add point"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // If the hull is starting to form defects then we're reaching numerical precision and we have to stop + bool has_defect = false; + for (const Triangle *nt : new_triangles) + if (nt->IsFacingOrigin()) + { + has_defect = true; + break; + } + if (has_defect) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Has defect"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + // When the hull has defects it is possible that the origin has been classified on the wrong side of the triangle + // so we do an additional check to see if the penetration in the -triangle normal direction is smaller than + // the penetration in the triangle normal direction. If so we must flip the sign of the penetration depth. + Vec3 w2 = inAIncludingConvexRadius.GetSupport(-t->mNormal) - inBIncludingConvexRadius.GetSupport(t->mNormal); + float dot2 = -t->mNormal.Dot(w2); + if (dot2 < dot) + flip_v_sign = true; + break; + } + } + while (hull.HasNextTriangle() && support_points.mY.size() < cMaxPoints); + + // Determine closest points, if last == null it means the hull was a plane so there's no penetration + if (last == nullptr) + return false; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Closest found"); + hull.DrawWireTriangle(*last, Color::sWhite); + hull.DrawArrow(last->mCentroid, last->mCentroid + last->mNormal.NormalizedOr(Vec3::sZero()), Color::sWhite, 0.1f); + hull.DrawState(); +#endif + + // Calculate penetration by getting the vector from the origin to the closest point on the triangle: + // distance = (centroid - origin) . normal / |normal|, closest = origin + distance * normal / |normal| + outV = (last->mCentroid.Dot(last->mNormal) / last->mNormal.LengthSq()) * last->mNormal; + + // If penetration is near zero, treat this as a non collision since we cannot find a good normal + if (outV.IsNearZero()) + return false; + + // Check if we have to flip the sign of the penetration depth + if (flip_v_sign) + outV = -outV; + + // Use the barycentric coordinates for the closest point to the origin to find the contact points on A and B + Vec3 p0 = support_points.mP[last->mEdge[0].mStartIdx]; + Vec3 p1 = support_points.mP[last->mEdge[1].mStartIdx]; + Vec3 p2 = support_points.mP[last->mEdge[2].mStartIdx]; + + Vec3 q0 = support_points.mQ[last->mEdge[0].mStartIdx]; + Vec3 q1 = support_points.mQ[last->mEdge[1].mStartIdx]; + Vec3 q2 = support_points.mQ[last->mEdge[2].mStartIdx]; + + if (last->mLambdaRelativeTo0) + { + // y0 was the reference vertex + outPointA = p0 + last->mLambda[0] * (p1 - p0) + last->mLambda[1] * (p2 - p0); + outPointB = q0 + last->mLambda[0] * (q1 - q0) + last->mLambda[1] * (q2 - q0); + } + else + { + // y1 was the reference vertex + outPointA = p1 + last->mLambda[0] * (p0 - p1) + last->mLambda[1] * (p2 - p1); + outPointB = q1 + last->mLambda[0] * (q0 - q1) + last->mLambda[1] * (q2 - q1); + } + + return true; + } + + /// This function combines the GJK and EPA steps and is provided as a convenience function. + /// Note: less performant since you're providing all support functions in one go + /// Note 2: You need to initialize ioV, see documentation at GetPenetrationDepthStepGJK! + template + bool GetPenetrationDepth(const AE &inAExcludingConvexRadius, const AI &inAIncludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, const BI &inBIncludingConvexRadius, float inConvexRadiusB, float inCollisionToleranceSq, float inPenetrationTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) + { + // Check result of collision detection + switch (GetPenetrationDepthStepGJK(inAExcludingConvexRadius, inConvexRadiusA, inBExcludingConvexRadius, inConvexRadiusB, inCollisionToleranceSq, ioV, outPointA, outPointB)) + { + case EPAPenetrationDepth::EStatus::Colliding: + return true; + + case EPAPenetrationDepth::EStatus::NotColliding: + return false; + + case EPAPenetrationDepth::EStatus::Indeterminate: + return GetPenetrationDepthStepEPA(inAIncludingConvexRadius, inBIncludingConvexRadius, inPenetrationTolerance, ioV, outPointA, outPointB); + } + + JPH_ASSERT(false); + return false; + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inCollisionTolerance The minimal distance between A and B before they are considered colliding + /// @param inPenetrationTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON. + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A. + /// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B. + /// @param inReturnDeepestPoint If the shapes are initially intersecting this determines if the EPA algorithm will run to find the deepest point + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// @param outPointA is the contact point on A + /// @param outPointB is the contact point on B + /// @param outContactNormal is either the contact normal when the objects are touching or the penetration axis when the objects are penetrating at the start of the sweep (pointing from A to B, length will not be 1) + /// + /// @return true if the a hit was found, in which case ioLambda, outPointA, outPointB and outSurfaceNormal are updated. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inCollisionTolerance, float inPenetrationTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, bool inReturnDeepestPoint, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outContactNormal) + { + JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inCollisionTolerance;) + + // First determine if there's a collision at all + if (!mGJK.CastShape(inStart, inDirection, inCollisionTolerance, inA, inB, inConvexRadiusA, inConvexRadiusB, ioLambda, outPointA, outPointB, outContactNormal)) + return false; + + // When our contact normal is too small, we don't have an accurate result + bool contact_normal_invalid = outContactNormal.IsNearZero(Square(inCollisionTolerance)); + + if (inReturnDeepestPoint + && ioLambda == 0.0f // Only when lambda = 0 we can have the bodies overlap + && (inConvexRadiusA + inConvexRadiusB == 0.0f // When no convex radius was provided we can never trust contact points at lambda = 0 + || contact_normal_invalid)) + { + // If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point + AddConvexRadius add_convex_a(inA, inConvexRadiusA); + AddConvexRadius add_convex_b(inB, inConvexRadiusB); + TransformedConvexObject> transformed_a(inStart, add_convex_a); + if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB)) + return false; + } + else if (contact_normal_invalid) + { + // If we weren't able to calculate a contact normal, use the cast direction instead + outContactNormal = inDirection; + } + + return true; + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h b/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h new file mode 100644 index 000000000000..bfa508faae86 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h @@ -0,0 +1,77 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Ellipse centered around the origin +/// @see https://en.wikipedia.org/wiki/Ellipse +class Ellipse +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct ellipse with radius A along the X-axis and B along the Y-axis + Ellipse(float inA, float inB) : mA(inA), mB(inB) { JPH_ASSERT(inA > 0.0f); JPH_ASSERT(inB > 0.0f); } + + /// Check if inPoint is inside the ellipse + bool IsInside(const Float2 &inPoint) const + { + return Square(inPoint.x / mA) + Square(inPoint.y / mB) <= 1.0f; + } + + /// Get the closest point on the ellipse to inPoint + /// Assumes inPoint is outside the ellipse + /// @see Rotation Joint Limits in Quaternion Space by Gino van den Bergen, section 10.1 in Game Engine Gems 3. + Float2 GetClosestPoint(const Float2 &inPoint) const + { + float a_sq = Square(mA); + float b_sq = Square(mB); + + // Equation of ellipse: f(x, y) = (x/a)^2 + (y/b)^2 - 1 = 0 [1] + // Normal on surface: (df/dx, df/dy) = (2 x / a^2, 2 y / b^2) + // Closest point (x', y') on ellipse to point (x, y): (x', y') + t (x / a^2, y / b^2) = (x, y) + // <=> (x', y') = (a^2 x / (t + a^2), b^2 y / (t + b^2)) + // Requiring point to be on ellipse (substituting into [1]): g(t) = (a x / (t + a^2))^2 + (b y / (t + b^2))^2 - 1 = 0 + + // Newton raphson iteration, starting at t = 0 + float t = 0.0f; + for (;;) + { + // Calculate g(t) + float t_plus_a_sq = t + a_sq; + float t_plus_b_sq = t + b_sq; + float gt = Square(mA * inPoint.x / t_plus_a_sq) + Square(mB * inPoint.y / t_plus_b_sq) - 1.0f; + + // Check if g(t) it is close enough to zero + if (abs(gt) < 1.0e-6f) + return Float2(a_sq * inPoint.x / t_plus_a_sq, b_sq * inPoint.y / t_plus_b_sq); + + // Get derivative dg/dt = g'(t) = -2 (b^2 y^2 / (t + b^2)^3 + a^2 x^2 / (t + a^2)^3) + float gt_accent = -2.0f * + (a_sq * Square(inPoint.x) / Cubed(t_plus_a_sq) + + b_sq * Square(inPoint.y) / Cubed(t_plus_b_sq)); + + // Calculate t for next iteration: tn+1 = tn - g(t) / g'(t) + float tn = t - gt / gt_accent; + t = tn; + } + } + + /// Get normal at point inPoint (non-normalized vector) + Float2 GetNormal(const Float2 &inPoint) const + { + // Calculated by [d/dx f(x, y), d/dy f(x, y)], where f(x, y) is the ellipse equation from above + return Float2(inPoint.x / Square(mA), inPoint.y / Square(mB)); + } + +private: + float mA; ///< Radius along X-axis + float mB; ///< Radius along Y-axis +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h b/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h new file mode 100644 index 000000000000..e5f49b65a0be --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h @@ -0,0 +1,946 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +//#define JPH_GJK_DEBUG +#ifdef JPH_GJK_DEBUG + #include + #include +#endif + +JPH_NAMESPACE_BEGIN + +/// Convex vs convex collision detection +/// Based on: A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen +class GJKClosestPoint : public NonCopyable +{ +private: + /// Get new closest point to origin given simplex mY of mNumPoints points + /// + /// @param inPrevVLenSq Length of |outV|^2 from the previous iteration, used as a maximum value when selecting a new closest point. + /// @param outV Closest point + /// @param outVLenSq |outV|^2 + /// @param outSet Set of points that form the new simplex closest to the origin (bit 1 = mY[0], bit 2 = mY[1], ...) + /// + /// If LastPointPartOfClosestFeature is true then the last point added will be assumed to be part of the closest feature and the function will do less work. + /// + /// @return True if new closest point was found. + /// False if the function failed, in this case the output variables are not modified + template + bool GetClosest(float inPrevVLenSq, Vec3 &outV, float &outVLenSq, uint32 &outSet) const + { +#ifdef JPH_GJK_DEBUG + for (int i = 0; i < mNumPoints; ++i) + Trace("y[%d] = [%s], |y[%d]| = %g", i, ConvertToString(mY[i]).c_str(), i, (double)mY[i].Length()); +#endif + + uint32 set; + Vec3 v; + + switch (mNumPoints) + { + case 1: + // Single point + set = 0b0001; + v = mY[0]; + break; + + case 2: + // Line segment + v = ClosestPoint::GetClosestPointOnLine(mY[0], mY[1], set); + break; + + case 3: + // Triangle + v = ClosestPoint::GetClosestPointOnTriangle(mY[0], mY[1], mY[2], set); + break; + + case 4: + // Tetrahedron + v = ClosestPoint::GetClosestPointOnTetrahedron(mY[0], mY[1], mY[2], mY[3], set); + break; + + default: + JPH_ASSERT(false); + return false; + } + +#ifdef JPH_GJK_DEBUG + Trace("GetClosest: set = 0b%s, v = [%s], |v| = %g", NibbleToBinary(set), ConvertToString(v).c_str(), (double)v.Length()); +#endif + + float v_len_sq = v.LengthSq(); + if (v_len_sq < inPrevVLenSq) // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false + { + // Return closest point + outV = v; + outVLenSq = v_len_sq; + outSet = set; + return true; + } + + // No better match found +#ifdef JPH_GJK_DEBUG + Trace("New closer point is further away, failed to converge"); +#endif + return false; + } + + // Get max(|Y_0|^2 .. |Y_n|^2) + float GetMaxYLengthSq() const + { + float y_len_sq = mY[0].LengthSq(); + for (int i = 1; i < mNumPoints; ++i) + y_len_sq = max(y_len_sq, mY[i].LengthSq()); + return y_len_sq; + } + + // Remove points that are not in the set, only updates mY + void UpdatePointSetY(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mY[num_points] = mY[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Remove points that are not in the set, only updates mP + void UpdatePointSetP(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mP[num_points] = mP[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Remove points that are not in the set, only updates mP and mQ + void UpdatePointSetPQ(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mP[num_points] = mP[i]; + mQ[num_points] = mQ[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Remove points that are not in the set, updates mY, mP and mQ + void UpdatePointSetYPQ(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mY[num_points] = mY[i]; + mP[num_points] = mP[i]; + mQ[num_points] = mQ[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Calculate closest points on A and B + void CalculatePointAAndB(Vec3 &outPointA, Vec3 &outPointB) const + { + switch (mNumPoints) + { + case 1: + outPointA = mP[0]; + outPointB = mQ[0]; + break; + + case 2: + { + float u, v; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], u, v); + outPointA = u * mP[0] + v * mP[1]; + outPointB = u * mQ[0] + v * mQ[1]; + } + break; + + case 3: + { + float u, v, w; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], u, v, w); + outPointA = u * mP[0] + v * mP[1] + w * mP[2]; + outPointB = u * mQ[0] + v * mQ[1] + w * mQ[2]; + } + break; + + case 4: + #ifdef JPH_DEBUG + memset(&outPointA, 0xcd, sizeof(outPointA)); + memset(&outPointB, 0xcd, sizeof(outPointB)); + #endif + break; + } + } + +public: + /// Test if inA and inB intersect + /// + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inTolerance Minimal distance between objects when the objects are considered to be colliding + /// @param ioV is used as initial separating axis (provide a zero vector if you don't know yet) + /// + /// @return True if they intersect (in which case ioV = (0, 0, 0)). + /// False if they don't intersect in which case ioV is a separating axis in the direction from A to B (magnitude is meaningless) + template + bool Intersects(const A &inA, const B &inB, float inTolerance, Vec3 &ioV) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + +#ifdef JPH_GJK_DEBUG + for (int i = 0; i < 4; ++i) + mY[i] = Vec3::sZero(); +#endif + + // Previous length^2 of v + float prev_v_len_sq = FLT_MAX; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints); +#endif + + // Get support points for shape A and B in the direction of v + Vec3 p = inA.GetSupport(ioV); + Vec3 q = inB.GetSupport(-ioV); + + // Get support point of the minkowski sum A - B of v + Vec3 w = p - q; + + // If the support point sA-B(v) is in the opposite direction as v, then we have found a separating axis and there is no intersection + if (ioV.Dot(w) < 0.0f) + { + // Separating axis found +#ifdef JPH_GJK_DEBUG + Trace("Separating axis"); +#endif + return false; + } + + // Store the point for later use + mY[mNumPoints] = w; + ++mNumPoints; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + // Determine the new closest point + float v_len_sq; // Length^2 of v + uint32 set; // Set of points that form the new simplex + if (!GetClosest(prev_v_len_sq, ioV, v_len_sq, set)) + return false; + + // If there are 4 points, the origin is inside the tetrahedron and we're done + if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // If v is very close to zero, we consider this a collision + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance zero"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // If v is very small compared to the length of y, we also consider this a collision + if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq()) + { +#ifdef JPH_GJK_DEBUG + Trace("Machine precision reached"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin + // Note: This must be done before terminating as converged since the separating axis is -v + ioV = -ioV; + + // If the squared length of v is not changing enough, we've converged and there is no collision + JPH_ASSERT(prev_v_len_sq >= v_len_sq); + if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq) + { + // v is a separating axis +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + return false; + } + prev_v_len_sq = v_len_sq; + + // Update the points of the simplex + UpdatePointSetY(set); + } + } + + /// Get closest points between inA and inB + /// + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inTolerance The minimal distance between A and B before the objects are considered colliding and processing is terminated. + /// @param inMaxDistSq The maximum squared distance between A and B before the objects are considered infinitely far away and processing is terminated. + /// @param ioV Initial guess for the separating axis. Start with any non-zero vector if you don't know. + /// If return value is 0, ioV = (0, 0, 0). + /// If the return value is bigger than 0 but smaller than FLT_MAX, ioV will be the separating axis in the direction from A to B and its length the squared distance between A and B. + /// If the return value is FLT_MAX, ioV will be the separating axis in the direction from A to B and the magnitude of the vector is meaningless. + /// @param outPointA , outPointB + /// If the return value is 0 the points are invalid. + /// If the return value is bigger than 0 but smaller than FLT_MAX these will contain the closest point on A and B. + /// If the return value is FLT_MAX the points are invalid. + /// + /// @return The squared distance between A and B or FLT_MAX when they are further away than inMaxDistSq. + template + float GetClosestPoints(const A &inA, const B &inB, float inTolerance, float inMaxDistSq, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + +#ifdef JPH_GJK_DEBUG + // Generate the hull of the Minkowski difference for visualization + MinkowskiDifference diff(inA, inB); + mGeometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); }); + + for (int i = 0; i < 4; ++i) + { + mY[i] = Vec3::sZero(); + mP[i] = Vec3::sZero(); + mQ[i] = Vec3::sZero(); + } +#endif + + // Length^2 of v + float v_len_sq = ioV.LengthSq(); + + // Previous length^2 of v + float prev_v_len_sq = FLT_MAX; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints); +#endif + + // Get support points for shape A and B in the direction of v + Vec3 p = inA.GetSupport(ioV); + Vec3 q = inB.GetSupport(-ioV); + + // Get support point of the minkowski sum A - B of v + Vec3 w = p - q; + + float dot = ioV.Dot(w); + +#ifdef JPH_GJK_DEBUG + // Draw -ioV to show the closest point to the origin from the previous simplex + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f); + + // Draw ioV to show where we're probing next + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + ioV, Color::sCyan, 0.05f); + + // Draw w, the support point + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + w, Color::sGreen, 0.05f); + DebugRenderer::sInstance->DrawMarker(mOffset + w, Color::sGreen, 1.0f); + + // Draw the simplex and the Minkowski difference around it + DrawState(); +#endif + + // Test if we have a separation of more than inMaxDistSq, in which case we terminate early + if (dot < 0.0f && dot * dot > v_len_sq * inMaxDistSq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance bigger than max"); +#endif +#ifdef JPH_DEBUG + memset(&outPointA, 0xcd, sizeof(outPointA)); + memset(&outPointB, 0xcd, sizeof(outPointB)); +#endif + return FLT_MAX; + } + + // Store the point for later use + mY[mNumPoints] = w; + mP[mNumPoints] = p; + mQ[mNumPoints] = q; + ++mNumPoints; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + uint32 set; + if (!GetClosest(prev_v_len_sq, ioV, v_len_sq, set)) + { + --mNumPoints; // Undo add last point + break; + } + + // If there are 4 points, the origin is inside the tetrahedron and we're done + if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // Update the points of the simplex + UpdatePointSetYPQ(set); + + // If v is very close to zero, we consider this a collision + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance zero"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // If v is very small compared to the length of y, we also consider this a collision +#ifdef JPH_GJK_DEBUG + Trace("Check v small compared to y: %g <= %g", (double)v_len_sq, (double)(FLT_EPSILON * GetMaxYLengthSq())); +#endif + if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq()) + { +#ifdef JPH_GJK_DEBUG + Trace("Machine precision reached"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin + // Note: This must be done before terminating as converged since the separating axis is -v + ioV = -ioV; + + // If the squared length of v is not changing enough, we've converged and there is no collision +#ifdef JPH_GJK_DEBUG + Trace("Check v not changing enough: %g <= %g", (double)(prev_v_len_sq - v_len_sq), (double)(FLT_EPSILON * prev_v_len_sq)); +#endif + JPH_ASSERT(prev_v_len_sq >= v_len_sq); + if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq) + { + // v is a separating axis +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + prev_v_len_sq = v_len_sq; + } + + // Get the closest points + CalculatePointAAndB(outPointA, outPointB); + +#ifdef JPH_GJK_DEBUG + Trace("Return: v = [%s], |v| = %g", ConvertToString(ioV).c_str(), (double)ioV.Length()); + + // Draw -ioV to show the closest point to the origin from the previous simplex + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f); + + // Draw the closest points + DebugRenderer::sInstance->DrawMarker(mOffset + outPointA, Color::sGreen, 1.0f); + DebugRenderer::sInstance->DrawMarker(mOffset + outPointB, Color::sPurple, 1.0f); + + // Draw the simplex and the Minkowski difference around it + DrawState(); +#endif + + JPH_ASSERT(ioV.LengthSq() == v_len_sq); + return v_len_sq; + } + + /// Get the resulting simplex after the GetClosestPoints algorithm finishes. + /// If it returned a squared distance of 0, the origin will be contained in the simplex. + void GetClosestPointsSimplex(Vec3 *outY, Vec3 *outP, Vec3 *outQ, uint &outNumPoints) const + { + uint size = sizeof(Vec3) * mNumPoints; + memcpy(outY, mY, size); + memcpy(outP, mP, size); + memcpy(outQ, mQ, size); + outNumPoints = mNumPoints; + } + + /// Test if a ray inRayOrigin + lambda * inRayDirection for lambda e [0, ioLambda> intersects inA + /// + /// Code based upon: Ray Casting against General Convex Objects with Application to Continuous Collision Detection - Gino van den Bergen + /// + /// @param inRayOrigin Origin of the ray + /// @param inRayDirection Direction of the ray (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between the ray and A before it is considered colliding + /// @param inA A convex object that has the GetSupport(Vec3) function + /// @param ioLambda The max fraction along the ray, on output updated with the actual collision fraction. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda. + template + bool CastRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inTolerance, const A &inA, float &ioLambda) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + + float lambda = 0.0f; + Vec3 x = inRayOrigin; + Vec3 v = x - inA.GetSupport(Vec3::sZero()); + float v_len_sq = FLT_MAX; + bool allow_restart = false; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints); +#endif + + // Get new support point + Vec3 p = inA.GetSupport(v); + Vec3 w = x - p; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + float v_dot_w = v.Dot(w); +#ifdef JPH_GJK_DEBUG + Trace("v . w = %g", (double)v_dot_w); +#endif + if (v_dot_w > 0.0f) + { + // If ray and normal are in the same direction, we've passed A and there's no collision + float v_dot_r = v.Dot(inRayDirection); +#ifdef JPH_GJK_DEBUG + Trace("v . r = %g", (double)v_dot_r); +#endif + if (v_dot_r >= 0.0f) + return false; + + // Update the lower bound for lambda + float delta = v_dot_w / v_dot_r; + float old_lambda = lambda; + lambda -= delta; +#ifdef JPH_GJK_DEBUG + Trace("lambda = %g, delta = %g", (double)lambda, (double)delta); +#endif + + // If lambda didn't change, we cannot converge any further and we assume a hit + if (old_lambda == lambda) + break; + + // If lambda is bigger or equal than max, we don't have a hit + if (lambda >= ioLambda) + return false; + + // Update x to new closest point on the ray + x = inRayOrigin + lambda * inRayDirection; + + // We've shifted x, so reset v_len_sq so that it is not used as early out for GetClosest + v_len_sq = FLT_MAX; + + // We allow rebuilding the simplex once after x changes because the simplex was built + // for another x and numerical round off builds up as you keep adding points to an + // existing simplex + allow_restart = true; + } + + // Add p to set P: P = P U {p} + mP[mNumPoints] = p; + ++mNumPoints; + + // Calculate Y = {x} - P + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - mP[i]; + + // Determine the new closest point from Y to origin + uint32 set; // Set of points that form the new simplex + if (!GetClosest(v_len_sq, v, v_len_sq, set)) + { +#ifdef JPH_GJK_DEBUG + Trace("Failed to converge"); +#endif + + // Only allow 1 restart, if we still can't get a closest point + // we're so close that we return this as a hit + if (!allow_restart) + break; + + // If we fail to converge, we start again with the last point as simplex +#ifdef JPH_GJK_DEBUG + Trace("Restarting"); +#endif + allow_restart = false; + mP[0] = p; + mNumPoints = 1; + v = x - p; + v_len_sq = FLT_MAX; + continue; + } + else if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + + // We're inside the tetrahedron, we have a hit (verify that length of v is 0) + JPH_ASSERT(v_len_sq == 0.0f); + break; + } + + // Update the points P to form the new simplex + // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration + UpdatePointSetP(set); + + // Check if x is close enough to inA + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + } + + // Store hit fraction + ioLambda = lambda; + return true; + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between A and B before they are considered colliding + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda) + { + // Transform the shape to be cast to the starting position + TransformedConvexObject transformed_a(inStart, inA); + + // Calculate the minkowski difference inB - inA + // inA is moving, so we need to add the back side of inB to the front side of inA + MinkowskiDifference difference(inB, transformed_a); + + // Do a raycast against the Minkowski difference + return CastRay(Vec3::sZero(), inDirection, inTolerance, difference, ioLambda); + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between A and B before they are considered colliding + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A. + /// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B. + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// @param outPointA is the contact point on A (if outSeparatingAxis is near zero, this may not be not the deepest point) + /// @param outPointB is the contact point on B (if outSeparatingAxis is near zero, this may not be not the deepest point) + /// @param outSeparatingAxis On return this will contain a vector that points from A to B along the smallest distance of separation. + /// The length of this vector indicates the separation of A and B without their convex radius. + /// If it is near zero, the direction may not be accurate as the bodies may overlap when lambda = 0. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda and outPoint and outSeparatingAxis are valid. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outSeparatingAxis) + { + float tolerance_sq = Square(inTolerance); + + // Calculate how close A and B (without their convex radius) need to be to each other in order for us to consider this a collision + float sum_convex_radius = inConvexRadiusA + inConvexRadiusB; + + // Transform the shape to be cast to the starting position + TransformedConvexObject transformed_a(inStart, inA); + + // Reset state + mNumPoints = 0; + + float lambda = 0.0f; + Vec3 x = Vec3::sZero(); // Since A is already transformed we can start the cast from zero + Vec3 v = -inB.GetSupport(Vec3::sZero()) + transformed_a.GetSupport(Vec3::sZero()); // See CastRay: v = x - inA.GetSupport(Vec3::sZero()) where inA is the Minkowski difference inB - transformed_a (see CastShape above) and x is zero + float v_len_sq = FLT_MAX; + bool allow_restart = false; + + // Keeps track of separating axis of the previous iteration. + // Initialized at zero as we don't know if our first v is actually a separating axis. + Vec3 prev_v = Vec3::sZero(); + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints); +#endif + + // Calculate the minkowski difference inB - inA + // inA is moving, so we need to add the back side of inB to the front side of inA + // Keep the support points on A and B separate so that in the end we can calculate a contact point + Vec3 p = transformed_a.GetSupport(-v); + Vec3 q = inB.GetSupport(v); + Vec3 w = x - (q - p); + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + // Difference from article to this code: + // We did not include the convex radius in p and q in order to be able to calculate a good separating axis at the end of the algorithm. + // However when moving forward along inDirection we do need to take this into account so that we keep A and B separated by the sum of their convex radii. + // From p we have to subtract: inConvexRadiusA * v / |v| + // To q we have to add: inConvexRadiusB * v / |v| + // This means that to w we have to add: -(inConvexRadiusA + inConvexRadiusB) * v / |v| + // So to v . w we have to add: v . (-(inConvexRadiusA + inConvexRadiusB) * v / |v|) = -(inConvexRadiusA + inConvexRadiusB) * |v| + float v_dot_w = v.Dot(w) - sum_convex_radius * v.Length(); +#ifdef JPH_GJK_DEBUG + Trace("v . w = %g", (double)v_dot_w); +#endif + if (v_dot_w > 0.0f) + { + // If ray and normal are in the same direction, we've passed A and there's no collision + float v_dot_r = v.Dot(inDirection); +#ifdef JPH_GJK_DEBUG + Trace("v . r = %g", (double)v_dot_r); +#endif + if (v_dot_r >= 0.0f) + return false; + + // Update the lower bound for lambda + float delta = v_dot_w / v_dot_r; + float old_lambda = lambda; + lambda -= delta; +#ifdef JPH_GJK_DEBUG + Trace("lambda = %g, delta = %g", (double)lambda, (double)delta); +#endif + + // If lambda didn't change, we cannot converge any further and we assume a hit + if (old_lambda == lambda) + break; + + // If lambda is bigger or equal than max, we don't have a hit + if (lambda >= ioLambda) + return false; + + // Update x to new closest point on the ray + x = lambda * inDirection; + + // We've shifted x, so reset v_len_sq so that it is not used as early out when GetClosest returns false + v_len_sq = FLT_MAX; + + // Now that we've moved, we know that A and B are not intersecting at lambda = 0, so we can update our tolerance to stop iterating + // as soon as A and B are inConvexRadiusA + inConvexRadiusB apart + tolerance_sq = Square(inTolerance + sum_convex_radius); + + // We allow rebuilding the simplex once after x changes because the simplex was built + // for another x and numerical round off builds up as you keep adding points to an + // existing simplex + allow_restart = true; + } + + // Add p to set P, q to set Q: P = P U {p}, Q = Q U {q} + mP[mNumPoints] = p; + mQ[mNumPoints] = q; + ++mNumPoints; + + // Calculate Y = {x} - (Q - P) + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - (mQ[i] - mP[i]); + + // Determine the new closest point from Y to origin + uint32 set; // Set of points that form the new simplex + if (!GetClosest(v_len_sq, v, v_len_sq, set)) + { +#ifdef JPH_GJK_DEBUG + Trace("Failed to converge"); +#endif + + // Only allow 1 restart, if we still can't get a closest point + // we're so close that we return this as a hit + if (!allow_restart) + break; + + // If we fail to converge, we start again with the last point as simplex +#ifdef JPH_GJK_DEBUG + Trace("Restarting"); +#endif + allow_restart = false; + mP[0] = p; + mQ[0] = q; + mNumPoints = 1; + v = x - q; + v_len_sq = FLT_MAX; + continue; + } + else if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + + // We're inside the tetrahedron, we have a hit (verify that length of v is 0) + JPH_ASSERT(v_len_sq == 0.0f); + break; + } + + // Update the points P and Q to form the new simplex + // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration + UpdatePointSetPQ(set); + + // Check if A and B are touching according to our tolerance + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + + // Store our v to return as separating axis + prev_v = v; + } + + // Calculate Y = {x} - (Q - P) again so we can calculate the contact points + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - (mQ[i] - mP[i]); + + // Calculate the offset we need to apply to A and B to correct for the convex radius + Vec3 normalized_v = v.NormalizedOr(Vec3::sZero()); + Vec3 convex_radius_a = inConvexRadiusA * normalized_v; + Vec3 convex_radius_b = inConvexRadiusB * normalized_v; + + // Get the contact point + // Note that A and B will coincide when lambda > 0. In this case we calculate only B as it is more accurate as it contains less terms. + switch (mNumPoints) + { + case 1: + outPointB = mQ[0] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : mP[0] - convex_radius_a; + break; + + case 2: + { + float bu, bv; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], bu, bv); + outPointB = bu * mQ[0] + bv * mQ[1] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] - convex_radius_a; + } + break; + + case 3: + case 4: // A full simplex, we can't properly determine a contact point! As contact point we take the closest point of the previous iteration. + { + float bu, bv, bw; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], bu, bv, bw); + outPointB = bu * mQ[0] + bv * mQ[1] + bw * mQ[2] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] + bw * mP[2] - convex_radius_a; + } + break; + } + + // Store separating axis, in case we have a convex radius we can just return v, + // otherwise v will be very small and we resort to returning previous v as an approximation. + outSeparatingAxis = sum_convex_radius > 0.0f? -v : -prev_v; + + // Store hit fraction + ioLambda = lambda; + return true; + } + +private: +#ifdef JPH_GJK_DEBUG + /// Draw state of algorithm + void DrawState() + { + RMat44 origin = RMat44::sTranslation(mOffset); + + // Draw origin + DebugRenderer::sInstance->DrawCoordinateSystem(origin, 1.0f); + + // Draw the hull + DebugRenderer::sInstance->DrawGeometry(origin, mGeometry->mBounds.Transformed(origin), mGeometry->mBounds.GetExtent().LengthSq(), Color::sYellow, mGeometry); + + // Draw Y + for (int i = 0; i < mNumPoints; ++i) + { + // Draw support point + RVec3 y_i = origin * mY[i]; + DebugRenderer::sInstance->DrawMarker(y_i, Color::sRed, 1.0f); + for (int j = i + 1; j < mNumPoints; ++j) + { + // Draw edge + RVec3 y_j = origin * mY[j]; + DebugRenderer::sInstance->DrawLine(y_i, y_j, Color::sRed); + for (int k = j + 1; k < mNumPoints; ++k) + { + // Make sure triangle faces the origin + RVec3 y_k = origin * mY[k]; + RVec3 center = (y_i + y_j + y_k) / Real(3); + RVec3 normal = (y_j - y_i).Cross(y_k - y_i); + if (normal.Dot(center) < Real(0)) + DebugRenderer::sInstance->DrawTriangle(y_i, y_j, y_k, Color::sLightGrey); + else + DebugRenderer::sInstance->DrawTriangle(y_i, y_k, y_j, Color::sLightGrey); + } + } + } + + // Offset to the right + mOffset += Vec3(mGeometry->mBounds.GetSize().GetX() + 2.0f, 0, 0); + } +#endif // JPH_GJK_DEBUG + + Vec3 mY[4]; ///< Support points on A - B + Vec3 mP[4]; ///< Support point on A + Vec3 mQ[4]; ///< Support point on B + int mNumPoints = 0; ///< Number of points in mY, mP and mQ that are valid + +#ifdef JPH_GJK_DEBUG + DebugRenderer::GeometryRef mGeometry; ///< A visualization of the minkowski difference for state drawing + RVec3 mOffset = RVec3::sZero(); ///< Offset to use for state drawing +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h b/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h new file mode 100644 index 000000000000..7ebffc29c514 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h @@ -0,0 +1,116 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Triangle with 32-bit indices +class IndexedTriangleNoMaterial +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + IndexedTriangleNoMaterial() = default; + constexpr IndexedTriangleNoMaterial(uint32 inI1, uint32 inI2, uint32 inI3) : mIdx { inI1, inI2, inI3 } { } + + /// Check if two triangles are identical + bool operator == (const IndexedTriangleNoMaterial &inRHS) const + { + return mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2]; + } + + /// Check if two triangles are equivalent (using the same vertices) + bool IsEquivalent(const IndexedTriangleNoMaterial &inRHS) const + { + return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2]) + || (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[0]) + || (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[1]); + } + + /// Check if two triangles are opposite (using the same vertices but in opposing order) + bool IsOpposite(const IndexedTriangleNoMaterial &inRHS) const + { + return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[1]) + || (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[2]) + || (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[0]); + } + + /// Check if triangle is degenerate + bool IsDegenerate(const VertexList &inVertices) const + { + Vec3 v0(inVertices[mIdx[0]]); + Vec3 v1(inVertices[mIdx[1]]); + Vec3 v2(inVertices[mIdx[2]]); + + return (v1 - v0).Cross(v2 - v0).IsNearZero(); + } + + /// Rotate the vertices so that the second vertex becomes first etc. This does not change the represented triangle. + void Rotate() + { + uint32 tmp = mIdx[0]; + mIdx[0] = mIdx[1]; + mIdx[1] = mIdx[2]; + mIdx[2] = tmp; + } + + /// Get center of triangle + Vec3 GetCentroid(const VertexList &inVertices) const + { + return (Vec3(inVertices[mIdx[0]]) + Vec3(inVertices[mIdx[1]]) + Vec3(inVertices[mIdx[2]])) / 3.0f; + } + + uint32 mIdx[3]; +}; + +/// Triangle with 32-bit indices and material index +class IndexedTriangle : public IndexedTriangleNoMaterial +{ +public: + using IndexedTriangleNoMaterial::IndexedTriangleNoMaterial; + + /// Constructor + constexpr IndexedTriangle(uint32 inI1, uint32 inI2, uint32 inI3, uint32 inMaterialIndex, uint inUserData = 0) : IndexedTriangleNoMaterial(inI1, inI2, inI3), mMaterialIndex(inMaterialIndex), mUserData(inUserData) { } + + /// Check if two triangles are identical + bool operator == (const IndexedTriangle &inRHS) const + { + return mMaterialIndex == inRHS.mMaterialIndex && mUserData == inRHS.mUserData && IndexedTriangleNoMaterial::operator==(inRHS); + } + + /// Rotate the vertices so that the lowest vertex becomes the first. This does not change the represented triangle. + IndexedTriangle GetLowestIndexFirst() const + { + if (mIdx[0] < mIdx[1]) + { + if (mIdx[0] < mIdx[2]) + return IndexedTriangle(mIdx[0], mIdx[1], mIdx[2], mMaterialIndex, mUserData); // 0 is smallest + else + return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex, mUserData); // 2 is smallest + } + else + { + if (mIdx[1] < mIdx[2]) + return IndexedTriangle(mIdx[1], mIdx[2], mIdx[0], mMaterialIndex, mUserData); // 1 is smallest + else + return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex, mUserData); // 2 is smallest + } + } + + uint32 mMaterialIndex = 0; + uint32 mUserData = 0; ///< User data that can be used for anything by the application, e.g. for tracking the original index of the triangle +}; + +using IndexedTriangleNoMaterialList = Array; +using IndexedTriangleList = Array; + +JPH_NAMESPACE_END + +// Create a std::hash/JPH::Hash for IndexedTriangleNoMaterial and IndexedTriangle +JPH_MAKE_HASHABLE(JPH::IndexedTriangleNoMaterial, t.mIdx[0], t.mIdx[1], t.mIdx[2]) +JPH_MAKE_HASHABLE(JPH::IndexedTriangle, t.mIdx[0], t.mIdx[1], t.mIdx[2], t.mMaterialIndex, t.mUserData) diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp new file mode 100644 index 000000000000..019dbeeda40e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp @@ -0,0 +1,222 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +static JPH_INLINE const Float3 &sIndexifyGetFloat3(const TriangleList &inTriangles, uint32 inVertexIndex) +{ + return inTriangles[inVertexIndex / 3].mV[inVertexIndex % 3]; +} + +static JPH_INLINE Vec3 sIndexifyGetVec3(const TriangleList &inTriangles, uint32 inVertexIndex) +{ + return Vec3::sLoadFloat3Unsafe(sIndexifyGetFloat3(inTriangles, inVertexIndex)); +} + +static void sIndexifyVerticesBruteForce(const TriangleList &inTriangles, const uint32 *inVertexIndices, const uint32 *inVertexIndicesEnd, Array &ioWeldedVertices, float inVertexWeldDistance) +{ + float weld_dist_sq = Square(inVertexWeldDistance); + + // Compare every vertex + for (const uint32 *v1_idx = inVertexIndices; v1_idx < inVertexIndicesEnd; ++v1_idx) + { + Vec3 v1 = sIndexifyGetVec3(inTriangles, *v1_idx); + + // with every other vertex... + for (const uint32 *v2_idx = v1_idx + 1; v2_idx < inVertexIndicesEnd; ++v2_idx) + { + Vec3 v2 = sIndexifyGetVec3(inTriangles, *v2_idx); + + // If they're weldable + if ((v2 - v1).LengthSq() <= weld_dist_sq) + { + // Find the lowest indices both indices link to + uint32 idx1 = *v1_idx; + for (;;) + { + uint32 new_idx1 = ioWeldedVertices[idx1]; + if (new_idx1 >= idx1) + break; + idx1 = new_idx1; + } + uint32 idx2 = *v2_idx; + for (;;) + { + uint32 new_idx2 = ioWeldedVertices[idx2]; + if (new_idx2 >= idx2) + break; + idx2 = new_idx2; + } + + // Order the vertices + uint32 lowest = min(idx1, idx2); + uint32 highest = max(idx1, idx2); + + // Link highest to lowest + ioWeldedVertices[highest] = lowest; + + // Also update the vertices we started from to avoid creating long chains + ioWeldedVertices[*v1_idx] = lowest; + ioWeldedVertices[*v2_idx] = lowest; + break; + } + } + } +} + +static void sIndexifyVerticesRecursively(const TriangleList &inTriangles, uint32 *ioVertexIndices, uint inNumVertices, uint32 *ioScratch, Array &ioWeldedVertices, float inVertexWeldDistance, uint inMaxRecursion) +{ + // Check if we have few enough vertices to do a brute force search + // Or if we've recursed too deep (this means we chipped off a few vertices each iteration because all points are very close) + if (inNumVertices <= 8 || inMaxRecursion == 0) + { + sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance); + return; + } + + // Calculate bounds + AABox bounds; + for (const uint32 *v = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices; v < v_end; ++v) + bounds.Encapsulate(sIndexifyGetVec3(inTriangles, *v)); + + // Determine split plane + int split_axis = bounds.GetExtent().GetHighestComponentIndex(); + float split_value = bounds.GetCenter()[split_axis]; + + // Partition vertices + uint32 *v_read = ioVertexIndices, *v_write = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices; + uint32 *scratch = ioScratch; + while (v_read < v_end) + { + // Calculate distance to plane + float distance_to_split_plane = sIndexifyGetFloat3(inTriangles, *v_read)[split_axis] - split_value; + if (distance_to_split_plane < -inVertexWeldDistance) + { + // Vertex is on the right side + *v_write = *v_read; + ++v_read; + ++v_write; + } + else if (distance_to_split_plane > inVertexWeldDistance) + { + // Vertex is on the wrong side, swap with the last vertex + --v_end; + std::swap(*v_read, *v_end); + } + else + { + // Vertex is too close to the split plane, it goes on both sides + *scratch++ = *v_read++; + } + } + + // Check if we made any progress + uint num_vertices_on_both_sides = (uint)(scratch - ioScratch); + if (num_vertices_on_both_sides == inNumVertices) + { + sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance); + return; + } + + // Calculate how we classified the vertices + uint num_vertices_left = (uint)(v_write - ioVertexIndices); + uint num_vertices_right = (uint)(ioVertexIndices + inNumVertices - v_end); + JPH_ASSERT(num_vertices_left + num_vertices_right + num_vertices_on_both_sides == inNumVertices); + memcpy(v_write, ioScratch, num_vertices_on_both_sides * sizeof(uint32)); + + // Recurse + uint max_recursion = inMaxRecursion - 1; + sIndexifyVerticesRecursively(inTriangles, ioVertexIndices, num_vertices_left + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion); + sIndexifyVerticesRecursively(inTriangles, ioVertexIndices + num_vertices_left, num_vertices_right + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion); +} + +void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance) +{ + uint num_triangles = (uint)inTriangles.size(); + uint num_vertices = num_triangles * 3; + + // Create a list of all vertex indices + Array vertex_indices; + vertex_indices.resize(num_vertices); + for (uint i = 0; i < num_vertices; ++i) + vertex_indices[i] = i; + + // Link each vertex to itself + Array welded_vertices; + welded_vertices.resize(num_vertices); + for (uint i = 0; i < num_vertices; ++i) + welded_vertices[i] = i; + + // A scope to free memory used by the scratch array + { + // Some scratch memory, used for the vertices that fall in both partitions + Array scratch; + scratch.resize(num_vertices); + + // Recursively split the vertices + sIndexifyVerticesRecursively(inTriangles, vertex_indices.data(), num_vertices, scratch.data(), welded_vertices, inVertexWeldDistance, 32); + } + + // Do a pass to complete the welding, linking each vertex to the vertex it is welded to + // (and since we're going from 0 to N we can be sure that the vertex we're linking to is already linked to the lowest vertex) + uint num_resulting_vertices = 0; + for (uint i = 0; i < num_vertices; ++i) + { + JPH_ASSERT(welded_vertices[welded_vertices[i]] <= welded_vertices[i]); + welded_vertices[i] = welded_vertices[welded_vertices[i]]; + if (welded_vertices[i] == i) + ++num_resulting_vertices; + } + + // Collect the vertices + outVertices.clear(); + outVertices.reserve(num_resulting_vertices); + for (uint i = 0; i < num_vertices; ++i) + if (welded_vertices[i] == i) + { + // New vertex + welded_vertices[i] = (uint32)outVertices.size(); + outVertices.push_back(sIndexifyGetFloat3(inTriangles, i)); + } + else + { + // Reused vertex, remap index + welded_vertices[i] = welded_vertices[welded_vertices[i]]; + } + + // Create indexed triangles + outTriangles.clear(); + outTriangles.reserve(num_triangles); + for (uint t = 0; t < num_triangles; ++t) + { + IndexedTriangle it; + it.mMaterialIndex = inTriangles[t].mMaterialIndex; + it.mUserData = inTriangles[t].mUserData; + for (int v = 0; v < 3; ++v) + it.mIdx[v] = welded_vertices[t * 3 + v]; + if (!it.IsDegenerate(outVertices)) + outTriangles.push_back(it); + } +} + +void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles) +{ + outTriangles.resize(inTriangles.size()); + for (size_t t = 0; t < inTriangles.size(); ++t) + { + const IndexedTriangle &in = inTriangles[t]; + Triangle &out = outTriangles[t]; + out.mMaterialIndex = in.mMaterialIndex; + out.mUserData = in.mUserData; + for (int v = 0; v < 3; ++v) + out.mV[v] = inVertices[in.mIdx[v]]; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h new file mode 100644 index 000000000000..01fb805305de --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Take a list of triangles and get the unique set of vertices and use them to create indexed triangles. +/// Vertices that are less than inVertexWeldDistance apart will be combined to a single vertex. +JPH_EXPORT void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance = 1.0e-4f); + +/// Take a list of indexed triangles and unpack them +JPH_EXPORT void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h b/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h new file mode 100644 index 000000000000..e750d7e6d1ba --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h @@ -0,0 +1,40 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class MortonCode +{ +public: + /// First converts a floating point value in the range [0, 1] to a 10 bit fixed point integer. + /// Then expands a 10-bit integer into 30 bits by inserting 2 zeros after each bit. + static uint32 sExpandBits(float inV) + { + JPH_ASSERT(inV >= 0.0f && inV <= 1.0f); + uint32 v = uint32(inV * 1023.0f + 0.5f); + JPH_ASSERT(v < 1024); + v = (v * 0x00010001u) & 0xFF0000FFu; + v = (v * 0x00000101u) & 0x0F00F00Fu; + v = (v * 0x00000011u) & 0xC30C30C3u; + v = (v * 0x00000005u) & 0x49249249u; + return v; + } + + /// Calculate the morton code for inVector, given that all vectors lie in inVectorBounds + static uint32 sGetMortonCode(Vec3Arg inVector, const AABox &inVectorBounds) + { + // Convert to 10 bit fixed point + Vec3 scaled = (inVector - inVectorBounds.mMin) / inVectorBounds.GetSize(); + uint x = sExpandBits(scaled.GetX()); + uint y = sExpandBits(scaled.GetY()); + uint z = sExpandBits(scaled.GetZ()); + return (x << 2) + (y << 1) + z; + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp new file mode 100644 index 000000000000..31c38c877e81 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp @@ -0,0 +1,178 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +bool OrientedBox::Overlaps(const AABox &inBox, float inEpsilon) const +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box) + + // Convert AABox to center / extent representation + Vec3 a_center = inBox.GetCenter(); + Vec3 a_half_extents = inBox.GetExtent(); + + // Compute rotation matrix expressing b in a's coordinate frame + Mat44 rot(mOrientation.GetColumn4(0), mOrientation.GetColumn4(1), mOrientation.GetColumn4(2), mOrientation.GetColumn4(3) - Vec4(a_center, 0)); + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon }; + + // Test axes L = A0, L = A1, L = A2 + float ra, rb; + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[i]; + rb = mHalfExtents[0] * abs_r[0][i] + mHalfExtents[1] * abs_r[1][i] + mHalfExtents[2] * abs_r[2][i]; + if (abs(rot(i, 3)) > ra + rb) return false; + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = a_half_extents.Dot(abs_r[i]); + rb = mHalfExtents[i]; + if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false; + } + + // Test axis L = A0 x B0 + ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1]; + rb = mHalfExtents[1] * abs_r[2][0] + mHalfExtents[2] * abs_r[1][0]; + if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false; + + // Test axis L = A0 x B1 + ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1]; + rb = mHalfExtents[0] * abs_r[2][0] + mHalfExtents[2] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false; + + // Test axis L = A0 x B2 + ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1]; + rb = mHalfExtents[0] * abs_r[1][0] + mHalfExtents[1] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false; + + // Test axis L = A1 x B0 + ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0]; + rb = mHalfExtents[1] * abs_r[2][1] + mHalfExtents[2] * abs_r[1][1]; + if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false; + + // Test axis L = A1 x B1 + ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0]; + rb = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[2] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false; + + // Test axis L = A1 x B2 + ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0]; + rb = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false; + + // Test axis L = A2 x B0 + ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0]; + rb = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[1][2]; + if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false; + + // Test axis L = A2 x B1 + ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0]; + rb = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false; + + // Test axis L = A2 x B2 + ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0]; + rb = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[1] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false; + + // Since no separating axis is found, the OBB and AAB must be intersecting + return true; +} + +bool OrientedBox::Overlaps(const OrientedBox &inBox, float inEpsilon) const +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that A is this, B is inBox + + // Compute rotation matrix expressing b in a's coordinate frame + Mat44 rot = mOrientation.InversedRotationTranslation() * inBox.mOrientation; + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon }; + + // Test axes L = A0, L = A1, L = A2 + float ra, rb; + for (int i = 0; i < 3; i++) + { + ra = mHalfExtents[i]; + rb = inBox.mHalfExtents[0] * abs_r[0][i] + inBox.mHalfExtents[1] * abs_r[1][i] + inBox.mHalfExtents[2] * abs_r[2][i]; + if (abs(rot(i, 3)) > ra + rb) return false; + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = mHalfExtents.Dot(abs_r[i]); + rb = inBox.mHalfExtents[i]; + if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false; + } + + // Test axis L = A0 x B0 + ra = mHalfExtents[1] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][1]; + rb = inBox.mHalfExtents[1] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[1][0]; + if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false; + + // Test axis L = A0 x B1 + ra = mHalfExtents[1] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][1]; + rb = inBox.mHalfExtents[0] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false; + + // Test axis L = A0 x B2 + ra = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][1]; + rb = inBox.mHalfExtents[0] * abs_r[1][0] + inBox.mHalfExtents[1] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false; + + // Test axis L = A1 x B0 + ra = mHalfExtents[0] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][0]; + rb = inBox.mHalfExtents[1] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[1][1]; + if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false; + + // Test axis L = A1 x B1 + ra = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][0]; + rb = inBox.mHalfExtents[0] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false; + + // Test axis L = A1 x B2 + ra = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][0]; + rb = inBox.mHalfExtents[0] * abs_r[1][1] + inBox.mHalfExtents[1] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false; + + // Test axis L = A2 x B0 + ra = mHalfExtents[0] * abs_r[0][1] + mHalfExtents[1] * abs_r[0][0]; + rb = inBox.mHalfExtents[1] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[1][2]; + if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false; + + // Test axis L = A2 x B1 + ra = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[1][0]; + rb = inBox.mHalfExtents[0] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false; + + // Test axis L = A2 x B2 + ra = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[1] * abs_r[2][0]; + rb = inBox.mHalfExtents[0] * abs_r[1][2] + inBox.mHalfExtents[1] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false; + + // Since no separating axis is found, the OBBs must be intersecting + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h new file mode 100644 index 000000000000..c5c2a0e16c84 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h @@ -0,0 +1,39 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class AABox; + +/// Oriented box +class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND OrientedBox +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + OrientedBox() = default; + OrientedBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents) : mOrientation(inOrientation), mHalfExtents(inHalfExtents) { } + + /// Construct from axis aligned box and transform. Only works for rotation/translation matrix (no scaling / shearing). + OrientedBox(Mat44Arg inOrientation, const AABox &inBox) : OrientedBox(inOrientation.PreTranslated(inBox.GetCenter()), inBox.GetExtent()) { } + + /// Test if oriented box overlaps with axis aligned box each other + bool Overlaps(const AABox &inBox, float inEpsilon = 1.0e-6f) const; + + /// Test if two oriented boxes overlap each other + bool Overlaps(const OrientedBox &inBox, float inEpsilon = 1.0e-6f) const; + + Mat44 mOrientation; ///< Transform that positions and rotates the local space axis aligned box into world space + Vec3 mHalfExtents; ///< Half extents (half the size of the edge) of the local space axis aligned box +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Plane.h b/thirdparty/jolt_physics/Jolt/Geometry/Plane.h new file mode 100644 index 000000000000..7e3b8a8c4fed --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Plane.h @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// An infinite plane described by the formula X . Normal + Constant = 0. +class [[nodiscard]] Plane +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Plane() = default; + explicit Plane(Vec4Arg inNormalAndConstant) : mNormalAndConstant(inNormalAndConstant) { } + Plane(Vec3Arg inNormal, float inConstant) : mNormalAndConstant(inNormal, inConstant) { } + + /// Create from point and normal + static Plane sFromPointAndNormal(Vec3Arg inPoint, Vec3Arg inNormal) { return Plane(Vec4(inNormal, -inNormal.Dot(inPoint))); } + + /// Create from point and normal, double precision version that more accurately calculates the plane constant + static Plane sFromPointAndNormal(DVec3Arg inPoint, Vec3Arg inNormal) { return Plane(Vec4(inNormal, -float(DVec3(inNormal).Dot(inPoint)))); } + + /// Create from 3 counter clockwise points + static Plane sFromPointsCCW(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) { return sFromPointAndNormal(inV1, (inV2 - inV1).Cross(inV3 - inV1).Normalized()); } + + // Properties + Vec3 GetNormal() const { return Vec3(mNormalAndConstant); } + void SetNormal(Vec3Arg inNormal) { mNormalAndConstant = Vec4(inNormal, mNormalAndConstant.GetW()); } + float GetConstant() const { return mNormalAndConstant.GetW(); } + void SetConstant(float inConstant) { mNormalAndConstant.SetW(inConstant); } + + /// Offset the plane (positive value means move it in the direction of the plane normal) + Plane Offset(float inDistance) const { return Plane(mNormalAndConstant - Vec4(Vec3::sZero(), inDistance)); } + + /// Transform the plane by a matrix + inline Plane GetTransformed(Mat44Arg inTransform) const + { + Vec3 transformed_normal = inTransform.Multiply3x3(GetNormal()); + return Plane(transformed_normal, GetConstant() - inTransform.GetTranslation().Dot(transformed_normal)); + } + + /// Scale the plane, can handle non-uniform and negative scaling + inline Plane Scaled(Vec3Arg inScale) const + { + Vec3 scaled_normal = GetNormal() / inScale; + float scaled_normal_length = scaled_normal.Length(); + return Plane(scaled_normal / scaled_normal_length, GetConstant() / scaled_normal_length); + } + + /// Distance point to plane + float SignedDistance(Vec3Arg inPoint) const { return inPoint.Dot(GetNormal()) + GetConstant(); } + + /// Project inPoint onto the plane + Vec3 ProjectPointOnPlane(Vec3Arg inPoint) const { return inPoint - GetNormal() * SignedDistance(inPoint); } + + /// Returns intersection point between 3 planes + static bool sIntersectPlanes(const Plane &inP1, const Plane &inP2, const Plane &inP3, Vec3 &outPoint) + { + // We solve the equation: + // |ax, ay, az, aw| | x | | 0 | + // |bx, by, bz, bw| * | y | = | 0 | + // |cx, cy, cz, cw| | z | | 0 | + // | 0, 0, 0, 1| | 1 | | 1 | + // Where normal of plane 1 = (ax, ay, az), plane constant of 1 = aw, normal of plane 2 = (bx, by, bz) etc. + // This involves inverting the matrix and multiplying it with [0, 0, 0, 1] + + // Fetch the normals and plane constants for the three planes + Vec4 a = inP1.mNormalAndConstant; + Vec4 b = inP2.mNormalAndConstant; + Vec4 c = inP3.mNormalAndConstant; + + // Result is a vector that we have to divide by: + float denominator = Vec3(a).Dot(Vec3(b).Cross(Vec3(c))); + if (denominator == 0.0f) + return false; + + // The numerator is: + // [aw*(bz*cy-by*cz)+ay*(bw*cz-bz*cw)+az*(by*cw-bw*cy)] + // [aw*(bx*cz-bz*cx)+ax*(bz*cw-bw*cz)+az*(bw*cx-bx*cw)] + // [aw*(by*cx-bx*cy)+ax*(bw*cy-by*cw)+ay*(bx*cw-bw*cx)] + Vec4 numerator = + a.SplatW() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()) + + a.Swizzle() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()) + + a.Swizzle() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()); + + outPoint = Vec3(numerator) / denominator; + return true; + } + +private: +#ifdef JPH_OBJECT_STREAM + friend void CreateRTTIPlane(class RTTI &); // For JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS +#endif + + Vec4 mNormalAndConstant; ///< XYZ = normal, W = constant, plane: x . normal + constant = 0 +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h b/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h new file mode 100644 index 000000000000..4506fadbb75b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h @@ -0,0 +1,241 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Helper structure holding the reciprocal of a ray for Ray vs AABox testing +class RayInvDirection +{ +public: + /// Constructors + inline RayInvDirection() = default; + inline explicit RayInvDirection(Vec3Arg inDirection) { Set(inDirection); } + + /// Set reciprocal from ray direction + inline void Set(Vec3Arg inDirection) + { + // if (abs(inDirection) <= Epsilon) the ray is nearly parallel to the slab. + mIsParallel = Vec3::sLessOrEqual(inDirection.Abs(), Vec3::sReplicate(1.0e-20f)); + + // Calculate 1 / direction while avoiding division by zero + mInvDirection = Vec3::sSelect(inDirection, Vec3::sReplicate(1.0f), mIsParallel).Reciprocal(); + } + + Vec3 mInvDirection; ///< 1 / ray direction + UVec4 mIsParallel; ///< for each component if it is parallel to the coordinate axis +}; + +/// Intersect AABB with ray, returns minimal distance along ray or FLT_MAX if no hit +/// Note: Can return negative value if ray starts in box +JPH_INLINE float RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY()); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ()); + return Vec3::sSelect(t_min, flt_max, no_intersection).GetX(); +} + +/// Intersect 4 AABBs with ray, returns minimal distance along ray or FLT_MAX if no hit +/// Note: Can return negative value if ray starts in box +JPH_INLINE Vec4 RayAABox4(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) +{ + // Constants + Vec4 flt_min = Vec4::sReplicate(-FLT_MAX); + Vec4 flt_max = Vec4::sReplicate(FLT_MAX); + + // Origin + Vec4 originx = inOrigin.SplatX(); + Vec4 originy = inOrigin.SplatY(); + Vec4 originz = inOrigin.SplatZ(); + + // Parallel + UVec4 parallelx = inInvDirection.mIsParallel.SplatX(); + UVec4 parallely = inInvDirection.mIsParallel.SplatY(); + UVec4 parallelz = inInvDirection.mIsParallel.SplatZ(); + + // Inverse direction + Vec4 invdirx = inInvDirection.mInvDirection.SplatX(); + Vec4 invdiry = inInvDirection.mInvDirection.SplatY(); + Vec4 invdirz = inInvDirection.mInvDirection.SplatZ(); + + // Test against all three axii simultaneously. + Vec4 t1x = (inBoundsMinX - originx) * invdirx; + Vec4 t1y = (inBoundsMinY - originy) * invdiry; + Vec4 t1z = (inBoundsMinZ - originz) * invdirz; + Vec4 t2x = (inBoundsMaxX - originx) * invdirx; + Vec4 t2y = (inBoundsMaxY - originy) * invdiry; + Vec4 t2z = (inBoundsMaxZ - originz) * invdirz; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec4 t_minx = Vec4::sSelect(Vec4::sMin(t1x, t2x), flt_min, parallelx); + Vec4 t_miny = Vec4::sSelect(Vec4::sMin(t1y, t2y), flt_min, parallely); + Vec4 t_minz = Vec4::sSelect(Vec4::sMin(t1z, t2z), flt_min, parallelz); + Vec4 t_maxx = Vec4::sSelect(Vec4::sMax(t1x, t2x), flt_max, parallelx); + Vec4 t_maxy = Vec4::sSelect(Vec4::sMax(t1y, t2y), flt_max, parallely); + Vec4 t_maxz = Vec4::sSelect(Vec4::sMax(t1z, t2z), flt_max, parallelz); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + Vec4 t_min = Vec4::sMax(Vec4::sMax(t_minx, t_miny), t_minz); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + Vec4 t_max = Vec4::sMin(Vec4::sMin(t_maxx, t_maxy), t_maxz); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec4::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec4::sLess(t_max, Vec4::sZero())); + + // if bounds are invalid return FLOAT_MAX; + UVec4 bounds_invalid = UVec4::sOr(UVec4::sOr(Vec4::sGreater(inBoundsMinX, inBoundsMaxX), Vec4::sGreater(inBoundsMinY, inBoundsMaxY)), Vec4::sGreater(inBoundsMinZ, inBoundsMaxZ)); + no_intersection = UVec4::sOr(no_intersection, bounds_invalid); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlapx = UVec4::sAnd(parallelx, UVec4::sOr(Vec4::sLess(originx, inBoundsMinX), Vec4::sGreater(originx, inBoundsMaxX))); + UVec4 no_parallel_overlapy = UVec4::sAnd(parallely, UVec4::sOr(Vec4::sLess(originy, inBoundsMinY), Vec4::sGreater(originy, inBoundsMaxY))); + UVec4 no_parallel_overlapz = UVec4::sAnd(parallelz, UVec4::sOr(Vec4::sLess(originz, inBoundsMinZ), Vec4::sGreater(originz, inBoundsMaxZ))); + no_intersection = UVec4::sOr(no_intersection, UVec4::sOr(UVec4::sOr(no_parallel_overlapx, no_parallel_overlapy), no_parallel_overlapz)); + return Vec4::sSelect(t_min, flt_max, no_intersection); +} + +/// Intersect AABB with ray, returns minimal and maximal distance along ray or FLT_MAX, -FLT_MAX if no hit +/// Note: Can return negative value for outMin if ray starts in box +JPH_INLINE void RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float &outMin, float &outMax) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY()); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ()); + outMin = Vec3::sSelect(t_min, flt_max, no_intersection).GetX(); + outMax = Vec3::sSelect(t_max, flt_min, no_intersection).GetX(); +} + +/// Intersect AABB with ray, returns true if there is a hit closer than inClosest +JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float inClosest) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return false; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return false; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (t_min > inClosest) return false; + no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater(t_min, Vec3::sReplicate(inClosest))); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return false; else return true; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + + return !no_intersection.TestAnyXYZTrue(); +} + +/// Intersect AABB with ray without hit fraction, based on separating axis test +/// @see http://www.codercorner.com/RayAABB.cpp +JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax) +{ + Vec3 extents = inBoundsMax - inBoundsMin; + + Vec3 diff = 2.0f * inOrigin - inBoundsMin - inBoundsMax; + Vec3 abs_diff = diff.Abs(); + + UVec4 no_intersection = UVec4::sAnd(Vec3::sGreater(abs_diff, extents), Vec3::sGreaterOrEqual(diff * inDirection, Vec3::sZero())); + + Vec3 abs_dir = inDirection.Abs(); + Vec3 abs_dir_yzz = abs_dir.Swizzle(); + Vec3 abs_dir_xyx = abs_dir.Swizzle(); + + Vec3 extents_yzz = extents.Swizzle(); + Vec3 extents_xyx = extents.Swizzle(); + + Vec3 diff_yzx = diff.Swizzle(); + + Vec3 dir_yzx = inDirection.Swizzle(); + + no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater((inDirection * diff_yzx - dir_yzx * diff).Abs(), extents_xyx * abs_dir_yzz + extents_yzz * abs_dir_xyx)); + + return !no_intersection.TestAnyXYZTrue(); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h b/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h new file mode 100644 index 000000000000..4862931b685d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection +/// against a capsule centered around the origin with its axis along the Y axis and half height specified. +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the capsule, the returned fraction will be 0. +/// @param inCapsuleHalfHeight Distance from the origin to the center of the top sphere (or that of the bottom) +/// @param inCapsuleRadius Radius of the top/bottom sphere +JPH_INLINE float RayCapsule(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCapsuleHalfHeight, float inCapsuleRadius) +{ + // Test infinite cylinder + float cylinder = RayCylinder(inRayOrigin, inRayDirection, inCapsuleRadius); + if (cylinder == FLT_MAX) + return FLT_MAX; + + // If this hit is in the finite cylinder we have our fraction + if (abs(inRayOrigin.GetY() + cylinder * inRayDirection.GetY()) <= inCapsuleHalfHeight) + return cylinder; + + // Test upper and lower sphere + Vec3 sphere_center(0, inCapsuleHalfHeight, 0); + float upper = RaySphere(inRayOrigin, inRayDirection, sphere_center, inCapsuleRadius); + float lower = RaySphere(inRayOrigin, inRayDirection, -sphere_center, inCapsuleRadius); + return min(upper, lower); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h b/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h new file mode 100644 index 000000000000..cabed0680a22 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection +/// against an infinite cylinder centered along the Y axis +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Direction of the ray. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0. +/// @param inCylinderRadius Radius of the infinite cylinder +JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderRadius) +{ + // Remove Y component of ray to see of ray intersects with infinite cylinder + UVec4 mask_y = UVec4(0, 0xffffffff, 0, 0); + Vec3 origin_xz = Vec3::sSelect(inRayOrigin, Vec3::sZero(), mask_y); + float origin_xz_len_sq = origin_xz.LengthSq(); + float r_sq = Square(inCylinderRadius); + if (origin_xz_len_sq > r_sq) + { + // Ray starts outside of the infinite cylinder + // Solve: |RayOrigin_xz + fraction * RayDirection_xz|^2 = r^2 to find fraction + Vec3 direction_xz = Vec3::sSelect(inRayDirection, Vec3::sZero(), mask_y); + float a = direction_xz.LengthSq(); + float b = 2.0f * origin_xz.Dot(direction_xz); + float c = origin_xz_len_sq - r_sq; + float fraction1, fraction2; + if (FindRoot(a, b, c, fraction1, fraction2) == 0) + return FLT_MAX; // No intersection with infinite cylinder + + // Get fraction corresponding to the ray entering the circle + float fraction = min(fraction1, fraction2); + if (fraction >= 0.0f) + return fraction; + } + else + { + // Ray starts inside the infinite cylinder + return 0.0f; + } + + // No collision + return FLT_MAX; +} + +/// Test a ray against a cylinder centered around the origin with its axis along the Y axis and half height specified. +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0. +/// @param inCylinderRadius Radius of the cylinder +/// @param inCylinderHalfHeight Distance from the origin to the top (or bottom) of the cylinder +JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderHalfHeight, float inCylinderRadius) +{ + // Test infinite cylinder + float fraction = RayCylinder(inRayOrigin, inRayDirection, inCylinderRadius); + if (fraction == FLT_MAX) + return FLT_MAX; + + // If this hit is in the finite cylinder we have our fraction + if (abs(inRayOrigin.GetY() + fraction * inRayDirection.GetY()) <= inCylinderHalfHeight) + return fraction; + + // Check if ray could hit the top or bottom plane of the cylinder + float direction_y = inRayDirection.GetY(); + if (direction_y != 0.0f) + { + // Solving line equation: x = ray_origin + fraction * ray_direction + // and plane equation: plane_normal . x + plane_constant = 0 + // fraction = (-plane_constant - plane_normal . ray_origin) / (plane_normal . ray_direction) + // when the ray_direction.y < 0: + // plane_constant = -cylinder_half_height, plane_normal = (0, 1, 0) + // else + // plane_constant = -cylinder_half_height, plane_normal = (0, -1, 0) + float origin_y = inRayOrigin.GetY(); + float plane_fraction; + if (direction_y < 0.0f) + plane_fraction = (inCylinderHalfHeight - origin_y) / direction_y; + else + plane_fraction = -(inCylinderHalfHeight + origin_y) / direction_y; + + // Check if the hit is in front of the ray + if (plane_fraction >= 0.0f) + { + // Test if this hit is inside the cylinder + Vec3 point = inRayOrigin + plane_fraction * inRayDirection; + float dist_sq = Square(point.GetX()) + Square(point.GetZ()); + if (dist_sq <= Square(inCylinderRadius)) + return plane_fraction; + } + } + + // No collision + return FLT_MAX; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h b/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h new file mode 100644 index 000000000000..93ad2682800b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h @@ -0,0 +1,96 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere, +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inSphereCenter Position of the center of the sphere +/// @param inSphereRadius Radius of the sphere +JPH_INLINE float RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius) +{ + // Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction + Vec3 center_origin = inRayOrigin - inSphereCenter; + float a = inRayDirection.LengthSq(); + float b = 2.0f * inRayDirection.Dot(center_origin); + float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius; + float fraction1, fraction2; + if (FindRoot(a, b, c, fraction1, fraction2) == 0) + return c <= 0.0f? 0.0f : FLT_MAX; // Return if origin is inside the sphere + + // Sort so that the smallest is first + if (fraction1 > fraction2) + std::swap(fraction1, fraction2); + + // Test solution with lowest fraction, this will be the ray entering the sphere + if (fraction1 >= 0.0f) + return fraction1; // Sphere is before the ray start + + // Test solution with highest fraction, this will be the ray leaving the sphere + if (fraction2 >= 0.0f) + return 0.0f; // We start inside the sphere + + // No solution + return FLT_MAX; +} + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere. +/// Outputs entry and exit points (outMinFraction and outMaxFraction) along the ray (which could be negative if the hit point is before the start of the ray). +/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inSphereCenter Position of the center of the sphere. +/// @param inSphereRadius Radius of the sphere. +/// @param outMinFraction Returned lowest intersection fraction +/// @param outMaxFraction Returned highest intersection fraction +/// @return The amount of intersections with the sphere. +/// If 1 intersection is returned outMinFraction will be equal to outMaxFraction +JPH_INLINE int RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius, float &outMinFraction, float &outMaxFraction) +{ + // Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction + Vec3 center_origin = inRayOrigin - inSphereCenter; + float a = inRayDirection.LengthSq(); + float b = 2.0f * inRayDirection.Dot(center_origin); + float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius; + float fraction1, fraction2; + switch (FindRoot(a, b, c, fraction1, fraction2)) + { + case 0: + if (c <= 0.0f) + { + // Origin inside sphere + outMinFraction = outMaxFraction = 0.0f; + return 1; + } + else + { + // Origin outside of the sphere + return 0; + } + break; + + case 1: + // Ray is touching the sphere + outMinFraction = outMaxFraction = fraction1; + return 1; + + default: + // Ray enters and exits the sphere + + // Sort so that the smallest is first + if (fraction1 > fraction2) + std::swap(fraction1, fraction2); + + outMinFraction = fraction1; + outMaxFraction = fraction2; + return 2; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h b/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h new file mode 100644 index 000000000000..dabd0275cc0c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Intersect ray with triangle, returns closest point or FLT_MAX if no hit (branch less version) +/// Adapted from: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm +JPH_INLINE float RayTriangle(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) +{ + // Epsilon + Vec3 epsilon = Vec3::sReplicate(1.0e-12f); + + // Zero & one + Vec3 zero = Vec3::sZero(); + Vec3 one = Vec3::sReplicate(1.0f); + + // Find vectors for two edges sharing inV0 + Vec3 e1 = inV1 - inV0; + Vec3 e2 = inV2 - inV0; + + // Begin calculating determinant - also used to calculate u parameter + Vec3 p = inDirection.Cross(e2); + + // if determinant is near zero, ray lies in plane of triangle + Vec3 det = Vec3::sReplicate(e1.Dot(p)); + + // Check if determinant is near zero + UVec4 det_near_zero = Vec3::sLess(det.Abs(), epsilon); + + // When the determinant is near zero, set it to one to avoid dividing by zero + det = Vec3::sSelect(det, Vec3::sReplicate(1.0f), det_near_zero); + + // Calculate distance from inV0 to ray origin + Vec3 s = inOrigin - inV0; + + // Calculate u parameter + Vec3 u = Vec3::sReplicate(s.Dot(p)) / det; + + // Prepare to test v parameter + Vec3 q = s.Cross(e1); + + // Calculate v parameter + Vec3 v = Vec3::sReplicate(inDirection.Dot(q)) / det; + + // Get intersection point + Vec3 t = Vec3::sReplicate(e2.Dot(q)) / det; + + // Check if there is an intersection + UVec4 no_intersection = + UVec4::sOr + ( + UVec4::sOr + ( + UVec4::sOr + ( + det_near_zero, + Vec3::sLess(u, zero) + ), + UVec4::sOr + ( + Vec3::sLess(v, zero), + Vec3::sGreater(u + v, one) + ) + ), + Vec3::sLess(t, zero) + ); + + // Select intersection point or FLT_MAX based on if there is an intersection or not + return Vec3::sSelect(t, Vec3::sReplicate(FLT_MAX), no_intersection).GetX(); +} + +/// Intersect ray with 4 triangles in SOA format, returns 4 vector of closest points or FLT_MAX if no hit (uses bit tricks to do less divisions) +JPH_INLINE Vec4 RayTriangle4(Vec3Arg inOrigin, Vec3Arg inDirection, Vec4Arg inV0X, Vec4Arg inV0Y, Vec4Arg inV0Z, Vec4Arg inV1X, Vec4Arg inV1Y, Vec4Arg inV1Z, Vec4Arg inV2X, Vec4Arg inV2Y, Vec4Arg inV2Z) +{ + // Epsilon + Vec4 epsilon = Vec4::sReplicate(1.0e-12f); + + // Zero + Vec4 zero = Vec4::sZero(); + + // Find vectors for two edges sharing inV0 + Vec4 e1x = inV1X - inV0X; + Vec4 e1y = inV1Y - inV0Y; + Vec4 e1z = inV1Z - inV0Z; + Vec4 e2x = inV2X - inV0X; + Vec4 e2y = inV2Y - inV0Y; + Vec4 e2z = inV2Z - inV0Z; + + // Get direction vector components + Vec4 dx = inDirection.SplatX(); + Vec4 dy = inDirection.SplatY(); + Vec4 dz = inDirection.SplatZ(); + + // Begin calculating determinant - also used to calculate u parameter + Vec4 px = dy * e2z - dz * e2y; + Vec4 py = dz * e2x - dx * e2z; + Vec4 pz = dx * e2y - dy * e2x; + + // if determinant is near zero, ray lies in plane of triangle + Vec4 det = e1x * px + e1y * py + e1z * pz; + + // Get sign bit for determinant and make positive + Vec4 det_sign = Vec4::sAnd(det, UVec4::sReplicate(0x80000000).ReinterpretAsFloat()); + det = Vec4::sXor(det, det_sign); + + // Check which determinants are near zero + UVec4 det_near_zero = Vec4::sLess(det, epsilon); + + // Set components of the determinant to 1 that are near zero to avoid dividing by zero + det = Vec4::sSelect(det, Vec4::sReplicate(1.0f), det_near_zero); + + // Calculate distance from inV0 to ray origin + Vec4 sx = inOrigin.SplatX() - inV0X; + Vec4 sy = inOrigin.SplatY() - inV0Y; + Vec4 sz = inOrigin.SplatZ() - inV0Z; + + // Calculate u parameter and flip sign if determinant was negative + Vec4 u = Vec4::sXor(sx * px + sy * py + sz * pz, det_sign); + + // Prepare to test v parameter + Vec4 qx = sy * e1z - sz * e1y; + Vec4 qy = sz * e1x - sx * e1z; + Vec4 qz = sx * e1y - sy * e1x; + + // Calculate v parameter and flip sign if determinant was negative + Vec4 v = Vec4::sXor(dx * qx + dy * qy + dz * qz, det_sign); + + // Get intersection point and flip sign if determinant was negative + Vec4 t = Vec4::sXor(e2x * qx + e2y * qy + e2z * qz, det_sign); + + // Check if there is an intersection + UVec4 no_intersection = + UVec4::sOr + ( + UVec4::sOr + ( + UVec4::sOr + ( + det_near_zero, + Vec4::sLess(u, zero) + ), + UVec4::sOr + ( + Vec4::sLess(v, zero), + Vec4::sGreater(u + v, det) + ) + ), + Vec4::sLess(t, zero) + ); + + // Select intersection point or FLT_MAX based on if there is an intersection or not + return Vec4::sSelect(t / det, Vec4::sReplicate(FLT_MAX), no_intersection); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h b/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h new file mode 100644 index 000000000000..05f7dd14f37d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] Sphere +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + inline Sphere() = default; + inline Sphere(const Float3 &inCenter, float inRadius) : mCenter(inCenter), mRadius(inRadius) { } + inline Sphere(Vec3Arg inCenter, float inRadius) : mRadius(inRadius) { inCenter.StoreFloat3(&mCenter); } + + /// Calculate the support vector for this convex shape. + inline Vec3 GetSupport(Vec3Arg inDirection) const + { + float length = inDirection.Length(); + return length > 0.0f ? Vec3::sLoadFloat3Unsafe(mCenter) + (mRadius/ length) * inDirection : Vec3::sLoadFloat3Unsafe(mCenter); + } + + // Properties + inline Vec3 GetCenter() const { return Vec3::sLoadFloat3Unsafe(mCenter); } + inline float GetRadius() const { return mRadius; } + + /// Test if two spheres overlap + inline bool Overlaps(const Sphere &inB) const + { + return (Vec3::sLoadFloat3Unsafe(mCenter) - Vec3::sLoadFloat3Unsafe(inB.mCenter)).LengthSq() <= Square(mRadius + inB.mRadius); + } + + /// Check if this sphere overlaps with a box + inline bool Overlaps(const AABox &inOther) const + { + return inOther.GetSqDistanceTo(GetCenter()) <= Square(mRadius); + } + + /// Create the minimal sphere that encapsulates this sphere and inPoint + inline void EncapsulatePoint(Vec3Arg inPoint) + { + // Calculate distance between point and center + Vec3 center = GetCenter(); + Vec3 d_vec = inPoint - center; + float d_sq = d_vec.LengthSq(); + if (d_sq > Square(mRadius)) + { + // It is further away than radius, we need to widen the sphere + // The diameter of the new sphere is radius + d, so the new radius is half of that + float d = sqrt(d_sq); + float radius = 0.5f * (mRadius + d); + + // The center needs to shift by new radius - old radius in the direction of d + center += (radius - mRadius) / d * d_vec; + + // Store new sphere + center.StoreFloat3(&mCenter); + mRadius = radius; + } + } + +private: + Float3 mCenter; + float mRadius; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h b/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h new file mode 100644 index 000000000000..7ad718fc2a8b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h @@ -0,0 +1,34 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// A simple triangle and its material +class Triangle +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Triangle() = default; + Triangle(const Float3 &inV1, const Float3 &inV2, const Float3 &inV3, uint32 inMaterialIndex = 0, uint32 inUserData = 0) : mV { inV1, inV2, inV3 }, mMaterialIndex(inMaterialIndex), mUserData(inUserData) { } + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, uint32 inMaterialIndex = 0, uint32 inUserData = 0) : mMaterialIndex(inMaterialIndex), mUserData(inUserData) { inV1.StoreFloat3(&mV[0]); inV2.StoreFloat3(&mV[1]); inV3.StoreFloat3(&mV[2]); } + + /// Get center of triangle + Vec3 GetCentroid() const + { + return (Vec3::sLoadFloat3Unsafe(mV[0]) + Vec3::sLoadFloat3Unsafe(mV[1]) + Vec3::sLoadFloat3Unsafe(mV[2])) * (1.0f / 3.0f); + } + + /// Vertices + Float3 mV[3]; + uint32 mMaterialIndex = 0; ///< Follows mV[3] so that we can read mV as 4 vectors + uint32 mUserData = 0; ///< User data that can be used for anything by the application, e.g. for tracking the original index of the triangle +}; + +using TriangleList = Array; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Jolt.h b/thirdparty/jolt_physics/Jolt/Jolt.h new file mode 100644 index 000000000000..acc400ce9e3c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Jolt.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Project includes +#include +#include +#include +#include +#include +#include +#include +#include +#include diff --git a/thirdparty/jolt_physics/Jolt/Jolt.natvis b/thirdparty/jolt_physics/Jolt/Jolt.natvis new file mode 100644 index 000000000000..bebafa1adcdd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Jolt.natvis @@ -0,0 +1,116 @@ + + + + r={(int)r}, g={(int)g}, b={(int)b}, a={(int)a} + + + {x}, {y} + + + {x}, {y}, {z} + + + {x}, {y}, {z}, {w} + + + {mF32[0]}, {mF32[1]}, {mF32[2]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]} + + + {mF64[0]}, {mF64[1]}, {mF64[2]}, L^2={mF64[0]*mF64[0]+mF64[1]*mF64[1]+mF64[2]*mF64[2]} + + + {mF32[0]}, {mF32[1]}, {mF32[2]}, {mF32[3]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]+mF32[3]*mF32[3]} + + + {mU32[0]}, {mU32[1]}, {mU32[2]}, {mU32[3]} + + + {uint(mU8[0])}, {uint(mU8[1])}, {uint(mU8[2])}, {uint(mU8[3])}, {uint(mU8[4])}, {uint(mU8[5])}, {uint(mU8[6])}, {uint(mU8[7])}, {uint(mU8[8])}, {uint(mU8[9])}, {uint(mU8[10])}, {uint(mU8[11])}, {uint(mU8[12])}, {uint(mU8[13])}, {uint(mU8[14])}, {uint(mU8[15])} + + + {mValue} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol[3].mF32[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol[3].mF32[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol[3].mF32[2]} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol[3].mF32[0]} + + + {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol[3].mF32[1]} + + + {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol[3].mF32[2]} + + + {mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, {mCol[3].mF32[3]} + + + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]} + + + {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]} + + + {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]} + + + {mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, 1} + + + + + min=({mMin}), max=({mMax}) + + + {mID} + + + {mDebugName}: p=({mPosition.mF32[0],g}, {mPosition.mF32[1],g}, {mPosition.mF32[2],g}), r=({mRotation.mValue.mF32[0],g}, {mRotation.mValue.mF32[1],g}, {mRotation.mValue.mF32[2],g}, {mRotation.mValue.mF32[3],g}), v=({mLinearVelocity.mF32[0],g}, {mLinearVelocity.mF32[1],g}, {mLinearVelocity.mF32[2],g}), w=({mAngularVelocity.mF32[0],g}, {mAngularVelocity.mF32[1],g}, {mAngularVelocity.mF32[2],g}) + + + bodies={mBodies._Mypair._Myval2._Mylast - mBodies._Mypair._Myval2._Myfirst}, active={mActiveBodies._Mypair._Myval2._Mylast - mActiveBodies._Mypair._Myval2._Myfirst} + + + size={mSize} + + mSize + + mSize + (value_type *)mElements + + + + + size={mSize} + + mSize + mCapacity + + mSize + mElements + + + + + size={mSize} + + mSize + mMaxSize + + mMaxSize + mData[$i] + "--Empty--" + "--Deleted--" + + + + + {(value_type *)mPtr}, stride={mStride} + + diff --git a/thirdparty/jolt_physics/Jolt/Math/BVec16.h b/thirdparty/jolt_physics/Jolt/Math/BVec16.h new file mode 100644 index 000000000000..a4ac12bf79d6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/BVec16.h @@ -0,0 +1,99 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// A vector consisting of 16 bytes +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) BVec16 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128i; +#elif defined(JPH_USE_NEON) + using Type = uint8x16_t; +#else + using Type = struct { uint64 mData[2]; }; +#endif + + /// Constructor + BVec16() = default; ///< Intentionally not initialized for performance reasons + BVec16(const BVec16 &inRHS) = default; + BVec16 & operator = (const BVec16 &inRHS) = default; + JPH_INLINE BVec16(Type inRHS) : mValue(inRHS) { } + + /// Create a vector from 16 bytes + JPH_INLINE BVec16(uint8 inB0, uint8 inB1, uint8 inB2, uint8 inB3, uint8 inB4, uint8 inB5, uint8 inB6, uint8 inB7, uint8 inB8, uint8 inB9, uint8 inB10, uint8 inB11, uint8 inB12, uint8 inB13, uint8 inB14, uint8 inB15); + + /// Create a vector from two uint64's + JPH_INLINE BVec16(uint64 inV0, uint64 inV1); + + /// Comparison + JPH_INLINE bool operator == (BVec16Arg inV2) const; + JPH_INLINE bool operator != (BVec16Arg inV2) const { return !(*this == inV2); } + + /// Vector with all zeros + static JPH_INLINE BVec16 sZero(); + + /// Replicate int inV across all components + static JPH_INLINE BVec16 sReplicate(uint8 inV); + + /// Load 16 bytes from memory + static JPH_INLINE BVec16 sLoadByte16(const uint8 *inV); + + /// Equals (component wise), highest bit of each component that is set is considered true + static JPH_INLINE BVec16 sEquals(BVec16Arg inV1, BVec16Arg inV2); + + /// Logical or (component wise) + static JPH_INLINE BVec16 sOr(BVec16Arg inV1, BVec16Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE BVec16 sXor(BVec16Arg inV1, BVec16Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE BVec16 sAnd(BVec16Arg inV1, BVec16Arg inV2); + + /// Logical not (component wise) + static JPH_INLINE BVec16 sNot(BVec16Arg inV1); + + /// Get component by index + JPH_INLINE uint8 operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 16); return mU8[inCoordinate]; } + JPH_INLINE uint8 & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 16); return mU8[inCoordinate]; } + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Store if mU8[0] is true in bit 0, mU8[1] in bit 1, etc. (true is when highest bit of component is set) + JPH_INLINE int GetTrues() const; + + /// To String + friend ostream & operator << (ostream &inStream, BVec16Arg inV) + { + inStream << uint(inV.mU8[0]) << ", " << uint(inV.mU8[1]) << ", " << uint(inV.mU8[2]) << ", " << uint(inV.mU8[3]) << ", " + << uint(inV.mU8[4]) << ", " << uint(inV.mU8[5]) << ", " << uint(inV.mU8[6]) << ", " << uint(inV.mU8[7]) << ", " + << uint(inV.mU8[8]) << ", " << uint(inV.mU8[9]) << ", " << uint(inV.mU8[10]) << ", " << uint(inV.mU8[11]) << ", " + << uint(inV.mU8[12]) << ", " << uint(inV.mU8[13]) << ", " << uint(inV.mU8[14]) << ", " << uint(inV.mU8[15]); + return inStream; + } + + union + { + Type mValue; + uint8 mU8[16]; + uint64 mU64[2]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "BVec16.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/BVec16.inl b/thirdparty/jolt_physics/Jolt/Math/BVec16.inl new file mode 100644 index 000000000000..91e063a0134c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/BVec16.inl @@ -0,0 +1,177 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +BVec16::BVec16(uint8 inB0, uint8 inB1, uint8 inB2, uint8 inB3, uint8 inB4, uint8 inB5, uint8 inB6, uint8 inB7, uint8 inB8, uint8 inB9, uint8 inB10, uint8 inB11, uint8 inB12, uint8 inB13, uint8 inB14, uint8 inB15) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_epi8(char(inB15), char(inB14), char(inB13), char(inB12), char(inB11), char(inB10), char(inB9), char(inB8), char(inB7), char(inB6), char(inB5), char(inB4), char(inB3), char(inB2), char(inB1), char(inB0)); +#elif defined(JPH_USE_NEON) + uint8x8_t v1 = vcreate_u8(uint64(inB0) | (uint64(inB1) << 8) | (uint64(inB2) << 16) | (uint64(inB3) << 24) | (uint64(inB4) << 32) | (uint64(inB5) << 40) | (uint64(inB6) << 48) | (uint64(inB7) << 56)); + uint8x8_t v2 = vcreate_u8(uint64(inB8) | (uint64(inB9) << 8) | (uint64(inB10) << 16) | (uint64(inB11) << 24) | (uint64(inB12) << 32) | (uint64(inB13) << 40) | (uint64(inB14) << 48) | (uint64(inB15) << 56)); + mValue = vcombine_u8(v1, v2); +#else + mU8[0] = inB0; + mU8[1] = inB1; + mU8[2] = inB2; + mU8[3] = inB3; + mU8[4] = inB4; + mU8[5] = inB5; + mU8[6] = inB6; + mU8[7] = inB7; + mU8[8] = inB8; + mU8[9] = inB9; + mU8[10] = inB10; + mU8[11] = inB11; + mU8[12] = inB12; + mU8[13] = inB13; + mU8[14] = inB14; + mU8[15] = inB15; +#endif +} + +BVec16::BVec16(uint64 inV0, uint64 inV1) +{ + mU64[0] = inV0; + mU64[1] = inV1; +} + +bool BVec16::operator == (BVec16Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +BVec16 BVec16::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_si128(); +#elif defined(JPH_USE_NEON) + return vdupq_n_u8(0); +#else + return BVec16(0, 0); +#endif +} + +BVec16 BVec16::sReplicate(uint8 inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_epi8(char(inV)); +#elif defined(JPH_USE_NEON) + return vdupq_n_u8(inV); +#else + uint64 v(inV); + v |= v << 8; + v |= v << 16; + v |= v << 32; + return BVec16(v, v); +#endif +} + +BVec16 BVec16::sLoadByte16(const uint8 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_loadu_si128(reinterpret_cast(inV)); +#elif defined(JPH_USE_NEON) + return vld1q_u8(inV); +#else + return BVec16(inV[0], inV[1], inV[2], inV[3], inV[4], inV[5], inV[6], inV[7], inV[8], inV[9], inV[10], inV[11], inV[12], inV[13], inV[14], inV[15]); +#endif +} + +BVec16 BVec16::sEquals(BVec16Arg inV1, BVec16Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_cmpeq_epi8(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vceqq_u8(inV1.mValue, inV2.mValue); +#else + auto equals = [](uint64 inV1, uint64 inV2) { + uint64 r = inV1 ^ ~inV2; // Bits that are equal are 1 + r &= r << 1; // Combine bit 0 through 1 + r &= r << 2; // Combine bit 0 through 3 + r &= r << 4; // Combine bit 0 through 7 + r &= 0x8080808080808080UL; // Keep only the highest bit of each byte + return r; + }; + return BVec16(equals(inV1.mU64[0], inV2.mU64[0]), equals(inV1.mU64[1], inV2.mU64[1])); +#endif +} + +BVec16 BVec16::sOr(BVec16Arg inV1, BVec16Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vorrq_u8(inV1.mValue, inV2.mValue); +#else + return BVec16(inV1.mU64[0] | inV2.mU64[0], inV1.mU64[1] | inV2.mU64[1]); +#endif +} + +BVec16 BVec16::sXor(BVec16Arg inV1, BVec16Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return veorq_u8(inV1.mValue, inV2.mValue); +#else + return BVec16(inV1.mU64[0] ^ inV2.mU64[0], inV1.mU64[1] ^ inV2.mU64[1]); +#endif +} + +BVec16 BVec16::sAnd(BVec16Arg inV1, BVec16Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vandq_u8(inV1.mValue, inV2.mValue); +#else + return BVec16(inV1.mU64[0] & inV2.mU64[0], inV1.mU64[1] & inV2.mU64[1]); +#endif +} + + +BVec16 BVec16::sNot(BVec16Arg inV1) +{ +#if defined(JPH_USE_SSE) + return sXor(inV1, sReplicate(0xff)); +#elif defined(JPH_USE_NEON) + return vmvnq_u8(inV1.mValue); +#else + return BVec16(~inV1.mU64[0], ~inV1.mU64[1]); +#endif +} + +int BVec16::GetTrues() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_epi8(mValue); +#else + int result = 0; + for (int i = 0; i < 16; ++i) + result |= int(mU8[i] >> 7) << i; + return result; +#endif +} + +bool BVec16::TestAnyTrue() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_epi8(mValue) != 0; +#else + return ((mU64[0] | mU64[1]) & 0x8080808080808080UL) != 0; +#endif +} + +bool BVec16::TestAllTrue() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_epi8(mValue) == 0b1111111111111111; +#else + return ((mU64[0] & mU64[1]) & 0x8080808080808080UL) == 0x8080808080808080UL; +#endif +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/DMat44.h b/thirdparty/jolt_physics/Jolt/Math/DMat44.h new file mode 100644 index 000000000000..cd8042107ada --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DMat44.h @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Holds a 4x4 matrix of floats with the last column consisting of doubles +class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DMat44 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying column type + using Type = Vec4::Type; + using DType = DVec3::Type; + using DTypeArg = DVec3::TypeArg; + + // Argument type + using ArgType = DMat44Arg; + + /// Constructor + DMat44() = default; ///< Intentionally not initialized for performance reasons + JPH_INLINE DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4); + DMat44(const DMat44 &inM2) = default; + DMat44 & operator = (const DMat44 &inM2) = default; + JPH_INLINE explicit DMat44(Mat44Arg inM); + JPH_INLINE DMat44(Mat44Arg inRot, DVec3Arg inT); + JPH_INLINE DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4); + + /// Zero matrix + static JPH_INLINE DMat44 sZero(); + + /// Identity matrix + static JPH_INLINE DMat44 sIdentity(); + + /// Rotate from quaternion + static JPH_INLINE DMat44 sRotation(QuatArg inQuat) { return DMat44(Mat44::sRotation(inQuat), DVec3::sZero()); } + + /// Get matrix that translates + static JPH_INLINE DMat44 sTranslation(DVec3Arg inV) { return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), inV); } + + /// Get matrix that rotates and translates + static JPH_INLINE DMat44 sRotationTranslation(QuatArg inR, DVec3Arg inT) { return DMat44(Mat44::sRotation(inR), inT); } + + /// Get inverse matrix of sRotationTranslation + static JPH_INLINE DMat44 sInverseRotationTranslation(QuatArg inR, DVec3Arg inT); + + /// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal) + static JPH_INLINE DMat44 sScale(Vec3Arg inV) { return DMat44(Mat44::sScale(inV), DVec3::sZero()); } + + /// Convert to Mat44 rounding to nearest + JPH_INLINE Mat44 ToMat44() const { return Mat44(mCol[0], mCol[1], mCol[2], Vec3(mCol3)); } + + /// Comparison + JPH_INLINE bool operator == (DMat44Arg inM2) const; + JPH_INLINE bool operator != (DMat44Arg inM2) const { return !(*this == inM2); } + + /// Test if two matrices are close + JPH_INLINE bool IsClose(DMat44Arg inM2, float inMaxDistSq = 1.0e-12f) const; + + /// Multiply matrix by matrix + JPH_INLINE DMat44 operator * (Mat44Arg inM) const; + + /// Multiply matrix by matrix + JPH_INLINE DMat44 operator * (DMat44Arg inM) const; + + /// Multiply vector by matrix + JPH_INLINE DVec3 operator * (Vec3Arg inV) const; + + /// Multiply vector by matrix + JPH_INLINE DVec3 operator * (DVec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const { return GetRotation().Multiply3x3(inV); } + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE DVec3 Multiply3x3(DVec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$) + JPH_INLINE Vec3 Multiply3x3Transposed(Vec3Arg inV) const { return GetRotation().Multiply3x3Transposed(inV); } + + /// Scale a matrix: result = this * Mat44::sScale(inScale) + JPH_INLINE DMat44 PreScaled(Vec3Arg inScale) const; + + /// Scale a matrix: result = Mat44::sScale(inScale) * this + JPH_INLINE DMat44 PostScaled(Vec3Arg inScale) const; + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE DMat44 PreTranslated(Vec3Arg inTranslation) const; + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE DMat44 PreTranslated(DVec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE DMat44 PostTranslated(Vec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE DMat44 PostTranslated(DVec3Arg inTranslation) const; + + /// Access to the columns + JPH_INLINE Vec3 GetAxisX() const { return Vec3(mCol[0]); } + JPH_INLINE void SetAxisX(Vec3Arg inV) { mCol[0] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisY() const { return Vec3(mCol[1]); } + JPH_INLINE void SetAxisY(Vec3Arg inV) { mCol[1] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisZ() const { return Vec3(mCol[2]); } + JPH_INLINE void SetAxisZ(Vec3Arg inV) { mCol[2] = Vec4(inV, 0.0f); } + JPH_INLINE DVec3 GetTranslation() const { return mCol3; } + JPH_INLINE void SetTranslation(DVec3Arg inV) { mCol3 = inV; } + JPH_INLINE Vec3 GetColumn3(uint inCol) const { JPH_ASSERT(inCol < 3); return Vec3(mCol[inCol]); } + JPH_INLINE void SetColumn3(uint inCol, Vec3Arg inV) { JPH_ASSERT(inCol < 3); mCol[inCol] = Vec4(inV, 0.0f); } + JPH_INLINE Vec4 GetColumn4(uint inCol) const { JPH_ASSERT(inCol < 3); return mCol[inCol]; } + JPH_INLINE void SetColumn4(uint inCol, Vec4Arg inV) { JPH_ASSERT(inCol < 3); mCol[inCol] = inV; } + + /// Transpose 3x3 subpart of matrix + JPH_INLINE Mat44 Transposed3x3() const { return GetRotation().Transposed3x3(); } + + /// Inverse 4x4 matrix + JPH_INLINE DMat44 Inversed() const; + + /// Inverse 4x4 matrix when it only contains rotation and translation + JPH_INLINE DMat44 InversedRotationTranslation() const; + + /// Get rotation part only (note: retains the first 3 values from the bottom row) + JPH_INLINE Mat44 GetRotation() const { return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); } + + /// Updates the rotation part of this matrix (the first 3 columns) + JPH_INLINE void SetRotation(Mat44Arg inRotation); + + /// Convert to quaternion + JPH_INLINE Quat GetQuaternion() const { return GetRotation().GetQuaternion(); } + + /// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved) + JPH_INLINE Mat44 GetDirectionPreservingMatrix() const { return GetRotation().Inversed3x3().Transposed3x3(); } + + /// Works identical to Mat44::Decompose + JPH_INLINE DMat44 Decompose(Vec3 &outScale) const { return DMat44(GetRotation().Decompose(outScale), mCol3); } + + /// To String + friend ostream & operator << (ostream &inStream, DMat44Arg inM) + { + inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol3; + return inStream; + } + +private: + Vec4 mCol[3]; ///< Rotation columns + DVec3 mCol3; ///< Translation column, 4th element is assumed to be 1 +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "DMat44.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/DMat44.inl b/thirdparty/jolt_physics/Jolt/Math/DMat44.inl new file mode 100644 index 000000000000..462cf79114c3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DMat44.inl @@ -0,0 +1,310 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +DMat44::DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4) : + mCol { inC1, inC2, inC3 }, + mCol3(inC4) +{ +} + +DMat44::DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4) : + mCol { inC1, inC2, inC3 }, + mCol3(inC4) +{ +} + +DMat44::DMat44(Mat44Arg inM) : + mCol { inM.GetColumn4(0), inM.GetColumn4(1), inM.GetColumn4(2) }, + mCol3(inM.GetTranslation()) +{ +} + +DMat44::DMat44(Mat44Arg inRot, DVec3Arg inT) : + mCol { inRot.GetColumn4(0), inRot.GetColumn4(1), inRot.GetColumn4(2) }, + mCol3(inT) +{ +} + +DMat44 DMat44::sZero() +{ + return DMat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), DVec3::sZero()); +} + +DMat44 DMat44::sIdentity() +{ + return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3::sZero()); +} + +DMat44 DMat44::sInverseRotationTranslation(QuatArg inR, DVec3Arg inT) +{ + Mat44 m = Mat44::sRotation(inR.Conjugated()); + DMat44 dm(m, DVec3::sZero()); + dm.SetTranslation(-dm.Multiply3x3(inT)); + return dm; +} + +bool DMat44::operator == (DMat44Arg inM2) const +{ + return mCol[0] == inM2.mCol[0] + && mCol[1] == inM2.mCol[1] + && mCol[2] == inM2.mCol[2] + && mCol3 == inM2.mCol3; +} + +bool DMat44::IsClose(DMat44Arg inM2, float inMaxDistSq) const +{ + for (int i = 0; i < 3; ++i) + if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq)) + return false; + return mCol3.IsClose(inM2.mCol3, double(inMaxDistSq)); +} + +DVec3 DMat44::operator * (Vec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + return DVec3::sFixW(_mm256_add_pd(mCol3.mValue, _mm256_cvtps_pd(t))); +#elif defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + __m128d low = _mm_add_pd(mCol3.mValue.mLow, _mm_cvtps_pd(t)); + __m128d high = _mm_add_pd(mCol3.mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(t, t, _MM_SHUFFLE(2, 2, 2, 2)))); + return DVec3({ low, high }); +#elif defined(JPH_USE_NEON) + float32x4_t t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + float64x2_t low = vaddq_f64(mCol3.mValue.val[0], vcvt_f64_f32(vget_low_f32(t))); + float64x2_t high = vaddq_f64(mCol3.mValue.val[1], vcvt_high_f64_f32(t)); + return DVec3::sFixW({ low, high }); +#else + return DVec3( + mCol3.mF64[0] + double(mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2]), + mCol3.mF64[1] + double(mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2]), + mCol3.mF64[2] + double(mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2])); +#endif +} + +DVec3 DMat44::operator * (DVec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m256d t = _mm256_add_pd(mCol3.mValue, _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2]))); + return DVec3::sFixW(t); +#elif defined(JPH_USE_SSE) + __m128d xxxx = _mm_set1_pd(inV.mF64[0]); + __m128d yyyy = _mm_set1_pd(inV.mF64[1]); + __m128d zzzz = _mm_set1_pd(inV.mF64[2]); + __m128 col0 = mCol[0].mValue; + __m128 col1 = mCol[1].mValue; + __m128 col2 = mCol[2].mValue; + __m128d t_low = _mm_add_pd(mCol3.mValue.mLow, _mm_mul_pd(_mm_cvtps_pd(col0), xxxx)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz)); + __m128d t_high = _mm_add_pd(mCol3.mValue.mHigh, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz)); + return DVec3({ t_low, t_high }); +#elif defined(JPH_USE_NEON) + float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0); + float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1); + float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0); + float32x4_t col0 = mCol[0].mValue; + float32x4_t col1 = mCol[1].mValue; + float32x4_t col2 = mCol[2].mValue; + float64x2_t t_low = vaddq_f64(mCol3.mValue.val[0], vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz)); + float64x2_t t_high = vaddq_f64(mCol3.mValue.val[1], vmulq_f64(vcvt_high_f64_f32(col0), xxxx)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz)); + return DVec3::sFixW({ t_low, t_high }); +#else + return DVec3( + mCol3.mF64[0] + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2], + mCol3.mF64[1] + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2], + mCol3.mF64[2] + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]); +#endif +} + +DVec3 DMat44::Multiply3x3(DVec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m256d t = _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0])); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2]))); + return DVec3::sFixW(t); +#elif defined(JPH_USE_SSE) + __m128d xxxx = _mm_set1_pd(inV.mF64[0]); + __m128d yyyy = _mm_set1_pd(inV.mF64[1]); + __m128d zzzz = _mm_set1_pd(inV.mF64[2]); + __m128 col0 = mCol[0].mValue; + __m128 col1 = mCol[1].mValue; + __m128 col2 = mCol[2].mValue; + __m128d t_low = _mm_mul_pd(_mm_cvtps_pd(col0), xxxx); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz)); + __m128d t_high = _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz)); + return DVec3({ t_low, t_high }); +#elif defined(JPH_USE_NEON) + float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0); + float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1); + float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0); + float32x4_t col0 = mCol[0].mValue; + float32x4_t col1 = mCol[1].mValue; + float32x4_t col2 = mCol[2].mValue; + float64x2_t t_low = vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz)); + float64x2_t t_high = vmulq_f64(vcvt_high_f64_f32(col0), xxxx); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz)); + return DVec3::sFixW({ t_low, t_high }); +#else + return DVec3( + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2], + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2], + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]); +#endif +} + +DMat44 DMat44::operator * (Mat44Arg inM) const +{ + DMat44 result; + + // Rotation part +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.GetColumn4(i).mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.GetColumn4(i).mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + { + Vec4 coli = inM.GetColumn4(i); + result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2]; + } +#endif + + // Translation part + result.mCol3 = *this * inM.GetTranslation(); + + return result; +} + +DMat44 DMat44::operator * (DMat44Arg inM) const +{ + DMat44 result; + + // Rotation part +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.GetColumn4(i).mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + { + Vec4 coli = inM.mCol[i]; + result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2]; + } +#endif + + // Translation part + result.mCol3 = *this * inM.GetTranslation(); + + return result; +} + +void DMat44::SetRotation(Mat44Arg inRotation) +{ + mCol[0] = inRotation.GetColumn4(0); + mCol[1] = inRotation.GetColumn4(1); + mCol[2] = inRotation.GetColumn4(2); +} + +DMat44 DMat44::PreScaled(Vec3Arg inScale) const +{ + return DMat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol3); +} + +DMat44 DMat44::PostScaled(Vec3Arg inScale) const +{ + Vec4 scale(inScale, 1); + return DMat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], DVec3(scale) * mCol3); +} + +DMat44 DMat44::PreTranslated(Vec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation)); +} + +DMat44 DMat44::PreTranslated(DVec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation)); +} + +DMat44 DMat44::PostTranslated(Vec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation); +} + +DMat44 DMat44::PostTranslated(DVec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation); +} + +DMat44 DMat44::Inversed() const +{ + DMat44 m(GetRotation().Inversed3x3()); + m.mCol3 = -m.Multiply3x3(mCol3); + return m; +} + +DMat44 DMat44::InversedRotationTranslation() const +{ + DMat44 m(GetRotation().Transposed3x3()); + m.mCol3 = -m.Multiply3x3(mCol3); + return m; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/DVec3.h b/thirdparty/jolt_physics/Jolt/Math/DVec3.h new file mode 100644 index 000000000000..74d209b2a849 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DVec3.h @@ -0,0 +1,288 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// 3 component vector of doubles (stored as 4 vectors). +/// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined +class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DVec3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_AVX) + using Type = __m256d; + using TypeArg = __m256d; +#elif defined(JPH_USE_SSE) + using Type = struct { __m128d mLow, mHigh; }; + using TypeArg = const Type &; +#elif defined(JPH_USE_NEON) + using Type = float64x2x2_t; + using TypeArg = const Type &; +#else + using Type = struct { double mData[4]; }; + using TypeArg = const Type &; +#endif + + // Argument type + using ArgType = DVec3Arg; + + /// Constructor + DVec3() = default; ///< Intentionally not initialized for performance reasons + DVec3(const DVec3 &inRHS) = default; + DVec3 & operator = (const DVec3 &inRHS) = default; + JPH_INLINE explicit DVec3(Vec3Arg inRHS); + JPH_INLINE explicit DVec3(Vec4Arg inRHS); + JPH_INLINE DVec3(TypeArg inRHS) : mValue(inRHS) { CheckW(); } + + /// Create a vector from 3 components + JPH_INLINE DVec3(double inX, double inY, double inZ); + + /// Load 3 doubles from memory + explicit JPH_INLINE DVec3(const Double3 &inV); + + /// Vector with all zeros + static JPH_INLINE DVec3 sZero(); + + /// Vectors with the principal axis + static JPH_INLINE DVec3 sAxisX() { return DVec3(1, 0, 0); } + static JPH_INLINE DVec3 sAxisY() { return DVec3(0, 1, 0); } + static JPH_INLINE DVec3 sAxisZ() { return DVec3(0, 0, 1); } + + /// Replicate inV across all components + static JPH_INLINE DVec3 sReplicate(double inV); + + /// Vector with all NaN's + static JPH_INLINE DVec3 sNaN(); + + /// Load 3 doubles from memory (reads 64 bits extra which it doesn't use) + static JPH_INLINE DVec3 sLoadDouble3Unsafe(const Double3 &inV); + + /// Store 3 doubles to memory + JPH_INLINE void StoreDouble3(Double3 *outV) const; + + /// Convert to float vector 3 rounding to nearest + JPH_INLINE explicit operator Vec3() const; + + /// Prepare to convert to float vector 3 rounding towards zero (returns DVec3 that can be converted to a Vec3 to get the rounding) + JPH_INLINE DVec3 PrepareRoundToZero() const; + + /// Prepare to convert to float vector 3 rounding towards positive/negative inf (returns DVec3 that can be converted to a Vec3 to get the rounding) + JPH_INLINE DVec3 PrepareRoundToInf() const; + + /// Convert to float vector 3 rounding down + JPH_INLINE Vec3 ToVec3RoundDown() const; + + /// Convert to float vector 3 rounding up + JPH_INLINE Vec3 ToVec3RoundUp() const; + + /// Return the minimum value of each of the components + static JPH_INLINE DVec3 sMin(DVec3Arg inV1, DVec3Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE DVec3 sMax(DVec3Arg inV1, DVec3Arg inV2); + + /// Clamp a vector between min and max (component wise) + static JPH_INLINE DVec3 sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax); + + /// Equals (component wise) + static JPH_INLINE DVec3 sEquals(DVec3Arg inV1, DVec3Arg inV2); + + /// Less than (component wise) + static JPH_INLINE DVec3 sLess(DVec3Arg inV1, DVec3Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE DVec3 sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE DVec3 sGreater(DVec3Arg inV1, DVec3Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE DVec3 sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE DVec3 sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd); + + /// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1 + static JPH_INLINE DVec3 sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE DVec3 sOr(DVec3Arg inV1, DVec3Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE DVec3 sXor(DVec3Arg inV1, DVec3Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE DVec3 sAnd(DVec3Arg inV1, DVec3Arg inV2); + + /// Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of component is set) + JPH_INLINE int GetTrues() const; + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Get individual components +#if defined(JPH_USE_AVX) + JPH_INLINE double GetX() const { return _mm_cvtsd_f64(_mm256_castpd256_pd128(mValue)); } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return mF64[2]; } +#elif defined(JPH_USE_SSE) + JPH_INLINE double GetX() const { return _mm_cvtsd_f64(mValue.mLow); } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return _mm_cvtsd_f64(mValue.mHigh); } +#elif defined(JPH_USE_NEON) + JPH_INLINE double GetX() const { return vgetq_lane_f64(mValue.val[0], 0); } + JPH_INLINE double GetY() const { return vgetq_lane_f64(mValue.val[0], 1); } + JPH_INLINE double GetZ() const { return vgetq_lane_f64(mValue.val[1], 0); } +#else + JPH_INLINE double GetX() const { return mF64[0]; } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return mF64[2]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(double inX) { mF64[0] = inX; } + JPH_INLINE void SetY(double inY) { mF64[1] = inY; } + JPH_INLINE void SetZ(double inZ) { mF64[2] = mF64[3] = inZ; } // Assure Z and W are the same + + /// Set all components + JPH_INLINE void Set(double inX, double inY, double inZ) { *this = DVec3(inX, inY, inZ); } + + /// Get double component by index + JPH_INLINE double operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 3); return mF64[inCoordinate]; } + + /// Set double component by index + JPH_INLINE void SetComponent(uint inCoordinate, double inValue) { JPH_ASSERT(inCoordinate < 3); mF64[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same + + /// Comparison + JPH_INLINE bool operator == (DVec3Arg inV2) const; + JPH_INLINE bool operator != (DVec3Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(DVec3Arg inV2, double inMaxDistSq = 1.0e-24) const; + + /// Test if vector is near zero + JPH_INLINE bool IsNearZero(double inMaxDistSq = 1.0e-24) const; + + /// Test if vector is normalized + JPH_INLINE bool IsNormalized(double inTolerance = 1.0e-12) const; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two double vectors (component wise) + JPH_INLINE DVec3 operator * (DVec3Arg inV2) const; + + /// Multiply vector with double + JPH_INLINE DVec3 operator * (double inV2) const; + + /// Multiply vector with double + friend JPH_INLINE DVec3 operator * (double inV1, DVec3Arg inV2); + + /// Divide vector by double + JPH_INLINE DVec3 operator / (double inV2) const; + + /// Multiply vector with double + JPH_INLINE DVec3 & operator *= (double inV2); + + /// Multiply vector with vector + JPH_INLINE DVec3 & operator *= (DVec3Arg inV2); + + /// Divide vector by double + JPH_INLINE DVec3 & operator /= (double inV2); + + /// Add two vectors (component wise) + JPH_INLINE DVec3 operator + (Vec3Arg inV2) const; + + /// Add two double vectors (component wise) + JPH_INLINE DVec3 operator + (DVec3Arg inV2) const; + + /// Add two vectors (component wise) + JPH_INLINE DVec3 & operator += (Vec3Arg inV2); + + /// Add two double vectors (component wise) + JPH_INLINE DVec3 & operator += (DVec3Arg inV2); + + /// Negate + JPH_INLINE DVec3 operator - () const; + + /// Subtract two vectors (component wise) + JPH_INLINE DVec3 operator - (Vec3Arg inV2) const; + + /// Subtract two double vectors (component wise) + JPH_INLINE DVec3 operator - (DVec3Arg inV2) const; + + /// Subtract two vectors (component wise) + JPH_INLINE DVec3 & operator -= (Vec3Arg inV2); + + /// Subtract two vectors (component wise) + JPH_INLINE DVec3 & operator -= (DVec3Arg inV2); + + /// Divide (component wise) + JPH_INLINE DVec3 operator / (DVec3Arg inV2) const; + + /// Return the absolute value of each of the components + JPH_INLINE DVec3 Abs() const; + + /// Reciprocal vector (1 / value) for each of the components + JPH_INLINE DVec3 Reciprocal() const; + + /// Cross product + JPH_INLINE DVec3 Cross(DVec3Arg inV2) const; + + /// Dot product + JPH_INLINE double Dot(DVec3Arg inV2) const; + + /// Squared length of vector + JPH_INLINE double LengthSq() const; + + /// Length of vector + JPH_INLINE double Length() const; + + /// Normalize vector + JPH_INLINE DVec3 Normalized() const; + + /// Component wise square root + JPH_INLINE DVec3 Sqrt() const; + + /// Get vector that contains the sign of each element (returns 1 if positive, -1 if negative) + JPH_INLINE DVec3 GetSign() const; + + /// To String + friend ostream & operator << (ostream &inStream, DVec3Arg inV) + { + inStream << inV.mF64[0] << ", " << inV.mF64[1] << ", " << inV.mF64[2]; + return inStream; + } + + /// Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate div by 0 + JPH_INLINE void CheckW() const; + + /// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero + static JPH_INLINE Type sFixW(TypeArg inValue); + + /// Representations of true and false for boolean operations + inline static const double cTrue = BitCast(~uint64(0)); + inline static const double cFalse = 0.0; + + union + { + Type mValue; + double mF64[4]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "DVec3.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/DVec3.inl b/thirdparty/jolt_physics/Jolt/Math/DVec3.inl new file mode 100644 index 000000000000..eb0bcf4393e8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DVec3.inl @@ -0,0 +1,936 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// Create a std::hash/JPH::Hash for DVec3 +JPH_MAKE_HASHABLE(JPH::DVec3, t.GetX(), t.GetY(), t.GetZ()) + +JPH_NAMESPACE_BEGIN + +DVec3::DVec3(Vec3Arg inRHS) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_cvtps_pd(inRHS.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_cvtps_pd(inRHS.mValue); + mValue.mHigh = _mm_cvtps_pd(_mm_shuffle_ps(inRHS.mValue, inRHS.mValue, _MM_SHUFFLE(2, 2, 2, 2))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vcvt_f64_f32(vget_low_f32(inRHS.mValue)); + mValue.val[1] = vcvt_high_f64_f32(inRHS.mValue); +#else + mF64[0] = (double)inRHS.GetX(); + mF64[1] = (double)inRHS.GetY(); + mF64[2] = (double)inRHS.GetZ(); + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +DVec3::DVec3(Vec4Arg inRHS) : + DVec3(Vec3(inRHS)) +{ +} + +DVec3::DVec3(double inX, double inY, double inZ) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_set_pd(inZ, inZ, inY, inX); // Assure Z and W are the same +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_set_pd(inY, inX); + mValue.mHigh = _mm_set1_pd(inZ); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vcombine_f64(vcreate_f64(BitCast(inX)), vcreate_f64(BitCast(inY))); + mValue.val[1] = vdupq_n_f64(inZ); +#else + mF64[0] = inX; + mF64[1] = inY; + mF64[2] = inZ; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +DVec3::DVec3(const Double3 &inV) +{ +#if defined(JPH_USE_AVX) + Type x = _mm256_castpd128_pd256(_mm_load_sd(&inV.x)); + Type y = _mm256_castpd128_pd256(_mm_load_sd(&inV.y)); + Type z = _mm256_broadcast_sd(&inV.z); + Type xy = _mm256_unpacklo_pd(x, y); + mValue = _mm256_blend_pd(xy, z, 0b1100); // Assure Z and W are the same +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_loadu_pd(&inV.x); + mValue.mHigh = _mm_set1_pd(inV.z); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vld1q_f64(&inV.x); + mValue.val[1] = vdupq_n_f64(inV.z); +#else + mF64[0] = inV.x; + mF64[1] = inV.y; + mF64[2] = inV.z; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +void DVec3::CheckW() const +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + // Avoid asserts when both components are NaN + JPH_ASSERT(reinterpret_cast(mF64)[2] == reinterpret_cast(mF64)[3]); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +/// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero +DVec3::Type DVec3::sFixW(TypeArg inValue) +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #if defined(JPH_USE_AVX) + return _mm256_shuffle_pd(inValue, inValue, 2); + #elif defined(JPH_USE_SSE) + Type value; + value.mLow = inValue.mLow; + value.mHigh = _mm_shuffle_pd(inValue.mHigh, inValue.mHigh, 0); + return value; + #elif defined(JPH_USE_NEON) + Type value; + value.val[0] = inValue.val[0]; + value.val[1] = vdupq_laneq_f64(inValue.val[1], 0); + return value; + #else + Type value; + value.mData[0] = inValue.mData[0]; + value.mData[1] = inValue.mData[1]; + value.mData[2] = inValue.mData[2]; + value.mData[3] = inValue.mData[2]; + return value; + #endif +#else + return inValue; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +DVec3 DVec3::sZero() +{ +#if defined(JPH_USE_AVX) + return _mm256_setzero_pd(); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ zero, zero }); +#elif defined(JPH_USE_NEON) + float64x2_t zero = vdupq_n_f64(0.0); + return DVec3({ zero, zero }); +#else + return DVec3(0, 0, 0); +#endif +} + +DVec3 DVec3::sReplicate(double inV) +{ +#if defined(JPH_USE_AVX) + return _mm256_set1_pd(inV); +#elif defined(JPH_USE_SSE) + __m128d value = _mm_set1_pd(inV); + return DVec3({ value, value }); +#elif defined(JPH_USE_NEON) + float64x2_t value = vdupq_n_f64(inV); + return DVec3({ value, value }); +#else + return DVec3(inV, inV, inV); +#endif +} + +DVec3 DVec3::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +DVec3 DVec3::sLoadDouble3Unsafe(const Double3 &inV) +{ +#if defined(JPH_USE_AVX) + Type v = _mm256_loadu_pd(&inV.x); +#elif defined(JPH_USE_SSE) + Type v; + v.mLow = _mm_loadu_pd(&inV.x); + v.mHigh = _mm_set1_pd(inV.z); +#elif defined(JPH_USE_NEON) + Type v = vld1q_f64_x2(&inV.x); +#else + Type v = { inV.x, inV.y, inV.z }; +#endif + return sFixW(v); +} + +void DVec3::StoreDouble3(Double3 *outV) const +{ + outV->x = mF64[0]; + outV->y = mF64[1]; + outV->z = mF64[2]; +} + +DVec3::operator Vec3() const +{ +#if defined(JPH_USE_AVX) + return _mm256_cvtpd_ps(mValue); +#elif defined(JPH_USE_SSE) + __m128 low = _mm_cvtpd_ps(mValue.mLow); + __m128 high = _mm_cvtpd_ps(mValue.mHigh); + return _mm_shuffle_ps(low, high, _MM_SHUFFLE(1, 0, 1, 0)); +#elif defined(JPH_USE_NEON) + return vcvt_high_f32_f64(vcvtx_f32_f64(mValue.val[0]), mValue.val[1]); +#else + return Vec3((float)GetX(), (float)GetY(), (float)GetZ()); +#endif +} + +DVec3 DVec3::sMin(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_min_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_min_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_min_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vminq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vminq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(min(inV1.mF64[0], inV2.mF64[0]), + min(inV1.mF64[1], inV2.mF64[1]), + min(inV1.mF64[2], inV2.mF64[2])); +#endif +} + +DVec3 DVec3::sMax(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_max_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_max_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_max_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmaxq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vmaxq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(max(inV1.mF64[0], inV2.mF64[0]), + max(inV1.mF64[1], inV2.mF64[1]), + max(inV1.mF64[2], inV2.mF64[2])); +#endif +} + +DVec3 DVec3::sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax) +{ + return sMax(sMin(inV, inMax), inMin); +} + +DVec3 DVec3::sEquals(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_EQ_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpeq_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpeq_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vceqq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vceqq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] == inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] == inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] == inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sLess(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LT_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmplt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmplt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vcltq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcltq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] < inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] < inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] < inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LE_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmple_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmple_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vcleq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcleq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] <= inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] <= inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] <= inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sGreater(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GT_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpgt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpgt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vcgtq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcgtq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] > inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] > inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] > inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GE_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpge_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpge_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vcgeq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcgeq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] >= inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] >= inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] >= inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd) +{ +#if defined(JPH_USE_AVX) + #ifdef JPH_USE_FMADD + return _mm256_fmadd_pd(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm256_add_pd(_mm256_mul_pd(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return DVec3({ vmlaq_f64(inAdd.mValue.val[0], inMul1.mValue.val[0], inMul2.mValue.val[0]), vmlaq_f64(inAdd.mValue.val[1], inMul1.mValue.val[1], inMul2.mValue.val[1]) }); +#else + return inMul1 * inMul2 + inAdd; +#endif +} + +DVec3 DVec3::sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl) +{ +#if defined(JPH_USE_AVX) + return _mm256_blendv_pd(inNotSet.mValue, inSet.mValue, inControl.mValue); +#elif defined(JPH_USE_SSE4_1) + Type v = { _mm_blendv_pd(inNotSet.mValue.mLow, inSet.mValue.mLow, inControl.mValue.mLow), _mm_blendv_pd(inNotSet.mValue.mHigh, inSet.mValue.mHigh, inControl.mValue.mHigh) }; + return sFixW(v); +#elif defined(JPH_USE_NEON) + Type v = { vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[0]), 63)), inSet.mValue.val[0], inNotSet.mValue.val[0]), + vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[1]), 63)), inSet.mValue.val[1], inNotSet.mValue.val[1]) }; + return sFixW(v); +#else + DVec3 result; + for (int i = 0; i < 3; i++) + result.mF64[i] = (BitCast(inControl.mF64[i]) & (uint64(1) << 63))? inSet.mF64[i] : inNotSet.mF64[i]; +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + result.mF64[3] = result.mF64[2]; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + return result; +#endif +} + +DVec3 DVec3::sOr(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_or_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_or_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_or_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))), + vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) | BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) | BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) | BitCast(inV2.mF64[2]))); +#endif +} + +DVec3 DVec3::sXor(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_xor_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_xor_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_xor_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))), + vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) ^ BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) ^ BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) ^ BitCast(inV2.mF64[2]))); +#endif +} + +DVec3 DVec3::sAnd(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_and_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_and_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_and_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))), + vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) & BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) & BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) & BitCast(inV2.mF64[2]))); +#endif +} + +int DVec3::GetTrues() const +{ +#if defined(JPH_USE_AVX) + return _mm256_movemask_pd(mValue) & 0x7; +#elif defined(JPH_USE_SSE) + return (_mm_movemask_pd(mValue.mLow) + (_mm_movemask_pd(mValue.mHigh) << 2)) & 0x7; +#else + return int((BitCast(mF64[0]) >> 63) | ((BitCast(mF64[1]) >> 63) << 1) | ((BitCast(mF64[2]) >> 63) << 2)); +#endif +} + +bool DVec3::TestAnyTrue() const +{ + return GetTrues() != 0; +} + +bool DVec3::TestAllTrue() const +{ + return GetTrues() == 0x7; +} + +bool DVec3::operator == (DVec3Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +bool DVec3::IsClose(DVec3Arg inV2, double inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool DVec3::IsNearZero(double inMaxDistSq) const +{ + return LengthSq() <= inMaxDistSq; +} + +DVec3 DVec3::operator * (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_mul_pd(mValue.mLow, inV2.mValue.mLow), _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_f64(mValue.val[0], inV2.mValue.val[0]), vmulq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] * inV2.mF64[0], mF64[1] * inV2.mF64[1], mF64[2] * inV2.mF64[2]); +#endif +} + +DVec3 DVec3::operator * (double inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + return DVec3({ _mm_mul_pd(mValue.mLow, v), _mm_mul_pd(mValue.mHigh, v) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_n_f64(mValue.val[0], inV2), vmulq_n_f64(mValue.val[1], inV2) }); +#else + return DVec3(mF64[0] * inV2, mF64[1] * inV2, mF64[2] * inV2); +#endif +} + +DVec3 operator * (double inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(_mm256_set1_pd(inV1), inV2.mValue); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV1); + return DVec3({ _mm_mul_pd(v, inV2.mValue.mLow), _mm_mul_pd(v, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_n_f64(inV2.mValue.val[0], inV1), vmulq_n_f64(inV2.mValue.val[1], inV1) }); +#else + return DVec3(inV1 * inV2.mF64[0], inV1 * inV2.mF64[1], inV1 * inV2.mF64[2]); +#endif +} + +DVec3 DVec3::operator / (double inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_div_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + return DVec3({ _mm_div_pd(mValue.mLow, v), _mm_div_pd(mValue.mHigh, v) }); +#elif defined(JPH_USE_NEON) + float64x2_t v = vdupq_n_f64(inV2); + return DVec3({ vdivq_f64(mValue.val[0], v), vdivq_f64(mValue.val[1], v) }); +#else + return DVec3(mF64[0] / inV2, mF64[1] / inV2, mF64[2] / inV2); +#endif +} + +DVec3 &DVec3::operator *= (double inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_mul_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + mValue.mLow = _mm_mul_pd(mValue.mLow, v); + mValue.mHigh = _mm_mul_pd(mValue.mHigh, v); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vmulq_n_f64(mValue.val[0], inV2); + mValue.val[1] = vmulq_n_f64(mValue.val[1], inV2); +#else + for (int i = 0; i < 3; ++i) + mF64[i] *= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator *= (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_mul_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vmulq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vmulq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] *= inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator /= (double inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_div_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + mValue.mLow = _mm_div_pd(mValue.mLow, v); + mValue.mHigh = _mm_div_pd(mValue.mHigh, v); +#elif defined(JPH_USE_NEON) + float64x2_t v = vdupq_n_f64(inV2); + mValue.val[0] = vdivq_f64(mValue.val[0], v); + mValue.val[1] = vdivq_f64(mValue.val[1], v); +#else + for (int i = 0; i < 3; ++i) + mF64[i] /= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator + (Vec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) }); +#else + return DVec3(mF64[0] + inV2.mF32[0], mF64[1] + inV2.mF32[1], mF64[2] + inV2.mF32[2]); +#endif +} + +DVec3 DVec3::operator + (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_add_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_add_pd(mValue.mLow, inV2.mValue.mLow), _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vaddq_f64(mValue.val[0], inV2.mValue.val[0]), vaddq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] + inV2.mF64[0], mF64[1] + inV2.mF64[1], mF64[2] + inV2.mF64[2]); +#endif +} + +DVec3 &DVec3::operator += (Vec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)); + mValue.mHigh = _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))); + mValue.val[1] = vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)); +#else + for (int i = 0; i < 3; ++i) + mF64[i] += inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator += (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_add_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_add_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vaddq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vaddq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] += inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator - () const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(_mm256_setzero_pd(), mValue); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ _mm_sub_pd(zero, mValue.mLow), _mm_sub_pd(zero, mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + float64x2_t zero = vdupq_n_f64(0); + return DVec3({ vsubq_f64(zero, mValue.val[0]), vsubq_f64(zero, mValue.val[1]) }); + #else + return DVec3({ vnegq_f64(mValue.val[0]), vnegq_f64(mValue.val[1]) }); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return DVec3(0.0 - mF64[0], 0.0 - mF64[1], 0.0 - mF64[2]); + #else + return DVec3(-mF64[0], -mF64[1], -mF64[2]); + #endif +#endif +} + +DVec3 DVec3::operator - (Vec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) }); +#else + return DVec3(mF64[0] - inV2.mF32[0], mF64[1] - inV2.mF32[1], mF64[2] - inV2.mF32[2]); +#endif +} + +DVec3 DVec3::operator - (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sub_pd(mValue.mLow, inV2.mValue.mLow), _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsubq_f64(mValue.val[0], inV2.mValue.val[0]), vsubq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] - inV2.mF64[0], mF64[1] - inV2.mF64[1], mF64[2] - inV2.mF64[2]); +#endif +} + +DVec3 &DVec3::operator -= (Vec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)); + mValue.mHigh = _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))); + mValue.val[1] = vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)); +#else + for (int i = 0; i < 3; ++i) + mF64[i] -= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator -= (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_sub_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_sub_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vsubq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vsubq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] -= inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator / (DVec3Arg inV2) const +{ + inV2.CheckW(); +#if defined(JPH_USE_AVX) + return _mm256_div_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_div_pd(mValue.mLow, inV2.mValue.mLow), _mm_div_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vdivq_f64(mValue.val[0], inV2.mValue.val[0]), vdivq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] / inV2.mF64[0], mF64[1] / inV2.mF64[1], mF64[2] / inV2.mF64[2]); +#endif +} + +DVec3 DVec3::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm256_range_pd(mValue, mValue, 0b1000); +#elif defined(JPH_USE_AVX) + return _mm256_max_pd(_mm256_sub_pd(_mm256_setzero_pd(), mValue), mValue); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ _mm_max_pd(_mm_sub_pd(zero, mValue.mLow), mValue.mLow), _mm_max_pd(_mm_sub_pd(zero, mValue.mHigh), mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vabsq_f64(mValue.val[0]), vabsq_f64(mValue.val[1]) }); +#else + return DVec3(abs(mF64[0]), abs(mF64[1]), abs(mF64[2])); +#endif +} + +DVec3 DVec3::Reciprocal() const +{ + return sReplicate(1.0) / mValue; +} + +DVec3 DVec3::Cross(DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX2) + __m256d t1 = _mm256_permute4x64_pd(inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t1 = _mm256_mul_pd(t1, mValue); + __m256d t2 = _mm256_permute4x64_pd(mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t2 = _mm256_mul_pd(t2, inV2.mValue); + __m256d t3 = _mm256_sub_pd(t1, t2); + return _mm256_permute4x64_pd(t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same +#else + return DVec3(mF64[1] * inV2.mF64[2] - mF64[2] * inV2.mF64[1], + mF64[2] * inV2.mF64[0] - mF64[0] * inV2.mF64[2], + mF64[0] * inV2.mF64[1] - mF64[1] * inV2.mF64[0]); +#endif +} + +double DVec3::Dot(DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + __m256d mul = _mm256_mul_pd(mValue, inV2.mValue); + __m128d xy = _mm256_castpd256_pd128(mul); + __m128d yx = _mm_shuffle_pd(xy, xy, 1); + __m128d sum = _mm_add_pd(xy, yx); + __m128d zw = _mm256_extractf128_pd(mul, 1); + sum = _mm_add_pd(sum, zw); + return _mm_cvtsd_f64(sum); +#elif defined(JPH_USE_SSE) + __m128d xy = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow); + __m128d yx = _mm_shuffle_pd(xy, xy, 1); + __m128d sum = _mm_add_pd(xy, yx); + __m128d z = _mm_mul_sd(mValue.mHigh, inV2.mValue.mHigh); + sum = _mm_add_pd(sum, z); + return _mm_cvtsd_f64(sum); +#elif defined(JPH_USE_NEON) + float64x2_t mul_low = vmulq_f64(mValue.val[0], inV2.mValue.val[0]); + float64x2_t mul_high = vmulq_f64(mValue.val[1], inV2.mValue.val[1]); + return vaddvq_f64(mul_low) + vgetq_lane_f64(mul_high, 0); +#else + double dot = 0.0; + for (int i = 0; i < 3; i++) + dot += mF64[i] * inV2.mF64[i]; + return dot; +#endif +} + +double DVec3::LengthSq() const +{ + return Dot(*this); +} + +DVec3 DVec3::Sqrt() const +{ +#if defined(JPH_USE_AVX) + return _mm256_sqrt_pd(mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sqrt_pd(mValue.mLow), _mm_sqrt_pd(mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsqrtq_f64(mValue.val[0]), vsqrtq_f64(mValue.val[1]) }); +#else + return DVec3(sqrt(mF64[0]), sqrt(mF64[1]), sqrt(mF64[2])); +#endif +} + +double DVec3::Length() const +{ + return sqrt(Dot(*this)); +} + +DVec3 DVec3::Normalized() const +{ + return *this / Length(); +} + +bool DVec3::IsNormalized(double inTolerance) const +{ + return abs(LengthSq() - 1.0) <= inTolerance; +} + +bool DVec3::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return (_mm256_fpclass_pd_mask(mValue, 0b10000001) & 0x7) != 0; +#elif defined(JPH_USE_AVX) + return (_mm256_movemask_pd(_mm256_cmp_pd(mValue, mValue, _CMP_UNORD_Q)) & 0x7) != 0; +#elif defined(JPH_USE_SSE) + return ((_mm_movemask_pd(_mm_cmpunord_pd(mValue.mLow, mValue.mLow)) + (_mm_movemask_pd(_mm_cmpunord_pd(mValue.mHigh, mValue.mHigh)) << 2)) & 0x7) != 0; +#else + return isnan(mF64[0]) || isnan(mF64[1]) || isnan(mF64[2]); +#endif +} + +DVec3 DVec3::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm256_fixupimm_pd(mValue, mValue, _mm256_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_AVX) + __m256d minus_one = _mm256_set1_pd(-1.0); + __m256d one = _mm256_set1_pd(1.0); + return _mm256_or_pd(_mm256_and_pd(mValue, minus_one), one); +#elif defined(JPH_USE_SSE) + __m128d minus_one = _mm_set1_pd(-1.0); + __m128d one = _mm_set1_pd(1.0); + return DVec3({ _mm_or_pd(_mm_and_pd(mValue.mLow, minus_one), one), _mm_or_pd(_mm_and_pd(mValue.mHigh, minus_one), one) }); +#elif defined(JPH_USE_NEON) + uint64x2_t minus_one = vreinterpretq_u64_f64(vdupq_n_f64(-1.0f)); + uint64x2_t one = vreinterpretq_u64_f64(vdupq_n_f64(1.0f)); + return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), minus_one), one)), + vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), minus_one), one)) }); +#else + return DVec3(std::signbit(mF64[0])? -1.0 : 1.0, + std::signbit(mF64[1])? -1.0 : 1.0, + std::signbit(mF64[2])? -1.0 : 1.0); +#endif +} + +DVec3 DVec3::PrepareRoundToZero() const +{ + // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float + constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1; + +#if defined(JPH_USE_AVX) + return _mm256_and_pd(mValue, _mm256_castsi256_pd(_mm256_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss)))); +#elif defined(JPH_USE_SSE) + __m128d mask = _mm_castsi128_pd(_mm_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss))); + return DVec3({ _mm_and_pd(mValue.mLow, mask), _mm_and_pd(mValue.mHigh, mask) }); +#elif defined(JPH_USE_NEON) + uint64x2_t mask = vdupq_n_u64(~cDoubleToFloatMantissaLoss); + return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mask)), + vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mask)) }); +#else + double x = BitCast(BitCast(mF64[0]) & ~cDoubleToFloatMantissaLoss); + double y = BitCast(BitCast(mF64[1]) & ~cDoubleToFloatMantissaLoss); + double z = BitCast(BitCast(mF64[2]) & ~cDoubleToFloatMantissaLoss); + + return DVec3(x, y, z); +#endif +} + +DVec3 DVec3::PrepareRoundToInf() const +{ + // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float + constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1; + +#if defined(JPH_USE_AVX512) + __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss); + __mmask8 is_zero = _mm256_testn_epi64_mask(_mm256_castpd_si256(mValue), mantissa_loss); + __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + return _mm256_mask_blend_pd(is_zero, value_or_mantissa_loss, mValue); +#elif defined(JPH_USE_AVX) + __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss); + __m256d value_and_mantissa_loss = _mm256_and_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + __m256d is_zero = _mm256_cmp_pd(value_and_mantissa_loss, _mm256_setzero_pd(), _CMP_EQ_OQ); + __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + return _mm256_blendv_pd(value_or_mantissa_loss, mValue, is_zero); +#elif defined(JPH_USE_SSE4_1) + __m128i mantissa_loss = _mm_set1_epi64x(cDoubleToFloatMantissaLoss); + __m128d zero = _mm_setzero_pd(); + __m128d value_and_mantissa_loss_low = _mm_and_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss)); + __m128d is_zero_low = _mm_cmpeq_pd(value_and_mantissa_loss_low, zero); + __m128d value_or_mantissa_loss_low = _mm_or_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss)); + __m128d value_and_mantissa_loss_high = _mm_and_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss)); + __m128d is_zero_high = _mm_cmpeq_pd(value_and_mantissa_loss_high, zero); + __m128d value_or_mantissa_loss_high = _mm_or_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss)); + return DVec3({ _mm_blendv_pd(value_or_mantissa_loss_low, mValue.mLow, is_zero_low), _mm_blendv_pd(value_or_mantissa_loss_high, mValue.mHigh, is_zero_high) }); +#elif defined(JPH_USE_NEON) + uint64x2_t mantissa_loss = vdupq_n_u64(cDoubleToFloatMantissaLoss); + float64x2_t zero = vdupq_n_f64(0.0); + float64x2_t value_and_mantissa_loss_low = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss)); + uint64x2_t is_zero_low = vceqq_f64(value_and_mantissa_loss_low, zero); + float64x2_t value_or_mantissa_loss_low = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss)); + float64x2_t value_and_mantissa_loss_high = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss)); + float64x2_t value_low = vbslq_f64(is_zero_low, mValue.val[0], value_or_mantissa_loss_low); + uint64x2_t is_zero_high = vceqq_f64(value_and_mantissa_loss_high, zero); + float64x2_t value_or_mantissa_loss_high = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss)); + float64x2_t value_high = vbslq_f64(is_zero_high, mValue.val[1], value_or_mantissa_loss_high); + return DVec3({ value_low, value_high }); +#else + uint64 ux = BitCast(mF64[0]); + uint64 uy = BitCast(mF64[1]); + uint64 uz = BitCast(mF64[2]); + + double x = BitCast((ux & cDoubleToFloatMantissaLoss) == 0? ux : (ux | cDoubleToFloatMantissaLoss)); + double y = BitCast((uy & cDoubleToFloatMantissaLoss) == 0? uy : (uy | cDoubleToFloatMantissaLoss)); + double z = BitCast((uz & cDoubleToFloatMantissaLoss) == 0? uz : (uz | cDoubleToFloatMantissaLoss)); + + return DVec3(x, y, z); +#endif +} + +Vec3 DVec3::ToVec3RoundDown() const +{ + DVec3 to_zero = PrepareRoundToZero(); + DVec3 to_inf = PrepareRoundToInf(); + return Vec3(DVec3::sSelect(to_zero, to_inf, DVec3::sLess(*this, DVec3::sZero()))); +} + +Vec3 DVec3::ToVec3RoundUp() const +{ + DVec3 to_zero = PrepareRoundToZero(); + DVec3 to_inf = PrepareRoundToInf(); + return Vec3(DVec3::sSelect(to_inf, to_zero, DVec3::sLess(*this, DVec3::sZero()))); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Double3.h b/thirdparty/jolt_physics/Jolt/Math/Double3.h new file mode 100644 index 000000000000..9b738ff8f8f8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Double3.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that holds 3 doubles. Used as a storage class. Convert to DVec3 for calculations. +class [[nodiscard]] Double3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Double3() = default; ///< Intentionally not initialized for performance reasons + Double3(const Double3 &inRHS) = default; + Double3 & operator = (const Double3 &inRHS) = default; + Double3(double inX, double inY, double inZ) : x(inX), y(inY), z(inZ) { } + + double operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 3); + return *(&x + inCoordinate); + } + + bool operator == (const Double3 &inRHS) const + { + return x == inRHS.x && y == inRHS.y && z == inRHS.z; + } + + bool operator != (const Double3 &inRHS) const + { + return x != inRHS.x || y != inRHS.y || z != inRHS.z; + } + + double x; + double y; + double z; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +// Create a std::hash/JPH::Hash for Double3 +JPH_MAKE_HASHABLE(JPH::Double3, t.x, t.y, t.z) diff --git a/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h b/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h new file mode 100644 index 000000000000..76db294c782c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Dynamic resizable matrix class +class [[nodiscard]] DynMatrix +{ +public: + /// Constructor + DynMatrix(const DynMatrix &) = default; + DynMatrix(uint inRows, uint inCols) : mRows(inRows), mCols(inCols) { mElements.resize(inRows * inCols); } + + /// Access an element + float operator () (uint inRow, uint inCol) const { JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; } + float & operator () (uint inRow, uint inCol) { JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; } + + /// Get dimensions + uint GetCols() const { return mCols; } + uint GetRows() const { return mRows; } + +private: + uint mRows; + uint mCols; + Array mElements; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h b/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h new file mode 100644 index 000000000000..920ddb9c9e72 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h @@ -0,0 +1,177 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Function to determine the eigen vectors and values of a N x N real symmetric matrix +/// by Jacobi transformations. This method is most suitable for N < 10. +/// +/// Taken and adapted from Numerical Recipies paragraph 11.1 +/// +/// An eigen vector is a vector v for which \f$A \: v = \lambda \: v\f$ +/// +/// Where: +/// A: A square matrix. +/// \f$\lambda\f$: a non-zero constant value. +/// +/// @see https://en.wikipedia.org/wiki/Eigenvalues_and_eigenvectors +/// +/// Matrix is a matrix type, which has dimensions N x N. +/// @param inMatrix is the matrix of which to return the eigenvalues and vectors +/// @param outEigVec will contain a matrix whose columns contain the normalized eigenvectors (must be identity before call) +/// @param outEigVal will contain the eigenvalues +template +bool EigenValueSymmetric(const Matrix &inMatrix, Matrix &outEigVec, Vector &outEigVal) +{ + // This algorithm can generate infinite values, see comment below + FPExceptionDisableInvalid disable_invalid; + (void)disable_invalid; + + // Maximum number of sweeps to make + const int cMaxSweeps = 50; + + // Get problem dimension + const uint n = inMatrix.GetRows(); + + // Make sure the dimensions are right + JPH_ASSERT(inMatrix.GetRows() == n); + JPH_ASSERT(inMatrix.GetCols() == n); + JPH_ASSERT(outEigVec.GetRows() == n); + JPH_ASSERT(outEigVec.GetCols() == n); + JPH_ASSERT(outEigVal.GetRows() == n); + JPH_ASSERT(outEigVec.IsIdentity()); + + // Get the matrix in a so we can mess with it + Matrix a = inMatrix; + + Vector b, z; + + for (uint ip = 0; ip < n; ++ip) + { + // Initialize b to diagonal of a + b[ip] = a(ip, ip); + + // Initialize output to diagonal of a + outEigVal[ip] = a(ip, ip); + + // Reset z + z[ip] = 0.0f; + } + + for (int sweep = 0; sweep < cMaxSweeps; ++sweep) + { + // Get the sum of the off-diagonal elements of a + float sm = 0.0f; + for (uint ip = 0; ip < n - 1; ++ip) + for (uint iq = ip + 1; iq < n; ++iq) + sm += abs(a(ip, iq)); + float avg_sm = sm / Square(n); + + // Normal return, convergence to machine underflow + if (avg_sm < FLT_MIN) // Original code: sm == 0.0f, when the average is denormal, we also consider it machine underflow + { + // Sanity checks + #ifdef JPH_ENABLE_ASSERTS + for (uint c = 0; c < n; ++c) + { + // Check if the eigenvector is normalized + JPH_ASSERT(outEigVec.GetColumn(c).IsNormalized()); + + // Check if inMatrix * eigen_vector = eigen_value * eigen_vector + Vector mat_eigvec = inMatrix * outEigVec.GetColumn(c); + Vector eigval_eigvec = outEigVal[c] * outEigVec.GetColumn(c); + JPH_ASSERT(mat_eigvec.IsClose(eigval_eigvec, max(mat_eigvec.LengthSq(), eigval_eigvec.LengthSq()) * 1.0e-6f)); + } + #endif + + // Success + return true; + } + + // On the first three sweeps use a fraction of the sum of the off diagonal elements as threshold + // Note that we pick a minimum threshold of FLT_MIN because dividing by a denormalized number is likely to result in infinity. + float tresh = sweep < 4? 0.2f * avg_sm : FLT_MIN; // Original code: 0.0f instead of FLT_MIN + + for (uint ip = 0; ip < n - 1; ++ip) + for (uint iq = ip + 1; iq < n; ++iq) + { + float &a_pq = a(ip, iq); + float &eigval_p = outEigVal[ip]; + float &eigval_q = outEigVal[iq]; + + float abs_a_pq = abs(a_pq); + float g = 100.0f * abs_a_pq; + + // After four sweeps, skip the rotation if the off-diagonal element is small + if (sweep > 4 + && abs(eigval_p) + g == abs(eigval_p) + && abs(eigval_q) + g == abs(eigval_q)) + { + a_pq = 0.0f; + } + else if (abs_a_pq > tresh) + { + float h = eigval_q - eigval_p; + float abs_h = abs(h); + + float t; + if (abs_h + g == abs_h) + { + t = a_pq / h; + } + else + { + float theta = 0.5f * h / a_pq; // Warning: Can become infinite if a(ip, iq) is very small which may trigger an invalid float exception + t = 1.0f / (abs(theta) + sqrt(1.0f + theta * theta)); // If theta becomes inf, t will be 0 so the infinite is not a problem for the algorithm + if (theta < 0.0f) t = -t; + } + + float c = 1.0f / sqrt(1.0f + t * t); + float s = t * c; + float tau = s / (1.0f + c); + h = t * a_pq; + + a_pq = 0.0f; + + z[ip] -= h; + z[iq] += h; + + eigval_p -= h; + eigval_q += h; + + #define JPH_EVS_ROTATE(a, i, j, k, l) \ + g = a(i, j), \ + h = a(k, l), \ + a(i, j) = g - s * (h + g * tau), \ + a(k, l) = h + s * (g - h * tau) + + uint j; + for (j = 0; j < ip; ++j) JPH_EVS_ROTATE(a, j, ip, j, iq); + for (j = ip + 1; j < iq; ++j) JPH_EVS_ROTATE(a, ip, j, j, iq); + for (j = iq + 1; j < n; ++j) JPH_EVS_ROTATE(a, ip, j, iq, j); + for (j = 0; j < n; ++j) JPH_EVS_ROTATE(outEigVec, j, ip, j, iq); + + #undef JPH_EVS_ROTATE + } + } + + // Update eigenvalues with the sum of ta_pq and reinitialize z + for (uint ip = 0; ip < n; ++ip) + { + b[ip] += z[ip]; + outEigVal[ip] = b[ip]; + z[ip] = 0.0f; + } + } + + // Failure + JPH_ASSERT(false, "Too many iterations"); + return false; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/FindRoot.h b/thirdparty/jolt_physics/Jolt/Math/FindRoot.h new file mode 100644 index 000000000000..21fef9f61f08 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/FindRoot.h @@ -0,0 +1,42 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Find the roots of \f$inA \: x^2 + inB \: x + inC = 0\f$. +/// @return The number of roots, actual roots in outX1 and outX2. +/// If number of roots returned is 1 then outX1 == outX2. +template +inline int FindRoot(const T inA, const T inB, const T inC, T &outX1, T &outX2) +{ + // Check if this is a linear equation + if (inA == T(0)) + { + // Check if this is a constant equation + if (inB == T(0)) + return 0; + + // Linear equation with 1 solution + outX1 = outX2 = -inC / inB; + return 1; + } + + // See Numerical Recipes in C, Chapter 5.6 Quadratic and Cubic Equations + T det = Square(inB) - T(4) * inA * inC; + if (det < T(0)) + return 0; + T q = (inB + Sign(inB) * sqrt(det)) / T(-2); + outX1 = q / inA; + if (q == T(0)) + { + outX2 = outX1; + return 1; + } + outX2 = inC / q; + return 2; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Float2.h b/thirdparty/jolt_physics/Jolt/Math/Float2.h new file mode 100644 index 000000000000..3e16e8c6c048 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Float2.h @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Class that holds 2 floats, used as a storage class mainly. +class [[nodiscard]] Float2 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Float2() = default; ///< Intentionally not initialized for performance reasons + Float2(const Float2 &inRHS) = default; + Float2 & operator = (const Float2 &inRHS) = default; + Float2(float inX, float inY) : x(inX), y(inY) { } + + bool operator == (const Float2 &inRHS) const { return x == inRHS.x && y == inRHS.y; } + bool operator != (const Float2 &inRHS) const { return x != inRHS.x || y != inRHS.y; } + + /// To String + friend ostream & operator << (ostream &inStream, const Float2 &inV) + { + inStream << inV.x << ", " << inV.y; + return inStream; + } + + float x; + float y; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Float3.h b/thirdparty/jolt_physics/Jolt/Math/Float3.h new file mode 100644 index 000000000000..1355e9ca0f81 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Float3.h @@ -0,0 +1,50 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that holds 3 floats. Used as a storage class. Convert to Vec3 for calculations. +class [[nodiscard]] Float3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Float3() = default; ///< Intentionally not initialized for performance reasons + Float3(const Float3 &inRHS) = default; + Float3 & operator = (const Float3 &inRHS) = default; + constexpr Float3(float inX, float inY, float inZ) : x(inX), y(inY), z(inZ) { } + + float operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 3); + return *(&x + inCoordinate); + } + + bool operator == (const Float3 &inRHS) const + { + return x == inRHS.x && y == inRHS.y && z == inRHS.z; + } + + bool operator != (const Float3 &inRHS) const + { + return x != inRHS.x || y != inRHS.y || z != inRHS.z; + } + + float x; + float y; + float z; +}; + +using VertexList = Array; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +// Create a std::hash/JPH::Hash for Float3 +JPH_MAKE_HASHABLE(JPH::Float3, t.x, t.y, t.z) diff --git a/thirdparty/jolt_physics/Jolt/Math/Float4.h b/thirdparty/jolt_physics/Jolt/Math/Float4.h new file mode 100644 index 000000000000..ca292b3ac3e4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Float4.h @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Class that holds 4 float values. Convert to Vec4 to perform calculations. +class [[nodiscard]] Float4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Float4() = default; ///< Intentionally not initialized for performance reasons + Float4(const Float4 &inRHS) = default; + Float4(float inX, float inY, float inZ, float inW) : x(inX), y(inY), z(inZ), w(inW) { } + + float operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 4); + return *(&x + inCoordinate); + } + + float x; + float y; + float z; + float w; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h b/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h new file mode 100644 index 000000000000..f986cf3b3b07 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h @@ -0,0 +1,102 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// This function performs Gauss-Jordan elimination to solve a matrix equation. +/// A must be an NxN matrix and B must be an NxM matrix forming the equation A * x = B +/// on output B will contain x and A will be destroyed. +/// +/// This code can be used for example to compute the inverse of a matrix. +/// Set A to the matrix to invert, set B to identity and let GaussianElimination solve +/// the equation, on return B will be the inverse of A. And A is destroyed. +/// +/// Taken and adapted from Numerical Recipies in C paragraph 2.1 +template +bool GaussianElimination(MatrixA &ioA, MatrixB &ioB, float inTolerance = 1.0e-16f) +{ + // Get problem dimensions + const uint n = ioA.GetCols(); + const uint m = ioB.GetCols(); + + // Check matrix requirement + JPH_ASSERT(ioA.GetRows() == n); + JPH_ASSERT(ioB.GetRows() == n); + + // Create array for bookkeeping on pivoting + int *ipiv = (int *)JPH_STACK_ALLOC(n * sizeof(int)); + memset(ipiv, 0, n * sizeof(int)); + + for (uint i = 0; i < n; ++i) + { + // Initialize pivot element as the diagonal + uint pivot_row = i, pivot_col = i; + + // Determine pivot element + float largest_element = 0.0f; + for (uint j = 0; j < n; ++j) + if (ipiv[j] != 1) + for (uint k = 0; k < n; ++k) + { + if (ipiv[k] == 0) + { + float element = abs(ioA(j, k)); + if (element >= largest_element) + { + largest_element = element; + pivot_row = j; + pivot_col = k; + } + } + else if (ipiv[k] > 1) + { + return false; + } + } + + // Mark this column as used + ++ipiv[pivot_col]; + + // Exchange rows when needed so that the pivot element is at ioA(pivot_col, pivot_col) instead of at ioA(pivot_row, pivot_col) + if (pivot_row != pivot_col) + { + for (uint j = 0; j < n; ++j) + std::swap(ioA(pivot_row, j), ioA(pivot_col, j)); + for (uint j = 0; j < m; ++j) + std::swap(ioB(pivot_row, j), ioB(pivot_col, j)); + } + + // Get diagonal element that we are about to set to 1 + float diagonal_element = ioA(pivot_col, pivot_col); + if (abs(diagonal_element) < inTolerance) + return false; + + // Divide the whole row by the pivot element, making ioA(pivot_col, pivot_col) = 1 + for (uint j = 0; j < n; ++j) + ioA(pivot_col, j) /= diagonal_element; + for (uint j = 0; j < m; ++j) + ioB(pivot_col, j) /= diagonal_element; + ioA(pivot_col, pivot_col) = 1.0f; + + // Next reduce the rows, except for the pivot one, + // after this step the pivot_col column is zero except for the pivot element which is 1 + for (uint j = 0; j < n; ++j) + if (j != pivot_col) + { + float element = ioA(j, pivot_col); + for (uint k = 0; k < n; ++k) + ioA(j, k) -= ioA(pivot_col, k) * element; + for (uint k = 0; k < m; ++k) + ioB(j, k) -= ioB(pivot_col, k) * element; + ioA(j, pivot_col) = 0.0f; + } + } + + // Success + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h b/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h new file mode 100644 index 000000000000..5b36cb095177 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h @@ -0,0 +1,204 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +using HalfFloat = uint16; + +// Define half float constant values +static constexpr HalfFloat HALF_FLT_MAX = 0x7bff; +static constexpr HalfFloat HALF_FLT_MAX_NEGATIVE = 0xfbff; +static constexpr HalfFloat HALF_FLT_INF = 0x7c00; +static constexpr HalfFloat HALF_FLT_INF_NEGATIVE = 0xfc00; +static constexpr HalfFloat HALF_FLT_NANQ = 0x7e00; +static constexpr HalfFloat HALF_FLT_NANQ_NEGATIVE = 0xfe00; + +namespace HalfFloatConversion { + +// Layout of a float +static constexpr int FLOAT_SIGN_POS = 31; +static constexpr int FLOAT_EXPONENT_POS = 23; +static constexpr int FLOAT_EXPONENT_BITS = 8; +static constexpr int FLOAT_EXPONENT_MASK = (1 << FLOAT_EXPONENT_BITS) - 1; +static constexpr int FLOAT_EXPONENT_BIAS = 127; +static constexpr int FLOAT_MANTISSA_BITS = 23; +static constexpr int FLOAT_MANTISSA_MASK = (1 << FLOAT_MANTISSA_BITS) - 1; +static constexpr int FLOAT_EXPONENT_AND_MANTISSA_MASK = FLOAT_MANTISSA_MASK + (FLOAT_EXPONENT_MASK << FLOAT_EXPONENT_POS); + +// Layout of half float +static constexpr int HALF_FLT_SIGN_POS = 15; +static constexpr int HALF_FLT_EXPONENT_POS = 10; +static constexpr int HALF_FLT_EXPONENT_BITS = 5; +static constexpr int HALF_FLT_EXPONENT_MASK = (1 << HALF_FLT_EXPONENT_BITS) - 1; +static constexpr int HALF_FLT_EXPONENT_BIAS = 15; +static constexpr int HALF_FLT_MANTISSA_BITS = 10; +static constexpr int HALF_FLT_MANTISSA_MASK = (1 << HALF_FLT_MANTISSA_BITS) - 1; +static constexpr int HALF_FLT_EXPONENT_AND_MANTISSA_MASK = HALF_FLT_MANTISSA_MASK + (HALF_FLT_EXPONENT_MASK << HALF_FLT_EXPONENT_POS); + +/// Define half-float rounding modes +enum ERoundingMode +{ + ROUND_TO_NEG_INF, ///< Round to negative infinity + ROUND_TO_POS_INF, ///< Round to positive infinity + ROUND_TO_NEAREST, ///< Round to nearest value +}; + +/// Convert a float (32-bits) to a half float (16-bits), fallback version when no intrinsics available +template +inline HalfFloat FromFloatFallback(float inV) +{ + // Reinterpret the float as an uint32 + uint32 value = BitCast(inV); + + // Extract exponent + uint32 exponent = (value >> FLOAT_EXPONENT_POS) & FLOAT_EXPONENT_MASK; + + // Extract mantissa + uint32 mantissa = value & FLOAT_MANTISSA_MASK; + + // Extract the sign and move it into the right spot for the half float (so we can just or it in at the end) + HalfFloat hf_sign = HalfFloat(value >> (FLOAT_SIGN_POS - HALF_FLT_SIGN_POS)) & (1 << HALF_FLT_SIGN_POS); + + // Check NaN or INF + if (exponent == FLOAT_EXPONENT_MASK) // NaN or INF + return hf_sign | (mantissa == 0? HALF_FLT_INF : HALF_FLT_NANQ); + + // Rebias the exponent for half floats + int rebiased_exponent = int(exponent) - FLOAT_EXPONENT_BIAS + HALF_FLT_EXPONENT_BIAS; + + // Check overflow to infinity + if (rebiased_exponent >= HALF_FLT_EXPONENT_MASK) + { + bool round_up = RoundingMode == ROUND_TO_NEAREST || (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF); + return hf_sign | (round_up? HALF_FLT_INF : HALF_FLT_MAX); + } + + // Check underflow to zero + if (rebiased_exponent < -HALF_FLT_MANTISSA_BITS) + { + bool round_up = RoundingMode != ROUND_TO_NEAREST && (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF) && (value & FLOAT_EXPONENT_AND_MANTISSA_MASK) != 0; + return hf_sign | (round_up? 1 : 0); + } + + HalfFloat hf_exponent; + int shift; + if (rebiased_exponent <= 0) + { + // Underflow to denormalized number + hf_exponent = 0; + mantissa |= 1 << FLOAT_MANTISSA_BITS; // Add the implicit 1 bit to the mantissa + shift = FLOAT_MANTISSA_BITS - HALF_FLT_MANTISSA_BITS + 1 - rebiased_exponent; + } + else + { + // Normal half float + hf_exponent = HalfFloat(rebiased_exponent << HALF_FLT_EXPONENT_POS); + shift = FLOAT_MANTISSA_BITS - HALF_FLT_MANTISSA_BITS; + } + + // Compose the half float + HalfFloat hf_mantissa = HalfFloat(mantissa >> shift); + HalfFloat hf = hf_sign | hf_exponent | hf_mantissa; + + // Calculate the remaining bits that we're discarding + uint remainder = mantissa & ((1 << shift) - 1); + + if constexpr (RoundingMode == ROUND_TO_NEAREST) + { + // Round to nearest + uint round_threshold = 1 << (shift - 1); + if (remainder > round_threshold // Above threshold, we must always round + || (remainder == round_threshold && (hf_mantissa & 1))) // When equal, round to nearest even + hf++; // May overflow to infinity + } + else + { + // Round up or down (truncate) depending on the rounding mode + bool round_up = (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF) && remainder != 0; + if (round_up) + hf++; // May overflow to infinity + } + + return hf; +} + +/// Convert a float (32-bits) to a half float (16-bits) +template +JPH_INLINE HalfFloat FromFloat(float inV) +{ +#ifdef JPH_USE_F16C + union + { + __m128i u128; + HalfFloat u16[8]; + } hf; + __m128 val = _mm_load_ss(&inV); + switch (RoundingMode) + { + case ROUND_TO_NEG_INF: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_NEG_INF); + break; + case ROUND_TO_POS_INF: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_POS_INF); + break; + case ROUND_TO_NEAREST: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_NEAREST_INT); + break; + } + return hf.u16[0]; +#else + return FromFloatFallback(inV); +#endif +} + +/// Convert 4 half floats (lower 64 bits) to floats, fallback version when no intrinsics available +inline Vec4 ToFloatFallback(UVec4Arg inValue) +{ + // Unpack half floats to 4 uint32's + UVec4 value = inValue.Expand4Uint16Lo(); + + // Normal half float path, extract the exponent and mantissa, shift them into place and update the exponent bias + UVec4 exponent_mantissa = UVec4::sAnd(value, UVec4::sReplicate(HALF_FLT_EXPONENT_AND_MANTISSA_MASK)).LogicalShiftLeft() + UVec4::sReplicate((FLOAT_EXPONENT_BIAS - HALF_FLT_EXPONENT_BIAS) << FLOAT_EXPONENT_POS); + + // Denormalized half float path, renormalize the float + UVec4 exponent_mantissa_denormalized = ((exponent_mantissa + UVec4::sReplicate(1 << FLOAT_EXPONENT_POS)).ReinterpretAsFloat() - UVec4::sReplicate((FLOAT_EXPONENT_BIAS - HALF_FLT_EXPONENT_BIAS + 1) << FLOAT_EXPONENT_POS).ReinterpretAsFloat()).ReinterpretAsInt(); + + // NaN / INF path, set all exponent bits + UVec4 exponent_mantissa_nan_inf = UVec4::sOr(exponent_mantissa, UVec4::sReplicate(FLOAT_EXPONENT_MASK << FLOAT_EXPONENT_POS)); + + // Get the exponent to determine which of the paths we should take + UVec4 exponent_mask = UVec4::sReplicate(HALF_FLT_EXPONENT_MASK << HALF_FLT_EXPONENT_POS); + UVec4 exponent = UVec4::sAnd(value, exponent_mask); + UVec4 is_denormalized = UVec4::sEquals(exponent, UVec4::sZero()); + UVec4 is_nan_inf = UVec4::sEquals(exponent, exponent_mask); + + // Select the correct result + UVec4 result_exponent_mantissa = UVec4::sSelect(UVec4::sSelect(exponent_mantissa, exponent_mantissa_nan_inf, is_nan_inf), exponent_mantissa_denormalized, is_denormalized); + + // Extract the sign bit and shift it to the left + UVec4 sign = UVec4::sAnd(value, UVec4::sReplicate(1 << HALF_FLT_SIGN_POS)).LogicalShiftLeft(); + + // Construct the float + return UVec4::sOr(sign, result_exponent_mantissa).ReinterpretAsFloat(); +} + +/// Convert 4 half floats (lower 64 bits) to floats +JPH_INLINE Vec4 ToFloat(UVec4Arg inValue) +{ +#if defined(JPH_USE_F16C) + return _mm_cvtph_ps(inValue.mValue); +#elif defined(JPH_USE_NEON) + return vcvt_f32_f16(vreinterpret_f16_u32(vget_low_u32(inValue.mValue))); +#else + return ToFloatFallback(inValue); +#endif +} + +} // HalfFloatConversion + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Mat44.h b/thirdparty/jolt_physics/Jolt/Math/Mat44.h new file mode 100644 index 000000000000..4774935efcc7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Mat44.h @@ -0,0 +1,243 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix. +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Mat44 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying column type + using Type = Vec4::Type; + + // Argument type + using ArgType = Mat44Arg; + + /// Constructor + Mat44() = default; ///< Intentionally not initialized for performance reasons + JPH_INLINE Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4); + JPH_INLINE Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4); + Mat44(const Mat44 &inM2) = default; + Mat44 & operator = (const Mat44 &inM2) = default; + JPH_INLINE Mat44(Type inC1, Type inC2, Type inC3, Type inC4); + + /// Zero matrix + static JPH_INLINE Mat44 sZero(); + + /// Identity matrix + static JPH_INLINE Mat44 sIdentity(); + + /// Matrix filled with NaN's + static JPH_INLINE Mat44 sNaN(); + + /// Load 16 floats from memory + static JPH_INLINE Mat44 sLoadFloat4x4(const Float4 *inV); + + /// Load 16 floats from memory, 16 bytes aligned + static JPH_INLINE Mat44 sLoadFloat4x4Aligned(const Float4 *inV); + + /// Rotate around X, Y or Z axis (angle in radians) + static JPH_INLINE Mat44 sRotationX(float inX); + static JPH_INLINE Mat44 sRotationY(float inY); + static JPH_INLINE Mat44 sRotationZ(float inZ); + + /// Rotate around arbitrary axis + static JPH_INLINE Mat44 sRotation(Vec3Arg inAxis, float inAngle); + + /// Rotate from quaternion + static JPH_INLINE Mat44 sRotation(QuatArg inQuat); + + /// Get matrix that translates + static JPH_INLINE Mat44 sTranslation(Vec3Arg inV); + + /// Get matrix that rotates and translates + static JPH_INLINE Mat44 sRotationTranslation(QuatArg inR, Vec3Arg inT); + + /// Get inverse matrix of sRotationTranslation + static JPH_INLINE Mat44 sInverseRotationTranslation(QuatArg inR, Vec3Arg inT); + + /// Get matrix that scales uniformly + static JPH_INLINE Mat44 sScale(float inScale); + + /// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal) + static JPH_INLINE Mat44 sScale(Vec3Arg inV); + + /// Get outer product of inV and inV2 (equivalent to \f$inV1 \otimes inV2\f$) + static JPH_INLINE Mat44 sOuterProduct(Vec3Arg inV1, Vec3Arg inV2); + + /// Get matrix that represents a cross product \f$A \times B = \text{sCrossProduct}(A) \: B\f$ + static JPH_INLINE Mat44 sCrossProduct(Vec3Arg inV); + + /// Returns matrix ML so that \f$ML(q) \: p = q \: p\f$ (where p and q are quaternions) + static JPH_INLINE Mat44 sQuatLeftMultiply(QuatArg inQ); + + /// Returns matrix MR so that \f$MR(q) \: p = p \: q\f$ (where p and q are quaternions) + static JPH_INLINE Mat44 sQuatRightMultiply(QuatArg inQ); + + /// Returns a look at matrix that transforms from world space to view space + /// @param inPos Position of the camera + /// @param inTarget Target of the camera + /// @param inUp Up vector + static JPH_INLINE Mat44 sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp); + + /// Returns a right-handed perspective projection matrix + static JPH_INLINE Mat44 sPerspective(float inFovY, float inAspect, float inNear, float inFar); + + /// Get float component by element index + JPH_INLINE float operator () (uint inRow, uint inColumn) const { JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; } + JPH_INLINE float & operator () (uint inRow, uint inColumn) { JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; } + + /// Comparison + JPH_INLINE bool operator == (Mat44Arg inM2) const; + JPH_INLINE bool operator != (Mat44Arg inM2) const { return !(*this == inM2); } + + /// Test if two matrices are close + JPH_INLINE bool IsClose(Mat44Arg inM2, float inMaxDistSq = 1.0e-12f) const; + + /// Multiply matrix by matrix + JPH_INLINE Mat44 operator * (Mat44Arg inM) const; + + /// Multiply vector by matrix + JPH_INLINE Vec3 operator * (Vec3Arg inV) const; + JPH_INLINE Vec4 operator * (Vec4Arg inV) const; + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$) + JPH_INLINE Vec3 Multiply3x3Transposed(Vec3Arg inV) const; + + /// Multiply 3x3 matrix by 3x3 matrix + JPH_INLINE Mat44 Multiply3x3(Mat44Arg inM) const; + + /// Multiply transpose of 3x3 matrix by 3x3 matrix (\f$result = this^T \: inM\f$) + JPH_INLINE Mat44 Multiply3x3LeftTransposed(Mat44Arg inM) const; + + /// Multiply 3x3 matrix by the transpose of a 3x3 matrix (\f$result = this \: inM^T\f$) + JPH_INLINE Mat44 Multiply3x3RightTransposed(Mat44Arg inM) const; + + /// Multiply matrix with float + JPH_INLINE Mat44 operator * (float inV) const; + friend JPH_INLINE Mat44 operator * (float inV, Mat44Arg inM) { return inM * inV; } + + /// Multiply matrix with float + JPH_INLINE Mat44 & operator *= (float inV); + + /// Per element addition of matrix + JPH_INLINE Mat44 operator + (Mat44Arg inM) const; + + /// Negate + JPH_INLINE Mat44 operator - () const; + + /// Per element subtraction of matrix + JPH_INLINE Mat44 operator - (Mat44Arg inM) const; + + /// Per element addition of matrix + JPH_INLINE Mat44 & operator += (Mat44Arg inM); + + /// Access to the columns + JPH_INLINE Vec3 GetAxisX() const { return Vec3(mCol[0]); } + JPH_INLINE void SetAxisX(Vec3Arg inV) { mCol[0] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisY() const { return Vec3(mCol[1]); } + JPH_INLINE void SetAxisY(Vec3Arg inV) { mCol[1] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisZ() const { return Vec3(mCol[2]); } + JPH_INLINE void SetAxisZ(Vec3Arg inV) { mCol[2] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetTranslation() const { return Vec3(mCol[3]); } + JPH_INLINE void SetTranslation(Vec3Arg inV) { mCol[3] = Vec4(inV, 1.0f); } + JPH_INLINE Vec3 GetDiagonal3() const { return Vec3(mCol[0][0], mCol[1][1], mCol[2][2]); } + JPH_INLINE void SetDiagonal3(Vec3Arg inV) { mCol[0][0] = inV.GetX(); mCol[1][1] = inV.GetY(); mCol[2][2] = inV.GetZ(); } + JPH_INLINE Vec4 GetDiagonal4() const { return Vec4(mCol[0][0], mCol[1][1], mCol[2][2], mCol[3][3]); } + JPH_INLINE void SetDiagonal4(Vec4Arg inV) { mCol[0][0] = inV.GetX(); mCol[1][1] = inV.GetY(); mCol[2][2] = inV.GetZ(); mCol[3][3] = inV.GetW(); } + JPH_INLINE Vec3 GetColumn3(uint inCol) const { JPH_ASSERT(inCol < 4); return Vec3(mCol[inCol]); } + JPH_INLINE void SetColumn3(uint inCol, Vec3Arg inV) { JPH_ASSERT(inCol < 4); mCol[inCol] = Vec4(inV, inCol == 3? 1.0f : 0.0f); } + JPH_INLINE Vec4 GetColumn4(uint inCol) const { JPH_ASSERT(inCol < 4); return mCol[inCol]; } + JPH_INLINE void SetColumn4(uint inCol, Vec4Arg inV) { JPH_ASSERT(inCol < 4); mCol[inCol] = inV; } + + /// Store matrix to memory + JPH_INLINE void StoreFloat4x4(Float4 *outV) const; + + /// Transpose matrix + JPH_INLINE Mat44 Transposed() const; + + /// Transpose 3x3 subpart of matrix + JPH_INLINE Mat44 Transposed3x3() const; + + /// Inverse 4x4 matrix + JPH_INLINE Mat44 Inversed() const; + + /// Inverse 4x4 matrix when it only contains rotation and translation + JPH_INLINE Mat44 InversedRotationTranslation() const; + + /// Get the determinant of a 3x3 matrix + JPH_INLINE float GetDeterminant3x3() const; + + /// Get the adjoint of a 3x3 matrix + JPH_INLINE Mat44 Adjointed3x3() const; + + /// Inverse 3x3 matrix + JPH_INLINE Mat44 Inversed3x3() const; + + /// *this = inM.Inversed3x3(), returns false if the matrix is singular in which case *this is unchanged + JPH_INLINE bool SetInversed3x3(Mat44Arg inM); + + /// Get rotation part only (note: retains the first 3 values from the bottom row) + JPH_INLINE Mat44 GetRotation() const; + + /// Get rotation part only (note: also clears the bottom row) + JPH_INLINE Mat44 GetRotationSafe() const; + + /// Updates the rotation part of this matrix (the first 3 columns) + JPH_INLINE void SetRotation(Mat44Arg inRotation); + + /// Convert to quaternion + JPH_INLINE Quat GetQuaternion() const; + + /// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved) + JPH_INLINE Mat44 GetDirectionPreservingMatrix() const { return GetRotation().Inversed3x3().Transposed3x3(); } + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE Mat44 PreTranslated(Vec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE Mat44 PostTranslated(Vec3Arg inTranslation) const; + + /// Scale a matrix: result = this * Mat44::sScale(inScale) + JPH_INLINE Mat44 PreScaled(Vec3Arg inScale) const; + + /// Scale a matrix: result = Mat44::sScale(inScale) * this + JPH_INLINE Mat44 PostScaled(Vec3Arg inScale) const; + + /// Decompose a matrix into a rotation & translation part and into a scale part so that: + /// this = return_value * Mat44::sScale(outScale). + /// This equation only holds when the matrix is orthogonal, if it is not the returned matrix + /// will be made orthogonal using the modified Gram-Schmidt algorithm (see: https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process) + JPH_INLINE Mat44 Decompose(Vec3 &outScale) const; + +#ifndef JPH_DOUBLE_PRECISION + /// In single precision mode just return the matrix itself + JPH_INLINE Mat44 ToMat44() const { return *this; } +#endif // !JPH_DOUBLE_PRECISION + + /// To String + friend ostream & operator << (ostream &inStream, Mat44Arg inM) + { + inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol[3]; + return inStream; + } + +private: + Vec4 mCol[4]; ///< Column +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Mat44.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/Mat44.inl b/thirdparty/jolt_physics/Jolt/Math/Mat44.inl new file mode 100644 index 000000000000..76577b7153a6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Mat44.inl @@ -0,0 +1,952 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#define JPH_EL(r, c) mCol[c].mF32[r] + +Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4) : + mCol { inC1, inC2, inC3, inC4 } +{ +} + +Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4) : + mCol { inC1, inC2, inC3, Vec4(inC4, 1.0f) } +{ +} + +Mat44::Mat44(Type inC1, Type inC2, Type inC3, Type inC4) : + mCol { inC1, inC2, inC3, inC4 } +{ +} + +Mat44 Mat44::sZero() +{ + return Mat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), Vec4::sZero()); +} + +Mat44 Mat44::sIdentity() +{ + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sNaN() +{ + return Mat44(Vec4::sNaN(), Vec4::sNaN(), Vec4::sNaN(), Vec4::sNaN()); +} + +Mat44 Mat44::sLoadFloat4x4(const Float4 *inV) +{ + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = Vec4::sLoadFloat4(inV + c); + return result; +} + +Mat44 Mat44::sLoadFloat4x4Aligned(const Float4 *inV) +{ + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = Vec4::sLoadFloat4Aligned(inV + c); + return result; +} + +Mat44 Mat44::sRotationX(float inX) +{ + Vec4 sv, cv; + Vec4::sReplicate(inX).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, c, s, 0), Vec4(0, -s, c, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotationY(float inY) +{ + Vec4 sv, cv; + Vec4::sReplicate(inY).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(c, 0, -s, 0), Vec4(0, 1, 0, 0), Vec4(s, 0, c, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotationZ(float inZ) +{ + Vec4 sv, cv; + Vec4::sReplicate(inZ).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(c, s, 0, 0), Vec4(-s, c, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotation(QuatArg inQuat) +{ + JPH_ASSERT(inQuat.IsNormalized()); + + // See: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation section 'Quaternion-derived rotation matrix' +#ifdef JPH_USE_SSE4_1 + __m128 xyzw = inQuat.mValue.mValue; + __m128 two_xyzw = _mm_add_ps(xyzw, xyzw); + __m128 yzxw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 0, 2, 1)); + __m128 two_yzxw = _mm_add_ps(yzxw, yzxw); + __m128 zxyw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 1, 0, 2)); + __m128 two_zxyw = _mm_add_ps(zxyw, zxyw); + __m128 wwww = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 3, 3, 3)); + __m128 diagonal = _mm_sub_ps(_mm_sub_ps(_mm_set1_ps(1.0f), _mm_mul_ps(two_yzxw, yzxw)), _mm_mul_ps(two_zxyw, zxyw)); // (1 - 2 y^2 - 2 z^2, 1 - 2 x^2 - 2 z^2, 1 - 2 x^2 - 2 y^2, 1 - 4 w^2) + __m128 plus = _mm_add_ps(_mm_mul_ps(two_xyzw, zxyw), _mm_mul_ps(two_yzxw, wwww)); // 2 * (xz + yw, xy + zw, yz + xw, ww) + __m128 minus = _mm_sub_ps(_mm_mul_ps(two_yzxw, xyzw), _mm_mul_ps(two_zxyw, wwww)); // 2 * (xy - zw, yz - xw, xz - yw, 0) + + // Workaround for compiler changing _mm_sub_ps(_mm_mul_ps(...), ...) into a fused multiply sub instruction, resulting in w not being 0 + // There doesn't appear to be a reliable way to turn this off in Clang + minus = _mm_insert_ps(minus, minus, 0b1000); + + __m128 col0 = _mm_blend_ps(_mm_blend_ps(plus, diagonal, 0b0001), minus, 0b1100); // (1 - 2 y^2 - 2 z^2, 2 xy + 2 zw, 2 xz - 2 yw, 0) + __m128 col1 = _mm_blend_ps(_mm_blend_ps(diagonal, minus, 0b1001), plus, 0b0100); // (2 xy - 2 zw, 1 - 2 x^2 - 2 z^2, 2 yz + 2 xw, 0) + __m128 col2 = _mm_blend_ps(_mm_blend_ps(minus, plus, 0b0001), diagonal, 0b0100); // (2 xz + 2 yw, 2 yz - 2 xw, 1 - 2 x^2 - 2 y^2, 0) + __m128 col3 = _mm_set_ps(1, 0, 0, 0); + + return Mat44(col0, col1, col2, col3); +#else + float x = inQuat.GetX(); + float y = inQuat.GetY(); + float z = inQuat.GetZ(); + float w = inQuat.GetW(); + + float tx = x + x; // Note: Using x + x instead of 2.0f * x to force this function to return the same value as the SSE4.1 version across platforms. + float ty = y + y; + float tz = z + z; + + float xx = tx * x; + float yy = ty * y; + float zz = tz * z; + float xy = tx * y; + float xz = tx * z; + float xw = tx * w; + float yz = ty * z; + float yw = ty * w; + float zw = tz * w; + + return Mat44(Vec4((1.0f - yy) - zz, xy + zw, xz - yw, 0.0f), // Note: Added extra brackets to force this function to return the same value as the SSE4.1 version across platforms. + Vec4(xy - zw, (1.0f - zz) - xx, yz + xw, 0.0f), + Vec4(xz + yw, yz - xw, (1.0f - xx) - yy, 0.0f), + Vec4(0.0f, 0.0f, 0.0f, 1.0f)); +#endif +} + +Mat44 Mat44::sRotation(Vec3Arg inAxis, float inAngle) +{ + return sRotation(Quat::sRotation(inAxis, inAngle)); +} + +Mat44 Mat44::sTranslation(Vec3Arg inV) +{ + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(inV, 1)); +} + +Mat44 Mat44::sRotationTranslation(QuatArg inR, Vec3Arg inT) +{ + Mat44 m = sRotation(inR); + m.SetTranslation(inT); + return m; +} + +Mat44 Mat44::sInverseRotationTranslation(QuatArg inR, Vec3Arg inT) +{ + Mat44 m = sRotation(inR.Conjugated()); + m.SetTranslation(-m.Multiply3x3(inT)); + return m; +} + +Mat44 Mat44::sScale(float inScale) +{ + return Mat44(Vec4(inScale, 0, 0, 0), Vec4(0, inScale, 0, 0), Vec4(0, 0, inScale, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sScale(Vec3Arg inV) +{ + return Mat44(Vec4(inV.GetX(), 0, 0, 0), Vec4(0, inV.GetY(), 0, 0), Vec4(0, 0, inV.GetZ(), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sOuterProduct(Vec3Arg inV1, Vec3Arg inV2) +{ + Vec4 v1(inV1, 0); + return Mat44(v1 * inV2.SplatX(), v1 * inV2.SplatY(), v1 * inV2.SplatZ(), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sCrossProduct(Vec3Arg inV) +{ +#ifdef JPH_USE_SSE4_1 + // Zero out the W component + __m128 zero = _mm_setzero_ps(); + __m128 v = _mm_blend_ps(inV.mValue, zero, 0b1000); + + // Negate + __m128 min_v = _mm_sub_ps(zero, v); + + return Mat44( + _mm_shuffle_ps(v, min_v, _MM_SHUFFLE(3, 1, 2, 3)), // [0, z, -y, 0] + _mm_shuffle_ps(min_v, v, _MM_SHUFFLE(3, 0, 3, 2)), // [-z, 0, x, 0] + _mm_blend_ps(_mm_shuffle_ps(v, v, _MM_SHUFFLE(3, 3, 3, 1)), _mm_shuffle_ps(min_v, min_v, _MM_SHUFFLE(3, 3, 0, 3)), 0b0010), // [y, -x, 0, 0] + Vec4(0, 0, 0, 1)); +#else + float x = inV.GetX(); + float y = inV.GetY(); + float z = inV.GetZ(); + + return Mat44( + Vec4(0, z, -y, 0), + Vec4(-z, 0, x, 0), + Vec4(y, -x, 0, 0), + Vec4(0, 0, 0, 1)); +#endif +} + +Mat44 Mat44::sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp) +{ + Vec3 direction = (inTarget - inPos).NormalizedOr(-Vec3::sAxisZ()); + Vec3 right = direction.Cross(inUp).NormalizedOr(Vec3::sAxisX()); + Vec3 up = right.Cross(direction); + + return Mat44(Vec4(right, 0), Vec4(up, 0), Vec4(-direction, 0), Vec4(inPos, 1)).InversedRotationTranslation(); +} + +Mat44 Mat44::sPerspective(float inFovY, float inAspect, float inNear, float inFar) +{ + float height = 1.0f / Tan(0.5f * inFovY); + float width = height / inAspect; + float range = inFar / (inNear - inFar); + + return Mat44(Vec4(width, 0.0f, 0.0f, 0.0f), Vec4(0.0f, height, 0.0f, 0.0f), Vec4(0.0f, 0.0f, range, -1.0f), Vec4(0.0f, 0.0f, range * inNear, 0.0f)); +} + +bool Mat44::operator == (Mat44Arg inM2) const +{ + return UVec4::sAnd( + UVec4::sAnd(Vec4::sEquals(mCol[0], inM2.mCol[0]), Vec4::sEquals(mCol[1], inM2.mCol[1])), + UVec4::sAnd(Vec4::sEquals(mCol[2], inM2.mCol[2]), Vec4::sEquals(mCol[3], inM2.mCol[3])) + ).TestAllTrue(); +} + +bool Mat44::IsClose(Mat44Arg inM2, float inMaxDistSq) const +{ + for (int i = 0; i < 4; ++i) + if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq)) + return false; + return true; +} + +Mat44 Mat44::operator * (Mat44Arg inM) const +{ + Mat44 result; +#if defined(JPH_USE_SSE) + for (int i = 0; i < 4; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 3, 3, 3)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 4; ++i) + { + Type c = inM.mCol[i].mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(c, 3)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2] + mCol[3] * inM.mCol[i].mF32[3]; +#endif + return result; +} + +Vec3 Mat44::operator * (Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, mCol[3].mValue); + return Vec3::sFixW(t); +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + t = vaddq_f32(t, mCol[3].mValue); // Don't combine this with the first mul into a fused multiply add, causes precision issues + return Vec3::sFixW(t); +#else + return Vec3( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2]); +#endif +} + +Vec4 Mat44::operator * (Vec4Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(3, 3, 3, 3)))); + return t; +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(inV.mValue, 3)); + return t; +#else + return Vec4( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0] * inV.mF32[3], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1] * inV.mF32[3], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2] * inV.mF32[3], + mCol[0].mF32[3] * inV.mF32[0] + mCol[1].mF32[3] * inV.mF32[1] + mCol[2].mF32[3] * inV.mF32[2] + mCol[3].mF32[3] * inV.mF32[3]); +#endif +} + +Vec3 Mat44::Multiply3x3(Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + return Vec3::sFixW(t); +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + return Vec3::sFixW(t); +#else + return Vec3( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]); +#endif +} + +Vec3 Mat44::Multiply3x3Transposed(Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE4_1) + __m128 x = _mm_dp_ps(mCol[0].mValue, inV.mValue, 0x7f); + __m128 y = _mm_dp_ps(mCol[1].mValue, inV.mValue, 0x7f); + __m128 xy = _mm_blend_ps(x, y, 0b0010); + __m128 z = _mm_dp_ps(mCol[2].mValue, inV.mValue, 0x7f); + __m128 xyzz = _mm_blend_ps(xy, z, 0b1100); + return xyzz; +#else + return Transposed3x3().Multiply3x3(inV); +#endif +} + +Mat44 Mat44::Multiply3x3(Mat44Arg inM) const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + Mat44 result; +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.mCol[i].mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2]; +#endif + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Multiply3x3LeftTransposed(Mat44Arg inM) const +{ + // Transpose left hand side + Mat44 trans = Transposed3x3(); + + // Do 3x3 matrix multiply + Mat44 result; + result.mCol[0] = trans.mCol[0] * inM.mCol[0].SplatX() + trans.mCol[1] * inM.mCol[0].SplatY() + trans.mCol[2] * inM.mCol[0].SplatZ(); + result.mCol[1] = trans.mCol[0] * inM.mCol[1].SplatX() + trans.mCol[1] * inM.mCol[1].SplatY() + trans.mCol[2] * inM.mCol[1].SplatZ(); + result.mCol[2] = trans.mCol[0] * inM.mCol[2].SplatX() + trans.mCol[1] * inM.mCol[2].SplatY() + trans.mCol[2] * inM.mCol[2].SplatZ(); + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Multiply3x3RightTransposed(Mat44Arg inM) const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + Mat44 result; + result.mCol[0] = mCol[0] * inM.mCol[0].SplatX() + mCol[1] * inM.mCol[1].SplatX() + mCol[2] * inM.mCol[2].SplatX(); + result.mCol[1] = mCol[0] * inM.mCol[0].SplatY() + mCol[1] * inM.mCol[1].SplatY() + mCol[2] * inM.mCol[2].SplatY(); + result.mCol[2] = mCol[0] * inM.mCol[0].SplatZ() + mCol[1] * inM.mCol[1].SplatZ() + mCol[2] * inM.mCol[2].SplatZ(); + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::operator * (float inV) const +{ + Vec4 multiplier = Vec4::sReplicate(inV); + + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = mCol[c] * multiplier; + return result; +} + +Mat44 &Mat44::operator *= (float inV) +{ + for (int c = 0; c < 4; ++c) + mCol[c] *= inV; + + return *this; +} + +Mat44 Mat44::operator + (Mat44Arg inM) const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[i] + inM.mCol[i]; + return result; +} + +Mat44 Mat44::operator - () const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = -mCol[i]; + return result; +} + +Mat44 Mat44::operator - (Mat44Arg inM) const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[i] - inM.mCol[i]; + return result; +} + +Mat44 &Mat44::operator += (Mat44Arg inM) +{ + for (int c = 0; c < 4; ++c) + mCol[c] += inM.mCol[c]; + + return *this; +} + +void Mat44::StoreFloat4x4(Float4 *outV) const +{ + for (int c = 0; c < 4; ++c) + mCol[c].StoreFloat4(outV + c); +} + +Mat44 Mat44::Transposed() const +{ +#if defined(JPH_USE_SSE) + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + + Mat44 result; + result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1)); + result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[3].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(3, 1, 3, 1)); + return result; +#elif defined(JPH_USE_NEON) + float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue); + float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, mCol[3].mValue); + float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]); + float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]); + + Mat44 result; + result.mCol[0].mValue = tmp3.val[0]; + result.mCol[1].mValue = tmp3.val[1]; + result.mCol[2].mValue = tmp4.val[0]; + result.mCol[3].mValue = tmp4.val[1]; + return result; +#else + Mat44 result; + for (int c = 0; c < 4; ++c) + for (int r = 0; r < 4; ++r) + result.mCol[r].mF32[c] = mCol[c].mF32[r]; + return result; +#endif +} + +Mat44 Mat44::Transposed3x3() const +{ +#if defined(JPH_USE_SSE) + __m128 zero = _mm_setzero_ps(); + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(3, 2, 3, 2)); + + Mat44 result; + result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1)); + result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0)); +#elif defined(JPH_USE_NEON) + float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue); + float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, vdupq_n_f32(0)); + float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]); + float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]); + + Mat44 result; + result.mCol[0].mValue = tmp3.val[0]; + result.mCol[1].mValue = tmp3.val[1]; + result.mCol[2].mValue = tmp4.val[0]; +#else + Mat44 result; + for (int c = 0; c < 3; ++c) + { + for (int r = 0; r < 3; ++r) + result.mCol[c].mF32[r] = mCol[r].mF32[c]; + result.mCol[c].mF32[3] = 0; + } +#endif + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Inversed() const +{ +#if defined(JPH_USE_SSE) + // Algorithm from: http://download.intel.com/design/PentiumIII/sml/24504301.pdf + // Streaming SIMD Extensions - Inverse of 4x4 Matrix + // Adapted to load data using _mm_shuffle_ps instead of loading from memory + // Replaced _mm_rcp_ps with _mm_div_ps for better accuracy + + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 row1 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0)); + row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1)); + tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 row3 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0)); + row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1)); + + tmp1 = _mm_mul_ps(row2, row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + __m128 minor0 = _mm_mul_ps(row1, tmp1); + __m128 minor1 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0); + minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1); + minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(row1, row2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0); + __m128 minor3 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1)); + minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3); + minor3 = _mm_shuffle_ps(minor3, minor3, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0); + __m128 minor2 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1)); + minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2); + minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(row0, row1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2); + minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2); + minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1)); + + tmp1 = _mm_mul_ps(row0, row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1)); + minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1); + minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1)); + + tmp1 = _mm_mul_ps(row0, row2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1); + minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1)); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1)); + minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3); + + __m128 det = _mm_mul_ps(row0, minor0); + det = _mm_add_ps(_mm_shuffle_ps(det, det, _MM_SHUFFLE(2, 3, 0, 1)), det); // Original code did (x + z) + (y + w), changed to (x + y) + (z + w) to match the ARM code below and make the result cross platform deterministic + det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det); + det = _mm_div_ss(_mm_set_ss(1.0f), det); + det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0)); + + Mat44 result; + result.mCol[0].mValue = _mm_mul_ps(det, minor0); + result.mCol[1].mValue = _mm_mul_ps(det, minor1); + result.mCol[2].mValue = _mm_mul_ps(det, minor2); + result.mCol[3].mValue = _mm_mul_ps(det, minor3); + return result; +#elif defined(JPH_USE_NEON) + // Adapted from the SSE version, there's surprising few articles about efficient ways of calculating an inverse for ARM on the internet + Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5); + Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 0, 1, 4, 5); + Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6); + row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7); + tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7); + Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 2, 3, 6, 7); + Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6); + row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7); + + tmp1 = vmulq_f32(row2, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + Type minor0 = vmulq_f32(row1, tmp1); + Type minor1 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0); + minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1); + minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1); + + tmp1 = vmulq_f32(row1, row2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0); + Type minor3 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1)); + minor3 = vsubq_f32(vmulq_f32(row0, tmp1), minor3); + minor3 = JPH_NEON_SHUFFLE_F32x4(minor3, minor3, 2, 3, 0, 1); + + tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1); + tmp1 = vmulq_f32(tmp1, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1); + minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0); + Type minor2 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1)); + minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2); + minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1); + + tmp1 = vmulq_f32(row0, row1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2); + minor3 = vsubq_f32(vmulq_f32(row2, tmp1), minor3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2); + minor3 = vsubq_f32(minor3, vmulq_f32(row2, tmp1)); + + tmp1 = vmulq_f32(row0, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1)); + minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1); + minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1)); + + tmp1 = vmulq_f32(row0, row2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1); + minor3 = vsubq_f32(minor3, vmulq_f32(row1, tmp1)); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1)); + minor3 = vaddq_f32(vmulq_f32(row1, tmp1), minor3); + + Type det = vmulq_f32(row0, minor0); + det = vdupq_n_f32(vaddvq_f32(det)); + det = vdivq_f32(vdupq_n_f32(1.0f), det); + + Mat44 result; + result.mCol[0].mValue = vmulq_f32(det, minor0); + result.mCol[1].mValue = vmulq_f32(det, minor1); + result.mCol[2].mValue = vmulq_f32(det, minor2); + result.mCol[3].mValue = vmulq_f32(det, minor3); + return result; +#else + float m00 = JPH_EL(0, 0), m10 = JPH_EL(1, 0), m20 = JPH_EL(2, 0), m30 = JPH_EL(3, 0); + float m01 = JPH_EL(0, 1), m11 = JPH_EL(1, 1), m21 = JPH_EL(2, 1), m31 = JPH_EL(3, 1); + float m02 = JPH_EL(0, 2), m12 = JPH_EL(1, 2), m22 = JPH_EL(2, 2), m32 = JPH_EL(3, 2); + float m03 = JPH_EL(0, 3), m13 = JPH_EL(1, 3), m23 = JPH_EL(2, 3), m33 = JPH_EL(3, 3); + + float m10211120 = m10 * m21 - m11 * m20; + float m10221220 = m10 * m22 - m12 * m20; + float m10231320 = m10 * m23 - m13 * m20; + float m10311130 = m10 * m31 - m11 * m30; + float m10321230 = m10 * m32 - m12 * m30; + float m10331330 = m10 * m33 - m13 * m30; + float m11221221 = m11 * m22 - m12 * m21; + float m11231321 = m11 * m23 - m13 * m21; + float m11321231 = m11 * m32 - m12 * m31; + float m11331331 = m11 * m33 - m13 * m31; + float m12231322 = m12 * m23 - m13 * m22; + float m12331332 = m12 * m33 - m13 * m32; + float m20312130 = m20 * m31 - m21 * m30; + float m20322230 = m20 * m32 - m22 * m30; + float m20332330 = m20 * m33 - m23 * m30; + float m21322231 = m21 * m32 - m22 * m31; + float m21332331 = m21 * m33 - m23 * m31; + float m22332332 = m22 * m33 - m23 * m32; + + Vec4 col0(m11 * m22332332 - m12 * m21332331 + m13 * m21322231, -m10 * m22332332 + m12 * m20332330 - m13 * m20322230, m10 * m21332331 - m11 * m20332330 + m13 * m20312130, -m10 * m21322231 + m11 * m20322230 - m12 * m20312130); + Vec4 col1(-m01 * m22332332 + m02 * m21332331 - m03 * m21322231, m00 * m22332332 - m02 * m20332330 + m03 * m20322230, -m00 * m21332331 + m01 * m20332330 - m03 * m20312130, m00 * m21322231 - m01 * m20322230 + m02 * m20312130); + Vec4 col2(m01 * m12331332 - m02 * m11331331 + m03 * m11321231, -m00 * m12331332 + m02 * m10331330 - m03 * m10321230, m00 * m11331331 - m01 * m10331330 + m03 * m10311130, -m00 * m11321231 + m01 * m10321230 - m02 * m10311130); + Vec4 col3(-m01 * m12231322 + m02 * m11231321 - m03 * m11221221, m00 * m12231322 - m02 * m10231320 + m03 * m10221220, -m00 * m11231321 + m01 * m10231320 - m03 * m10211120, m00 * m11221221 - m01 * m10221220 + m02 * m10211120); + + float det = m00 * col0.mF32[0] + m01 * col0.mF32[1] + m02 * col0.mF32[2] + m03 * col0.mF32[3]; + + return Mat44(col0 / det, col1 / det, col2 / det, col3 / det); +#endif +} + +Mat44 Mat44::InversedRotationTranslation() const +{ + Mat44 m = Transposed3x3(); + m.SetTranslation(-m.Multiply3x3(GetTranslation())); + return m; +} + +float Mat44::GetDeterminant3x3() const +{ + return GetAxisX().Dot(GetAxisY().Cross(GetAxisZ())); +} + +Mat44 Mat44::Adjointed3x3() const +{ + return Mat44( + Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0) + - Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0), + Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0) + - Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0), + Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) + - Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0), + Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::Inversed3x3() const +{ + float det = GetDeterminant3x3(); + + return Mat44( + (Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0) + - Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)) / det, + (Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0) + - Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)) / det, + (Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) + - Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0)) / det, + Vec4(0, 0, 0, 1)); +} + +bool Mat44::SetInversed3x3(Mat44Arg inM) +{ + float det = inM.GetDeterminant3x3(); + + // If the determinant is zero the matrix is singular and we return false + if (det == 0.0f) + return false; + + // Finish calculating the inverse + *this = inM.Adjointed3x3(); + mCol[0] /= det; + mCol[1] /= det; + mCol[2] /= det; + return true; +} + +Quat Mat44::GetQuaternion() const +{ + float tr = mCol[0].mF32[0] + mCol[1].mF32[1] + mCol[2].mF32[2]; + + if (tr >= 0.0f) + { + float s = sqrt(tr + 1.0f); + float is = 0.5f / s; + return Quat( + (mCol[1].mF32[2] - mCol[2].mF32[1]) * is, + (mCol[2].mF32[0] - mCol[0].mF32[2]) * is, + (mCol[0].mF32[1] - mCol[1].mF32[0]) * is, + 0.5f * s); + } + else + { + int i = 0; + if (mCol[1].mF32[1] > mCol[0].mF32[0]) i = 1; + if (mCol[2].mF32[2] > mCol[i].mF32[i]) i = 2; + + if (i == 0) + { + float s = sqrt(mCol[0].mF32[0] - (mCol[1].mF32[1] + mCol[2].mF32[2]) + 1); + float is = 0.5f / s; + return Quat( + 0.5f * s, + (mCol[1].mF32[0] + mCol[0].mF32[1]) * is, + (mCol[0].mF32[2] + mCol[2].mF32[0]) * is, + (mCol[1].mF32[2] - mCol[2].mF32[1]) * is); + } + else if (i == 1) + { + float s = sqrt(mCol[1].mF32[1] - (mCol[2].mF32[2] + mCol[0].mF32[0]) + 1); + float is = 0.5f / s; + return Quat( + (mCol[1].mF32[0] + mCol[0].mF32[1]) * is, + 0.5f * s, + (mCol[2].mF32[1] + mCol[1].mF32[2]) * is, + (mCol[2].mF32[0] - mCol[0].mF32[2]) * is); + } + else + { + JPH_ASSERT(i == 2); + + float s = sqrt(mCol[2].mF32[2] - (mCol[0].mF32[0] + mCol[1].mF32[1]) + 1); + float is = 0.5f / s; + return Quat( + (mCol[0].mF32[2] + mCol[2].mF32[0]) * is, + (mCol[2].mF32[1] + mCol[1].mF32[2]) * is, + 0.5f * s, + (mCol[0].mF32[1] - mCol[1].mF32[0]) * is); + } + } +} + +Mat44 Mat44::sQuatLeftMultiply(QuatArg inQ) +{ + return Mat44( + Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle(), + Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle(), + Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle(), + inQ.mValue); +} + +Mat44 Mat44::sQuatRightMultiply(QuatArg inQ) +{ + return Mat44( + Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle(), + Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle(), + Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle(), + inQ.mValue); +} + +Mat44 Mat44::GetRotation() const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::GetRotationSafe() const +{ +#if defined(JPH_USE_AVX512) + return Mat44(_mm_maskz_mov_ps(0b0111, mCol[0].mValue), + _mm_maskz_mov_ps(0b0111, mCol[1].mValue), + _mm_maskz_mov_ps(0b0111, mCol[2].mValue), + Vec4(0, 0, 0, 1)); +#elif defined(JPH_USE_SSE4_1) + __m128 zero = _mm_setzero_ps(); + return Mat44(_mm_blend_ps(mCol[0].mValue, zero, 8), + _mm_blend_ps(mCol[1].mValue, zero, 8), + _mm_blend_ps(mCol[2].mValue, zero, 8), + Vec4(0, 0, 0, 1)); +#elif defined(JPH_USE_NEON) + return Mat44(vsetq_lane_f32(0, mCol[0].mValue, 3), + vsetq_lane_f32(0, mCol[1].mValue, 3), + vsetq_lane_f32(0, mCol[2].mValue, 3), + Vec4(0, 0, 0, 1)); +#else + return Mat44(Vec4(mCol[0].mF32[0], mCol[0].mF32[1], mCol[0].mF32[2], 0), + Vec4(mCol[1].mF32[0], mCol[1].mF32[1], mCol[1].mF32[2], 0), + Vec4(mCol[2].mF32[0], mCol[2].mF32[1], mCol[2].mF32[2], 0), + Vec4(0, 0, 0, 1)); +#endif +} + +void Mat44::SetRotation(Mat44Arg inRotation) +{ + mCol[0] = inRotation.mCol[0]; + mCol[1] = inRotation.mCol[1]; + mCol[2] = inRotation.mCol[2]; +} + +Mat44 Mat44::PreTranslated(Vec3Arg inTranslation) const +{ + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + Multiply3x3(inTranslation), 1)); +} + +Mat44 Mat44::PostTranslated(Vec3Arg inTranslation) const +{ + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + inTranslation, 1)); +} + +Mat44 Mat44::PreScaled(Vec3Arg inScale) const +{ + return Mat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol[3]); +} + +Mat44 Mat44::PostScaled(Vec3Arg inScale) const +{ + Vec4 scale(inScale, 1); + return Mat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], scale * mCol[3]); +} + +Mat44 Mat44::Decompose(Vec3 &outScale) const +{ + // Start the modified Gram-Schmidt algorithm + // X axis will just be normalized + Vec3 x = GetAxisX(); + + // Make Y axis perpendicular to X + Vec3 y = GetAxisY(); + float x_dot_x = x.LengthSq(); + y -= (x.Dot(y) / x_dot_x) * x; + + // Make Z axis perpendicular to X + Vec3 z = GetAxisZ(); + z -= (x.Dot(z) / x_dot_x) * x; + + // Make Z axis perpendicular to Y + float y_dot_y = y.LengthSq(); + z -= (y.Dot(z) / y_dot_y) * y; + + // Determine the scale + float z_dot_z = z.LengthSq(); + outScale = Vec3(x_dot_x, y_dot_y, z_dot_z).Sqrt(); + + // If the resulting x, y and z vectors don't form a right handed matrix, flip the z axis. + if (x.Cross(y).Dot(z) < 0.0f) + outScale.SetZ(-outScale.GetZ()); + + // Determine the rotation and translation + return Mat44(Vec4(x / outScale.GetX(), 0), Vec4(y / outScale.GetY(), 0), Vec4(z / outScale.GetZ(), 0), GetColumn4(3)); +} + +#undef JPH_EL + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Math.h b/thirdparty/jolt_physics/Jolt/Math/Math.h new file mode 100644 index 000000000000..729d5403e393 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Math.h @@ -0,0 +1,205 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// The constant \f$\pi\f$ +static constexpr float JPH_PI = 3.14159265358979323846f; + +/// Convert a value from degrees to radians +JPH_INLINE constexpr float DegreesToRadians(float inV) +{ + return inV * (JPH_PI / 180.0f); +} + +/// Convert a value from radians to degrees +JPH_INLINE constexpr float RadiansToDegrees(float inV) +{ + return inV * (180.0f / JPH_PI); +} + +/// Convert angle in radians to the range \f$[-\pi, \pi]\f$ +inline float CenterAngleAroundZero(float inV) +{ + if (inV < -JPH_PI) + { + do + inV += 2.0f * JPH_PI; + while (inV < -JPH_PI); + } + else if (inV > JPH_PI) + { + do + inV -= 2.0f * JPH_PI; + while (inV > JPH_PI); + } + JPH_ASSERT(inV >= -JPH_PI && inV <= JPH_PI); + return inV; +} + +/// Clamp a value between two values +template +JPH_INLINE constexpr T Clamp(T inV, T inMin, T inMax) +{ + return min(max(inV, inMin), inMax); +} + +/// Square a value +template +JPH_INLINE constexpr T Square(T inV) +{ + return inV * inV; +} + +/// Returns \f$inV^3\f$. +template +JPH_INLINE constexpr T Cubed(T inV) +{ + return inV * inV * inV; +} + +/// Get the sign of a value +template +JPH_INLINE constexpr T Sign(T inV) +{ + return inV < 0? T(-1) : T(1); +} + +/// Check if inV is a power of 2 +template +constexpr bool IsPowerOf2(T inV) +{ + return (inV & (inV - 1)) == 0; +} + +/// Align inV up to the next inAlignment bytes +template +inline T AlignUp(T inV, uint64 inAlignment) +{ + JPH_ASSERT(IsPowerOf2(inAlignment)); + return T((uint64(inV) + inAlignment - 1) & ~(inAlignment - 1)); +} + +/// Check if inV is inAlignment aligned +template +inline bool IsAligned(T inV, uint64 inAlignment) +{ + JPH_ASSERT(IsPowerOf2(inAlignment)); + return (uint64(inV) & (inAlignment - 1)) == 0; +} + +/// Compute number of trailing zero bits (how many low bits are zero) +inline uint CountTrailingZeros(uint32 inValue) +{ +#if defined(JPH_CPU_X86) || defined(JPH_CPU_WASM) + #if defined(JPH_USE_TZCNT) + return _tzcnt_u32(inValue); + #elif defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanForward(&result, inValue); + return result; + #else + if (inValue == 0) + return 32; + return __builtin_ctz(inValue); + #endif +#elif defined(JPH_CPU_ARM) + #if defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanForward(&result, inValue); + return result; + #else + if (inValue == 0) + return 32; + return __builtin_ctz(inValue); + #endif +#elif defined(JPH_CPU_E2K) + return inValue ? __builtin_ctz(inValue) : 32; +#else + #error Undefined +#endif +} + +/// Compute the number of leading zero bits (how many high bits are zero) +inline uint CountLeadingZeros(uint32 inValue) +{ +#if defined(JPH_CPU_X86) || defined(JPH_CPU_WASM) + #if defined(JPH_USE_LZCNT) + return _lzcnt_u32(inValue); + #elif defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanReverse(&result, inValue); + return 31 - result; + #else + if (inValue == 0) + return 32; + return __builtin_clz(inValue); + #endif +#elif defined(JPH_CPU_ARM) + #if defined(JPH_COMPILER_MSVC) + return _CountLeadingZeros(inValue); + #else + return __builtin_clz(inValue); + #endif +#elif defined(JPH_CPU_E2K) + return inValue ? __builtin_clz(inValue) : 32; +#else + #error Undefined +#endif +} + +/// Count the number of 1 bits in a value +inline uint CountBits(uint32 inValue) +{ +#if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC) + return __builtin_popcount(inValue); +#elif defined(JPH_COMPILER_MSVC) + #if defined(JPH_USE_SSE4_2) + return _mm_popcnt_u32(inValue); + #elif defined(JPH_USE_NEON) && (_MSC_VER >= 1930) // _CountOneBits not available on MSVC2019 + return _CountOneBits(inValue); + #else + inValue = inValue - ((inValue >> 1) & 0x55555555); + inValue = (inValue & 0x33333333) + ((inValue >> 2) & 0x33333333); + inValue = (inValue + (inValue >> 4)) & 0x0F0F0F0F; + return (inValue * 0x01010101) >> 24; + #endif +#else + #error Undefined +#endif +} + +/// Get the next higher power of 2 of a value, or the value itself if the value is already a power of 2 +inline uint32 GetNextPowerOf2(uint32 inValue) +{ + return inValue <= 1? uint32(1) : uint32(1) << (32 - CountLeadingZeros(inValue - 1)); +} + +// Simple implementation of C++20 std::bit_cast (unfortunately not constexpr) +template +JPH_INLINE To BitCast(const From &inValue) +{ + static_assert(std::is_trivially_constructible_v); + static_assert(sizeof(From) == sizeof(To)); + + union FromTo + { + To mTo; + From mFrom; + }; + + FromTo convert; + convert.mFrom = inValue; + return convert.mTo; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/MathTypes.h b/thirdparty/jolt_physics/Jolt/Math/MathTypes.h new file mode 100644 index 000000000000..e58dc9dabec9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/MathTypes.h @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Vec3; +class DVec3; +class Vec4; +class UVec4; +class BVec16; +class Quat; +class Mat44; +class DMat44; + +// Types to use for passing arguments to functions +using Vec3Arg = const Vec3; +#ifdef JPH_USE_AVX + using DVec3Arg = const DVec3; +#else + using DVec3Arg = const DVec3 &; +#endif +using Vec4Arg = const Vec4; +using UVec4Arg = const UVec4; +using BVec16Arg = const BVec16; +using QuatArg = const Quat; +using Mat44Arg = const Mat44 &; +using DMat44Arg = const DMat44 &; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Matrix.h b/thirdparty/jolt_physics/Jolt/Math/Matrix.h new file mode 100644 index 000000000000..031665bbe718 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Matrix.h @@ -0,0 +1,259 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Templatized matrix class +template +class [[nodiscard]] Matrix +{ +public: + /// Constructor + inline Matrix() = default; + inline Matrix(const Matrix &inM2) { *this = inM2; } + + /// Dimensions + inline uint GetRows() const { return Rows; } + inline uint GetCols() const { return Cols; } + + /// Zero matrix + inline void SetZero() + { + for (uint c = 0; c < Cols; ++c) + mCol[c].SetZero(); + } + + inline static Matrix sZero() { Matrix m; m.SetZero(); return m; } + + /// Check if this matrix consists of all zeros + inline bool IsZero() const + { + for (uint c = 0; c < Cols; ++c) + if (!mCol[c].IsZero()) + return false; + + return true; + } + + /// Identity matrix + inline void SetIdentity() + { + // Clear matrix + SetZero(); + + // Set diagonal to 1 + for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc) + mCol[rc].mF32[rc] = 1.0f; + } + + inline static Matrix sIdentity() { Matrix m; m.SetIdentity(); return m; } + + /// Check if this matrix is identity + bool IsIdentity() const { return *this == sIdentity(); } + + /// Diagonal matrix + inline void SetDiagonal(const Vector &inV) + { + // Clear matrix + SetZero(); + + // Set diagonal + for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc) + mCol[rc].mF32[rc] = inV[rc]; + } + + inline static Matrix sDiagonal(const Vector &inV) + { + Matrix m; + m.SetDiagonal(inV); + return m; + } + + /// Copy a (part) of another matrix into this matrix + template + void CopyPart(const OtherMatrix &inM, uint inSourceRow, uint inSourceCol, uint inNumRows, uint inNumCols, uint inDestRow, uint inDestCol) + { + for (uint c = 0; c < inNumCols; ++c) + for (uint r = 0; r < inNumRows; ++r) + mCol[inDestCol + c].mF32[inDestRow + r] = inM(inSourceRow + r, inSourceCol + c); + } + + /// Get float component by element index + inline float operator () (uint inRow, uint inColumn) const + { + JPH_ASSERT(inRow < Rows); + JPH_ASSERT(inColumn < Cols); + return mCol[inColumn].mF32[inRow]; + } + + inline float & operator () (uint inRow, uint inColumn) + { + JPH_ASSERT(inRow < Rows); + JPH_ASSERT(inColumn < Cols); + return mCol[inColumn].mF32[inRow]; + } + + /// Comparison + inline bool operator == (const Matrix &inM2) const + { + for (uint c = 0; c < Cols; ++c) + if (mCol[c] != inM2.mCol[c]) + return false; + return true; + } + + inline bool operator != (const Matrix &inM2) const + { + for (uint c = 0; c < Cols; ++c) + if (mCol[c] != inM2.mCol[c]) + return true; + return false; + } + + /// Assignment + inline Matrix & operator = (const Matrix &inM2) + { + for (uint c = 0; c < Cols; ++c) + mCol[c] = inM2.mCol[c]; + return *this; + } + + /// Multiply matrix by matrix + template + inline Matrix operator * (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < OtherCols; ++c) + for (uint r = 0; r < Rows; ++r) + { + float dot = 0.0f; + for (uint i = 0; i < Cols; ++i) + dot += mCol[i].mF32[r] * inM.mCol[c].mF32[i]; + m.mCol[c].mF32[r] = dot; + } + return m; + } + + /// Multiply vector by matrix + inline Vector operator * (const Vector &inV) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + { + float dot = 0.0f; + for (uint c = 0; c < Cols; ++c) + dot += mCol[c].mF32[r] * inV.mF32[c]; + v.mF32[r] = dot; + } + return v; + } + + /// Multiply matrix with float + inline Matrix operator * (float inV) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] * inV; + return m; + } + + inline friend Matrix operator * (float inV, const Matrix &inM) + { + return inM * inV; + } + + /// Per element addition of matrix + inline Matrix operator + (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] + inM.mCol[c]; + return m; + } + + /// Per element subtraction of matrix + inline Matrix operator - (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] - inM.mCol[c]; + return m; + } + + /// Transpose matrix + inline Matrix Transposed() const + { + Matrix m; + for (uint r = 0; r < Rows; ++r) + for (uint c = 0; c < Cols; ++c) + m.mCol[r].mF32[c] = mCol[c].mF32[r]; + return m; + } + + /// Inverse matrix + bool SetInversed(const Matrix &inM) + { + if constexpr (Rows != Cols) JPH_ASSERT(false); + Matrix copy(inM); + SetIdentity(); + return GaussianElimination(copy, *this); + } + + inline Matrix Inversed() const + { + Matrix m; + m.SetInversed(*this); + return m; + } + + /// To String + friend ostream & operator << (ostream &inStream, const Matrix &inM) + { + for (uint i = 0; i < Cols - 1; ++i) + inStream << inM.mCol[i] << ", "; + inStream << inM.mCol[Cols - 1]; + return inStream; + } + + /// Column access + const Vector & GetColumn(int inIdx) const { return mCol[inIdx]; } + Vector & GetColumn(int inIdx) { return mCol[inIdx]; } + + Vector mCol[Cols]; ///< Column +}; + +// The template specialization doesn't sit well with Doxygen +#ifndef JPH_PLATFORM_DOXYGEN + +/// Specialization of SetInversed for 2x2 matrix +template <> +inline bool Matrix<2, 2>::SetInversed(const Matrix<2, 2> &inM) +{ + // Fetch elements + float a = inM.mCol[0].mF32[0]; + float b = inM.mCol[1].mF32[0]; + float c = inM.mCol[0].mF32[1]; + float d = inM.mCol[1].mF32[1]; + + // Calculate determinant + float det = a * d - b * c; + if (det == 0.0f) + return false; + + // Construct inverse + mCol[0].mF32[0] = d / det; + mCol[1].mF32[0] = -b / det; + mCol[0].mF32[1] = -c / det; + mCol[1].mF32[1] = a / det; + return true; +} + +#endif // !JPH_PLATFORM_DOXYGEN + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.h b/thirdparty/jolt_physics/Jolt/Math/Quat.h new file mode 100644 index 000000000000..a68e45be294f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Quat.h @@ -0,0 +1,255 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Quaternion class, quaternions are 4 dimensional vectors which can describe rotations in 3 dimensional +/// space if their length is 1. +/// +/// They are written as: +/// +/// \f$q = w + x \: i + y \: j + z \: k\f$ +/// +/// or in vector notation: +/// +/// \f$q = [w, v] = [w, x, y, z]\f$ +/// +/// Where: +/// +/// w = the real part +/// v = the imaginary part, (x, y, z) +/// +/// Note that we store the quaternion in a Vec4 as [x, y, z, w] because that makes +/// it easy to extract the rotation axis of the quaternion: +/// +/// q = [cos(angle / 2), sin(angle / 2) * rotation_axis] +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat +{ +public: + JPH_OVERRIDE_NEW_DELETE + + ///@name Constructors + ///@{ + inline Quat() = default; ///< Intentionally not initialized for performance reasons + Quat(const Quat &inRHS) = default; + Quat & operator = (const Quat &inRHS) = default; + inline Quat(float inX, float inY, float inZ, float inW) : mValue(inX, inY, inZ, inW) { } + inline explicit Quat(Vec4Arg inV) : mValue(inV) { } + ///@} + + ///@name Tests + ///@{ + + /// Check if two quaternions are exactly equal + inline bool operator == (QuatArg inRHS) const { return mValue == inRHS.mValue; } + + /// Check if two quaternions are different + inline bool operator != (QuatArg inRHS) const { return mValue != inRHS.mValue; } + + /// If this quaternion is close to inRHS. Note that q and -q represent the same rotation, this is not checked here. + inline bool IsClose(QuatArg inRHS, float inMaxDistSq = 1.0e-12f) const { return mValue.IsClose(inRHS.mValue, inMaxDistSq); } + + /// If the length of this quaternion is 1 +/- inTolerance + inline bool IsNormalized(float inTolerance = 1.0e-5f) const { return mValue.IsNormalized(inTolerance); } + + /// If any component of this quaternion is a NaN (not a number) + inline bool IsNaN() const { return mValue.IsNaN(); } + + ///@} + ///@name Get components + ///@{ + + /// Get X component (imaginary part i) + JPH_INLINE float GetX() const { return mValue.GetX(); } + + /// Get Y component (imaginary part j) + JPH_INLINE float GetY() const { return mValue.GetY(); } + + /// Get Z component (imaginary part k) + JPH_INLINE float GetZ() const { return mValue.GetZ(); } + + /// Get W component (real part) + JPH_INLINE float GetW() const { return mValue.GetW(); } + + /// Get the imaginary part of the quaternion + JPH_INLINE Vec3 GetXYZ() const { return Vec3(mValue); } + + /// Get the quaternion as a Vec4 + JPH_INLINE Vec4 GetXYZW() const { return mValue; } + + /// Set individual components + JPH_INLINE void SetX(float inX) { mValue.SetX(inX); } + JPH_INLINE void SetY(float inY) { mValue.SetY(inY); } + JPH_INLINE void SetZ(float inZ) { mValue.SetZ(inZ); } + JPH_INLINE void SetW(float inW) { mValue.SetW(inW); } + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ, float inW) { mValue.Set(inX, inY, inZ, inW); } + + ///@} + ///@name Default quaternions + ///@{ + + /// @return [0, 0, 0, 0] + JPH_INLINE static Quat sZero() { return Quat(Vec4::sZero()); } + + /// @return [1, 0, 0, 0] (or in storage format Quat(0, 0, 0, 1)) + JPH_INLINE static Quat sIdentity() { return Quat(0, 0, 0, 1); } + + ///@} + + /// Rotation from axis and angle + JPH_INLINE static Quat sRotation(Vec3Arg inAxis, float inAngle); + + /// Get axis and angle that represents this quaternion, outAngle will always be in the range \f$[0, \pi]\f$ + JPH_INLINE void GetAxisAngle(Vec3 &outAxis, float &outAngle) const; + + /// Create quaternion that rotates a vector from the direction of inFrom to the direction of inTo along the shortest path + /// @see https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm + JPH_INLINE static Quat sFromTo(Vec3Arg inFrom, Vec3Arg inTo); + + /// Random unit quaternion + template + inline static Quat sRandom(Random &inRandom); + + /// Conversion from Euler angles. Rotation order is X then Y then Z (RotZ * RotY * RotX). Angles in radians. + inline static Quat sEulerAngles(Vec3Arg inAngles); + + /// Conversion to Euler angles. Rotation order is X then Y then Z (RotZ * RotY * RotX). Angles in radians. + inline Vec3 GetEulerAngles() const; + + ///@name Length / normalization operations + ///@{ + + /// Squared length of quaternion. + /// @return Squared length of quaternion (\f$|v|^2\f$) + JPH_INLINE float LengthSq() const { return mValue.LengthSq(); } + + /// Length of quaternion. + /// @return Length of quaternion (\f$|v|\f$) + JPH_INLINE float Length() const { return mValue.Length(); } + + /// Normalize the quaternion (make it length 1) + JPH_INLINE Quat Normalized() const { return Quat(mValue.Normalized()); } + + ///@} + ///@name Additions / multiplications + ///@{ + + JPH_INLINE void operator += (QuatArg inRHS) { mValue += inRHS.mValue; } + JPH_INLINE void operator -= (QuatArg inRHS) { mValue -= inRHS.mValue; } + JPH_INLINE void operator *= (float inValue) { mValue *= inValue; } + JPH_INLINE void operator /= (float inValue) { mValue /= inValue; } + JPH_INLINE Quat operator - () const { return Quat(-mValue); } + JPH_INLINE Quat operator + (QuatArg inRHS) const { return Quat(mValue + inRHS.mValue); } + JPH_INLINE Quat operator - (QuatArg inRHS) const { return Quat(mValue - inRHS.mValue); } + JPH_INLINE Quat operator * (QuatArg inRHS) const; + JPH_INLINE Quat operator * (float inValue) const { return Quat(mValue * inValue); } + inline friend Quat operator * (float inValue, QuatArg inRHS) { return Quat(inRHS.mValue * inValue); } + JPH_INLINE Quat operator / (float inValue) const { return Quat(mValue / inValue); } + + ///@} + + /// Rotate a vector by this quaternion + JPH_INLINE Vec3 operator * (Vec3Arg inValue) const; + + /// Rotate a vector by the inverse of this quaternion + JPH_INLINE Vec3 InverseRotate(Vec3Arg inValue) const; + + /// Rotate a the vector (1, 0, 0) with this quaternion + JPH_INLINE Vec3 RotateAxisX() const; + + /// Rotate a the vector (0, 1, 0) with this quaternion + JPH_INLINE Vec3 RotateAxisY() const; + + /// Rotate a the vector (0, 0, 1) with this quaternion + JPH_INLINE Vec3 RotateAxisZ() const; + + /// Dot product + JPH_INLINE float Dot(QuatArg inRHS) const { return mValue.Dot(inRHS.mValue); } + + /// The conjugate [w, -x, -y, -z] is the same as the inverse for unit quaternions + JPH_INLINE Quat Conjugated() const { return Quat(Vec4::sXor(mValue, UVec4(0x80000000, 0x80000000, 0x80000000, 0).ReinterpretAsFloat())); } + + /// Get inverse quaternion + JPH_INLINE Quat Inversed() const { return Conjugated() / Length(); } + + /// Ensures that the W component is positive by negating the entire quaternion if it is not. This is useful when you want to store a quaternion as a 3 vector by discarding W and reconstructing it as sqrt(1 - x^2 - y^2 - z^2). + JPH_INLINE Quat EnsureWPositive() const { return Quat(Vec4::sXor(mValue, Vec4::sAnd(mValue.SplatW(), UVec4::sReplicate(0x80000000).ReinterpretAsFloat()))); } + + /// Get a quaternion that is perpendicular to this quaternion + JPH_INLINE Quat GetPerpendicular() const { return Quat(Vec4(1, -1, 1, -1) * mValue.Swizzle()); } + + /// Get rotation angle around inAxis (uses Swing Twist Decomposition to get the twist quaternion and uses q(axis, angle) = [cos(angle / 2), axis * sin(angle / 2)]) + JPH_INLINE float GetRotationAngle(Vec3Arg inAxis) const { return GetW() == 0.0f? JPH_PI : 2.0f * ATan(GetXYZ().Dot(inAxis) / GetW()); } + + /// Swing Twist Decomposition: any quaternion can be split up as: + /// + /// \f[q = q_{swing} \: q_{twist}\f] + /// + /// where \f$q_{twist}\f$ rotates only around axis v. + /// + /// \f$q_{twist}\f$ is: + /// + /// \f[q_{twist} = \frac{[q_w, q_{ijk} \cdot v \: v]}{\left|[q_w, q_{ijk} \cdot v \: v]\right|}\f] + /// + /// where q_w is the real part of the quaternion and q_i the imaginary part (a 3 vector). + /// + /// The swing can then be calculated as: + /// + /// \f[q_{swing} = q \: q_{twist}^* \f] + /// + /// Where \f$q_{twist}^*\f$ = complex conjugate of \f$q_{twist}\f$ + JPH_INLINE Quat GetTwist(Vec3Arg inAxis) const; + + /// Decomposes quaternion into swing and twist component: + /// + /// \f$q = q_{swing} \: q_{twist}\f$ + /// + /// where \f$q_{swing} \: \hat{x} = q_{twist} \: \hat{y} = q_{twist} \: \hat{z} = 0\f$ + /// + /// In other words: + /// + /// - \f$q_{twist}\f$ only rotates around the X-axis. + /// - \f$q_{swing}\f$ only rotates around the Y and Z-axis. + /// + /// @see Gino van den Bergen - Rotational Joint Limits in Quaternion Space - GDC 2016 + JPH_INLINE void GetSwingTwist(Quat &outSwing, Quat &outTwist) const; + + /// Linear interpolation between two quaternions (for small steps). + /// @param inFraction is in the range [0, 1] + /// @param inDestination The destination quaternion + /// @return (1 - inFraction) * this + fraction * inDestination + JPH_INLINE Quat LERP(QuatArg inDestination, float inFraction) const; + + /// Spherical linear interpolation between two quaternions. + /// @param inFraction is in the range [0, 1] + /// @param inDestination The destination quaternion + /// @return When fraction is zero this quaternion is returned, when fraction is 1 inDestination is returned. + /// When fraction is between 0 and 1 an interpolation along the shortest path is returned. + JPH_INLINE Quat SLERP(QuatArg inDestination, float inFraction) const; + + /// Load 3 floats from memory (X, Y and Z component and then calculates W) reads 32 bits extra which it doesn't use + static JPH_INLINE Quat sLoadFloat3Unsafe(const Float3 &inV); + + /// Store 3 as floats to memory (X, Y and Z component) + JPH_INLINE void StoreFloat3(Float3 *outV) const; + + /// To String + friend ostream & operator << (ostream &inStream, QuatArg inQ) { inStream << inQ.mValue; return inStream; } + + /// 4 vector that stores [x, y, z, w] parts of the quaternion + Vec4 mValue; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Quat.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.inl b/thirdparty/jolt_physics/Jolt/Math/Quat.inl new file mode 100644 index 000000000000..201481929d55 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Quat.inl @@ -0,0 +1,328 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +Quat Quat::operator * (QuatArg inRHS) const +{ +#if defined(JPH_USE_SSE4_1) + // Taken from: http://momchil-velikov.blogspot.nl/2013/10/fast-sse-quternion-multiplication.html + __m128 abcd = mValue.mValue; + __m128 xyzw = inRHS.mValue.mValue; + + __m128 t0 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(3, 3, 3, 3)); + __m128 t1 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 3, 0, 1)); + + __m128 t3 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(0, 0, 0, 0)); + __m128 t4 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(1, 0, 3, 2)); + + __m128 t5 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(1, 1, 1, 1)); + __m128 t6 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 0, 3, 1)); + + // [d,d,d,d] * [z,w,x,y] = [dz,dw,dx,dy] + __m128 m0 = _mm_mul_ps(t0, t1); + + // [a,a,a,a] * [y,x,w,z] = [ay,ax,aw,az] + __m128 m1 = _mm_mul_ps(t3, t4); + + // [b,b,b,b] * [z,x,w,y] = [bz,bx,bw,by] + __m128 m2 = _mm_mul_ps(t5, t6); + + // [c,c,c,c] * [w,z,x,y] = [cw,cz,cx,cy] + __m128 t7 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(2, 2, 2, 2)); + __m128 t8 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 2, 0, 1)); + __m128 m3 = _mm_mul_ps(t7, t8); + + // [dz,dw,dx,dy] + -[ay,ax,aw,az] = [dz+ay,dw-ax,dx+aw,dy-az] + __m128 e = _mm_addsub_ps(m0, m1); + + // [dx+aw,dz+ay,dy-az,dw-ax] + e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(1, 3, 0, 2)); + + // [dx+aw,dz+ay,dy-az,dw-ax] + -[bz,bx,bw,by] = [dx+aw+bz,dz+ay-bx,dy-az+bw,dw-ax-by] + e = _mm_addsub_ps(e, m2); + + // [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz] + e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 0, 1, 3)); + + // [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz] + -[cw,cz,cx,cy] = [dz+ay-bx+cw,dw-ax-by-cz,dy-az+bw+cx,dx+aw+bz-cy] + e = _mm_addsub_ps(e, m3); + + // [dw-ax-by-cz,dz+ay-bx+cw,dy-az+bw+cx,dx+aw+bz-cy] + return Quat(Vec4(_mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 3, 1, 0)))); +#else + float lx = mValue.GetX(); + float ly = mValue.GetY(); + float lz = mValue.GetZ(); + float lw = mValue.GetW(); + + float rx = inRHS.mValue.GetX(); + float ry = inRHS.mValue.GetY(); + float rz = inRHS.mValue.GetZ(); + float rw = inRHS.mValue.GetW(); + + float x = lw * rx + lx * rw + ly * rz - lz * ry; + float y = lw * ry - lx * rz + ly * rw + lz * rx; + float z = lw * rz + lx * ry - ly * rx + lz * rw; + float w = lw * rw - lx * rx - ly * ry - lz * rz; + + return Quat(x, y, z, w); +#endif +} + +Quat Quat::sRotation(Vec3Arg inAxis, float inAngle) +{ + // returns [inAxis * sin(0.5f * inAngle), cos(0.5f * inAngle)] + JPH_ASSERT(inAxis.IsNormalized()); + Vec4 s, c; + Vec4::sReplicate(0.5f * inAngle).SinCos(s, c); + return Quat(Vec4::sSelect(Vec4(inAxis) * s, c, UVec4(0, 0, 0, 0xffffffffU))); +} + +void Quat::GetAxisAngle(Vec3 &outAxis, float &outAngle) const +{ + JPH_ASSERT(IsNormalized()); + Quat w_pos = EnsureWPositive(); + float abs_w = w_pos.GetW(); + if (abs_w >= 1.0f) + { + outAxis = Vec3::sZero(); + outAngle = 0.0f; + } + else + { + outAngle = 2.0f * ACos(abs_w); + outAxis = w_pos.GetXYZ().NormalizedOr(Vec3::sZero()); + } +} + +Quat Quat::sFromTo(Vec3Arg inFrom, Vec3Arg inTo) +{ + /* + Uses (inFrom = v1, inTo = v2): + + angle = arcos(v1 . v2 / |v1||v2|) + axis = normalize(v1 x v2) + + Quaternion is then: + + s = sin(angle / 2) + x = axis.x * s + y = axis.y * s + z = axis.z * s + w = cos(angle / 2) + + Using identities: + + sin(2 * a) = 2 * sin(a) * cos(a) + cos(2 * a) = cos(a)^2 - sin(a)^2 + sin(a)^2 + cos(a)^2 = 1 + + This reduces to: + + x = (v1 x v2).x + y = (v1 x v2).y + z = (v1 x v2).z + w = |v1||v2| + v1 . v2 + + which then needs to be normalized because the whole equation was multiplied by 2 cos(angle / 2) + */ + + float len_v1_v2 = sqrt(inFrom.LengthSq() * inTo.LengthSq()); + float w = len_v1_v2 + inFrom.Dot(inTo); + + if (w == 0.0f) + { + if (len_v1_v2 == 0.0f) + { + // If either of the vectors has zero length, there is no rotation and we return identity + return Quat::sIdentity(); + } + else + { + // If vectors are perpendicular, take one of the many 180 degree rotations that exist + return Quat(Vec4(inFrom.GetNormalizedPerpendicular(), 0)); + } + } + + Vec3 v = inFrom.Cross(inTo); + return Quat(Vec4(v, w)).Normalized(); +} + +template +Quat Quat::sRandom(Random &inRandom) +{ + std::uniform_real_distribution zero_to_one(0.0f, 1.0f); + float x0 = zero_to_one(inRandom); + float r1 = sqrt(1.0f - x0), r2 = sqrt(x0); + std::uniform_real_distribution zero_to_two_pi(0.0f, 2.0f * JPH_PI); + Vec4 s, c; + Vec4(zero_to_two_pi(inRandom), zero_to_two_pi(inRandom), 0, 0).SinCos(s, c); + return Quat(s.GetX() * r1, c.GetX() * r1, s.GetY() * r2, c.GetY() * r2); +} + +Quat Quat::sEulerAngles(Vec3Arg inAngles) +{ + Vec4 half(0.5f * inAngles); + Vec4 s, c; + half.SinCos(s, c); + + float cx = c.GetX(); + float sx = s.GetX(); + float cy = c.GetY(); + float sy = s.GetY(); + float cz = c.GetZ(); + float sz = s.GetZ(); + + return Quat( + cz * sx * cy - sz * cx * sy, + cz * cx * sy + sz * sx * cy, + sz * cx * cy - cz * sx * sy, + cz * cx * cy + sz * sx * sy); +} + +Vec3 Quat::GetEulerAngles() const +{ + float y_sq = GetY() * GetY(); + + // X + float t0 = 2.0f * (GetW() * GetX() + GetY() * GetZ()); + float t1 = 1.0f - 2.0f * (GetX() * GetX() + y_sq); + + // Y + float t2 = 2.0f * (GetW() * GetY() - GetZ() * GetX()); + t2 = t2 > 1.0f? 1.0f : t2; + t2 = t2 < -1.0f? -1.0f : t2; + + // Z + float t3 = 2.0f * (GetW() * GetZ() + GetX() * GetY()); + float t4 = 1.0f - 2.0f * (y_sq + GetZ() * GetZ()); + + return Vec3(ATan2(t0, t1), ASin(t2), ATan2(t3, t4)); +} + +Quat Quat::GetTwist(Vec3Arg inAxis) const +{ + Quat twist(Vec4(GetXYZ().Dot(inAxis) * inAxis, GetW())); + float twist_len = twist.LengthSq(); + if (twist_len != 0.0f) + return twist / sqrt(twist_len); + else + return Quat::sIdentity(); +} + +void Quat::GetSwingTwist(Quat &outSwing, Quat &outTwist) const +{ + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float s = sqrt(Square(w) + Square(x)); + if (s != 0.0f) + { + outTwist = Quat(x / s, 0, 0, w / s); + outSwing = Quat(0, (w * y - x * z) / s, (w * z + x * y) / s, s); + } + else + { + // If both x and w are zero, this must be a 180 degree rotation around either y or z + outTwist = Quat::sIdentity(); + outSwing = *this; + } +} + +Quat Quat::LERP(QuatArg inDestination, float inFraction) const +{ + float scale0 = 1.0f - inFraction; + return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(inFraction) * inDestination.mValue); +} + +Quat Quat::SLERP(QuatArg inDestination, float inFraction) const +{ + // Difference at which to LERP instead of SLERP + const float delta = 0.0001f; + + // Calc cosine + float sign_scale1 = 1.0f; + float cos_omega = Dot(inDestination); + + // Adjust signs (if necessary) + if (cos_omega < 0.0f) + { + cos_omega = -cos_omega; + sign_scale1 = -1.0f; + } + + // Calculate coefficients + float scale0, scale1; + if (1.0f - cos_omega > delta) + { + // Standard case (slerp) + float omega = ACos(cos_omega); + float sin_omega = Sin(omega); + scale0 = Sin((1.0f - inFraction) * omega) / sin_omega; + scale1 = sign_scale1 * Sin(inFraction * omega) / sin_omega; + } + else + { + // Quaternions are very close so we can do a linear interpolation + scale0 = 1.0f - inFraction; + scale1 = sign_scale1 * inFraction; + } + + // Interpolate between the two quaternions + return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(scale1) * inDestination.mValue).Normalized(); +} + +Vec3 Quat::operator * (Vec3Arg inValue) const +{ + // Rotating a vector by a quaternion is done by: p' = q * p * q^-1 (q^-1 = conjugated(q) for a unit quaternion) + JPH_ASSERT(IsNormalized()); + return Vec3((*this * Quat(Vec4(inValue, 0)) * Conjugated()).mValue); +} + +Vec3 Quat::InverseRotate(Vec3Arg inValue) const +{ + JPH_ASSERT(IsNormalized()); + return Vec3((Conjugated() * Quat(Vec4(inValue, 0)) * *this).mValue); +} + +Vec3 Quat::RotateAxisX() const +{ + // This is *this * Vec3::sAxisX() written out: + JPH_ASSERT(IsNormalized()); + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float tx = 2.0f * x, tw = 2.0f * w; + return Vec3(tx * x + tw * w - 1.0f, tx * y + z * tw, tx * z - y * tw); +} + +Vec3 Quat::RotateAxisY() const +{ + // This is *this * Vec3::sAxisY() written out: + JPH_ASSERT(IsNormalized()); + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float ty = 2.0f * y, tw = 2.0f * w; + return Vec3(x * ty - z * tw, tw * w + ty * y - 1.0f, x * tw + ty * z); +} + +Vec3 Quat::RotateAxisZ() const +{ + // This is *this * Vec3::sAxisZ() written out: + JPH_ASSERT(IsNormalized()); + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float tz = 2.0f * z, tw = 2.0f * w; + return Vec3(x * tz + y * tw, y * tz - x * tw, tw * w + tz * z - 1.0f); +} + +void Quat::StoreFloat3(Float3 *outV) const +{ + JPH_ASSERT(IsNormalized()); + EnsureWPositive().GetXYZ().StoreFloat3(outV); +} + +Quat Quat::sLoadFloat3Unsafe(const Float3 &inV) +{ + Vec3 v = Vec3::sLoadFloat3Unsafe(inV); + float w = sqrt(max(1.0f - v.LengthSq(), 0.0f)); // It is possible that the length of v is a fraction above 1, and we don't want to introduce NaN's in that case so we clamp to 0 + return Quat(Vec4(v, w)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Real.h b/thirdparty/jolt_physics/Jolt/Math/Real.h new file mode 100644 index 000000000000..ca8cf5049eb1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Real.h @@ -0,0 +1,44 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DOUBLE_PRECISION + +// Define real to double +using Real = double; +using Real3 = Double3; +using RVec3 = DVec3; +using RVec3Arg = DVec3Arg; +using RMat44 = DMat44; +using RMat44Arg = DMat44Arg; + +#define JPH_RVECTOR_ALIGNMENT JPH_DVECTOR_ALIGNMENT + +#else + +// Define real to float +using Real = float; +using Real3 = Float3; +using RVec3 = Vec3; +using RVec3Arg = Vec3Arg; +using RMat44 = Mat44; +using RMat44Arg = Mat44Arg; + +#define JPH_RVECTOR_ALIGNMENT JPH_VECTOR_ALIGNMENT + +#endif // JPH_DOUBLE_PRECISION + +// Put the 'real' operator in a namespace so that users can opt in to use it: +// using namespace JPH::literals; +namespace literals { + constexpr Real operator ""_r (long double inValue) { return Real(inValue); } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Swizzle.h b/thirdparty/jolt_physics/Jolt/Math/Swizzle.h new file mode 100644 index 000000000000..ad8dfbc144a5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Swizzle.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum indicating which component to use when swizzling +enum +{ + SWIZZLE_X = 0, ///< Use the X component + SWIZZLE_Y = 1, ///< Use the Y component + SWIZZLE_Z = 2, ///< Use the Z component + SWIZZLE_W = 3, ///< Use the W component + SWIZZLE_UNUSED = 2, ///< We always use the Z component when we don't specifically want to initialize a value, this is consistent with what is done in Vec3(x, y, z), Vec3(Float3 &) and Vec3::sLoadFloat3Unsafe +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h b/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h new file mode 100644 index 000000000000..3503dd17bd49 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h @@ -0,0 +1,79 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +// Note that this file exists because std::sin etc. are not platform independent and will lead to non-deterministic simulation + +/// Sine of x (input in radians) +JPH_INLINE float Sin(float inX) +{ + Vec4 s, c; + Vec4::sReplicate(inX).SinCos(s, c); + return s.GetX(); +} + +/// Cosine of x (input in radians) +JPH_INLINE float Cos(float inX) +{ + Vec4 s, c; + Vec4::sReplicate(inX).SinCos(s, c); + return c.GetX(); +} + +/// Tangent of x (input in radians) +JPH_INLINE float Tan(float inX) +{ + return Vec4::sReplicate(inX).Tan().GetX(); +} + +/// Arc sine of x (returns value in the range [-PI / 2, PI / 2]) +/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::asin +JPH_INLINE float ASin(float inX) +{ + return Vec4::sReplicate(inX).ASin().GetX(); +} + +/// Arc cosine of x (returns value in the range [0, PI]) +/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::acos +JPH_INLINE float ACos(float inX) +{ + return Vec4::sReplicate(inX).ACos().GetX(); +} + +/// An approximation of ACos, max error is 4.2e-3 over the entire range [-1, 1], is approximately 2.5x faster than ACos +JPH_INLINE float ACosApproximate(float inX) +{ + // See: https://www.johndcook.com/blog/2022/09/06/inverse-cosine-near-1/ + // See also: https://seblagarde.wordpress.com/2014/12/01/inverse-trigonometric-functions-gpu-optimization-for-amd-gcn-architecture/ + // Taylor of cos(x) = 1 - x^2 / 2 + ... + // Substitute x = sqrt(2 y) we get: cos(sqrt(2 y)) = 1 - y + // Substitute z = 1 - y we get: cos(sqrt(2 (1 - z))) = z <=> acos(z) = sqrt(2 (1 - z)) + // To avoid the discontinuity at 1, instead of using the Taylor expansion of acos(x) we use acos(x) / sqrt(2 (1 - x)) = 1 + (1 - x) / 12 + ... + // Since the approximation was made at 1, it has quite a large error at 0 meaning that if we want to extend to the + // range [-1, 1] by mirroring the range [0, 1], the value at 0+ is not the same as 0-. + // So we observe that the form of the Taylor expansion is f(x) = sqrt(1 - x) * (a + b x) and we fit the function so that f(0) = pi / 2 + // this gives us a = pi / 2. f(1) = 0 regardless of b. We search for a constant b that minimizes the error in the range [0, 1]. + float abs_x = min(abs(inX), 1.0f); // Ensure that we don't get a value larger than 1 + float val = sqrt(1.0f - abs_x) * (JPH_PI / 2 - 0.175394f * abs_x); + + // Our approximation is valid in the range [0, 1], extend it to the range [-1, 1] + return inX < 0? JPH_PI - val : val; +} + +/// Arc tangent of x (returns value in the range [-PI / 2, PI / 2]) +JPH_INLINE float ATan(float inX) +{ + return Vec4::sReplicate(inX).ATan().GetX(); +} + +/// Arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI]) +JPH_INLINE float ATan2(float inY, float inX) +{ + return Vec4::sATan2(Vec4::sReplicate(inY), Vec4::sReplicate(inX)).GetX(); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/UVec4.h b/thirdparty/jolt_physics/Jolt/Math/UVec4.h new file mode 100644 index 000000000000..3f22d43ca6e1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/UVec4.h @@ -0,0 +1,220 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) UVec4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128i; +#elif defined(JPH_USE_NEON) + using Type = uint32x4_t; +#else + using Type = struct { uint32 mData[4]; }; +#endif + + /// Constructor + UVec4() = default; ///< Intentionally not initialized for performance reasons + UVec4(const UVec4 &inRHS) = default; + UVec4 & operator = (const UVec4 &inRHS) = default; + JPH_INLINE UVec4(Type inRHS) : mValue(inRHS) { } + + /// Create a vector from 4 integer components + JPH_INLINE UVec4(uint32 inX, uint32 inY, uint32 inZ, uint32 inW); + + /// Comparison + JPH_INLINE bool operator == (UVec4Arg inV2) const; + JPH_INLINE bool operator != (UVec4Arg inV2) const { return !(*this == inV2); } + + /// Swizzle the elements in inV + template + JPH_INLINE UVec4 Swizzle() const; + + /// Vector with all zeros + static JPH_INLINE UVec4 sZero(); + + /// Replicate int inV across all components + static JPH_INLINE UVec4 sReplicate(uint32 inV); + + /// Load 1 int from memory and place it in the X component, zeros Y, Z and W + static JPH_INLINE UVec4 sLoadInt(const uint32 *inV); + + /// Load 4 ints from memory + static JPH_INLINE UVec4 sLoadInt4(const uint32 *inV); + + /// Load 4 ints from memory, aligned to 16 bytes + static JPH_INLINE UVec4 sLoadInt4Aligned(const uint32 *inV); + + /// Gather 4 ints from memory at inBase + inOffsets[i] * Scale + template + static JPH_INLINE UVec4 sGatherInt4(const uint32 *inBase, UVec4Arg inOffsets); + + /// Return the minimum value of each of the components + static JPH_INLINE UVec4 sMin(UVec4Arg inV1, UVec4Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE UVec4 sMax(UVec4Arg inV1, UVec4Arg inV2); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(UVec4Arg inV1, UVec4Arg inV2); + + /// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1 + static JPH_INLINE UVec4 sSelect(UVec4Arg inNotSet, UVec4Arg inSet, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE UVec4 sOr(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE UVec4 sXor(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE UVec4 sAnd(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical not (component wise) + static JPH_INLINE UVec4 sNot(UVec4Arg inV1); + + /// Sorts the elements in inIndex so that the values that correspond to trues in inValue are the first elements. + /// The remaining elements will be set to inValue.w. + /// I.e. if inValue = (true, false, true, false) and inIndex = (1, 2, 3, 4) the function returns (1, 3, 4, 4). + static JPH_INLINE UVec4 sSort4True(UVec4Arg inValue, UVec4Arg inIndex); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE uint32 GetX() const { return uint32(_mm_cvtsi128_si32(mValue)); } + JPH_INLINE uint32 GetY() const { return mU32[1]; } + JPH_INLINE uint32 GetZ() const { return mU32[2]; } + JPH_INLINE uint32 GetW() const { return mU32[3]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE uint32 GetX() const { return vgetq_lane_u32(mValue, 0); } + JPH_INLINE uint32 GetY() const { return vgetq_lane_u32(mValue, 1); } + JPH_INLINE uint32 GetZ() const { return vgetq_lane_u32(mValue, 2); } + JPH_INLINE uint32 GetW() const { return vgetq_lane_u32(mValue, 3); } +#else + JPH_INLINE uint32 GetX() const { return mU32[0]; } + JPH_INLINE uint32 GetY() const { return mU32[1]; } + JPH_INLINE uint32 GetZ() const { return mU32[2]; } + JPH_INLINE uint32 GetW() const { return mU32[3]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(uint32 inX) { mU32[0] = inX; } + JPH_INLINE void SetY(uint32 inY) { mU32[1] = inY; } + JPH_INLINE void SetZ(uint32 inZ) { mU32[2] = inZ; } + JPH_INLINE void SetW(uint32 inW) { mU32[3] = inW; } + + /// Get component by index + JPH_INLINE uint32 operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 4); return mU32[inCoordinate]; } + JPH_INLINE uint32 & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 4); return mU32[inCoordinate]; } + + /// Multiplies each of the 4 integer components with an integer (discards any overflow) + JPH_INLINE UVec4 operator * (UVec4Arg inV2) const; + + /// Adds an integer value to all integer components (discards any overflow) + JPH_INLINE UVec4 operator + (UVec4Arg inV2); + + /// Add two integer vectors (component wise) + JPH_INLINE UVec4 & operator += (UVec4Arg inV2); + + /// Replicate the X component to all components + JPH_INLINE UVec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE UVec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE UVec4 SplatZ() const; + + /// Replicate the W component to all components + JPH_INLINE UVec4 SplatW() const; + + /// Convert each component from an int to a float + JPH_INLINE Vec4 ToFloat() const; + + /// Reinterpret UVec4 as a Vec4 (doesn't change the bits) + JPH_INLINE Vec4 ReinterpretAsFloat() const; + + /// Store 4 ints to memory + JPH_INLINE void StoreInt4(uint32 *outV) const; + + /// Store 4 ints to memory, aligned to 16 bytes + JPH_INLINE void StoreInt4Aligned(uint32 *outV) const; + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if any of X, Y or Z components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyXYZTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Test if X, Y and Z components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllXYZTrue() const; + + /// Count the number of components that are true (true is when highest bit of component is set) + JPH_INLINE int CountTrues() const; + + /// Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of component is set) + JPH_INLINE int GetTrues() const; + + /// Shift all components by Count bits to the left (filling with zeros from the left) + template + JPH_INLINE UVec4 LogicalShiftLeft() const; + + /// Shift all components by Count bits to the right (filling with zeros from the right) + template + JPH_INLINE UVec4 LogicalShiftRight() const; + + /// Shift all components by Count bits to the right (shifting in the value of the highest bit) + template + JPH_INLINE UVec4 ArithmeticShiftRight() const; + + /// Takes the lower 4 16 bits and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Uint16Lo() const; + + /// Takes the upper 4 16 bits and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Uint16Hi() const; + + /// Takes byte 0 .. 3 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte0() const; + + /// Takes byte 4 .. 7 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte4() const; + + /// Takes byte 8 .. 11 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte8() const; + + /// Takes byte 12 .. 15 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte12() const; + + /// Shift vector components by 4 - Count floats to the left, so if Count = 1 the resulting vector is (W, 0, 0, 0), when Count = 3 the resulting vector is (Y, Z, W, 0) + JPH_INLINE UVec4 ShiftComponents4Minus(int inCount) const; + + /// To String + friend ostream & operator << (ostream &inStream, UVec4Arg inV) + { + inStream << inV.mU32[0] << ", " << inV.mU32[1] << ", " << inV.mU32[2] << ", " << inV.mU32[3]; + return inStream; + } + + union + { + Type mValue; + uint32 mU32[4]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "UVec4.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/UVec4.inl b/thirdparty/jolt_physics/Jolt/Math/UVec4.inl new file mode 100644 index 000000000000..f9014acc28dc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/UVec4.inl @@ -0,0 +1,581 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +UVec4::UVec4(uint32 inX, uint32 inY, uint32 inZ, uint32 inW) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_epi32(int(inW), int(inZ), int(inY), int(inX)); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_u32(static_cast(inX) | (static_cast(inY) << 32)); + uint32x2_t zw = vcreate_u32(static_cast(inZ) | (static_cast(inW) << 32)); + mValue = vcombine_u32(xy, zw); +#else + mU32[0] = inX; + mU32[1] = inY; + mU32[2] = inZ; + mU32[3] = inW; +#endif +} + +bool UVec4::operator == (UVec4Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +template +UVec4 UVec4::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX)); +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_U32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleW); +#else + return UVec4(mU32[SwizzleX], mU32[SwizzleY], mU32[SwizzleZ], mU32[SwizzleW]); +#endif +} + +UVec4 UVec4::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_si128(); +#elif defined(JPH_USE_NEON) + return vdupq_n_u32(0); +#else + return UVec4(0, 0, 0, 0); +#endif +} + +UVec4 UVec4::sReplicate(uint32 inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_epi32(int(inV)); +#elif defined(JPH_USE_NEON) + return vdupq_n_u32(inV); +#else + return UVec4(inV, inV, inV, inV); +#endif +} + +UVec4 UVec4::sLoadInt(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_load_ss(reinterpret_cast(inV))); +#elif defined(JPH_USE_NEON) + return vsetq_lane_u32(*inV, vdupq_n_u32(0), 0); +#else + return UVec4(*inV, 0, 0, 0); +#endif +} + +UVec4 UVec4::sLoadInt4(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_loadu_si128(reinterpret_cast(inV)); +#elif defined(JPH_USE_NEON) + return vld1q_u32(inV); +#else + return UVec4(inV[0], inV[1], inV[2], inV[3]); +#endif +} + +UVec4 UVec4::sLoadInt4Aligned(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_load_si128(reinterpret_cast(inV)); +#elif defined(JPH_USE_NEON) + return vld1q_u32(inV); // ARM doesn't make distinction between aligned or not +#else + return UVec4(inV[0], inV[1], inV[2], inV[3]); +#endif +} + +template +UVec4 UVec4::sGatherInt4(const uint32 *inBase, UVec4Arg inOffsets) +{ +#ifdef JPH_USE_AVX2 + return _mm_i32gather_epi32(reinterpret_cast(inBase), inOffsets.mValue, Scale); +#else + const uint8 *base = reinterpret_cast(inBase); + uint32 x = *reinterpret_cast(base + inOffsets.GetX() * Scale); + uint32 y = *reinterpret_cast(base + inOffsets.GetY() * Scale); + uint32 z = *reinterpret_cast(base + inOffsets.GetZ() * Scale); + uint32 w = *reinterpret_cast(base + inOffsets.GetW() * Scale); + return UVec4(x, y, z, w); +#endif +} + +UVec4 UVec4::sMin(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE4_1) + return _mm_min_epu32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_u32(inV1.mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = min(inV1.mU32[i], inV2.mU32[i]); + return result; +#endif +} + +UVec4 UVec4::sMax(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE4_1) + return _mm_max_epu32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_u32(inV1.mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = max(inV1.mU32[i], inV2.mU32[i]); + return result; +#endif +} + +UVec4 UVec4::sEquals(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_cmpeq_epi32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vceqq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] == inV2.mU32[0]? 0xffffffffu : 0, + inV1.mU32[1] == inV2.mU32[1]? 0xffffffffu : 0, + inV1.mU32[2] == inV2.mU32[2]? 0xffffffffu : 0, + inV1.mU32[3] == inV2.mU32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 UVec4::sSelect(UVec4Arg inNotSet, UVec4Arg inSet, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox + return _mm_castps_si128(_mm_blendv_ps(_mm_castsi128_ps(inNotSet.mValue), _mm_castsi128_ps(inSet.mValue), _mm_castsi128_ps(inControl.mValue))); +#elif defined(JPH_USE_SSE) + __m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31)); + return _mm_castps_si128(_mm_or_ps(_mm_and_ps(is_set, _mm_castsi128_ps(inSet.mValue)), _mm_andnot_ps(is_set, _mm_castsi128_ps(inNotSet.mValue)))); +#elif defined(JPH_USE_NEON) + return vbslq_u32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mU32[i] : inNotSet.mU32[i]; + return result; +#endif +} + +UVec4 UVec4::sOr(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vorrq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] | inV2.mU32[0], + inV1.mU32[1] | inV2.mU32[1], + inV1.mU32[2] | inV2.mU32[2], + inV1.mU32[3] | inV2.mU32[3]); +#endif +} + +UVec4 UVec4::sXor(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return veorq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] ^ inV2.mU32[0], + inV1.mU32[1] ^ inV2.mU32[1], + inV1.mU32[2] ^ inV2.mU32[2], + inV1.mU32[3] ^ inV2.mU32[3]); +#endif +} + +UVec4 UVec4::sAnd(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vandq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] & inV2.mU32[0], + inV1.mU32[1] & inV2.mU32[1], + inV1.mU32[2] & inV2.mU32[2], + inV1.mU32[3] & inV2.mU32[3]); +#endif +} + + +UVec4 UVec4::sNot(UVec4Arg inV1) +{ +#if defined(JPH_USE_AVX512) + return _mm_ternarylogic_epi32(inV1.mValue, inV1.mValue, inV1.mValue, 0b01010101); +#elif defined(JPH_USE_SSE) + return sXor(inV1, sReplicate(0xffffffff)); +#elif defined(JPH_USE_NEON) + return vmvnq_u32(inV1.mValue); +#else + return UVec4(~inV1.mU32[0], ~inV1.mU32[1], ~inV1.mU32[2], ~inV1.mU32[3]); +#endif +} + +UVec4 UVec4::sSort4True(UVec4Arg inValue, UVec4Arg inIndex) +{ + // If inValue.z is false then shift W to Z + UVec4 v = UVec4::sSelect(inIndex.Swizzle(), inIndex, inValue.SplatZ()); + + // If inValue.y is false then shift Z and further to Y and further + v = UVec4::sSelect(v.Swizzle(), v, inValue.SplatY()); + + // If inValue.x is false then shift X and further to Y and further + v = UVec4::sSelect(v.Swizzle(), v, inValue.SplatX()); + + return v; +} + +UVec4 UVec4::operator * (UVec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_mullo_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_u32(mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = mU32[i] * inV2.mU32[i]; + return result; +#endif +} + +UVec4 UVec4::operator + (UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_add_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_u32(mValue, inV2.mValue); +#else + return UVec4(mU32[0] + inV2.mU32[0], + mU32[1] + inV2.mU32[1], + mU32[2] + inV2.mU32[2], + mU32[3] + inV2.mU32[3]); +#endif +} + +UVec4 &UVec4::operator += (UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_add_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_u32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mU32[i] += inV2.mU32[i]; +#endif + return *this; +} + +UVec4 UVec4::SplatX() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(0, 0, 0, 0)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 0); +#else + return UVec4(mU32[0], mU32[0], mU32[0], mU32[0]); +#endif +} + +UVec4 UVec4::SplatY() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(1, 1, 1, 1)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 1); +#else + return UVec4(mU32[1], mU32[1], mU32[1], mU32[1]); +#endif +} + +UVec4 UVec4::SplatZ() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(2, 2, 2, 2)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 2); +#else + return UVec4(mU32[2], mU32[2], mU32[2], mU32[2]); +#endif +} + +UVec4 UVec4::SplatW() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(3, 3, 3, 3)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 3); +#else + return UVec4(mU32[3], mU32[3], mU32[3], mU32[3]); +#endif +} + +Vec4 UVec4::ToFloat() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvtepi32_ps(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_f32_u32(mValue); +#else + return Vec4((float)mU32[0], (float)mU32[1], (float)mU32[2], (float)mU32[3]); +#endif +} + +Vec4 UVec4::ReinterpretAsFloat() const +{ +#if defined(JPH_USE_SSE) + return Vec4(_mm_castsi128_ps(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +void UVec4::StoreInt4(uint32 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_storeu_si128(reinterpret_cast<__m128i *>(outV), mValue); +#elif defined(JPH_USE_NEON) + vst1q_u32(outV, mValue); +#else + for (int i = 0; i < 4; ++i) + outV[i] = mU32[i]; +#endif +} + +void UVec4::StoreInt4Aligned(uint32 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_store_si128(reinterpret_cast<__m128i *>(outV), mValue); +#elif defined(JPH_USE_NEON) + vst1q_u32(outV, mValue); // ARM doesn't make distinction between aligned or not +#else + for (int i = 0; i < 4; ++i) + outV[i] = mU32[i]; +#endif +} + +int UVec4::CountTrues() const +{ +#if defined(JPH_USE_SSE) + return CountBits(_mm_movemask_ps(_mm_castsi128_ps(mValue))); +#elif defined(JPH_USE_NEON) + return vaddvq_u32(vshrq_n_u32(mValue, 31)); +#else + return (mU32[0] >> 31) + (mU32[1] >> 31) + (mU32[2] >> 31) + (mU32[3] >> 31); +#endif +} + +int UVec4::GetTrues() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_ps(_mm_castsi128_ps(mValue)); +#elif defined(JPH_USE_NEON) + int32x4_t shift = JPH_NEON_INT32x4(0, 1, 2, 3); + return vaddvq_u32(vshlq_u32(vshrq_n_u32(mValue, 31), shift)); +#else + return (mU32[0] >> 31) | ((mU32[1] >> 31) << 1) | ((mU32[2] >> 31) << 2) | ((mU32[3] >> 31) << 3); +#endif +} + +bool UVec4::TestAnyTrue() const +{ + return GetTrues() != 0; +} + +bool UVec4::TestAnyXYZTrue() const +{ + return (GetTrues() & 0b111) != 0; +} + +bool UVec4::TestAllTrue() const +{ + return GetTrues() == 0b1111; +} + +bool UVec4::TestAllXYZTrue() const +{ + return (GetTrues() & 0b111) == 0b111; +} + +template +UVec4 UVec4::LogicalShiftLeft() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_slli_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vshlq_n_u32(mValue, Count); +#else + return UVec4(mU32[0] << Count, mU32[1] << Count, mU32[2] << Count, mU32[3] << Count); +#endif +} + +template +UVec4 UVec4::LogicalShiftRight() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_srli_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vshrq_n_u32(mValue, Count); +#else + return UVec4(mU32[0] >> Count, mU32[1] >> Count, mU32[2] >> Count, mU32[3] >> Count); +#endif +} + +template +UVec4 UVec4::ArithmeticShiftRight() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_srai_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(mValue), Count)); +#else + return UVec4(uint32(int32_t(mU32[0]) >> Count), + uint32(int32_t(mU32[1]) >> Count), + uint32(int32_t(mU32[2]) >> Count), + uint32(int32_t(mU32[3]) >> Count)); +#endif +} + +UVec4 UVec4::Expand4Uint16Lo() const +{ +#if defined(JPH_USE_SSE) + return _mm_unpacklo_epi16(mValue, _mm_castps_si128(_mm_setzero_ps())); +#elif defined(JPH_USE_NEON) + uint16x4_t value = vget_low_u16(vreinterpretq_u16_u32(mValue)); + uint16x4_t zero = vdup_n_u16(0); + return vreinterpretq_u32_u16(vcombine_u16(vzip1_u16(value, zero), vzip2_u16(value, zero))); +#else + return UVec4(mU32[0] & 0xffff, + (mU32[0] >> 16) & 0xffff, + mU32[1] & 0xffff, + (mU32[1] >> 16) & 0xffff); +#endif +} + +UVec4 UVec4::Expand4Uint16Hi() const +{ +#if defined(JPH_USE_SSE) + return _mm_unpackhi_epi16(mValue, _mm_castps_si128(_mm_setzero_ps())); +#elif defined(JPH_USE_NEON) + uint16x4_t value = vget_high_u16(vreinterpretq_u16_u32(mValue)); + uint16x4_t zero = vdup_n_u16(0); + return vreinterpretq_u32_u16(vcombine_u16(vzip1_u16(value, zero), vzip2_u16(value, zero))); +#else + return UVec4(mU32[2] & 0xffff, + (mU32[2] >> 16) & 0xffff, + mU32[3] & 0xffff, + (mU32[3] >> 16) & 0xffff); +#endif +} + +UVec4 UVec4::Expand4Byte0() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff03), int(0xffffff02), int(0xffffff01), int(0xffffff00))); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = JPH_NEON_UINT8x16(0x00, 0x7f, 0x7f, 0x7f, 0x01, 0x7f, 0x7f, 0x7f, 0x02, 0x7f, 0x7f, 0x7f, 0x03, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[0] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte4() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff07), int(0xffffff06), int(0xffffff05), int(0xffffff04))); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = JPH_NEON_UINT8x16(0x04, 0x7f, 0x7f, 0x7f, 0x05, 0x7f, 0x7f, 0x7f, 0x06, 0x7f, 0x7f, 0x7f, 0x07, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[1] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte8() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff0b), int(0xffffff0a), int(0xffffff09), int(0xffffff08))); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = JPH_NEON_UINT8x16(0x08, 0x7f, 0x7f, 0x7f, 0x09, 0x7f, 0x7f, 0x7f, 0x0a, 0x7f, 0x7f, 0x7f, 0x0b, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[2] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte12() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff0f), int(0xffffff0e), int(0xffffff0d), int(0xffffff0c))); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = JPH_NEON_UINT8x16(0x0c, 0x7f, 0x7f, 0x7f, 0x0d, 0x7f, 0x7f, 0x7f, 0x0e, 0x7f, 0x7f, 0x7f, 0x0f, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[3] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::ShiftComponents4Minus(int inCount) const +{ +#if defined(JPH_USE_SSE4_1) || defined(JPH_USE_NEON) + alignas(UVec4) static constexpr uint32 sFourMinusXShuffle[5][4] = + { + { 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff }, + { 0x0f0e0d0c, 0xffffffff, 0xffffffff, 0xffffffff }, + { 0x0b0a0908, 0x0f0e0d0c, 0xffffffff, 0xffffffff }, + { 0x07060504, 0x0b0a0908, 0x0f0e0d0c, 0xffffffff }, + { 0x03020100, 0x07060504, 0x0b0a0908, 0x0f0e0d0c } + }; +#endif + +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, *reinterpret_cast(sFourMinusXShuffle[inCount])); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = vreinterpretq_u8_u32(*reinterpret_cast(sFourMinusXShuffle[inCount])); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result = UVec4::sZero(); + for (int i = 0; i < inCount; i++) + result.mU32[i] = mU32[i + 4 - inCount]; + return result; +#endif +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp b/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp new file mode 100644 index 000000000000..c865387f79e6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp @@ -0,0 +1,71 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +static void sAddVertex(StaticArray &ioVertices, Vec3Arg inVertex) +{ + bool found = false; + for (const Vec3 &v : ioVertices) + if (v == inVertex) + { + found = true; + break; + } + if (!found) + ioVertices.push_back(inVertex); +} + +static void sCreateVertices(StaticArray &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel) +{ + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + sAddVertex(ioVertices, center1); + sAddVertex(ioVertices, center2); + sAddVertex(ioVertices, center3); + + if (inLevel > 0) + { + int new_level = inLevel - 1; + sCreateVertices(ioVertices, inDir1, center1, center3, new_level); + sCreateVertices(ioVertices, center1, center2, center3, new_level); + sCreateVertices(ioVertices, center1, inDir2, center2, new_level); + sCreateVertices(ioVertices, center3, center2, inDir3, new_level); + } +} + +const StaticArray Vec3::sUnitSphere = []() { + + const int level = 3; + + StaticArray verts; + + // Add unit axis + verts.push_back(Vec3::sAxisX()); + verts.push_back(-Vec3::sAxisX()); + verts.push_back(Vec3::sAxisY()); + verts.push_back(-Vec3::sAxisY()); + verts.push_back(Vec3::sAxisZ()); + verts.push_back(-Vec3::sAxisZ()); + + // Subdivide + sCreateVertices(verts, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), level); + + return verts; +}(); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.h b/thirdparty/jolt_physics/Jolt/Math/Vec3.h new file mode 100644 index 000000000000..18264db7e27f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.h @@ -0,0 +1,295 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// 3 component vector (stored as 4 vectors). +/// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128; +#elif defined(JPH_USE_NEON) + using Type = float32x4_t; +#else + using Type = Vec4::Type; +#endif + + // Argument type + using ArgType = Vec3Arg; + + /// Constructor + Vec3() = default; ///< Intentionally not initialized for performance reasons + Vec3(const Vec3 &inRHS) = default; + Vec3 & operator = (const Vec3 &inRHS) = default; + explicit JPH_INLINE Vec3(Vec4Arg inRHS); + JPH_INLINE Vec3(Type inRHS) : mValue(inRHS) { CheckW(); } + + /// Load 3 floats from memory + explicit JPH_INLINE Vec3(const Float3 &inV); + + /// Create a vector from 3 components + JPH_INLINE Vec3(float inX, float inY, float inZ); + + /// Vector with all zeros + static JPH_INLINE Vec3 sZero(); + + /// Vector with all NaN's + static JPH_INLINE Vec3 sNaN(); + + /// Vectors with the principal axis + static JPH_INLINE Vec3 sAxisX() { return Vec3(1, 0, 0); } + static JPH_INLINE Vec3 sAxisY() { return Vec3(0, 1, 0); } + static JPH_INLINE Vec3 sAxisZ() { return Vec3(0, 0, 1); } + + /// Replicate inV across all components + static JPH_INLINE Vec3 sReplicate(float inV); + + /// Load 3 floats from memory (reads 32 bits extra which it doesn't use) + static JPH_INLINE Vec3 sLoadFloat3Unsafe(const Float3 &inV); + + /// Return the minimum value of each of the components + static JPH_INLINE Vec3 sMin(Vec3Arg inV1, Vec3Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE Vec3 sMax(Vec3Arg inV1, Vec3Arg inV2); + + /// Clamp a vector between min and max (component wise) + static JPH_INLINE Vec3 sClamp(Vec3Arg inV, Vec3Arg inMin, Vec3Arg inMax); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(Vec3Arg inV1, Vec3Arg inV2); + + /// Less than (component wise) + static JPH_INLINE UVec4 sLess(Vec3Arg inV1, Vec3Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE UVec4 sLessOrEqual(Vec3Arg inV1, Vec3Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE UVec4 sGreater(Vec3Arg inV1, Vec3Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE UVec4 sGreaterOrEqual(Vec3Arg inV1, Vec3Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE Vec3 sFusedMultiplyAdd(Vec3Arg inMul1, Vec3Arg inMul2, Vec3Arg inAdd); + + /// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1 + static JPH_INLINE Vec3 sSelect(Vec3Arg inNotSet, Vec3Arg inSet, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE Vec3 sOr(Vec3Arg inV1, Vec3Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE Vec3 sXor(Vec3Arg inV1, Vec3Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE Vec3 sAnd(Vec3Arg inV1, Vec3Arg inV2); + + /// Get unit vector given spherical coordinates + /// inTheta \f$\in [0, \pi]\f$ is angle between vector and z-axis + /// inPhi \f$\in [0, 2 \pi]\f$ is the angle in the xy-plane starting from the x axis and rotating counter clockwise around the z-axis + static JPH_INLINE Vec3 sUnitSpherical(float inTheta, float inPhi); + + /// A set of vectors uniformly spanning the surface of a unit sphere, usable for debug purposes + JPH_EXPORT static const StaticArray sUnitSphere; + + /// Get random unit vector + template + static inline Vec3 sRandom(Random &inRandom); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE float GetX() const { return _mm_cvtss_f32(mValue); } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE float GetX() const { return vgetq_lane_f32(mValue, 0); } + JPH_INLINE float GetY() const { return vgetq_lane_f32(mValue, 1); } + JPH_INLINE float GetZ() const { return vgetq_lane_f32(mValue, 2); } +#else + JPH_INLINE float GetX() const { return mF32[0]; } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(float inX) { mF32[0] = inX; } + JPH_INLINE void SetY(float inY) { mF32[1] = inY; } + JPH_INLINE void SetZ(float inZ) { mF32[2] = mF32[3] = inZ; } // Assure Z and W are the same + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ) { *this = Vec3(inX, inY, inZ); } + + /// Get float component by index + JPH_INLINE float operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 3); return mF32[inCoordinate]; } + + /// Set float component by index + JPH_INLINE void SetComponent(uint inCoordinate, float inValue) { JPH_ASSERT(inCoordinate < 3); mF32[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same + + /// Comparison + JPH_INLINE bool operator == (Vec3Arg inV2) const; + JPH_INLINE bool operator != (Vec3Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(Vec3Arg inV2, float inMaxDistSq = 1.0e-12f) const; + + /// Test if vector is near zero + JPH_INLINE bool IsNearZero(float inMaxDistSq = 1.0e-12f) const; + + /// Test if vector is normalized + JPH_INLINE bool IsNormalized(float inTolerance = 1.0e-6f) const; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two float vectors (component wise) + JPH_INLINE Vec3 operator * (Vec3Arg inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec3 operator * (float inV2) const; + + /// Multiply vector with float + friend JPH_INLINE Vec3 operator * (float inV1, Vec3Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec3 operator / (float inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec3 & operator *= (float inV2); + + /// Multiply vector with vector + JPH_INLINE Vec3 & operator *= (Vec3Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec3 & operator /= (float inV2); + + /// Add two float vectors (component wise) + JPH_INLINE Vec3 operator + (Vec3Arg inV2) const; + + /// Add two float vectors (component wise) + JPH_INLINE Vec3 & operator += (Vec3Arg inV2); + + /// Negate + JPH_INLINE Vec3 operator - () const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec3 operator - (Vec3Arg inV2) const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec3 & operator -= (Vec3Arg inV2); + + /// Divide (component wise) + JPH_INLINE Vec3 operator / (Vec3Arg inV2) const; + + /// Swizzle the elements in inV + template + JPH_INLINE Vec3 Swizzle() const; + + /// Replicate the X component to all components + JPH_INLINE Vec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE Vec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE Vec4 SplatZ() const; + + /// Get index of component with lowest value + JPH_INLINE int GetLowestComponentIndex() const; + + /// Get index of component with highest value + JPH_INLINE int GetHighestComponentIndex() const; + + /// Return the absolute value of each of the components + JPH_INLINE Vec3 Abs() const; + + /// Reciprocal vector (1 / value) for each of the components + JPH_INLINE Vec3 Reciprocal() const; + + /// Cross product + JPH_INLINE Vec3 Cross(Vec3Arg inV2) const; + + /// Dot product, returns the dot product in X, Y and Z components + JPH_INLINE Vec3 DotV(Vec3Arg inV2) const; + + /// Dot product, returns the dot product in X, Y, Z and W components + JPH_INLINE Vec4 DotV4(Vec3Arg inV2) const; + + /// Dot product + JPH_INLINE float Dot(Vec3Arg inV2) const; + + /// Squared length of vector + JPH_INLINE float LengthSq() const; + + /// Length of vector + JPH_INLINE float Length() const; + + /// Normalize vector + JPH_INLINE Vec3 Normalized() const; + + /// Normalize vector or return inZeroValue if the length of the vector is zero + JPH_INLINE Vec3 NormalizedOr(Vec3Arg inZeroValue) const; + + /// Store 3 floats to memory + JPH_INLINE void StoreFloat3(Float3 *outV) const; + + /// Convert each component from a float to an int + JPH_INLINE UVec4 ToInt() const; + + /// Reinterpret Vec3 as a UVec4 (doesn't change the bits) + JPH_INLINE UVec4 ReinterpretAsInt() const; + + /// Get the minimum of X, Y and Z + JPH_INLINE float ReduceMin() const; + + /// Get the maximum of X, Y and Z + JPH_INLINE float ReduceMax() const; + + /// Component wise square root + JPH_INLINE Vec3 Sqrt() const; + + /// Get normalized vector that is perpendicular to this vector + JPH_INLINE Vec3 GetNormalizedPerpendicular() const; + + /// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative) + JPH_INLINE Vec3 GetSign() const; + + /// To String + friend ostream & operator << (ostream &inStream, Vec3Arg inV) + { + inStream << inV.mF32[0] << ", " << inV.mF32[1] << ", " << inV.mF32[2]; + return inStream; + } + + /// Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate div by 0 + JPH_INLINE void CheckW() const; + + /// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero + static JPH_INLINE Type sFixW(Type inValue); + + union + { + Type mValue; + float mF32[4]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Vec3.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.inl b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl new file mode 100644 index 000000000000..5a47f40ce6ba --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl @@ -0,0 +1,859 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +// Create a std::hash/JPH::Hash for Vec3 +JPH_MAKE_HASHABLE(JPH::Vec3, t.GetX(), t.GetY(), t.GetZ()) + +JPH_NAMESPACE_BEGIN + +void Vec3::CheckW() const +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + // Avoid asserts when both components are NaN + JPH_ASSERT(reinterpret_cast(mF32)[2] == reinterpret_cast(mF32)[3]); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +JPH_INLINE Vec3::Type Vec3::sFixW(Type inValue) +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #if defined(JPH_USE_SSE) + return _mm_shuffle_ps(inValue, inValue, _MM_SHUFFLE(2, 2, 1, 0)); + #elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(inValue, inValue, 0, 1, 2, 2); + #else + Type value; + value.mData[0] = inValue.mData[0]; + value.mData[1] = inValue.mData[1]; + value.mData[2] = inValue.mData[2]; + value.mData[3] = inValue.mData[2]; + return value; + #endif +#else + return inValue; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +Vec3::Vec3(Vec4Arg inRHS) : + mValue(sFixW(inRHS.mValue)) +{ +} + +Vec3::Vec3(const Float3 &inV) +{ +#if defined(JPH_USE_SSE) + Type x = _mm_load_ss(&inV.x); + Type y = _mm_load_ss(&inV.y); + Type z = _mm_load_ss(&inV.z); + Type xy = _mm_unpacklo_ps(x, y); + mValue = _mm_shuffle_ps(xy, z, _MM_SHUFFLE(0, 0, 1, 0)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + float32x2_t xy = vld1_f32(&inV.x); + float32x2_t zz = vdup_n_f32(inV.z); // Assure Z and W are the same + mValue = vcombine_f32(xy, zz); +#else + mF32[0] = inV[0]; + mF32[1] = inV[1]; + mF32[2] = inV[2]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = inV[2]; + #endif +#endif +} + +Vec3::Vec3(float inX, float inY, float inZ) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_ps(inZ, inZ, inY, inX); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_u32(static_cast(BitCast(inX)) | (static_cast(BitCast(inY)) << 32)); + uint32x2_t zz = vreinterpret_u32_f32(vdup_n_f32(inZ)); + mValue = vreinterpretq_f32_u32(vcombine_u32(xy, zz)); +#else + mF32[0] = inX; + mF32[1] = inY; + mF32[2] = inZ; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = inZ; + #endif +#endif +} + +template +Vec3 Vec3::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleZ, SwizzleZ, SwizzleY, SwizzleX)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleZ); +#else + return Vec3(mF32[SwizzleX], mF32[SwizzleY], mF32[SwizzleZ]); +#endif +} + +Vec3 Vec3::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_ps(); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(0); +#else + return Vec3(0, 0, 0); +#endif +} + +Vec3 Vec3::sReplicate(float inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_ps(inV); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(inV); +#else + return Vec3(inV, inV, inV); +#endif +} + +Vec3 Vec3::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +Vec3 Vec3::sLoadFloat3Unsafe(const Float3 &inV) +{ +#if defined(JPH_USE_SSE) + Type v = _mm_loadu_ps(&inV.x); +#elif defined(JPH_USE_NEON) + Type v = vld1q_f32(&inV.x); +#else + Type v = { inV.x, inV.y, inV.z }; +#endif + return sFixW(v); +} + +Vec3 Vec3::sMin(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_min_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_f32(inV1.mValue, inV2.mValue); +#else + return Vec3(min(inV1.mF32[0], inV2.mF32[0]), + min(inV1.mF32[1], inV2.mF32[1]), + min(inV1.mF32[2], inV2.mF32[2])); +#endif +} + +Vec3 Vec3::sMax(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_max_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_f32(inV1.mValue, inV2.mValue); +#else + return Vec3(max(inV1.mF32[0], inV2.mF32[0]), + max(inV1.mF32[1], inV2.mF32[1]), + max(inV1.mF32[2], inV2.mF32[2])); +#endif +} + +Vec3 Vec3::sClamp(Vec3Arg inV, Vec3Arg inMin, Vec3Arg inMax) +{ + return sMax(sMin(inV, inMax), inMin); +} + +UVec4 Vec3::sEquals(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpeq_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vceqq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] == inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] == inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] == inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sLess(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmplt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcltq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] < inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] < inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] < inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sLessOrEqual(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmple_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcleq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] <= inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] <= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] <= inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sGreater(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpgt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgtq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] > inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] > inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] > inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sGreaterOrEqual(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpge_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgeq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] >= inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] >= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] >= inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +Vec3 Vec3::sFusedMultiplyAdd(Vec3Arg inMul1, Vec3Arg inMul2, Vec3Arg inAdd) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_FMADD + return _mm_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm_add_ps(_mm_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return vmlaq_f32(inAdd.mValue, inMul1.mValue, inMul2.mValue); +#else + return Vec3(inMul1.mF32[0] * inMul2.mF32[0] + inAdd.mF32[0], + inMul1.mF32[1] * inMul2.mF32[1] + inAdd.mF32[1], + inMul1.mF32[2] * inMul2.mF32[2] + inAdd.mF32[2]); +#endif +} + +Vec3 Vec3::sSelect(Vec3Arg inNotSet, Vec3Arg inSet, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox + Type v = _mm_blendv_ps(inNotSet.mValue, inSet.mValue, _mm_castsi128_ps(inControl.mValue)); + return sFixW(v); +#elif defined(JPH_USE_SSE) + __m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31)); + Type v = _mm_or_ps(_mm_and_ps(is_set, inSet.mValue), _mm_andnot_ps(is_set, inNotSet.mValue)); + return sFixW(v); +#elif defined(JPH_USE_NEON) + Type v = vbslq_f32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue); + return sFixW(v); +#else + Vec3 result; + for (int i = 0; i < 3; i++) + result.mF32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mF32[i] : inNotSet.mF32[i]; +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + result.mF32[3] = result.mF32[2]; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + return result; +#endif +} + +Vec3 Vec3::sOr(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return Vec3(UVec4::sOr(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sXor(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return Vec3(UVec4::sXor(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sAnd(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return Vec3(UVec4::sAnd(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sUnitSpherical(float inTheta, float inPhi) +{ + Vec4 s, c; + Vec4(inTheta, inPhi, 0, 0).SinCos(s, c); + return Vec3(s.GetX() * c.GetY(), s.GetX() * s.GetY(), c.GetX()); +} + +template +Vec3 Vec3::sRandom(Random &inRandom) +{ + std::uniform_real_distribution zero_to_one(0.0f, 1.0f); + float theta = JPH_PI * zero_to_one(inRandom); + float phi = 2.0f * JPH_PI * zero_to_one(inRandom); + return sUnitSpherical(theta, phi); +} + +bool Vec3::operator == (Vec3Arg inV2) const +{ + return sEquals(*this, inV2).TestAllXYZTrue(); +} + +bool Vec3::IsClose(Vec3Arg inV2, float inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool Vec3::IsNearZero(float inMaxDistSq) const +{ + return LengthSq() <= inMaxDistSq; +} + +Vec3 Vec3::operator * (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] * inV2.mF32[0], mF32[1] * inV2.mF32[1], mF32[2] * inV2.mF32[2]); +#endif +} + +Vec3 Vec3::operator * (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(mValue, inV2); +#else + return Vec3(mF32[0] * inV2, mF32[1] * inV2, mF32[2] * inV2); +#endif +} + +Vec3 operator * (float inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(_mm_set1_ps(inV1), inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(inV2.mValue, inV1); +#else + return Vec3(inV1 * inV2.mF32[0], inV1 * inV2.mF32[1], inV1 * inV2.mF32[2]); +#endif +} + +Vec3 Vec3::operator / (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + return Vec3(mF32[0] / inV2, mF32[1] / inV2, mF32[2] / inV2); +#endif +} + +Vec3 &Vec3::operator *= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vmulq_n_f32(mValue, inV2); +#else + for (int i = 0; i < 3; ++i) + mF32[i] *= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 &Vec3::operator *= (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vmulq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] *= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 &Vec3::operator /= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + for (int i = 0; i < 3; ++i) + mF32[i] /= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator + (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] + inV2.mF32[0], mF32[1] + inV2.mF32[1], mF32[2] + inV2.mF32[2]); +#endif +} + +Vec3 &Vec3::operator += (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] += inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator - () const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(_mm_setzero_ps(), mValue); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return vsubq_f32(vdupq_n_f32(0), mValue); + #else + return vnegq_f32(mValue); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return Vec3(0.0f - mF32[0], 0.0f - mF32[1], 0.0f - mF32[2]); + #else + return Vec3(-mF32[0], -mF32[1], -mF32[2]); + #endif +#endif +} + +Vec3 Vec3::operator - (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vsubq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] - inV2.mF32[0], mF32[1] - inV2.mF32[1], mF32[2] - inV2.mF32[2]); +#endif +} + +Vec3 &Vec3::operator -= (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vsubq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] -= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator / (Vec3Arg inV2) const +{ + inV2.CheckW(); // Check W equals Z to avoid div by zero +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] / inV2.mF32[0], mF32[1] / inV2.mF32[1], mF32[2] / inV2.mF32[2]); +#endif +} + +Vec4 Vec3::SplatX() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 0, 0)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 0); +#else + return Vec4(mF32[0], mF32[0], mF32[0], mF32[0]); +#endif +} + +Vec4 Vec3::SplatY() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(1, 1, 1, 1)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 1); +#else + return Vec4(mF32[1], mF32[1], mF32[1], mF32[1]); +#endif +} + +Vec4 Vec3::SplatZ() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(2, 2, 2, 2)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 2); +#else + return Vec4(mF32[2], mF32[2], mF32[2], mF32[2]); +#endif +} + +int Vec3::GetLowestComponentIndex() const +{ + return GetX() < GetY() ? (GetZ() < GetX() ? 2 : 0) : (GetZ() < GetY() ? 2 : 1); +} + +int Vec3::GetHighestComponentIndex() const +{ + return GetX() > GetY() ? (GetZ() > GetX() ? 2 : 0) : (GetZ() > GetY() ? 2 : 1); +} + +Vec3 Vec3::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm_range_ps(mValue, mValue, 0b1000); +#elif defined(JPH_USE_SSE) + return _mm_max_ps(_mm_sub_ps(_mm_setzero_ps(), mValue), mValue); +#elif defined(JPH_USE_NEON) + return vabsq_f32(mValue); +#else + return Vec3(abs(mF32[0]), abs(mF32[1]), abs(mF32[2])); +#endif +} + +Vec3 Vec3::Reciprocal() const +{ + return sReplicate(1.0f) / mValue; +} + +Vec3 Vec3::Cross(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + Type t1 = _mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t1 = _mm_mul_ps(t1, mValue); + Type t2 = _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t2 = _mm_mul_ps(t2, inV2.mValue); + Type t3 = _mm_sub_ps(t1, t2); + return _mm_shuffle_ps(t3, t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + Type t1 = JPH_NEON_SHUFFLE_F32x4(inV2.mValue, inV2.mValue, 1, 2, 0, 0); // Assure Z and W are the same + t1 = vmulq_f32(t1, mValue); + Type t2 = JPH_NEON_SHUFFLE_F32x4(mValue, mValue, 1, 2, 0, 0); // Assure Z and W are the same + t2 = vmulq_f32(t2, inV2.mValue); + Type t3 = vsubq_f32(t1, t2); + return JPH_NEON_SHUFFLE_F32x4(t3, t3, 1, 2, 0, 0); // Assure Z and W are the same +#else + return Vec3(mF32[1] * inV2.mF32[2] - mF32[2] * inV2.mF32[1], + mF32[2] * inV2.mF32[0] - mF32[0] * inV2.mF32[2], + mF32[0] * inV2.mF32[1] - mF32[1] * inV2.mF32[0]); +#endif +} + +Vec3 Vec3::DotV(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0x7f); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return Vec3::sReplicate(dot); +#endif +} + +Vec4 Vec3::DotV4(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0x7f); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return Vec4::sReplicate(dot); +#endif +} + +float Vec3::Dot(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, inV2.mValue, 0x7f)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vaddvq_f32(mul); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return dot; +#endif +} + +float Vec3::LengthSq() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, mValue, 0x7f)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vaddvq_f32(mul); +#else + float len_sq = 0.0f; + for (int i = 0; i < 3; i++) + len_sq += mF32[i] * mF32[i]; + return len_sq; +#endif +} + +float Vec3::Length() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(mValue, mValue, 0x7f))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x2_t sum = vdup_n_f32(vaddvq_f32(mul)); + return vget_lane_f32(vsqrt_f32(sum), 0); +#else + return sqrt(LengthSq()); +#endif +} + +Vec3 Vec3::Sqrt() const +{ +#if defined(JPH_USE_SSE) + return _mm_sqrt_ps(mValue); +#elif defined(JPH_USE_NEON) + return vsqrtq_f32(mValue); +#else + return Vec3(sqrt(mF32[0]), sqrt(mF32[1]), sqrt(mF32[2])); +#endif +} + +Vec3 Vec3::Normalized() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_div_ps(mValue, _mm_sqrt_ps(_mm_dp_ps(mValue, mValue, 0x7f))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul)); + return vdivq_f32(mValue, vsqrtq_f32(sum)); +#else + return *this / Length(); +#endif +} + +Vec3 Vec3::NormalizedOr(Vec3Arg inZeroValue) const +{ +#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox + Type len_sq = _mm_dp_ps(mValue, mValue, 0x7f); + // clang with '-ffast-math' (which you should not use!) can generate _mm_rsqrt_ps + // instructions which produce INFs/NaNs when they get a denormal float as input. + // We therefore treat denormals as zero here. + Type is_zero = _mm_cmple_ps(len_sq, _mm_set1_ps(FLT_MIN)); +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + if (_mm_movemask_ps(is_zero) == 0xf) + return inZeroValue; + else + return _mm_div_ps(mValue, _mm_sqrt_ps(len_sq)); +#else + return _mm_blendv_ps(_mm_div_ps(mValue, _mm_sqrt_ps(len_sq)), inZeroValue.mValue, is_zero); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x4_t len_sq = vdupq_n_f32(vaddvq_f32(mul)); + uint32x4_t is_zero = vcleq_f32(len_sq, vdupq_n_f32(FLT_MIN)); + return vbslq_f32(is_zero, inZeroValue.mValue, vdivq_f32(mValue, vsqrtq_f32(len_sq))); +#else + float len_sq = LengthSq(); + if (len_sq <= FLT_MIN) + return inZeroValue; + else + return *this / sqrt(len_sq); +#endif +} + +bool Vec3::IsNormalized(float inTolerance) const +{ + return abs(LengthSq() - 1.0f) <= inTolerance; +} + +bool Vec3::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return (_mm_fpclass_ps_mask(mValue, 0b10000001) & 0x7) != 0; +#elif defined(JPH_USE_SSE) + return (_mm_movemask_ps(_mm_cmpunord_ps(mValue, mValue)) & 0x7) != 0; +#elif defined(JPH_USE_NEON) + uint32x4_t mask = JPH_NEON_UINT32x4(1, 1, 1, 0); + uint32x4_t is_equal = vceqq_f32(mValue, mValue); // If a number is not equal to itself it's a NaN + return vaddvq_u32(vandq_u32(is_equal, mask)) != 3; +#else + return isnan(mF32[0]) || isnan(mF32[1]) || isnan(mF32[2]); +#endif +} + +void Vec3::StoreFloat3(Float3 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_store_ss(&outV->x, mValue); + Vec3 t = Swizzle(); + _mm_store_ss(&outV->y, t.mValue); + t = t.Swizzle(); + _mm_store_ss(&outV->z, t.mValue); +#elif defined(JPH_USE_NEON) + float32x2_t xy = vget_low_f32(mValue); + vst1_f32(&outV->x, xy); + vst1q_lane_f32(&outV->z, mValue, 2); +#else + outV->x = mF32[0]; + outV->y = mF32[1]; + outV->z = mF32[2]; +#endif +} + +UVec4 Vec3::ToInt() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvttps_epi32(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_u32_f32(mValue); +#else + return UVec4(uint32(mF32[0]), uint32(mF32[1]), uint32(mF32[2]), uint32(mF32[3])); +#endif +} + +UVec4 Vec3::ReinterpretAsInt() const +{ +#if defined(JPH_USE_SSE) + return UVec4(_mm_castps_si128(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_u32_f32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +float Vec3::ReduceMin() const +{ + Vec3 v = sMin(mValue, Swizzle()); + v = sMin(v, v.Swizzle()); + return v.GetX(); +} + +float Vec3::ReduceMax() const +{ + Vec3 v = sMax(mValue, Swizzle()); + v = sMax(v, v.Swizzle()); + return v.GetX(); +} + +Vec3 Vec3::GetNormalizedPerpendicular() const +{ + if (abs(mF32[0]) > abs(mF32[1])) + { + float len = sqrt(mF32[0] * mF32[0] + mF32[2] * mF32[2]); + return Vec3(mF32[2], 0.0f, -mF32[0]) / len; + } + else + { + float len = sqrt(mF32[1] * mF32[1] + mF32[2] * mF32[2]); + return Vec3(0.0f, mF32[2], -mF32[1]) / len; + } +} + +Vec3 Vec3::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fixupimm_ps(mValue, mValue, _mm_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_SSE) + Type minus_one = _mm_set1_ps(-1.0f); + Type one = _mm_set1_ps(1.0f); + return _mm_or_ps(_mm_and_ps(mValue, minus_one), one); +#elif defined(JPH_USE_NEON) + Type minus_one = vdupq_n_f32(-1.0f); + Type one = vdupq_n_f32(1.0f); + return vreinterpretq_f32_u32(vorrq_u32(vandq_u32(vreinterpretq_u32_f32(mValue), vreinterpretq_u32_f32(minus_one)), vreinterpretq_u32_f32(one))); +#else + return Vec3(std::signbit(mF32[0])? -1.0f : 1.0f, + std::signbit(mF32[1])? -1.0f : 1.0f, + std::signbit(mF32[2])? -1.0f : 1.0f); +#endif +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.h b/thirdparty/jolt_physics/Jolt/Math/Vec4.h new file mode 100644 index 000000000000..eb924a0f679c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.h @@ -0,0 +1,283 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128; +#elif defined(JPH_USE_NEON) + using Type = float32x4_t; +#else + using Type = struct { float mData[4]; }; +#endif + + /// Constructor + Vec4() = default; ///< Intentionally not initialized for performance reasons + Vec4(const Vec4 &inRHS) = default; + Vec4 & operator = (const Vec4 &inRHS) = default; + explicit JPH_INLINE Vec4(Vec3Arg inRHS); ///< WARNING: W component undefined! + JPH_INLINE Vec4(Vec3Arg inRHS, float inW); + JPH_INLINE Vec4(Type inRHS) : mValue(inRHS) { } + + /// Create a vector from 4 components + JPH_INLINE Vec4(float inX, float inY, float inZ, float inW); + + /// Vector with all zeros + static JPH_INLINE Vec4 sZero(); + + /// Vector with all NaN's + static JPH_INLINE Vec4 sNaN(); + + /// Replicate inV across all components + static JPH_INLINE Vec4 sReplicate(float inV); + + /// Load 4 floats from memory + static JPH_INLINE Vec4 sLoadFloat4(const Float4 *inV); + + /// Load 4 floats from memory, 16 bytes aligned + static JPH_INLINE Vec4 sLoadFloat4Aligned(const Float4 *inV); + + /// Gather 4 floats from memory at inBase + inOffsets[i] * Scale + template + static JPH_INLINE Vec4 sGatherFloat4(const float *inBase, UVec4Arg inOffsets); + + /// Return the minimum value of each of the components + static JPH_INLINE Vec4 sMin(Vec4Arg inV1, Vec4Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE Vec4 sMax(Vec4Arg inV1, Vec4Arg inV2); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(Vec4Arg inV1, Vec4Arg inV2); + + /// Less than (component wise) + static JPH_INLINE UVec4 sLess(Vec4Arg inV1, Vec4Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE UVec4 sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE UVec4 sGreater(Vec4Arg inV1, Vec4Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE UVec4 sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE Vec4 sFusedMultiplyAdd(Vec4Arg inMul1, Vec4Arg inMul2, Vec4Arg inAdd); + + /// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1 + static JPH_INLINE Vec4 sSelect(Vec4Arg inNotSet, Vec4Arg inSet, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE Vec4 sOr(Vec4Arg inV1, Vec4Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE Vec4 sXor(Vec4Arg inV1, Vec4Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE Vec4 sAnd(Vec4Arg inV1, Vec4Arg inV2); + + /// Sort the four elements of ioValue and sort ioIndex at the same time. + /// Based on a sorting network: http://en.wikipedia.org/wiki/Sorting_network + static JPH_INLINE void sSort4(Vec4 &ioValue, UVec4 &ioIndex); + + /// Reverse sort the four elements of ioValue (highest first) and sort ioIndex at the same time. + /// Based on a sorting network: http://en.wikipedia.org/wiki/Sorting_network + static JPH_INLINE void sSort4Reverse(Vec4 &ioValue, UVec4 &ioIndex); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE float GetX() const { return _mm_cvtss_f32(mValue); } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } + JPH_INLINE float GetW() const { return mF32[3]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE float GetX() const { return vgetq_lane_f32(mValue, 0); } + JPH_INLINE float GetY() const { return vgetq_lane_f32(mValue, 1); } + JPH_INLINE float GetZ() const { return vgetq_lane_f32(mValue, 2); } + JPH_INLINE float GetW() const { return vgetq_lane_f32(mValue, 3); } +#else + JPH_INLINE float GetX() const { return mF32[0]; } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } + JPH_INLINE float GetW() const { return mF32[3]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(float inX) { mF32[0] = inX; } + JPH_INLINE void SetY(float inY) { mF32[1] = inY; } + JPH_INLINE void SetZ(float inZ) { mF32[2] = inZ; } + JPH_INLINE void SetW(float inW) { mF32[3] = inW; } + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ, float inW) { *this = Vec4(inX, inY, inZ, inW); } + + /// Get float component by index + JPH_INLINE float operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 4); return mF32[inCoordinate]; } + JPH_INLINE float & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 4); return mF32[inCoordinate]; } + + /// Comparison + JPH_INLINE bool operator == (Vec4Arg inV2) const; + JPH_INLINE bool operator != (Vec4Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(Vec4Arg inV2, float inMaxDistSq = 1.0e-12f) const; + + /// Test if vector is normalized + JPH_INLINE bool IsNormalized(float inTolerance = 1.0e-6f) const; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two float vectors (component wise) + JPH_INLINE Vec4 operator * (Vec4Arg inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec4 operator * (float inV2) const; + + /// Multiply vector with float + friend JPH_INLINE Vec4 operator * (float inV1, Vec4Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec4 operator / (float inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec4 & operator *= (float inV2); + + /// Multiply vector with vector + JPH_INLINE Vec4 & operator *= (Vec4Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec4 & operator /= (float inV2); + + /// Add two float vectors (component wise) + JPH_INLINE Vec4 operator + (Vec4Arg inV2) const; + + /// Add two float vectors (component wise) + JPH_INLINE Vec4 & operator += (Vec4Arg inV2); + + /// Negate + JPH_INLINE Vec4 operator - () const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec4 operator - (Vec4Arg inV2) const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec4 & operator -= (Vec4Arg inV2); + + /// Divide (component wise) + JPH_INLINE Vec4 operator / (Vec4Arg inV2) const; + + /// Swizzle the elements in inV + template + JPH_INLINE Vec4 Swizzle() const; + + /// Replicate the X component to all components + JPH_INLINE Vec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE Vec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE Vec4 SplatZ() const; + + /// Replicate the W component to all components + JPH_INLINE Vec4 SplatW() const; + + /// Return the absolute value of each of the components + JPH_INLINE Vec4 Abs() const; + + /// Reciprocal vector (1 / value) for each of the components + JPH_INLINE Vec4 Reciprocal() const; + + /// Dot product, returns the dot product in X, Y and Z components + JPH_INLINE Vec4 DotV(Vec4Arg inV2) const; + + /// Dot product + JPH_INLINE float Dot(Vec4Arg inV2) const; + + /// Squared length of vector + JPH_INLINE float LengthSq() const; + + /// Length of vector + JPH_INLINE float Length() const; + + /// Normalize vector + JPH_INLINE Vec4 Normalized() const; + + /// Store 4 floats to memory + JPH_INLINE void StoreFloat4(Float4 *outV) const; + + /// Convert each component from a float to an int + JPH_INLINE UVec4 ToInt() const; + + /// Reinterpret Vec4 as a UVec4 (doesn't change the bits) + JPH_INLINE UVec4 ReinterpretAsInt() const; + + /// Store if X is negative in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 + JPH_INLINE int GetSignBits() const; + + /// Get the minimum of X, Y, Z and W + JPH_INLINE float ReduceMin() const; + + /// Get the maximum of X, Y, Z and W + JPH_INLINE float ReduceMax() const; + + /// Component wise square root + JPH_INLINE Vec4 Sqrt() const; + + /// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative) + JPH_INLINE Vec4 GetSign() const; + + /// Calculate the sine and cosine for each element of this vector (input in radians) + inline void SinCos(Vec4 &outSin, Vec4 &outCos) const; + + /// Calculate the tangent for each element of this vector (input in radians) + inline Vec4 Tan() const; + + /// Calculate the arc sine for each element of this vector (returns value in the range [-PI / 2, PI / 2]) + /// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::asin + inline Vec4 ASin() const; + + /// Calculate the arc cosine for each element of this vector (returns value in the range [0, PI]) + /// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::acos + inline Vec4 ACos() const; + + /// Calculate the arc tangent for each element of this vector (returns value in the range [-PI / 2, PI / 2]) + inline Vec4 ATan() const; + + /// Calculate the arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI]) + inline static Vec4 sATan2(Vec4Arg inY, Vec4Arg inX); + + /// To String + friend ostream & operator << (ostream &inStream, Vec4Arg inV) + { + inStream << inV.mF32[0] << ", " << inV.mF32[1] << ", " << inV.mF32[2] << ", " << inV.mF32[3]; + return inStream; + } + + union + { + Type mValue; + float mF32[4]; + }; +}; + +static_assert(std::is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Vec4.inl" diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.inl b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl new file mode 100644 index 000000000000..43676361c57e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl @@ -0,0 +1,981 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +// Constructor +Vec4::Vec4(Vec3Arg inRHS) : + mValue(inRHS.mValue) +{ +} + +Vec4::Vec4(Vec3Arg inRHS, float inW) +{ +#if defined(JPH_USE_SSE4_1) + mValue = _mm_blend_ps(inRHS.mValue, _mm_set1_ps(inW), 8); +#elif defined(JPH_USE_NEON) + mValue = vsetq_lane_f32(inW, inRHS.mValue, 3); +#else + for (int i = 0; i < 3; i++) + mF32[i] = inRHS.mF32[i]; + mF32[3] = inW; +#endif +} + +Vec4::Vec4(float inX, float inY, float inZ, float inW) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_ps(inW, inZ, inY, inX); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_u32(static_cast(BitCast(inX)) | (static_cast(BitCast(inY)) << 32)); + uint32x2_t zw = vcreate_u32(static_cast(BitCast(inZ)) | (static_cast(BitCast(inW)) << 32)); + mValue = vreinterpretq_f32_u32(vcombine_u32(xy, zw)); +#else + mF32[0] = inX; + mF32[1] = inY; + mF32[2] = inZ; + mF32[3] = inW; +#endif +} + +template +Vec4 Vec4::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX)); +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleW); +#else + return Vec4(mF32[SwizzleX], mF32[SwizzleY], mF32[SwizzleZ], mF32[SwizzleW]); +#endif +} + +Vec4 Vec4::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_ps(); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(0); +#else + return Vec4(0, 0, 0, 0); +#endif +} + +Vec4 Vec4::sReplicate(float inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_ps(inV); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(inV); +#else + return Vec4(inV, inV, inV, inV); +#endif +} + +Vec4 Vec4::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +Vec4 Vec4::sLoadFloat4(const Float4 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_loadu_ps(&inV->x); +#elif defined(JPH_USE_NEON) + return vld1q_f32(&inV->x); +#else + return Vec4(inV->x, inV->y, inV->z, inV->w); +#endif +} + +Vec4 Vec4::sLoadFloat4Aligned(const Float4 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_load_ps(&inV->x); +#elif defined(JPH_USE_NEON) + return vld1q_f32(&inV->x); +#else + return Vec4(inV->x, inV->y, inV->z, inV->w); +#endif +} + +template +Vec4 Vec4::sGatherFloat4(const float *inBase, UVec4Arg inOffsets) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_AVX2 + return _mm_i32gather_ps(inBase, inOffsets.mValue, Scale); + #else + const uint8 *base = reinterpret_cast(inBase); + Type x = _mm_load_ss(reinterpret_cast(base + inOffsets.GetX() * Scale)); + Type y = _mm_load_ss(reinterpret_cast(base + inOffsets.GetY() * Scale)); + Type xy = _mm_unpacklo_ps(x, y); + Type z = _mm_load_ss(reinterpret_cast(base + inOffsets.GetZ() * Scale)); + Type w = _mm_load_ss(reinterpret_cast(base + inOffsets.GetW() * Scale)); + Type zw = _mm_unpacklo_ps(z, w); + return _mm_movelh_ps(xy, zw); + #endif +#else + const uint8 *base = reinterpret_cast(inBase); + float x = *reinterpret_cast(base + inOffsets.GetX() * Scale); + float y = *reinterpret_cast(base + inOffsets.GetY() * Scale); + float z = *reinterpret_cast(base + inOffsets.GetZ() * Scale); + float w = *reinterpret_cast(base + inOffsets.GetW() * Scale); + return Vec4(x, y, z, w); +#endif +} + +Vec4 Vec4::sMin(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_min_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_f32(inV1.mValue, inV2.mValue); +#else + return Vec4(min(inV1.mF32[0], inV2.mF32[0]), + min(inV1.mF32[1], inV2.mF32[1]), + min(inV1.mF32[2], inV2.mF32[2]), + min(inV1.mF32[3], inV2.mF32[3])); +#endif +} + +Vec4 Vec4::sMax(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_max_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_f32(inV1.mValue, inV2.mValue); +#else + return Vec4(max(inV1.mF32[0], inV2.mF32[0]), + max(inV1.mF32[1], inV2.mF32[1]), + max(inV1.mF32[2], inV2.mF32[2]), + max(inV1.mF32[3], inV2.mF32[3])); +#endif +} + +UVec4 Vec4::sEquals(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpeq_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vceqq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] == inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] == inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] == inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] == inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sLess(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmplt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcltq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] < inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] < inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] < inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] < inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmple_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcleq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] <= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] <= inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] <= inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] <= inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sGreater(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpgt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgtq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] > inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] > inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] > inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] > inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpge_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgeq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] >= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] >= inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] >= inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] >= inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +Vec4 Vec4::sFusedMultiplyAdd(Vec4Arg inMul1, Vec4Arg inMul2, Vec4Arg inAdd) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_FMADD + return _mm_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm_add_ps(_mm_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return vmlaq_f32(inAdd.mValue, inMul1.mValue, inMul2.mValue); +#else + return Vec4(inMul1.mF32[0] * inMul2.mF32[0] + inAdd.mF32[0], + inMul1.mF32[1] * inMul2.mF32[1] + inAdd.mF32[1], + inMul1.mF32[2] * inMul2.mF32[2] + inAdd.mF32[2], + inMul1.mF32[3] * inMul2.mF32[3] + inAdd.mF32[3]); +#endif +} + +Vec4 Vec4::sSelect(Vec4Arg inNotSet, Vec4Arg inSet, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox + return _mm_blendv_ps(inNotSet.mValue, inSet.mValue, _mm_castsi128_ps(inControl.mValue)); +#elif defined(JPH_USE_SSE) + __m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31)); + return _mm_or_ps(_mm_and_ps(is_set, inSet.mValue), _mm_andnot_ps(is_set, inNotSet.mValue)); +#elif defined(JPH_USE_NEON) + return vbslq_f32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue); +#else + Vec4 result; + for (int i = 0; i < 4; i++) + result.mF32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mF32[i] : inNotSet.mF32[i]; + return result; +#endif +} + +Vec4 Vec4::sOr(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return UVec4::sOr(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +Vec4 Vec4::sXor(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return UVec4::sXor(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +Vec4 Vec4::sAnd(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue))); +#else + return UVec4::sAnd(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +void Vec4::sSort4(Vec4 &ioValue, UVec4 &ioIndex) +{ + // Pass 1, test 1st vs 3rd, 2nd vs 4th + Vec4 v1 = ioValue.Swizzle(); + UVec4 i1 = ioIndex.Swizzle(); + UVec4 c1 = sLess(ioValue, v1).Swizzle(); + ioValue = sSelect(ioValue, v1, c1); + ioIndex = UVec4::sSelect(ioIndex, i1, c1); + + // Pass 2, test 1st vs 2nd, 3rd vs 4th + Vec4 v2 = ioValue.Swizzle(); + UVec4 i2 = ioIndex.Swizzle(); + UVec4 c2 = sLess(ioValue, v2).Swizzle(); + ioValue = sSelect(ioValue, v2, c2); + ioIndex = UVec4::sSelect(ioIndex, i2, c2); + + // Pass 3, test 2nd vs 3rd component + Vec4 v3 = ioValue.Swizzle(); + UVec4 i3 = ioIndex.Swizzle(); + UVec4 c3 = sLess(ioValue, v3).Swizzle(); + ioValue = sSelect(ioValue, v3, c3); + ioIndex = UVec4::sSelect(ioIndex, i3, c3); +} + +void Vec4::sSort4Reverse(Vec4 &ioValue, UVec4 &ioIndex) +{ + // Pass 1, test 1st vs 3rd, 2nd vs 4th + Vec4 v1 = ioValue.Swizzle(); + UVec4 i1 = ioIndex.Swizzle(); + UVec4 c1 = sGreater(ioValue, v1).Swizzle(); + ioValue = sSelect(ioValue, v1, c1); + ioIndex = UVec4::sSelect(ioIndex, i1, c1); + + // Pass 2, test 1st vs 2nd, 3rd vs 4th + Vec4 v2 = ioValue.Swizzle(); + UVec4 i2 = ioIndex.Swizzle(); + UVec4 c2 = sGreater(ioValue, v2).Swizzle(); + ioValue = sSelect(ioValue, v2, c2); + ioIndex = UVec4::sSelect(ioIndex, i2, c2); + + // Pass 3, test 2nd vs 3rd component + Vec4 v3 = ioValue.Swizzle(); + UVec4 i3 = ioIndex.Swizzle(); + UVec4 c3 = sGreater(ioValue, v3).Swizzle(); + ioValue = sSelect(ioValue, v3, c3); + ioIndex = UVec4::sSelect(ioIndex, i3, c3); +} + +bool Vec4::operator == (Vec4Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +bool Vec4::IsClose(Vec4Arg inV2, float inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool Vec4::IsNormalized(float inTolerance) const +{ + return abs(LengthSq() - 1.0f) <= inTolerance; +} + +bool Vec4::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fpclass_ps_mask(mValue, 0b10000001) != 0; +#elif defined(JPH_USE_SSE) + return _mm_movemask_ps(_mm_cmpunord_ps(mValue, mValue)) != 0; +#elif defined(JPH_USE_NEON) + uint32x4_t is_equal = vceqq_f32(mValue, mValue); // If a number is not equal to itself it's a NaN + return vaddvq_u32(vshrq_n_u32(is_equal, 31)) != 4; +#else + return isnan(mF32[0]) || isnan(mF32[1]) || isnan(mF32[2]) || isnan(mF32[3]); +#endif +} + +Vec4 Vec4::operator * (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] * inV2.mF32[0], + mF32[1] * inV2.mF32[1], + mF32[2] * inV2.mF32[2], + mF32[3] * inV2.mF32[3]); +#endif +} + +Vec4 Vec4::operator * (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(mValue, inV2); +#else + return Vec4(mF32[0] * inV2, mF32[1] * inV2, mF32[2] * inV2, mF32[3] * inV2); +#endif +} + +/// Multiply vector with float +Vec4 operator * (float inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(_mm_set1_ps(inV1), inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(inV2.mValue, inV1); +#else + return Vec4(inV1 * inV2.mF32[0], + inV1 * inV2.mF32[1], + inV1 * inV2.mF32[2], + inV1 * inV2.mF32[3]); +#endif +} + +Vec4 Vec4::operator / (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + return Vec4(mF32[0] / inV2, mF32[1] / inV2, mF32[2] / inV2, mF32[3] / inV2); +#endif +} + +Vec4 &Vec4::operator *= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vmulq_n_f32(mValue, inV2); +#else + for (int i = 0; i < 4; ++i) + mF32[i] *= inV2; +#endif + return *this; +} + +Vec4 &Vec4::operator *= (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vmulq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] *= inV2.mF32[i]; +#endif + return *this; +} + +Vec4 &Vec4::operator /= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + for (int i = 0; i < 4; ++i) + mF32[i] /= inV2; +#endif + return *this; +} + +Vec4 Vec4::operator + (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] + inV2.mF32[0], + mF32[1] + inV2.mF32[1], + mF32[2] + inV2.mF32[2], + mF32[3] + inV2.mF32[3]); +#endif +} + +Vec4 &Vec4::operator += (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] += inV2.mF32[i]; +#endif + return *this; +} + +Vec4 Vec4::operator - () const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(_mm_setzero_ps(), mValue); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return vsubq_f32(vdupq_n_f32(0), mValue); + #else + return vnegq_f32(mValue); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return Vec4(0.0f - mF32[0], 0.0f - mF32[1], 0.0f - mF32[2], 0.0f - mF32[3]); + #else + return Vec4(-mF32[0], -mF32[1], -mF32[2], -mF32[3]); + #endif +#endif +} + +Vec4 Vec4::operator - (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vsubq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] - inV2.mF32[0], + mF32[1] - inV2.mF32[1], + mF32[2] - inV2.mF32[2], + mF32[3] - inV2.mF32[3]); +#endif +} + +Vec4 &Vec4::operator -= (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vsubq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] -= inV2.mF32[i]; +#endif + return *this; +} + +Vec4 Vec4::operator / (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] / inV2.mF32[0], + mF32[1] / inV2.mF32[1], + mF32[2] / inV2.mF32[2], + mF32[3] / inV2.mF32[3]); +#endif +} + +Vec4 Vec4::SplatX() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 0, 0)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 0); +#else + return Vec4(mF32[0], mF32[0], mF32[0], mF32[0]); +#endif +} + +Vec4 Vec4::SplatY() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(1, 1, 1, 1)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 1); +#else + return Vec4(mF32[1], mF32[1], mF32[1], mF32[1]); +#endif +} + +Vec4 Vec4::SplatZ() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(2, 2, 2, 2)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 2); +#else + return Vec4(mF32[2], mF32[2], mF32[2], mF32[2]); +#endif +} + +Vec4 Vec4::SplatW() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(3, 3, 3, 3)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 3); +#else + return Vec4(mF32[3], mF32[3], mF32[3], mF32[3]); +#endif +} + +Vec4 Vec4::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm_range_ps(mValue, mValue, 0b1000); +#elif defined(JPH_USE_SSE) + return _mm_max_ps(_mm_sub_ps(_mm_setzero_ps(), mValue), mValue); +#elif defined(JPH_USE_NEON) + return vabsq_f32(mValue); +#else + return Vec4(abs(mF32[0]), abs(mF32[1]), abs(mF32[2]), abs(mF32[3])); +#endif +} + +Vec4 Vec4::Reciprocal() const +{ + return sReplicate(1.0f) / mValue; +} + +Vec4 Vec4::DotV(Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0xff); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + // Brackets placed so that the order is consistent with the vectorized version + return Vec4::sReplicate((mF32[0] * inV2.mF32[0] + mF32[1] * inV2.mF32[1]) + (mF32[2] * inV2.mF32[2] + mF32[3] * inV2.mF32[3])); +#endif +} + +float Vec4::Dot(Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, inV2.mValue, 0xff)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + return vaddvq_f32(mul); +#else + // Brackets placed so that the order is consistent with the vectorized version + return (mF32[0] * inV2.mF32[0] + mF32[1] * inV2.mF32[1]) + (mF32[2] * inV2.mF32[2] + mF32[3] * inV2.mF32[3]); +#endif +} + +float Vec4::LengthSq() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, mValue, 0xff)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + return vaddvq_f32(mul); +#else + // Brackets placed so that the order is consistent with the vectorized version + return (mF32[0] * mF32[0] + mF32[1] * mF32[1]) + (mF32[2] * mF32[2] + mF32[3] * mF32[3]); +#endif +} + +float Vec4::Length() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(mValue, mValue, 0xff))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + float32x2_t sum = vdup_n_f32(vaddvq_f32(mul)); + return vget_lane_f32(vsqrt_f32(sum), 0); +#else + // Brackets placed so that the order is consistent with the vectorized version + return sqrt((mF32[0] * mF32[0] + mF32[1] * mF32[1]) + (mF32[2] * mF32[2] + mF32[3] * mF32[3])); +#endif +} + +Vec4 Vec4::Sqrt() const +{ +#if defined(JPH_USE_SSE) + return _mm_sqrt_ps(mValue); +#elif defined(JPH_USE_NEON) + return vsqrtq_f32(mValue); +#else + return Vec4(sqrt(mF32[0]), sqrt(mF32[1]), sqrt(mF32[2]), sqrt(mF32[3])); +#endif +} + + +Vec4 Vec4::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fixupimm_ps(mValue, mValue, _mm_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_SSE) + Type minus_one = _mm_set1_ps(-1.0f); + Type one = _mm_set1_ps(1.0f); + return _mm_or_ps(_mm_and_ps(mValue, minus_one), one); +#elif defined(JPH_USE_NEON) + Type minus_one = vdupq_n_f32(-1.0f); + Type one = vdupq_n_f32(1.0f); + return vreinterpretq_f32_u32(vorrq_u32(vandq_u32(vreinterpretq_u32_f32(mValue), vreinterpretq_u32_f32(minus_one)), vreinterpretq_u32_f32(one))); +#else + return Vec4(std::signbit(mF32[0])? -1.0f : 1.0f, + std::signbit(mF32[1])? -1.0f : 1.0f, + std::signbit(mF32[2])? -1.0f : 1.0f, + std::signbit(mF32[3])? -1.0f : 1.0f); +#endif +} + +Vec4 Vec4::Normalized() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_div_ps(mValue, _mm_sqrt_ps(_mm_dp_ps(mValue, mValue, 0xff))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul)); + return vdivq_f32(mValue, vsqrtq_f32(sum)); +#else + return *this / Length(); +#endif +} + +void Vec4::StoreFloat4(Float4 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_storeu_ps(&outV->x, mValue); +#elif defined(JPH_USE_NEON) + vst1q_f32(&outV->x, mValue); +#else + for (int i = 0; i < 4; ++i) + (&outV->x)[i] = mF32[i]; +#endif +} + +UVec4 Vec4::ToInt() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvttps_epi32(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_u32_f32(mValue); +#else + return UVec4(uint32(mF32[0]), uint32(mF32[1]), uint32(mF32[2]), uint32(mF32[3])); +#endif +} + +UVec4 Vec4::ReinterpretAsInt() const +{ +#if defined(JPH_USE_SSE) + return UVec4(_mm_castps_si128(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_u32_f32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +int Vec4::GetSignBits() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_ps(mValue); +#elif defined(JPH_USE_NEON) + int32x4_t shift = JPH_NEON_INT32x4(0, 1, 2, 3); + return vaddvq_u32(vshlq_u32(vshrq_n_u32(vreinterpretq_u32_f32(mValue), 31), shift)); +#else + return (std::signbit(mF32[0])? 1 : 0) | (std::signbit(mF32[1])? 2 : 0) | (std::signbit(mF32[2])? 4 : 0) | (std::signbit(mF32[3])? 8 : 0); +#endif +} + +float Vec4::ReduceMin() const +{ + Vec4 v = sMin(mValue, Swizzle()); + v = sMin(v, v.Swizzle()); + return v.GetX(); +} + +float Vec4::ReduceMax() const +{ + Vec4 v = sMax(mValue, Swizzle()); + v = sMax(v, v.Swizzle()); + return v.GetX(); +} + +void Vec4::SinCos(Vec4 &outSin, Vec4 &outCos) const +{ + // Implementation based on sinf.c from the cephes library, combines sinf and cosf in a single function, changes octants to quadrants and vectorizes it + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive and remember sign for sin only since cos is symmetric around x (highest bit of a float is the sign bit) + UVec4 sin_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, sin_sign.ReinterpretAsFloat()); + + // x / (PI / 2) rounded to nearest int gives us the quadrant closest to x + UVec4 quadrant = (0.6366197723675814f * x + Vec4::sReplicate(0.5f)).ToInt(); + + // Make x relative to the closest quadrant. + // This does x = x - quadrant * PI / 2 using a two step Cody-Waite argument reduction. + // This improves the accuracy of the result by avoiding loss of significant bits in the subtraction. + // We start with x = x - quadrant * PI / 2, PI / 2 in hexadecimal notation is 0x3fc90fdb, we remove the lowest 16 bits to + // get 0x3fc90000 (= 1.5703125) this means we can now multiply with a number of up to 2^16 without losing any bits. + // This leaves us with: x = (x - quadrant * 1.5703125) - quadrant * (PI / 2 - 1.5703125). + // PI / 2 - 1.5703125 in hexadecimal is 0x39fdaa22, stripping the lowest 12 bits we get 0x39fda000 (= 0.0004837512969970703125) + // This leaves uw with: x = ((x - quadrant * 1.5703125) - quadrant * 0.0004837512969970703125) - quadrant * (PI / 2 - 1.5703125 - 0.0004837512969970703125) + // See: https://stackoverflow.com/questions/42455143/sine-cosine-modular-extended-precision-arithmetic + // After this we have x in the range [-PI / 4, PI / 4]. + Vec4 float_quadrant = quadrant.ToFloat(); + x = ((x - float_quadrant * 1.5703125f) - float_quadrant * 0.0004837512969970703125f) - float_quadrant * 7.549789948768648e-8f; + + // Calculate x2 = x^2 + Vec4 x2 = x * x; + + // Taylor expansion: + // Cos(x) = 1 - x^2/2! + x^4/4! - x^6/6! + x^8/8! + ... = (((x2/8!- 1/6!) * x2 + 1/4!) * x2 - 1/2!) * x2 + 1 + Vec4 taylor_cos = ((2.443315711809948e-5f * x2 - Vec4::sReplicate(1.388731625493765e-3f)) * x2 + Vec4::sReplicate(4.166664568298827e-2f)) * x2 * x2 - 0.5f * x2 + Vec4::sReplicate(1.0f); + // Sin(x) = x - x^3/3! + x^5/5! - x^7/7! + ... = ((-x2/7! + 1/5!) * x2 - 1/3!) * x2 * x + x + Vec4 taylor_sin = ((-1.9515295891e-4f * x2 + Vec4::sReplicate(8.3321608736e-3f)) * x2 - Vec4::sReplicate(1.6666654611e-1f)) * x2 * x + x; + + // The lowest 2 bits of quadrant indicate the quadrant that we are in. + // Let x be the original input value and x' our value that has been mapped to the range [-PI / 4, PI / 4]. + // since cos(x) = sin(x - PI / 2) and since we want to use the Taylor expansion as close as possible to 0, + // we can alternate between using the Taylor expansion for sin and cos according to the following table: + // + // quadrant sin(x) cos(x) + // XXX00b sin(x') cos(x') + // XXX01b cos(x') -sin(x') + // XXX10b -sin(x') -cos(x') + // XXX11b -cos(x') sin(x') + // + // So: sin_sign = bit2, cos_sign = bit1 ^ bit2, bit1 determines if we use sin or cos Taylor expansion + UVec4 bit1 = quadrant.LogicalShiftLeft<31>(); + UVec4 bit2 = UVec4::sAnd(quadrant.LogicalShiftLeft<30>(), UVec4::sReplicate(0x80000000U)); + + // Select which one of the results is sin and which one is cos + Vec4 s = Vec4::sSelect(taylor_sin, taylor_cos, bit1); + Vec4 c = Vec4::sSelect(taylor_cos, taylor_sin, bit1); + + // Update the signs + sin_sign = UVec4::sXor(sin_sign, bit2); + UVec4 cos_sign = UVec4::sXor(bit1, bit2); + + // Correct the signs + outSin = Vec4::sXor(s, sin_sign.ReinterpretAsFloat()); + outCos = Vec4::sXor(c, cos_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::Tan() const +{ + // Implementation based on tanf.c from the cephes library, see Vec4::SinCos for further details + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 tan_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, tan_sign.ReinterpretAsFloat()); + + // x / (PI / 2) rounded to nearest int gives us the quadrant closest to x + UVec4 quadrant = (0.6366197723675814f * x + Vec4::sReplicate(0.5f)).ToInt(); + + // Remap x to range [-PI / 4, PI / 4], see Vec4::SinCos + Vec4 float_quadrant = quadrant.ToFloat(); + x = ((x - float_quadrant * 1.5703125f) - float_quadrant * 0.0004837512969970703125f) - float_quadrant * 7.549789948768648e-8f; + + // Calculate x2 = x^2 + Vec4 x2 = x * x; + + // Roughly equivalent to the Taylor expansion: + // Tan(x) = x + x^3/3 + 2*x^5/15 + 17*x^7/315 + 62*x^9/2835 + ... + Vec4 tan = + (((((9.38540185543e-3f * x2 + Vec4::sReplicate(3.11992232697e-3f)) * x2 + Vec4::sReplicate(2.44301354525e-2f)) * x2 + + Vec4::sReplicate(5.34112807005e-2f)) * x2 + Vec4::sReplicate(1.33387994085e-1f)) * x2 + Vec4::sReplicate(3.33331568548e-1f)) * x2 * x + x; + + // For the 2nd and 4th quadrant we need to invert the value + UVec4 bit1 = quadrant.LogicalShiftLeft<31>(); + tan = Vec4::sSelect(tan, Vec4::sReplicate(-1.0f) / (tan JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(+ Vec4::sReplicate(FLT_MIN))), bit1); // Add small epsilon to prevent div by zero, works because tan is always positive + + // Put the sign back + return Vec4::sXor(tan, tan_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::ASin() const +{ + // Implementation based on asinf.c from the cephes library + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 asin_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 a = Vec4::sXor(*this, asin_sign.ReinterpretAsFloat()); + + // ASin is not defined outside the range [-1, 1] but it often happens that a value is slightly above 1 so we just clamp here + a = Vec4::sMin(a, Vec4::sReplicate(1.0f)); + + // When |x| <= 0.5 we use the asin approximation as is + Vec4 z1 = a * a; + Vec4 x1 = a; + + // When |x| > 0.5 we use the identity asin(x) = PI / 2 - 2 * asin(sqrt((1 - x) / 2)) + Vec4 z2 = 0.5f * (Vec4::sReplicate(1.0f) - a); + Vec4 x2 = z2.Sqrt(); + + // Select which of the two situations we have + UVec4 greater = Vec4::sGreater(a, Vec4::sReplicate(0.5f)); + Vec4 z = Vec4::sSelect(z1, z2, greater); + Vec4 x = Vec4::sSelect(x1, x2, greater); + + // Polynomial approximation of asin + z = ((((4.2163199048e-2f * z + Vec4::sReplicate(2.4181311049e-2f)) * z + Vec4::sReplicate(4.5470025998e-2f)) * z + Vec4::sReplicate(7.4953002686e-2f)) * z + Vec4::sReplicate(1.6666752422e-1f)) * z * x + x; + + // If |x| > 0.5 we need to apply the remainder of the identity above + z = Vec4::sSelect(z, Vec4::sReplicate(0.5f * JPH_PI) - (z + z), greater); + + // Put the sign back + return Vec4::sXor(z, asin_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::ACos() const +{ + // Not the most accurate, but simple + return Vec4::sReplicate(0.5f * JPH_PI) - ASin(); +} + +Vec4 Vec4::ATan() const +{ + // Implementation based on atanf.c from the cephes library + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 atan_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, atan_sign.ReinterpretAsFloat()); + Vec4 y = Vec4::sZero(); + + // If x > Tan(PI / 8) + UVec4 greater1 = Vec4::sGreater(x, Vec4::sReplicate(0.4142135623730950f)); + Vec4 x1 = (x - Vec4::sReplicate(1.0f)) / (x + Vec4::sReplicate(1.0f)); + + // If x > Tan(3 * PI / 8) + UVec4 greater2 = Vec4::sGreater(x, Vec4::sReplicate(2.414213562373095f)); + Vec4 x2 = Vec4::sReplicate(-1.0f) / (x JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(+ Vec4::sReplicate(FLT_MIN))); // Add small epsilon to prevent div by zero, works because x is always positive + + // Apply first if + x = Vec4::sSelect(x, x1, greater1); + y = Vec4::sSelect(y, Vec4::sReplicate(0.25f * JPH_PI), greater1); + + // Apply second if + x = Vec4::sSelect(x, x2, greater2); + y = Vec4::sSelect(y, Vec4::sReplicate(0.5f * JPH_PI), greater2); + + // Polynomial approximation + Vec4 z = x * x; + y += (((8.05374449538e-2f * z - Vec4::sReplicate(1.38776856032e-1f)) * z + Vec4::sReplicate(1.99777106478e-1f)) * z - Vec4::sReplicate(3.33329491539e-1f)) * z * x + x; + + // Put the sign back + return Vec4::sXor(y, atan_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::sATan2(Vec4Arg inY, Vec4Arg inX) +{ + UVec4 sign_mask = UVec4::sReplicate(0x80000000U); + + // Determine absolute value and sign of y + UVec4 y_sign = UVec4::sAnd(inY.ReinterpretAsInt(), sign_mask); + Vec4 y_abs = Vec4::sXor(inY, y_sign.ReinterpretAsFloat()); + + // Determine absolute value and sign of x + UVec4 x_sign = UVec4::sAnd(inX.ReinterpretAsInt(), sign_mask); + Vec4 x_abs = Vec4::sXor(inX, x_sign.ReinterpretAsFloat()); + + // Always divide smallest / largest to avoid dividing by zero + UVec4 x_is_numerator = Vec4::sLess(x_abs, y_abs); + Vec4 numerator = Vec4::sSelect(y_abs, x_abs, x_is_numerator); + Vec4 denominator = Vec4::sSelect(x_abs, y_abs, x_is_numerator); + Vec4 atan = (numerator / denominator).ATan(); + + // If we calculated x / y instead of y / x the result is PI / 2 - result (note that this is true because we know the result is positive because the input was positive) + atan = Vec4::sSelect(atan, Vec4::sReplicate(0.5f * JPH_PI) - atan, x_is_numerator); + + // Now we need to map to the correct quadrant + // x_sign y_sign result + // +1 +1 atan + // -1 +1 -atan + PI + // -1 -1 atan - PI + // +1 -1 -atan + // This can be written as: x_sign * y_sign * (atan - (x_sign < 0? PI : 0)) + atan -= Vec4::sAnd(x_sign.ArithmeticShiftRight<31>().ReinterpretAsFloat(), Vec4::sReplicate(JPH_PI)); + atan = Vec4::sXor(atan, UVec4::sXor(x_sign, y_sign).ReinterpretAsFloat()); + return atan; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vector.h b/thirdparty/jolt_physics/Jolt/Math/Vector.h new file mode 100644 index 000000000000..b51a93c07b07 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Math/Vector.h @@ -0,0 +1,211 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Templatized vector class +template +class [[nodiscard]] Vector +{ +public: + /// Constructor + inline Vector() = default; + inline Vector(const Vector &) = default; + + /// Dimensions + inline uint GetRows() const { return Rows; } + + /// Vector with all zeros + inline void SetZero() + { + for (uint r = 0; r < Rows; ++r) + mF32[r] = 0.0f; + } + + inline static Vector sZero() { Vector v; v.SetZero(); return v; } + + /// Copy a (part) of another vector into this vector + template + void CopyPart(const OtherVector &inV, uint inSourceRow, uint inNumRows, uint inDestRow) + { + for (uint r = 0; r < inNumRows; ++r) + mF32[inDestRow + r] = inV[inSourceRow + r]; + } + + /// Get float component by index + inline float operator [] (uint inCoordinate) const + { + JPH_ASSERT(inCoordinate < Rows); + return mF32[inCoordinate]; + } + + inline float & operator [] (uint inCoordinate) + { + JPH_ASSERT(inCoordinate < Rows); + return mF32[inCoordinate]; + } + + /// Comparison + inline bool operator == (const Vector &inV2) const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != inV2.mF32[r]) + return false; + return true; + } + + inline bool operator != (const Vector &inV2) const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != inV2.mF32[r]) + return true; + return false; + } + + /// Test if vector consists of all zeros + inline bool IsZero() const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != 0.0f) + return false; + return true; + } + + /// Test if two vectors are close to each other + inline bool IsClose(const Vector &inV2, float inMaxDistSq = 1.0e-12f) const + { + return (inV2 - *this).LengthSq() <= inMaxDistSq; + } + + /// Assignment + inline Vector & operator = (const Vector &) = default; + + /// Multiply vector with float + inline Vector operator * (const float inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] * inV2; + return v; + } + + inline Vector & operator *= (const float inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] *= inV2; + return *this; + } + + /// Multiply vector with float + inline friend Vector operator * (const float inV1, const Vector &inV2) + { + return inV2 * inV1; + } + + /// Divide vector by float + inline Vector operator / (float inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] / inV2; + return v; + } + + inline Vector & operator /= (float inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] /= inV2; + return *this; + } + + /// Add two float vectors (component wise) + inline Vector operator + (const Vector &inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] + inV2.mF32[r]; + return v; + } + + inline Vector & operator += (const Vector &inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] += inV2.mF32[r]; + return *this; + } + + /// Negate + inline Vector operator - () const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = -mF32[r]; + return v; + } + + /// Subtract two float vectors (component wise) + inline Vector operator - (const Vector &inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] - inV2.mF32[r]; + return v; + } + + inline Vector & operator -= (const Vector &inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] -= inV2.mF32[r]; + return *this; + } + + /// Dot product + inline float Dot(const Vector &inV2) const + { + float dot = 0.0f; + for (uint r = 0; r < Rows; ++r) + dot += mF32[r] * inV2.mF32[r]; + return dot; + } + + /// Squared length of vector + inline float LengthSq() const + { + return Dot(*this); + } + + /// Length of vector + inline float Length() const + { + return sqrt(LengthSq()); + } + + /// Check if vector is normalized + inline bool IsNormalized(float inToleranceSq = 1.0e-6f) + { + return abs(LengthSq() - 1.0f) <= inToleranceSq; + } + + /// Normalize vector + inline Vector Normalized() const + { + return *this / Length(); + } + + /// To String + friend ostream & operator << (ostream &inStream, const Vector &inV) + { + inStream << "["; + for (uint i = 0; i < Rows - 1; ++i) + inStream << inV.mF32[i] << ", "; + inStream << inV.mF32[Rows - 1] << "]"; + return inStream; + } + + float mF32[Rows]; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h new file mode 100644 index 000000000000..adbfbb773c66 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h @@ -0,0 +1,333 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Base class for object stream input and output streams. +class JPH_EXPORT ObjectStream : public NonCopyable +{ +public: + /// Stream type + enum class EStreamType + { + Text, + Binary, + }; + +protected: + /// Destructor + virtual ~ObjectStream() = default; + + /// Identifier for objects + using Identifier = uint32; + + static constexpr int sVersion = 1; + static constexpr int sRevision = 0; + static constexpr Identifier sNullIdentifier = 0; +}; + +/// Interface class for reading from an object stream +class JPH_EXPORT IObjectStreamIn : public ObjectStream +{ +public: + ///@name Input type specific operations + virtual bool ReadDataType(EOSDataType &outType) = 0; + virtual bool ReadName(String &outName) = 0; + virtual bool ReadIdentifier(Identifier &outIdentifier) = 0; + virtual bool ReadCount(uint32 &outCount) = 0; + + ///@name Read primitives + virtual bool ReadPrimitiveData(uint8 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint16 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(int &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint32 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint64 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(float &outPrimitive) = 0; + virtual bool ReadPrimitiveData(double &outPrimitive) = 0; + virtual bool ReadPrimitiveData(bool &outPrimitive) = 0; + virtual bool ReadPrimitiveData(String &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Float3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Double3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Vec3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(DVec3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Vec4 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Quat &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Mat44 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(DMat44 &outPrimitive) = 0; + + ///@name Read compounds + virtual bool ReadClassData(const char *inClassName, void *inInstance) = 0; + virtual bool ReadPointerData(const RTTI *inRTTI, void **inPointer, int inRefCountOffset = -1) = 0; +}; + +/// Interface class for writing to an object stream +class JPH_EXPORT IObjectStreamOut : public ObjectStream +{ +public: + ///@name Output type specific operations + virtual void WriteDataType(EOSDataType inType) = 0; + virtual void WriteName(const char *inName) = 0; + virtual void WriteIdentifier(Identifier inIdentifier) = 0; + virtual void WriteCount(uint32 inCount) = 0; + + ///@name Write primitives + virtual void WritePrimitiveData(const uint8 &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint16 &inPrimitive) = 0; + virtual void WritePrimitiveData(const int &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint32 &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint64 &inPrimitive) = 0; + virtual void WritePrimitiveData(const float &inPrimitive) = 0; + virtual void WritePrimitiveData(const double &inPrimitive) = 0; + virtual void WritePrimitiveData(const bool &inPrimitive) = 0; + virtual void WritePrimitiveData(const String &inPrimitive) = 0; + virtual void WritePrimitiveData(const Float3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Double3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Vec3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const DVec3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Vec4 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Quat &inPrimitive) = 0; + virtual void WritePrimitiveData(const Mat44 &inPrimitive) = 0; + virtual void WritePrimitiveData(const DMat44 &inPrimitive) = 0; + + ///@name Write compounds + virtual void WritePointerData(const RTTI *inRTTI, const void *inPointer) = 0; + virtual void WriteClassData(const RTTI *inRTTI, const void *inInstance) = 0; + + ///@name Layout hints (for text output) + virtual void HintNextItem() { /* Default is do nothing */ } + virtual void HintIndentUp() { /* Default is do nothing */ } + virtual void HintIndentDown() { /* Default is do nothing */ } +}; + +// Define macro to declare functions for a specific primitive type +#define JPH_DECLARE_PRIMITIVE(name) \ + JPH_EXPORT bool OSIsType(name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + JPH_EXPORT bool OSReadData(IObjectStreamIn &ioStream, name &outPrimitive); \ + JPH_EXPORT void OSWriteDataType(IObjectStreamOut &ioStream, name *); \ + JPH_EXPORT void OSWriteData(IObjectStreamOut &ioStream, const name &inPrimitive); + +// This file uses the JPH_DECLARE_PRIMITIVE macro to define all types +#include + +// Define serialization templates +template +bool OSIsType(Array *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(StaticArray *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(T (*)[N], int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(Ref *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return OSIsType(static_cast(nullptr), inArrayDepth, inDataType, inClassName); +} + +template +bool OSIsType(RefConst *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return OSIsType(static_cast(nullptr), inArrayDepth, inDataType, inClassName); +} + +/// Define serialization templates for dynamic arrays +template +bool OSReadData(IObjectStreamIn &ioStream, Array &inArray) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + + // Read array items + if (continue_reading) + { + inArray.clear(); + inArray.resize(array_length); + for (uint32 el = 0; el < array_length && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + } + + return continue_reading; +} + +/// Define serialization templates for static arrays +template +bool OSReadData(IObjectStreamIn &ioStream, StaticArray &inArray) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + + // Check if we can fit this many elements + if (array_length > N) + return false; + + // Read array items + if (continue_reading) + { + inArray.clear(); + inArray.resize(array_length); + for (uint32 el = 0; el < array_length && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + } + + return continue_reading; +} + +/// Define serialization templates for C style arrays +template +bool OSReadData(IObjectStreamIn &ioStream, T (&inArray)[N]) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + if (array_length != N) + return false; + + // Read array items + for (uint32 el = 0; el < N && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + + return continue_reading; +} + +/// Define serialization templates for references +template +bool OSReadData(IObjectStreamIn &ioStream, Ref &inRef) +{ + return ioStream.ReadPointerData(JPH_RTTI(T), inRef.InternalGetPointer(), T::sInternalGetRefCountOffset()); +} + +template +bool OSReadData(IObjectStreamIn &ioStream, RefConst &inRef) +{ + return ioStream.ReadPointerData(JPH_RTTI(T), inRef.InternalGetPointer(), T::sInternalGetRefCountOffset()); +} + +// Define serialization templates for dynamic arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, Array *) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const Array &inArray) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(static_cast(inArray.size())); + + // Write data in array + ioStream.HintIndentUp(); + for (const T &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for static arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, StaticArray *) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const StaticArray &inArray) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(inArray.size()); + + // Write data in array + ioStream.HintIndentUp(); + for (const typename StaticArray::value_type &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for C style arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, T (*)[N]) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const T (&inArray)[N]) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(uint32(N)); + + // Write data in array + ioStream.HintIndentUp(); + for (const T &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for references +template +void OSWriteDataType(IObjectStreamOut &ioStream, Ref *) +{ + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const Ref &inRef) +{ + if (inRef != nullptr) + ioStream.WritePointerData(GetRTTI(inRef.GetPtr()), inRef.GetPtr()); + else + ioStream.WritePointerData(nullptr, nullptr); +} + +template +void OSWriteDataType(IObjectStreamOut &ioStream, RefConst *) +{ + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const RefConst &inRef) +{ + if (inRef != nullptr) + ioStream.WritePointerData(GetRTTI(inRef.GetPtr()), inRef.GetPtr()); + else + ioStream.WritePointerData(nullptr, nullptr); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h new file mode 100644 index 000000000000..9a6d8802187e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +class RTTI; +class IObjectStreamIn; +class IObjectStreamOut; + +/// Data type +enum class EOSDataType +{ + /// Control codes + Declare, ///< Used to declare the attributes of a new object type + Object, ///< Start of a new object + Instance, ///< Used in attribute declaration, indicates that an object is an instanced attribute (no pointer) + Pointer, ///< Used in attribute declaration, indicates that an object is a pointer attribute + Array, ///< Used in attribute declaration, indicates that this is an array of objects + + // Basic types (primitives) + #define JPH_DECLARE_PRIMITIVE(name) T_##name, + + // This file uses the JPH_DECLARE_PRIMITIVE macro to define all types + #include + + // Error values for read functions + Invalid, ///< Next token on the stream was not a valid data type +}; + +/// Attributes are members of classes that need to be serialized. +class SerializableAttribute +{ +public: + ///@ Serialization functions + using pGetMemberPrimitiveType = const RTTI * (*)(); + using pIsType = bool (*)(int inArrayDepth, EOSDataType inDataType, const char *inClassName); + using pReadData = bool (*)(IObjectStreamIn &ioStream, void *inObject); + using pWriteData = void (*)(IObjectStreamOut &ioStream, const void *inObject); + using pWriteDataType = void (*)(IObjectStreamOut &ioStream); + + /// Constructor + SerializableAttribute(const char *inName, uint inMemberOffset, pGetMemberPrimitiveType inGetMemberPrimitiveType, pIsType inIsType, pReadData inReadData, pWriteData inWriteData, pWriteDataType inWriteDataType) : mName(inName), mMemberOffset(inMemberOffset), mGetMemberPrimitiveType(inGetMemberPrimitiveType), mIsType(inIsType), mReadData(inReadData), mWriteData(inWriteData), mWriteDataType(inWriteDataType) { } + + /// Construct from other attribute with base class offset + SerializableAttribute(const SerializableAttribute &inOther, int inBaseOffset) : mName(inOther.mName), mMemberOffset(inOther.mMemberOffset + inBaseOffset), mGetMemberPrimitiveType(inOther.mGetMemberPrimitiveType), mIsType(inOther.mIsType), mReadData(inOther.mReadData), mWriteData(inOther.mWriteData), mWriteDataType(inOther.mWriteDataType) { } + + /// Name of the attribute + void SetName(const char *inName) { mName = inName; } + const char * GetName() const { return mName; } + + /// Access to the memory location that contains the member + template + inline T * GetMemberPointer(void *inObject) const { return reinterpret_cast(reinterpret_cast(inObject) + mMemberOffset); } + template + inline const T * GetMemberPointer(const void *inObject) const { return reinterpret_cast(reinterpret_cast(inObject) + mMemberOffset); } + + /// In case this attribute contains an RTTI type, return it (note that a Array will return the rtti of sometype) + const RTTI * GetMemberPrimitiveType() const + { + return mGetMemberPrimitiveType(); + } + + /// Check if this attribute is of a specific type + bool IsType(int inArrayDepth, EOSDataType inDataType, const char *inClassName) const + { + return mIsType(inArrayDepth, inDataType, inClassName); + } + + /// Read the data for this attribute into attribute containing class inObject + bool ReadData(IObjectStreamIn &ioStream, void *inObject) const + { + return mReadData(ioStream, GetMemberPointer(inObject)); + } + + /// Write the data for this attribute from attribute containing class inObject + void WriteData(IObjectStreamOut &ioStream, const void *inObject) const + { + mWriteData(ioStream, GetMemberPointer(inObject)); + } + + /// Write the data type of this attribute to a stream + void WriteDataType(IObjectStreamOut &ioStream) const + { + mWriteDataType(ioStream); + } + +private: + // Name of the attribute + const char * mName; + + // Offset of the member relative to the class + uint mMemberOffset; + + // In case this attribute contains an RTTI type, return it (note that a Array will return the rtti of sometype) + pGetMemberPrimitiveType mGetMemberPrimitiveType; + + // Serialization operations + pIsType mIsType; + pReadData mReadData; + pWriteData mWriteData; + pWriteDataType mWriteDataType; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h new file mode 100644 index 000000000000..c6f241ea41c4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h @@ -0,0 +1,67 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_OBJECT_STREAM + +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to add properties to be serialized +////////////////////////////////////////////////////////////////////////////////////////// + +template +inline void AddSerializableAttributeEnum(RTTI &inRTTI, uint inOffset, const char *inName) +{ + inRTTI.AddAttribute(SerializableAttribute(inName, inOffset, + []() -> const RTTI * + { + return nullptr; + }, + [](int inArrayDepth, EOSDataType inDataType, [[maybe_unused]] const char *inClassName) + { + return inArrayDepth == 0 && inDataType == EOSDataType::T_uint32; + }, + [](IObjectStreamIn &ioStream, void *inObject) + { + uint32 temporary; + if (OSReadData(ioStream, temporary)) + { + *reinterpret_cast(inObject) = static_cast(temporary); + return true; + } + return false; + }, + [](IObjectStreamOut &ioStream, const void *inObject) + { + static_assert(sizeof(MemberType) <= sizeof(uint32)); + uint32 temporary = uint32(*reinterpret_cast(inObject)); + OSWriteData(ioStream, temporary); + }, + [](IObjectStreamOut &ioStream) + { + ioStream.WriteDataType(EOSDataType::T_uint32); + })); +} + +// JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS +#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \ + AddSerializableAttributeEnum(inRTTI, offsetof(class_name, member_name), alias_name); + +// JPH_ADD_ENUM_ATTRIBUTE +#define JPH_ADD_ENUM_ATTRIBUTE(class_name, member_name) \ + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name); + +JPH_NAMESPACE_END + +#else + +#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(...) +#define JPH_ADD_ENUM_ATTRIBUTE(...) + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h new file mode 100644 index 000000000000..ce18325c46cb --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h @@ -0,0 +1,60 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_OBJECT_STREAM + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to add properties to be serialized +////////////////////////////////////////////////////////////////////////////////////////// + +template +inline void AddSerializableAttributeTyped(RTTI &inRTTI, uint inOffset, const char *inName) +{ + inRTTI.AddAttribute(SerializableAttribute(inName, inOffset, + []() + { + return GetPrimitiveTypeOfType((MemberType *)nullptr); + }, + [](int inArrayDepth, EOSDataType inDataType, const char *inClassName) + { + return OSIsType((MemberType *)nullptr, inArrayDepth, inDataType, inClassName); + }, + [](IObjectStreamIn &ioStream, void *inObject) + { + return OSReadData(ioStream, *reinterpret_cast(inObject)); + }, + [](IObjectStreamOut &ioStream, const void *inObject) + { + OSWriteData(ioStream, *reinterpret_cast(inObject)); + }, + [](IObjectStreamOut &ioStream) + { + OSWriteDataType(ioStream, (MemberType *)nullptr); + })); +} + +// JPH_ADD_ATTRIBUTE +#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \ + AddSerializableAttributeTyped(inRTTI, offsetof(class_name, member_name), alias_name); + +// JPH_ADD_ATTRIBUTE +#define JPH_ADD_ATTRIBUTE(class_name, member_name) \ + JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name) + +JPH_NAMESPACE_END + +#else + +#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(...) +#define JPH_ADD_ATTRIBUTE(...) + +#endif // JPH_OBJECT_STREAM diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp new file mode 100644 index 000000000000..98d3b3cf8d09 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp @@ -0,0 +1,15 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(SerializableObject) +{ +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h new file mode 100644 index 000000000000..86b8830c259a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h @@ -0,0 +1,164 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Helper macros +////////////////////////////////////////////////////////////////////////////////////////// + +#ifdef JPH_OBJECT_STREAM + +// JPH_DECLARE_SERIALIZATION_FUNCTIONS +#define JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, prefix, class_name) \ + linkage prefix bool OSReadData(IObjectStreamIn &ioStream, class_name &inInstance); \ + linkage prefix bool OSReadData(IObjectStreamIn &ioStream, class_name *&inPointer); \ + linkage prefix bool OSIsType(class_name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + linkage prefix bool OSIsType(class_name **, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + linkage prefix void OSWriteData(IObjectStreamOut &ioStream, const class_name &inInstance); \ + linkage prefix void OSWriteData(IObjectStreamOut &ioStream, class_name *const &inPointer); \ + linkage prefix void OSWriteDataType(IObjectStreamOut &ioStream, class_name *); \ + linkage prefix void OSWriteDataType(IObjectStreamOut &ioStream, class_name **); + +// JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS +#define JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + bool OSReadData(IObjectStreamIn &ioStream, class_name &inInstance) \ + { \ + return ioStream.ReadClassData(#class_name, (void *)&inInstance); \ + } \ + bool OSReadData(IObjectStreamIn &ioStream, class_name *&inPointer) \ + { \ + return ioStream.ReadPointerData(JPH_RTTI(class_name), (void **)&inPointer); \ + } \ + bool OSIsType(class_name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) \ + { \ + return inArrayDepth == 0 && inDataType == EOSDataType::Instance && strcmp(inClassName, #class_name) == 0; \ + } \ + bool OSIsType(class_name **, int inArrayDepth, EOSDataType inDataType, const char *inClassName) \ + { \ + return inArrayDepth == 0 && inDataType == EOSDataType::Pointer && strcmp(inClassName, #class_name) == 0; \ + } \ + void OSWriteData(IObjectStreamOut &ioStream, const class_name &inInstance) \ + { \ + ioStream.WriteClassData(JPH_RTTI(class_name), (void *)&inInstance); \ + } \ + void OSWriteData(IObjectStreamOut &ioStream, class_name *const &inPointer) \ + { \ + if (inPointer) \ + ioStream.WritePointerData(GetRTTI(inPointer), (void *)inPointer); \ + else \ + ioStream.WritePointerData(nullptr, nullptr); \ + } \ + void OSWriteDataType(IObjectStreamOut &ioStream, class_name *) \ + { \ + ioStream.WriteDataType(EOSDataType::Instance); \ + ioStream.WriteName(#class_name); \ + } \ + void OSWriteDataType(IObjectStreamOut &ioStream, class_name **) \ + { \ + ioStream.WriteDataType(EOSDataType::Pointer); \ + ioStream.WriteName(#class_name); \ + } + +#else + +#define JPH_DECLARE_SERIALIZATION_FUNCTIONS(...) +#define JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(...) + +#endif // JPH_OBJECT_STREAM + +////////////////////////////////////////////////////////////////////////////////////////// +// Use these macros on non-virtual objects to make them serializable +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL +#define JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_NON_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL +#define JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_NON_VIRTUAL(class_name) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but when you cannot insert the declaration in the class itself +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS +#define JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(linkage, class_name) \ + JPH_DECLARE_RTTI_OUTSIDE_CLASS(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, extern, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS +#define JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(class_name) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but for classes that have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_VIRTUAL - Use for concrete, non-base classes +#define JPH_DECLARE_SERIALIZABLE_VIRTUAL(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL +#define JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_ABSTRACT - Use for abstract, non-base classes +#define JPH_DECLARE_SERIALIZABLE_ABSTRACT(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_ABSTRACT(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT +#define JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE - Use for concrete base classes +#define JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_VIRTUAL_BASE(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL_BASE +#define JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL_BASE(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE - Use for abstract base class +#define JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_ABSTRACT_BASE(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE +#define JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(class_name) + +/// Classes must be derived from SerializableObject if you want to be able to save pointers or +/// reference counting pointers to objects of this or derived classes. The type will automatically +/// be determined during serialization and upon deserialization it will be restored correctly. +class JPH_EXPORT SerializableObject : public NonCopyable +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(JPH_EXPORT, SerializableObject) + +public: + /// Constructor + virtual ~SerializableObject() = default; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h new file mode 100644 index 000000000000..015e1dbf1445 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h @@ -0,0 +1,42 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint8); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint16); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, int); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint32); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint64); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, float); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, double); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, bool); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, String); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Float3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Double3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Vec3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, DVec3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Vec4); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Quat); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Mat44); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, DMat44); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Color); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, AABox); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Triangle); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, IndexedTriangle); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Plane); + +JPH_NAMESPACE_END + +// These need to be added after all types have been registered or else clang under linux will not find GetRTTIOfType for the type +#include +#include diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h b/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h new file mode 100644 index 000000000000..8445cb186335 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h @@ -0,0 +1,68 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used in BodyCreationSettings and MotionProperties to indicate which degrees of freedom a body has +enum class EAllowedDOFs : uint8 +{ + None = 0b000000, ///< No degrees of freedom are allowed. Note that this is not valid and will crash. Use a static body instead. + All = 0b111111, ///< All degrees of freedom are allowed + TranslationX = 0b000001, ///< Body can move in world space X axis + TranslationY = 0b000010, ///< Body can move in world space Y axis + TranslationZ = 0b000100, ///< Body can move in world space Z axis + RotationX = 0b001000, ///< Body can rotate around world space X axis + RotationY = 0b010000, ///< Body can rotate around world space Y axis + RotationZ = 0b100000, ///< Body can rotate around world space Z axis + Plane2D = TranslationX | TranslationY | RotationZ, ///< Body can only move in X and Y axis and rotate around Z axis +}; + +/// Bitwise OR operator for EAllowedDOFs +constexpr EAllowedDOFs operator | (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) | uint8(inRHS)); +} + +/// Bitwise AND operator for EAllowedDOFs +constexpr EAllowedDOFs operator & (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) & uint8(inRHS)); +} + +/// Bitwise XOR operator for EAllowedDOFs +constexpr EAllowedDOFs operator ^ (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) ^ uint8(inRHS)); +} + +/// Bitwise NOT operator for EAllowedDOFs +constexpr EAllowedDOFs operator ~ (EAllowedDOFs inAllowedDOFs) +{ + return EAllowedDOFs(~uint8(inAllowedDOFs)); +} + +/// Bitwise OR assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator |= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS | inRHS; + return ioLHS; +} + +/// Bitwise AND assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator &= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS & inRHS; + return ioLHS; +} + +/// Bitwise XOR assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator ^= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS ^ inRHS; + return ioLHS; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp new file mode 100644 index 000000000000..4386ec2db252 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp @@ -0,0 +1,423 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +static const EmptyShape sFixedToWorldShape; +Body Body::sFixedToWorld(false); + +Body::Body(bool) : + mPosition(Vec3::sZero()), + mRotation(Quat::sIdentity()), + mShape(&sFixedToWorldShape), // Dummy shape + mFriction(0.0f), + mRestitution(0.0f), + mObjectLayer(cObjectLayerInvalid), + mMotionType(EMotionType::Static) +{ + sFixedToWorldShape.SetEmbedded(); +} + +void Body::SetMotionType(EMotionType inMotionType) +{ + if (mMotionType == inMotionType) + return; + + JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set tot true"); + JPH_ASSERT(inMotionType != EMotionType::Static || !IsActive(), "Deactivate body first"); + JPH_ASSERT(inMotionType == EMotionType::Dynamic || !IsSoftBody(), "Soft bodies can only be dynamic, you can make individual vertices kinematic by setting their inverse mass to 0"); + + // Store new motion type + mMotionType = inMotionType; + + if (mMotionProperties != nullptr) + { + // Update cache + JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = inMotionType;) + + switch (inMotionType) + { + case EMotionType::Static: + // Stop the object + mMotionProperties->mLinearVelocity = Vec3::sZero(); + mMotionProperties->mAngularVelocity = Vec3::sZero(); + [[fallthrough]]; + + case EMotionType::Kinematic: + // Cancel forces + mMotionProperties->ResetForce(); + mMotionProperties->ResetTorque(); + break; + + case EMotionType::Dynamic: + break; + } + } +} + +void Body::SetAllowSleeping(bool inAllow) +{ + mMotionProperties->mAllowSleeping = inAllow; + if (inAllow) + ResetSleepTimer(); +} + +void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies + JPH_ASSERT(!IsStatic()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + // Calculate center of mass at end situation + RVec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass(); + + // Calculate delta position and rotation + Vec3 delta_pos = Vec3(new_com - mPosition); + Quat delta_rotation = inTargetRotation * mRotation.Conjugated(); + + mMotionProperties->MoveKinematic(delta_pos, delta_rotation, inDeltaTime); +} + +void Body::CalculateWorldSpaceBoundsInternal() +{ + mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); +} + +void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); + + mPosition = inPosition + inRotation * mShape->GetCenterOfMass(); + mRotation = inRotation; + + // Initialize bounding box + CalculateWorldSpaceBoundsInternal(); + + // Reset sleeping test + if (inResetSleepTimer && mMotionProperties != nullptr) + ResetSleepTimer(); +} + +void Body::UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties) +{ + // Update center of mass position so the world position for this body stays the same + mPosition += mRotation * (mShape->GetCenterOfMass() - inPreviousCenterOfMass); + + // Recalculate mass and inertia if requested + if (inUpdateMassProperties && mMotionProperties != nullptr) + mMotionProperties->SetMassProperties(mMotionProperties->GetAllowedDOFs(), mShape->GetMassProperties()); +} + +void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties) +{ + JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); + + // Get the old center of mass + Vec3 old_com = mShape->GetCenterOfMass(); + + // Update the shape + mShape = inShape; + + // Update center of mass + UpdateCenterOfMassInternal(old_com, inUpdateMassProperties); + + // Recalculate bounding box + CalculateWorldSpaceBoundsInternal(); +} + +ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep) +{ + // Check override & sensors will never go to sleep (they would stop detecting collisions with sleeping bodies) + if (!mMotionProperties->mAllowSleeping || IsSensor()) + return ECanSleep::CannotSleep; + + // Get the points to test + RVec3 points[3]; + GetSleepTestPoints(points); + +#ifdef JPH_DOUBLE_PRECISION + // Get base offset for spheres + DVec3 offset = mMotionProperties->GetSleepTestOffset(); +#endif // JPH_DOUBLE_PRECISION + + for (int i = 0; i < 3; ++i) + { + Sphere &sphere = mMotionProperties->mSleepTestSpheres[i]; + + // Make point relative to base offset +#ifdef JPH_DOUBLE_PRECISION + Vec3 p = Vec3(points[i] - offset); +#else + Vec3 p = points[i]; +#endif // JPH_DOUBLE_PRECISION + + // Encapsulate the point in a sphere + sphere.EncapsulatePoint(p); + + // Test if it exceeded the max movement + if (sphere.GetRadius() > inMaxMovement) + { + // Body is not sleeping, reset test + mMotionProperties->ResetSleepTestSpheres(points); + return ECanSleep::CannotSleep; + } + } + + return mMotionProperties->AccumulateSleepTime(inDeltaTime, inTimeBeforeSleep); +} + +void Body::GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const +{ + // For GetSubmergedVolume we transform the surface relative to the body position for increased precision + Mat44 rotation = Mat44::sRotation(mRotation); + Plane surface_relative_to_body = Plane::sFromPointAndNormal(inSurfacePosition - mPosition, inSurfaceNormal); + + // Calculate amount of volume that is submerged and what the center of buoyancy is + mShape->GetSubmergedVolume(rotation, Vec3::sReplicate(1.0f), surface_relative_to_body, outTotalVolume, outSubmergedVolume, outRelativeCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, mPosition)); +} + +bool Body::ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); // Only implemented for rigid bodies currently + + // We follow the approach from 'Game Programming Gems 6' 2.5 Exact Buoyancy for Polyhedra + // All quantities below are in world space + + // If we're not submerged, there's no point in doing the rest of the calculations + if (inSubmergedVolume > 0.0f) + { + #ifdef JPH_DEBUG_RENDERER + // Draw submerged volume properties + if (Shape::sDrawSubmergedVolumes) + { + RVec3 center_of_buoyancy = mPosition + inRelativeCenterOfBuoyancy; + DebugRenderer::sInstance->DrawMarker(center_of_buoyancy, Color::sWhite, 2.0f); + DebugRenderer::sInstance->DrawText3D(center_of_buoyancy, StringFormat("%.3f / %.3f", (double)inSubmergedVolume, (double)inTotalVolume)); + } + #endif // JPH_DEBUG_RENDERER + + // When buoyancy is 1 we want neutral buoyancy, this means that the density of the liquid is the same as the density of the body at that point. + // Buoyancy > 1 should make the object float, < 1 should make it sink. + float inverse_mass = mMotionProperties->GetInverseMass(); + float fluid_density = inBuoyancy / (inTotalVolume * inverse_mass); + + // Buoyancy force = Density of Fluid * Submerged volume * Magnitude of gravity * Up direction (eq 2.5.1) + // Impulse = Force * Delta time + // We should apply this at the center of buoyancy (= center of mass of submerged volume) + Vec3 buoyancy_impulse = -fluid_density * inSubmergedVolume * mMotionProperties->GetGravityFactor() * inGravity * inDeltaTime; + + // Calculate the velocity of the center of buoyancy relative to the fluid + Vec3 linear_velocity = mMotionProperties->GetLinearVelocity(); + Vec3 angular_velocity = mMotionProperties->GetAngularVelocity(); + Vec3 center_of_buoyancy_velocity = linear_velocity + angular_velocity.Cross(inRelativeCenterOfBuoyancy); + Vec3 relative_center_of_buoyancy_velocity = inFluidVelocity - center_of_buoyancy_velocity; + + // Here we deviate from the article, instead of eq 2.5.14 we use a quadratic drag formula: https://en.wikipedia.org/wiki/Drag_%28physics%29 + // Drag force = 0.5 * Fluid Density * (Velocity of fluid - Velocity of center of buoyancy)^2 * Linear Drag * Area Facing the Relative Fluid Velocity + // Again Impulse = Force * Delta Time + // We should apply this at the center of buoyancy (= center of mass for submerged volume with no center of mass offset) + + // Get size of local bounding box + Vec3 size = mShape->GetLocalBounds().GetSize(); + + // Determine area of the local space bounding box in the direction of the relative velocity between the fluid and the center of buoyancy + float area = 0.0f; + float relative_center_of_buoyancy_velocity_len_sq = relative_center_of_buoyancy_velocity.LengthSq(); + if (relative_center_of_buoyancy_velocity_len_sq > 1.0e-12f) + { + Vec3 local_relative_center_of_buoyancy_velocity = GetRotation().Conjugated() * relative_center_of_buoyancy_velocity; + area = local_relative_center_of_buoyancy_velocity.Abs().Dot(size.Swizzle() * size.Swizzle()) / sqrt(relative_center_of_buoyancy_velocity_len_sq); + } + + // Calculate the impulse + Vec3 drag_impulse = (0.5f * fluid_density * inLinearDrag * area * inDeltaTime) * relative_center_of_buoyancy_velocity * relative_center_of_buoyancy_velocity.Length(); + + // Clamp magnitude against current linear velocity to prevent overshoot + float linear_velocity_len_sq = linear_velocity.LengthSq(); + float drag_delta_linear_velocity_len_sq = (drag_impulse * inverse_mass).LengthSq(); + if (drag_delta_linear_velocity_len_sq > linear_velocity_len_sq) + drag_impulse *= sqrt(linear_velocity_len_sq / drag_delta_linear_velocity_len_sq); + + // Calculate the resulting delta linear velocity due to buoyancy and drag + Vec3 delta_linear_velocity = (drag_impulse + buoyancy_impulse) * inverse_mass; + mMotionProperties->AddLinearVelocityStep(delta_linear_velocity); + + // Determine average width of the body (across the three axis) + float l = (size.GetX() + size.GetY() + size.GetZ()) / 3.0f; + + // Drag torque = -Angular Drag * Mass * Submerged volume / Total volume * (Average width of body)^2 * Angular velocity (eq 2.5.15) + Vec3 drag_angular_impulse = (-inAngularDrag * inSubmergedVolume / inTotalVolume * inDeltaTime * Square(l) / inverse_mass) * angular_velocity; + Mat44 inv_inertia = GetInverseInertia(); + Vec3 drag_delta_angular_velocity = inv_inertia * drag_angular_impulse; + + // Clamp magnitude against the current angular velocity to prevent overshoot + float angular_velocity_len_sq = angular_velocity.LengthSq(); + float drag_delta_angular_velocity_len_sq = drag_delta_angular_velocity.LengthSq(); + if (drag_delta_angular_velocity_len_sq > angular_velocity_len_sq) + drag_delta_angular_velocity *= sqrt(angular_velocity_len_sq / drag_delta_angular_velocity_len_sq); + + // Calculate total delta angular velocity due to drag and buoyancy + Vec3 delta_angular_velocity = drag_delta_angular_velocity + inv_inertia * inRelativeCenterOfBuoyancy.Cross(buoyancy_impulse + drag_impulse); + mMotionProperties->AddAngularVelocityStep(delta_angular_velocity); + return true; + } + + return false; +} + +bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + float total_volume, submerged_volume; + Vec3 relative_center_of_buoyancy; + GetSubmergedVolume(inSurfacePosition, inSurfaceNormal, total_volume, submerged_volume, relative_center_of_buoyancy); + + return ApplyBuoyancyImpulse(total_volume, submerged_volume, relative_center_of_buoyancy, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime); +} + +void Body::SaveState(StateRecorder &inStream) const +{ + // Only write properties that can change at runtime + inStream.Write(mPosition); + inStream.Write(mRotation); + + if (mMotionProperties != nullptr) + { + if (IsSoftBody()) + static_cast(mMotionProperties)->SaveState(inStream); + else + mMotionProperties->SaveState(inStream); + } +} + +void Body::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + + if (mMotionProperties != nullptr) + { + if (IsSoftBody()) + static_cast(mMotionProperties)->RestoreState(inStream); + else + mMotionProperties->RestoreState(inStream); + + JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = mMotionType); + } + + // Initialize bounding box + CalculateWorldSpaceBoundsInternal(); +} + +BodyCreationSettings Body::GetBodyCreationSettings() const +{ + JPH_ASSERT(IsRigidBody()); + + BodyCreationSettings result; + + result.mPosition = GetPosition(); + result.mRotation = GetRotation(); + result.mLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); + result.mAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); + result.mObjectLayer = GetObjectLayer(); + result.mUserData = mUserData; + result.mCollisionGroup = GetCollisionGroup(); + result.mMotionType = GetMotionType(); + result.mAllowedDOFs = mMotionProperties != nullptr? mMotionProperties->GetAllowedDOFs() : EAllowedDOFs::All; + result.mAllowDynamicOrKinematic = mMotionProperties != nullptr; + result.mIsSensor = IsSensor(); + result.mCollideKinematicVsNonDynamic = GetCollideKinematicVsNonDynamic(); + result.mUseManifoldReduction = GetUseManifoldReduction(); + result.mApplyGyroscopicForce = GetApplyGyroscopicForce(); + result.mMotionQuality = mMotionProperties != nullptr? mMotionProperties->GetMotionQuality() : EMotionQuality::Discrete; + result.mEnhancedInternalEdgeRemoval = GetEnhancedInternalEdgeRemoval(); + result.mAllowSleeping = mMotionProperties != nullptr? GetAllowSleeping() : true; + result.mFriction = GetFriction(); + result.mRestitution = GetRestitution(); + result.mLinearDamping = mMotionProperties != nullptr? mMotionProperties->GetLinearDamping() : 0.0f; + result.mAngularDamping = mMotionProperties != nullptr? mMotionProperties->GetAngularDamping() : 0.0f; + result.mMaxLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxLinearVelocity() : 0.0f; + result.mMaxAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxAngularVelocity() : 0.0f; + result.mGravityFactor = mMotionProperties != nullptr? mMotionProperties->GetGravityFactor() : 1.0f; + result.mNumVelocityStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumVelocityStepsOverride() : 0; + result.mNumPositionStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumPositionStepsOverride() : 0; + result.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + + // Invert inertia and mass + if (mMotionProperties != nullptr) + { + float inv_mass = mMotionProperties->GetInverseMassUnchecked(); + Mat44 inv_inertia = mMotionProperties->GetLocalSpaceInverseInertiaUnchecked(); + + // Get mass + result.mMassPropertiesOverride.mMass = inv_mass != 0.0f? 1.0f / inv_mass : FLT_MAX; + + // Get inertia + Mat44 inertia; + if (inertia.SetInversed3x3(inv_inertia)) + { + // Inertia was invertible, we can use it + result.mMassPropertiesOverride.mInertia = inertia; + } + else + { + // Prevent division by zero + Vec3 diagonal = Vec3::sMax(inv_inertia.GetDiagonal3(), Vec3::sReplicate(FLT_MIN)); + result.mMassPropertiesOverride.mInertia = Mat44::sScale(diagonal.Reciprocal()); + } + } + else + { + result.mMassPropertiesOverride.mMass = FLT_MAX; + result.mMassPropertiesOverride.mInertia = Mat44::sScale(Vec3::sReplicate(FLT_MAX)); + } + + result.SetShape(GetShape()); + + return result; +} + +SoftBodyCreationSettings Body::GetSoftBodyCreationSettings() const +{ + JPH_ASSERT(IsSoftBody()); + + SoftBodyCreationSettings result; + + result.mPosition = GetPosition(); + result.mRotation = GetRotation(); + result.mUserData = mUserData; + result.mObjectLayer = GetObjectLayer(); + result.mCollisionGroup = GetCollisionGroup(); + result.mFriction = GetFriction(); + result.mRestitution = GetRestitution(); + const SoftBodyMotionProperties *mp = static_cast(mMotionProperties); + result.mNumIterations = mp->GetNumIterations(); + result.mLinearDamping = mp->GetLinearDamping(); + result.mMaxLinearVelocity = mp->GetMaxLinearVelocity(); + result.mGravityFactor = mp->GetGravityFactor(); + result.mPressure = mp->GetPressure(); + result.mUpdatePosition = mp->GetUpdatePosition(); + result.mSettings = mp->GetSettings(); + + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h new file mode 100644 index 000000000000..63034f909fe4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h @@ -0,0 +1,429 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StateRecorder; +class BodyCreationSettings; +class SoftBodyCreationSettings; + +/// A rigid body that can be simulated using the physics system +/// +/// Note that internally all properties (position, velocity etc.) are tracked relative to the center of mass of the object to simplify the simulation of the object. +/// +/// The offset between the position of the body and the center of mass position of the body is GetShape()->GetCenterOfMass(). +/// The functions that get/set the position of the body all indicate if they are relative to the center of mass or to the original position in which the shape was created. +/// +/// The linear velocity is also velocity of the center of mass, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$. +class alignas(JPH_RVECTOR_ALIGNMENT) JPH_EXPORT_GCC_BUG_WORKAROUND Body : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Get the id of this body + inline const BodyID & GetID() const { return mID; } + + /// Get the type of body (rigid or soft) + inline EBodyType GetBodyType() const { return mBodyType; } + + /// Check if this body is a rigid body + inline bool IsRigidBody() const { return mBodyType == EBodyType::RigidBody; } + + /// Check if this body is a soft body + inline bool IsSoftBody() const { return mBodyType == EBodyType::SoftBody; } + + // See comment at GetIndexInActiveBodiesInternal for reasoning why TSAN is disabled here + JPH_TSAN_NO_SANITIZE + /// If this body is currently actively simulating (true) or sleeping (false) + inline bool IsActive() const { return mMotionProperties != nullptr && mMotionProperties->mIndexInActiveBodies != cInactiveIndex; } + + /// Check if this body is static (not movable) + inline bool IsStatic() const { return mMotionType == EMotionType::Static; } + + /// Check if this body is kinematic (keyframed), which means that it will move according to its current velocity, but forces don't affect it + inline bool IsKinematic() const { return mMotionType == EMotionType::Kinematic; } + + /// Check if this body is dynamic, which means that it moves and forces can act on it + inline bool IsDynamic() const { return mMotionType == EMotionType::Dynamic; } + + /// Check if a body could be made kinematic or dynamic (if it was created dynamic or with mAllowDynamicOrKinematic set to true) + inline bool CanBeKinematicOrDynamic() const { return mMotionProperties != nullptr; } + + /// Change the body to a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. + /// The cheapest sensor (in terms of CPU usage) is a sensor with motion type Static (they can be moved around using BodyInterface::SetPosition/SetPositionAndRotation). + /// These sensors will only detect collisions with active Dynamic or Kinematic bodies. As soon as a body go to sleep, the contact point with the sensor will be lost. + /// If you make a sensor Dynamic or Kinematic and activate them, the sensor will be able to detect collisions with sleeping bodies too. An active sensor will never go to sleep automatically. + /// When you make a Dynamic or Kinematic sensor, make sure it is in an ObjectLayer that does not collide with Static bodies or other sensors to avoid extra overhead in the broad phase. + inline void SetIsSensor(bool inIsSensor) { JPH_ASSERT(IsRigidBody()); if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); } + + /// Check if this body is a sensor. + inline bool IsSensor() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; } + + /// If kinematic objects can generate contact points against other kinematic or static objects. + /// Note that turning this on can be CPU intensive as much more collision detection work will be done without any effect on the simulation (kinematic objects are not affected by other kinematic/static objects). + /// This can be used to make sensors detect static objects. Note that the sensor must be kinematic and active for it to detect static objects. + inline void SetCollideKinematicVsNonDynamic(bool inCollide) { JPH_ASSERT(IsRigidBody()); if (inCollide) mFlags.fetch_or(uint8(EFlags::CollideKinematicVsNonDynamic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::CollideKinematicVsNonDynamic)), memory_order_relaxed); } + + /// Check if kinematic objects can generate contact points against other kinematic or static objects. + inline bool GetCollideKinematicVsNonDynamic() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::CollideKinematicVsNonDynamic)) != 0; } + + /// If PhysicsSettings::mUseManifoldReduction is true, this allows turning off manifold reduction for this specific body. + /// Manifold reduction by default will combine contacts with similar normals that come from different SubShapeIDs (e.g. different triangles in a mesh shape or different compound shapes). + /// If the application requires tracking exactly which SubShapeIDs are in contact, you can turn off manifold reduction. Note that this comes at a performance cost. + /// Consider using BodyInterface::SetUseManifoldReduction if the body could already be in contact with other bodies to ensure that the contact cache is invalidated and you get the correct contact callbacks. + inline void SetUseManifoldReduction(bool inUseReduction) { JPH_ASSERT(IsRigidBody()); if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); } + + /// Check if this body can use manifold reduction. + inline bool GetUseManifoldReduction() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::UseManifoldReduction)) != 0; } + + /// Checks if the combination of this body and inBody2 should use manifold reduction + inline bool GetUseManifoldReductionWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) & inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::UseManifoldReduction)) != 0; } + + /// Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + inline void SetApplyGyroscopicForce(bool inApply) { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::ApplyGyroscopicForce), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::ApplyGyroscopicForce)), memory_order_relaxed); } + + /// Check if the gyroscopic force is being applied for this body + inline bool GetApplyGyroscopicForce() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::ApplyGyroscopicForce)) != 0; } + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + inline void SetEnhancedInternalEdgeRemoval(bool inApply) { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::EnhancedInternalEdgeRemoval), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::EnhancedInternalEdgeRemoval)), memory_order_relaxed); } + + /// Check if enhanced internal edge removal is turned on + inline bool GetEnhancedInternalEdgeRemoval() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; } + + /// Checks if the combination of this body and inBody2 should use enhanced internal edge removal + inline bool GetEnhancedInternalEdgeRemovalWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) | inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; } + + /// Get the bodies motion type. + inline EMotionType GetMotionType() const { return mMotionType; } + + /// Set the motion type of this body. Consider using BodyInterface::SetMotionType instead of this function if the body may be active or if it needs to be activated. + void SetMotionType(EMotionType inMotionType); + + /// Get broadphase layer, this determines in which broad phase sub-tree the object is placed + inline BroadPhaseLayer GetBroadPhaseLayer() const { return mBroadPhaseLayer; } + + /// Get object layer, this determines which other objects it collides with + inline ObjectLayer GetObjectLayer() const { return mObjectLayer; } + + /// Collision group and sub-group ID, determines which other objects it collides with + const CollisionGroup & GetCollisionGroup() const { return mCollisionGroup; } + CollisionGroup & GetCollisionGroup() { return mCollisionGroup; } + void SetCollisionGroup(const CollisionGroup &inGroup) { mCollisionGroup = inGroup; } + + /// If this body can go to sleep. Note that disabling sleeping on a sleeping object will not wake it up. + bool GetAllowSleeping() const { return mMotionProperties->mAllowSleeping; } + void SetAllowSleeping(bool inAllow); + + /// Resets the sleep timer. This does not wake up the body if it is sleeping, but allows resetting the system that detects when a body is sleeping. + inline void ResetSleepTimer(); + + /// Friction (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + inline float GetFriction() const { return mFriction; } + void SetFriction(float inFriction) { mFriction = inFriction; } + + /// Restitution (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + inline float GetRestitution() const { return mRestitution; } + void SetRestitution(float inRestitution) { mRestitution = inRestitution; } + + /// Get world space linear velocity of the center of mass (unit: m/s) + inline Vec3 GetLinearVelocity() const { return !IsStatic()? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); } + + /// Set world space linear velocity of the center of mass (unit: m/s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead. + void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocity(inLinearVelocity); } + + /// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity. + /// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead. + void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocityClamped(inLinearVelocity); } + + /// Get world space angular velocity of the center of mass (unit: rad/s) + inline Vec3 GetAngularVelocity() const { return !IsStatic()? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); } + + /// Set world space angular velocity of the center of mass (unit: rad/s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead. + void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocity(inAngularVelocity); } + + /// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity. + /// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead. + void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocityClamped(inAngularVelocity); } + + /// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s) + inline Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return !IsStatic()? mMotionProperties->GetPointVelocityCOM(inPointRelativeToCOM) : Vec3::sZero(); } + + /// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s) + inline Vec3 GetPointVelocity(RVec3Arg inPoint) const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return GetPointVelocityCOM(Vec3(inPoint - mPosition)); } + + /// Add force (unit: N) at center of mass for the next time step, will be reset after the next call to PhysicsSystem::Update. + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead. + inline void AddForce(Vec3Arg inForce) { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); } + + /// Add force (unit: N) at inPosition for the next time step, will be reset after the next call to PhysicsSystem::Update. + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead. + inline void AddForce(Vec3Arg inForce, RVec3Arg inPosition); + + /// Add torque (unit: N m) for the next time step, will be reset after the next call to PhysicsSystem::Update. + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddTorque instead. + inline void AddTorque(Vec3Arg inTorque) { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); } + + // Get the total amount of force applied to the center of mass this time step (through AddForce calls). Note that it will reset to zero after PhysicsSystem::Update. + inline Vec3 GetAccumulatedForce() const { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedForce(); } + + // Get the total amount of torque applied to the center of mass this time step (through AddForce/AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update. + inline Vec3 GetAccumulatedTorque() const { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedTorque(); } + + // Reset the total accumulated force, not that this will be done automatically after every time step. + JPH_INLINE void ResetForce() { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetForce(); } + + // Reset the total accumulated torque, not that this will be done automatically after every time step. + JPH_INLINE void ResetTorque() { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetTorque(); } + + // Reset the current velocity and accumulated force and torque. + JPH_INLINE void ResetMotion() { JPH_ASSERT(!IsStatic()); return mMotionProperties->ResetMotion(); } + + /// Get inverse inertia tensor in world space + inline Mat44 GetInverseInertia() const; + + /// Add impulse to center of mass (unit: kg m/s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead. + inline void AddImpulse(Vec3Arg inImpulse); + + /// Add impulse to point in world space (unit: kg m/s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead. + inline void AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition); + + /// Add angular impulse in world space (unit: N m s). + /// If you want the body to wake up when it is sleeping, use BodyInterface::AddAngularImpulse instead. + inline void AddAngularImpulse(Vec3Arg inAngularImpulse); + + /// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds. + /// If you want the body to wake up when it is sleeping, use BodyInterface::MoveKinematic instead. + void MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime); + + /// Gets the properties needed to do buoyancy calculations + /// @param inSurfacePosition Position of the fluid surface in world space + /// @param inSurfaceNormal Normal of the fluid surface (should point up) + /// @param outTotalVolume On return this contains the total volume of the shape + /// @param outSubmergedVolume On return this contains the submerged volume of the shape + /// @param outRelativeCenterOfBuoyancy On return this contains the center of mass of the submerged volume relative to the center of mass of the body + void GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const; + + /// Applies an impulse to the body that simulates fluid buoyancy and drag. + /// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead. + /// @param inSurfacePosition Position of the fluid surface in world space + /// @param inSurfaceNormal Normal of the fluid surface (should point up) + /// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2] + /// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5) + /// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01) + /// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides + /// @param inGravity The gravity vector (pointing down) + /// @param inDeltaTime Delta time of the next simulation step (in s) + /// @return true if an impulse was applied, false if the body was not in the fluid + bool ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); + + /// Applies an impulse to the body that simulates fluid buoyancy and drag. + /// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead. + /// @param inTotalVolume Total volume of the shape of this body (m^3) + /// @param inSubmergedVolume Submerged volume of the shape of this body (m^3) + /// @param inRelativeCenterOfBuoyancy The center of mass of the submerged volume relative to the center of mass of the body + /// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2] + /// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5) + /// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01) + /// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides + /// @param inGravity The gravity vector (pointing down) + /// @param inDeltaTime Delta time of the next simulation step (in s) + /// @return true if an impulse was applied, false if the body was not in the fluid + bool ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); + + /// Check if this body has been added to the physics system + inline bool IsInBroadPhase() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsInBroadPhase)) != 0; } + + /// Check if this body has been changed in such a way that the collision cache should be considered invalid for any body interacting with this body + inline bool IsCollisionCacheInvalid() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) != 0; } + + /// Get the shape of this body + inline const Shape * GetShape() const { return mShape; } + + /// World space position of the body + inline RVec3 GetPosition() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); } + + /// World space rotation of the body + inline Quat GetRotation() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mRotation; } + + /// Calculates the transform of this body + inline RMat44 GetWorldTransform() const; + + /// Gets the world space position of this body's center of mass + inline RVec3 GetCenterOfMassPosition() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition; } + + /// Calculates the transform for this body's center of mass + inline RMat44 GetCenterOfMassTransform() const; + + /// Calculates the inverse of the transform for this body's center of mass + inline RMat44 GetInverseCenterOfMassTransform() const; + + /// Get world space bounding box + inline const AABox & GetWorldSpaceBounds() const { return mBounds; } + + /// Access to the motion properties + const MotionProperties *GetMotionProperties() const { JPH_ASSERT(!IsStatic()); return mMotionProperties; } + MotionProperties * GetMotionProperties() { JPH_ASSERT(!IsStatic()); return mMotionProperties; } + + /// Access to the motion properties (version that does not check if the object is kinematic or dynamic) + const MotionProperties *GetMotionPropertiesUnchecked() const { return mMotionProperties; } + MotionProperties * GetMotionPropertiesUnchecked() { return mMotionProperties; } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Get surface normal of a particular sub shape and its world space surface position on this body + inline Vec3 GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const; + + /// Get the transformed shape of this body, which can be used to do collision detection outside of a body lock + inline TransformedShape GetTransformedShape() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return TransformedShape(mPosition, mRotation, mShape, mID); } + + /// Debug function to convert a body back to a body creation settings object to be able to save/recreate the body later + BodyCreationSettings GetBodyCreationSettings() const; + + /// Debug function to convert a soft body back to a soft body creation settings object to be able to save/recreate the body later + SoftBodyCreationSettings GetSoftBodyCreationSettings() const; + + /// A dummy body that can be used by constraints to attach a constraint to the world instead of another body + static Body sFixedToWorld; + + ///@name THESE FUNCTIONS ARE FOR INTERNAL USE ONLY AND SHOULD NOT BE CALLED BY THE APPLICATION + ///@{ + + /// Helper function for BroadPhase::FindCollidingPairs that returns true when two bodies can collide + /// It assumes that body 1 is dynamic and active and guarantees that it body 1 collides with body 2 that body 2 will not collide with body 1 in order to avoid finding duplicate collision pairs + static inline bool sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2); + + /// Update position using an Euler step (used during position integrate & constraint solving) + inline void AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); } + inline void SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); } + + /// Update rotation using an Euler step (used during position integrate & constraint solving) + inline void AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime); + inline void SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime); + + /// Flag if body is in the broadphase (should only be called by the BroadPhase) + inline void SetInBroadPhaseInternal(bool inInBroadPhase) { if (inInBroadPhase) mFlags.fetch_or(uint8(EFlags::IsInBroadPhase), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsInBroadPhase)), memory_order_relaxed); } + + /// Invalidate the contact cache (should only be called by the BodyManager), will be reset the next simulation step. Returns true if the contact cache was still valid. + inline bool InvalidateContactCacheInternal() { return (mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) == 0; } + + /// Reset the collision cache invalid flag (should only be called by the BodyManager). + inline void ValidateContactCacheInternal() { JPH_IF_ENABLE_ASSERTS(uint8 old_val = ) mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); JPH_ASSERT((old_val & uint8(EFlags::InvalidateContactCache)) != 0); } + + /// Updates world space bounding box (should only be called by the PhysicsSystem) + void CalculateWorldSpaceBoundsInternal(); + + /// Function to update body's position (should only be called by the BodyInterface since it also requires updating the broadphase) + void SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer = true); + + /// Updates the center of mass and optionally mass properties after shifting the center of mass or changes to the shape (should only be called by the BodyInterface since it also requires updating the broadphase) + /// @param inPreviousCenterOfMass Center of mass of the shape before the alterations + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + void UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties); + + /// Function to update a body's shape (should only be called by the BodyInterface since it also requires updating the broadphase) + /// @param inShape The new shape for this body + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + void SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties); + + // TSAN detects a race between BodyManager::AddBodyToActiveBodies coming from PhysicsSystem::ProcessBodyPair and Body::GetIndexInActiveBodiesInternal coming from PhysicsSystem::ProcessBodyPair. + // When PhysicsSystem::ProcessBodyPair activates a body, it updates mIndexInActiveBodies and then updates BodyManager::mNumActiveBodies with release semantics. PhysicsSystem::ProcessBodyPair will + // then finish its loop of active bodies and at the end of the loop it will read BodyManager::mNumActiveBodies with acquire semantics to see if any bodies were activated during the loop. + // This means that changes to mIndexInActiveBodies must be visible to the thread, so TSANs report must be a false positive. We suppress the warning here. + JPH_TSAN_NO_SANITIZE + /// Access to the index in the BodyManager::mActiveBodies list + uint32 GetIndexInActiveBodiesInternal() const { return mMotionProperties != nullptr? mMotionProperties->mIndexInActiveBodies : cInactiveIndex; } + + /// Update eligibility for sleeping + ECanSleep UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep); + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + ///@} + + static constexpr uint32 cInactiveIndex = MotionProperties::cInactiveIndex; ///< Constant indicating that body is not active + +private: + friend class BodyManager; + friend class BodyWithMotionProperties; + friend class SoftBodyWithMotionPropertiesAndShape; + + Body() = default; ///< Bodies must be created through BodyInterface::CreateBody + + explicit Body(bool); ///< Alternative constructor that initializes all members + + ~Body() { JPH_ASSERT(mMotionProperties == nullptr); } ///< Bodies must be destroyed through BodyInterface::DestroyBody + + inline void GetSleepTestPoints(RVec3 *outPoints) const; ///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis + + enum class EFlags : uint8 + { + IsSensor = 1 << 0, ///< If this object is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. + CollideKinematicVsNonDynamic = 1 << 1, ///< If kinematic objects can generate contact points against other kinematic or static objects. + IsInBroadPhase = 1 << 2, ///< Set this bit to indicate that the body is in the broadphase + InvalidateContactCache = 1 << 3, ///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step. + UseManifoldReduction = 1 << 4, ///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true) + ApplyGyroscopicForce = 1 << 5, ///< Set this bit to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + EnhancedInternalEdgeRemoval = 1 << 6, ///< Set this bit to indicate that enhanced internal edge removal should be used for this body (see BodyCreationSettings::mEnhancedInternalEdgeRemoval) + }; + + // 16 byte aligned + RVec3 mPosition; ///< World space position of center of mass + Quat mRotation; ///< World space rotation of center of mass + AABox mBounds; ///< World space bounding box of the body + + // 8 byte aligned + RefConst mShape; ///< Shape representing the volume of this body + MotionProperties * mMotionProperties = nullptr; ///< If this is a keyframed or dynamic object, this object holds all information about the movement + uint64 mUserData = 0; ///< User data, can be used for anything by the application + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + // 4 byte aligned + float mFriction; ///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + float mRestitution; ///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + BodyID mID; ///< ID of the body (index in the bodies array) + + // 2 or 4 bytes aligned + ObjectLayer mObjectLayer; ///< The collision layer this body belongs to (determines if two objects can collide) + + // 1 byte aligned + EBodyType mBodyType; ///< Type of body (rigid or soft) + BroadPhaseLayer mBroadPhaseLayer; ///< The broad phase layer this body belongs to + EMotionType mMotionType; ///< Type of motion (static, dynamic or kinematic) + atomic mFlags = 0; ///< See EFlags for possible flags + + // 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer) +}; + +static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect"); +static_assert(alignof(Body) == JPH_RVECTOR_ALIGNMENT, "Body should properly align"); + +JPH_NAMESPACE_END + +#include "Body.inl" diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl new file mode 100644 index 000000000000..dbbd64dd7f94 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl @@ -0,0 +1,197 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +RMat44 Body::GetWorldTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass()); +} + +RMat44 Body::GetCenterOfMassTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + return RMat44::sRotationTranslation(mRotation, mPosition); +} + +RMat44 Body::GetInverseCenterOfMassTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + return RMat44::sInverseRotationTranslation(mRotation, mPosition); +} + +inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2) +{ + // First body should never be a soft body + JPH_ASSERT(!inBody1.IsSoftBody()); + + // One of these conditions must be true + // - We always allow detecting collisions between kinematic and non-dynamic bodies + // - One of the bodies must be dynamic to collide + // - A kinematic object can collide with a sensor + if (!inBody1.GetCollideKinematicVsNonDynamic() + && !inBody2.GetCollideKinematicVsNonDynamic() + && (!inBody1.IsDynamic() && !inBody2.IsDynamic()) + && !(inBody1.IsKinematic() && inBody2.IsSensor()) + && !(inBody2.IsKinematic() && inBody1.IsSensor())) + return false; + + // Check that body 1 is active + uint32 body1_index_in_active_bodies = inBody1.GetIndexInActiveBodiesInternal(); + JPH_ASSERT(!inBody1.IsStatic() && body1_index_in_active_bodies != Body::cInactiveIndex, "This function assumes that Body 1 is active"); + + // If the pair A, B collides we need to ensure that the pair B, A does not collide or else we will handle the collision twice. + // If A is the same body as B we don't want to collide (1) + // If A is dynamic / kinematic and B is static we should collide (2) + // If A is dynamic / kinematic and B is dynamic / kinematic we should only collide if + // - A is active and B is not active (3) + // - A is active and B will become active during this simulation step (4) + // - A is active and B is active, we require a condition that makes A, B collide and B, A not (5) + // + // In order to implement this we use the index in the active body list and make use of the fact that + // a body not in the active list has Body.Index = 0xffffffff which is the highest possible value for an uint32. + // + // Because we know that A is active we know that A.Index != 0xffffffff: + // (1) Because A.Index != 0xffffffff, if A.Index = B.Index then A = B, so to collide A.Index != B.Index + // (2) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's static and cannot be in the active list), so to collide A.Index != B.Index + // (3) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's not yet active), so to collide A.Index != B.Index + // (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it + // will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate + // bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index. + // (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't + static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value"); + if (!inBody2.IsSoftBody() && body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal()) + return false; + JPH_ASSERT(inBody1.GetID() != inBody2.GetID(), "Read the comment above, A and B are the same body which should not be possible!"); + + // Check collision group filter + if (!inBody1.GetCollisionGroup().CanCollide(inBody2.GetCollisionGroup())) + return false; + + return true; +} + +void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); + + // This used to use the equation: d/dt R(t) = 1/2 * w(t) * R(t) so that R(t + dt) = R(t) + 1/2 * w(t) * R(t) * dt + // See: Appendix B of An Introduction to Physically Based Modeling: Rigid Body Simulation II-Nonpenetration Constraints + // URL: https://www.cs.cmu.edu/~baraff/sigcourse/notesd2.pdf + // But this is a first order approximation and does not work well for kinematic ragdolls that are driven to a new + // pose if the poses differ enough. So now we split w(t) * dt into an axis and angle part and create a quaternion with it. + // Note that the resulting quaternion is normalized since otherwise numerical drift will eventually make the rotation non-normalized. + float len = inAngularVelocityTimesDeltaTime.Length(); + if (len > 1.0e-6f) + { + mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized(); + JPH_ASSERT(!mRotation.IsNaN()); + } +} + +void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); + + // See comment at Body::AddRotationStep + float len = inAngularVelocityTimesDeltaTime.Length(); + if (len > 1.0e-6f) + { + mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized(); + JPH_ASSERT(!mRotation.IsNaN()); + } +} + +Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const +{ + RMat44 inv_com = GetInverseCenterOfMassTransform(); + return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, Vec3(inv_com * inPosition))).Normalized(); +} + +Mat44 Body::GetInverseInertia() const +{ + JPH_ASSERT(IsDynamic()); + + return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation)); +} + +void Body::AddForce(Vec3Arg inForce, RVec3Arg inPosition) +{ + AddForce(inForce); + AddTorque(Vec3(inPosition - mPosition).Cross(inForce)); +} + +void Body::AddImpulse(Vec3Arg inImpulse) +{ + JPH_ASSERT(IsDynamic()); + + SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass()); +} + +void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition) +{ + JPH_ASSERT(IsDynamic()); + + SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass()); + + SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, Vec3(inPosition - mPosition).Cross(inImpulse))); +} + +void Body::AddAngularImpulse(Vec3Arg inAngularImpulse) +{ + JPH_ASSERT(IsDynamic()); + + SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, inAngularImpulse)); +} + +void Body::GetSleepTestPoints(RVec3 *outPoints) const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + + // Center of mass is the first position + outPoints[0] = mPosition; + + // The second and third position are on the largest axis of the bounding box + Vec3 extent = mShape->GetLocalBounds().GetExtent(); + int lowest_component = extent.GetLowestComponentIndex(); + Mat44 rotation = Mat44::sRotation(mRotation); + switch (lowest_component) + { + case 0: + outPoints[1] = mPosition + extent.GetY() * rotation.GetColumn3(1); + outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2); + break; + + case 1: + outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0); + outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2); + break; + + case 2: + outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0); + outPoints[2] = mPosition + extent.GetY() * rotation.GetColumn3(1); + break; + + default: + JPH_ASSERT(false); + break; + } +} + +void Body::ResetSleepTimer() +{ + RVec3 points[3]; + GetSleepTestPoints(points); + mMotionProperties->ResetSleepTestSpheres(points); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h new file mode 100644 index 000000000000..a7fd6a4ac3b9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h @@ -0,0 +1,68 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_BEGIN + +class JPH_EXPORT BodyAccess +{ +public: + /// Access rules, used to detect race conditions during simulation + enum class EAccess : uint8 + { + None = 0, + Read = 1, + ReadWrite = 3, + }; + + /// Grant a scope specific access rights on the current thread + class Grant + { + public: + inline Grant(EAccess inVelocity, EAccess inPosition) + { + EAccess &velocity = sVelocityAccess(); + EAccess &position = sPositionAccess(); + + JPH_ASSERT(velocity == EAccess::ReadWrite); + JPH_ASSERT(position == EAccess::ReadWrite); + + velocity = inVelocity; + position = inPosition; + } + + inline ~Grant() + { + sVelocityAccess() = EAccess::ReadWrite; + sPositionAccess() = EAccess::ReadWrite; + } + }; + + /// Check if we have permission + static inline bool sCheckRights(EAccess inRights, EAccess inDesiredRights) + { + return (uint8(inRights) & uint8(inDesiredRights)) == uint8(inDesiredRights); + } + + /// Access to read/write velocities + static inline EAccess & sVelocityAccess() + { + static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite; + return sAccess; + } + + /// Access to read/write positions + static inline EAccess & sPositionAccess() + { + static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite; + return sAccess; + } +}; + +JPH_NAMESPACE_END + +#endif // JPH_ENABLE_ASSERTS diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h new file mode 100644 index 000000000000..2c8808a15058 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class BodyID; + +/// A listener class that receives events when a body activates or deactivates. +/// It can be registered with the BodyManager (or PhysicsSystem). +class BodyActivationListener +{ +public: + /// Ensure virtual destructor + virtual ~BodyActivationListener() = default; + + /// Called whenever a body activates, note this can be called from any thread so make sure your code is thread safe. + /// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback. + virtual void OnBodyActivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0; + + /// Called whenever a body deactivates, note this can be called from any thread so make sure your code is thread safe. + /// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback. + virtual void OnBodyDeactivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp new file mode 100644 index 000000000000..9b6d3f92c41a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp @@ -0,0 +1,234 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings) +{ + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mPosition) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRotation) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUserData) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mShape) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mCollisionGroup) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mObjectLayer) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionType) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mAllowedDOFs) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowDynamicOrKinematic) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor) + JPH_ADD_ATTRIBUTE_WITH_ALIAS(BodyCreationSettings, mCollideKinematicVsNonDynamic, "mSensorDetectsStatic") // This is the old name to keep backwards compatibility + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUseManifoldReduction) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mApplyGyroscopicForce) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionQuality) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mEnhancedInternalEdgeRemoval) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowSleeping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mFriction) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRestitution) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearDamping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularDamping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxLinearVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxAngularVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mGravityFactor) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumVelocityStepsOverride) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumPositionStepsOverride) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mOverrideMassProperties) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mInertiaMultiplier) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMassPropertiesOverride) +} + +void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mLinearVelocity); + inStream.Write(mAngularVelocity); + mCollisionGroup.SaveBinaryState(inStream); + inStream.Write(mObjectLayer); + inStream.Write(mMotionType); + inStream.Write(mAllowedDOFs); + inStream.Write(mAllowDynamicOrKinematic); + inStream.Write(mIsSensor); + inStream.Write(mCollideKinematicVsNonDynamic); + inStream.Write(mUseManifoldReduction); + inStream.Write(mApplyGyroscopicForce); + inStream.Write(mMotionQuality); + inStream.Write(mEnhancedInternalEdgeRemoval); + inStream.Write(mAllowSleeping); + inStream.Write(mFriction); + inStream.Write(mRestitution); + inStream.Write(mLinearDamping); + inStream.Write(mAngularDamping); + inStream.Write(mMaxLinearVelocity); + inStream.Write(mMaxAngularVelocity); + inStream.Write(mGravityFactor); + inStream.Write(mNumVelocityStepsOverride); + inStream.Write(mNumPositionStepsOverride); + inStream.Write(mOverrideMassProperties); + inStream.Write(mInertiaMultiplier); + mMassPropertiesOverride.SaveBinaryState(inStream); +} + +void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mLinearVelocity); + inStream.Read(mAngularVelocity); + mCollisionGroup.RestoreBinaryState(inStream); + inStream.Read(mObjectLayer); + inStream.Read(mMotionType); + inStream.Read(mAllowedDOFs); + inStream.Read(mAllowDynamicOrKinematic); + inStream.Read(mIsSensor); + inStream.Read(mCollideKinematicVsNonDynamic); + inStream.Read(mUseManifoldReduction); + inStream.Read(mApplyGyroscopicForce); + inStream.Read(mMotionQuality); + inStream.Read(mEnhancedInternalEdgeRemoval); + inStream.Read(mAllowSleeping); + inStream.Read(mFriction); + inStream.Read(mRestitution); + inStream.Read(mLinearDamping); + inStream.Read(mAngularDamping); + inStream.Read(mMaxLinearVelocity); + inStream.Read(mMaxAngularVelocity); + inStream.Read(mGravityFactor); + inStream.Read(mNumVelocityStepsOverride); + inStream.Read(mNumPositionStepsOverride); + inStream.Read(mOverrideMassProperties); + inStream.Read(mInertiaMultiplier); + mMassPropertiesOverride.RestoreBinaryState(inStream); +} + +Shape::ShapeResult BodyCreationSettings::ConvertShapeSettings() +{ + // If we already have a shape, return it + if (mShapePtr != nullptr) + { + mShape = nullptr; + + Shape::ShapeResult result; + result.Set(const_cast(mShapePtr.GetPtr())); + return result; + } + + // Check if we have shape settings + if (mShape == nullptr) + { + Shape::ShapeResult result; + result.SetError("No shape present!"); + return result; + } + + // Create the shape + Shape::ShapeResult result = mShape->Create(); + if (result.IsValid()) + mShapePtr = result.Get(); + mShape = nullptr; + return result; +} + +const Shape *BodyCreationSettings::GetShape() const +{ + // If we already have a shape, return it + if (mShapePtr != nullptr) + return mShapePtr; + + // Check if we have shape settings + if (mShape == nullptr) + return nullptr; + + // Create the shape + Shape::ShapeResult result = mShape->Create(); + if (result.IsValid()) + return result.Get(); + + Trace("Error: %s", result.GetError().c_str()); + JPH_ASSERT(false, "An error occurred during shape creation. Use ConvertShapeSettings() to convert the shape and get the error!"); + return nullptr; +} + +MassProperties BodyCreationSettings::GetMassProperties() const +{ + // Calculate mass properties + MassProperties mass_properties; + switch (mOverrideMassProperties) + { + case EOverrideMassProperties::CalculateMassAndInertia: + mass_properties = GetShape()->GetMassProperties(); + mass_properties.mInertia *= mInertiaMultiplier; + mass_properties.mInertia(3, 3) = 1.0f; + break; + case EOverrideMassProperties::CalculateInertia: + mass_properties = GetShape()->GetMassProperties(); + mass_properties.ScaleToMass(mMassPropertiesOverride.mMass); + mass_properties.mInertia *= mInertiaMultiplier; + mass_properties.mInertia(3, 3) = 1.0f; + break; + case EOverrideMassProperties::MassAndInertiaProvided: + mass_properties = mMassPropertiesOverride; + break; + } + return mass_properties; +} + +void BodyCreationSettings::SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const +{ + // Save creation settings + SaveBinaryState(inStream); + + // Save shape + if (ioShapeMap != nullptr && ioMaterialMap != nullptr) + GetShape()->SaveWithChildren(inStream, *ioShapeMap, *ioMaterialMap); + else + inStream.Write(~uint32(0)); + + // Save group filter + StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap); +} + +BodyCreationSettings::BCSResult BodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap) +{ + BCSResult result; + + // Read creation settings + BodyCreationSettings settings; + settings.RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Error reading body creation settings"); + return result; + } + + // Read shape + Shape::ShapeResult shape_result = Shape::sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap); + if (shape_result.HasError()) + { + result.SetError(shape_result.GetError()); + return result; + } + settings.SetShape(shape_result.Get()); + + // Read group filter + Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap); + if (gfresult.HasError()) + { + result.SetError(gfresult.GetError()); + return result; + } + settings.mCollisionGroup.SetGroupFilter(gfresult.Get()); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h new file mode 100644 index 000000000000..8949b2ec3e0c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h @@ -0,0 +1,124 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Enum used in BodyCreationSettings to indicate how mass and inertia should be calculated +enum class EOverrideMassProperties : uint8 +{ + CalculateMassAndInertia, ///< Tells the system to calculate the mass and inertia based on density + CalculateInertia, ///< Tells the system to take the mass from mMassPropertiesOverride and to calculate the inertia based on density of the shapes and to scale it to the provided mass + MassAndInertiaProvided ///< Tells the system to take the mass and inertia from mMassPropertiesOverride +}; + +/// Settings for constructing a rigid body +class JPH_EXPORT BodyCreationSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, BodyCreationSettings) + +public: + /// Constructor + BodyCreationSettings() = default; + BodyCreationSettings(const ShapeSettings *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShape(inShape) { } + BodyCreationSettings(const Shape *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShapePtr(inShape) { } + + /// Access to the shape settings object. This contains serializable (non-runtime optimized) information about the Shape. + const ShapeSettings * GetShapeSettings() const { return mShape; } + void SetShapeSettings(const ShapeSettings *inShape) { mShape = inShape; mShapePtr = nullptr; } + + /// Convert ShapeSettings object into a Shape object. This will free the ShapeSettings object and make the object ready for runtime. Serialization is no longer possible after this. + Shape::ShapeResult ConvertShapeSettings(); + + /// Access to the run-time shape object. Will convert from ShapeSettings object if needed. + const Shape * GetShape() const; + void SetShape(const Shape *inShape) { mShapePtr = inShape; mShape = nullptr; } + + /// Check if the mass properties of this body will be calculated (only relevant for kinematic or dynamic objects that need a MotionProperties object) + bool HasMassProperties() const { return mAllowDynamicOrKinematic || mMotionType != EMotionType::Static; } + + /// Calculate (or return when overridden) the mass and inertia for this body + MassProperties GetMassProperties() const; + + /// Saves the state of this object in binary form to inStream. Doesn't store the shape nor the group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the shape nor the group filter. + void RestoreBinaryState(StreamIn &inStream); + + using GroupFilterToIDMap = StreamUtils::ObjectToIDMap; + using IDToGroupFilterMap = StreamUtils::IDToObjectMap; + using ShapeToIDMap = Shape::ShapeToIDMap; + using IDToShapeMap = Shape::IDToShapeMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + /// Pass nullptr to ioShapeMap and ioMaterial map to skip saving shapes + /// Pass nullptr to ioGroupFilterMap to skip saving group filters + void SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const; + + using BCSResult = Result; + + /// Restore body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static BCSResult sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap); + + RVec3 mPosition = RVec3::sZero(); ///< Position of the body (not of the center of mass) + Quat mRotation = Quat::sIdentity(); ///< Rotation of the body + Vec3 mLinearVelocity = Vec3::sZero(); ///< World space linear velocity of the center of mass (m/s) + Vec3 mAngularVelocity = Vec3::sZero(); ///< World space angular velocity (rad/s) + + /// User data value (can be used by application) + uint64 mUserData = 0; + + ///@name Collision settings + ObjectLayer mObjectLayer = 0; ///< The collision layer this body belongs to (determines if two objects can collide) + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + ///@name Simulation properties + EMotionType mMotionType = EMotionType::Dynamic; ///< Motion type, determines if the object is static, dynamic or kinematic + EAllowedDOFs mAllowedDOFs = EAllowedDOFs::All; ///< Which degrees of freedom this body has (can be used to limit simulation to 2D) + bool mAllowDynamicOrKinematic = false; ///< When this body is created as static, this setting tells the system to create a MotionProperties object so that the object can be switched to kinematic or dynamic + bool mIsSensor = false; ///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor. + bool mCollideKinematicVsNonDynamic = false; ///< If kinematic objects can generate contact points against other kinematic or static objects. See description at Body::SetCollideKinematicVsNonDynamic. + bool mUseManifoldReduction = true; ///< If this body should use manifold reduction (see description at Body::SetUseManifoldReduction) + bool mApplyGyroscopicForce = false; ///< Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + EMotionQuality mMotionQuality = EMotionQuality::Discrete; ///< Motion quality, or how well it detects collisions when it has a high velocity + bool mEnhancedInternalEdgeRemoval = false; ///< Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool mAllowSleeping = true; ///< If this body can go to sleep or not + float mFriction = 0.2f; ///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + float mRestitution = 0.0f; ///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + float mLinearDamping = 0.05f; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + float mAngularDamping = 0.05f; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + float mMaxLinearVelocity = 500.0f; ///< Maximum linear velocity that this body can reach (m/s) + float mMaxAngularVelocity = 0.25f * JPH_PI * 60.0f; ///< Maximum angular velocity that this body can reach (rad/s) + float mGravityFactor = 1.0f; ///< Value to multiply gravity with for this body + uint mNumVelocityStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumPositionStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + + ///@name Mass properties of the body (by default calculated by the shape) + EOverrideMassProperties mOverrideMassProperties = EOverrideMassProperties::CalculateMassAndInertia; ///< Determines how mMassPropertiesOverride will be used + float mInertiaMultiplier = 1.0f; ///< When calculating the inertia (not when it is provided) the calculated inertia will be multiplied by this value + MassProperties mMassPropertiesOverride; ///< Contains replacement mass settings which override the automatically calculated values + +private: + /// Collision volume for the body + RefConst mShape; ///< Shape settings, can be serialized. Mutually exclusive with mShapePtr + RefConst mShapePtr; ///< Actual shape, cannot be serialized. Mutually exclusive with mShape +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h new file mode 100644 index 000000000000..111d514018b6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; + +/// Class function to filter out bodies, returns true if test should collide with body +class JPH_EXPORT BodyFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BodyFilter() = default; + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide([[maybe_unused]] const BodyID &inBodyID) const + { + return true; + } + + /// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members) + virtual bool ShouldCollideLocked([[maybe_unused]] const Body &inBody) const + { + return true; + } +}; + +/// A simple body filter implementation that ignores a single, specified body +class JPH_EXPORT IgnoreSingleBodyFilter : public BodyFilter +{ +public: + /// Constructor, pass the body you want to ignore + explicit IgnoreSingleBodyFilter(const BodyID &inBodyID) : + mBodyID(inBodyID) + { + } + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide(const BodyID &inBodyID) const override + { + return mBodyID != inBodyID; + } + +private: + BodyID mBodyID; +}; + +/// A simple body filter implementation that ignores multiple, specified bodies +class JPH_EXPORT IgnoreMultipleBodiesFilter : public BodyFilter +{ +public: + /// Remove all bodies from the filter + void Clear() + { + mBodyIDs.clear(); + } + + /// Reserve space for inSize body ID's + void Reserve(uint inSize) + { + mBodyIDs.reserve(inSize); + } + + /// Add a body to be ignored + void IgnoreBody(const BodyID &inBodyID) + { + mBodyIDs.push_back(inBodyID); + } + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide(const BodyID &inBodyID) const override + { + return std::find(mBodyIDs.begin(), mBodyIDs.end(), inBodyID) == mBodyIDs.end(); + } + +private: + Array mBodyIDs; +}; + +/// Ignores a single body and chains the filter to another filter +class JPH_EXPORT IgnoreSingleBodyFilterChained : public BodyFilter +{ +public: + /// Constructor + explicit IgnoreSingleBodyFilterChained(const BodyID inBodyID, const BodyFilter &inFilter) : + mBodyID(inBodyID), + mFilter(inFilter) + { + } + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide(const BodyID &inBodyID) const override + { + return inBodyID != mBodyID && mFilter.ShouldCollide(inBodyID); + } + + /// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members) + virtual bool ShouldCollideLocked(const Body &inBody) const override + { + return mFilter.ShouldCollideLocked(inBody); + } + +private: + BodyID mBodyID; + const BodyFilter & mFilter; +}; + +#ifdef JPH_DEBUG_RENDERER +/// Class function to filter out bodies for debug rendering, returns true if body should be rendered +class JPH_EXPORT BodyDrawFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BodyDrawFilter() = default; + + /// Filter function. Returns true if inBody should be rendered + virtual bool ShouldDraw([[maybe_unused]] const Body& inBody) const + { + return true; + } +}; +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h new file mode 100644 index 000000000000..d2bbaccff7d2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h @@ -0,0 +1,100 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding race conditions. +class BodyID +{ +public: + JPH_OVERRIDE_NEW_DELETE + + static constexpr uint32 cInvalidBodyID = 0xffffffff; ///< The value for an invalid body ID + static constexpr uint32 cBroadPhaseBit = 0x00800000; ///< This bit is used by the broadphase + static constexpr uint32 cMaxBodyIndex = 0x7fffff; ///< Maximum value for body index (also the maximum amount of bodies supported - 1) + static constexpr uint8 cMaxSequenceNumber = 0xff; ///< Maximum value for the sequence number + + /// Construct invalid body ID + BodyID() : + mID(cInvalidBodyID) + { + } + + /// Construct from index and sequence number combined in a single uint32 (use with care!) + explicit BodyID(uint32 inID) : + mID(inID) + { + JPH_ASSERT((inID & cBroadPhaseBit) == 0 || inID == cInvalidBodyID); // Check bit used by broadphase + } + + /// Construct from index and sequence number + explicit BodyID(uint32 inID, uint8 inSequenceNumber) : + mID((uint32(inSequenceNumber) << 24) | inID) + { + JPH_ASSERT(inID < cMaxBodyIndex); // Should not use bit pattern for invalid ID and should not use the broadphase bit + } + + /// Get index in body array + inline uint32 GetIndex() const + { + return mID & cMaxBodyIndex; + } + + /// Get sequence number of body. + /// The sequence number can be used to check if a body ID with the same body index has been reused by another body. + /// It is mainly used in multi threaded situations where a body is removed and its body index is immediately reused by a body created from another thread. + /// Functions querying the broadphase can (after acquiring a body lock) detect that the body has been removed (we assume that this won't happen more than 128 times in a row). + inline uint8 GetSequenceNumber() const + { + return uint8(mID >> 24); + } + + /// Returns the index and sequence number combined in an uint32 + inline uint32 GetIndexAndSequenceNumber() const + { + return mID; + } + + /// Check if the ID is valid + inline bool IsInvalid() const + { + return mID == cInvalidBodyID; + } + + /// Equals check + inline bool operator == (const BodyID &inRHS) const + { + return mID == inRHS.mID; + } + + /// Not equals check + inline bool operator != (const BodyID &inRHS) const + { + return mID != inRHS.mID; + } + + /// Smaller than operator, can be used for sorting bodies + inline bool operator < (const BodyID &inRHS) const + { + return mID < inRHS.mID; + } + + /// Greater than operator, can be used for sorting bodies + inline bool operator > (const BodyID &inRHS) const + { + return mID > inRHS.mID; + } + +private: + uint32 mID; +}; + +JPH_NAMESPACE_END + +// Create a std::hash/JPH::Hash for BodyID +JPH_MAKE_HASHABLE(JPH::BodyID, t.GetIndexAndSequenceNumber()) diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp new file mode 100644 index 000000000000..cc5f27de4191 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp @@ -0,0 +1,1034 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void BodyInterface::ActivateBodyInternal(Body &ioBody) const +{ + // Activate body or reset its sleep timer. + // Note that BodyManager::ActivateBodies also resets the sleep timer internally, but we avoid a mutex lock if the body is already active by calling ResetSleepTimer directly. + if (!ioBody.IsActive()) + mBodyManager->ActivateBodies(&ioBody.GetID(), 1); + else + ioBody.ResetSleepTimer(); +} + +Body *BodyInterface::CreateBody(const BodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateBody(inSettings); + if (!mBodyManager->AddBody(body)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateSoftBody(const SoftBodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateSoftBody(inSettings); + if (!mBodyManager->AddBody(body)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateBody(inSettings); + if (!mBodyManager->AddBodyWithCustomID(body, inBodyID)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateSoftBody(inSettings); + if (!mBodyManager->AddBodyWithCustomID(body, inBodyID)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateBodyWithoutID(const BodyCreationSettings &inSettings) const +{ + return mBodyManager->AllocateBody(inSettings); +} + +Body *BodyInterface::CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const +{ + return mBodyManager->AllocateSoftBody(inSettings); +} + +void BodyInterface::DestroyBodyWithoutID(Body *inBody) const +{ + mBodyManager->FreeBody(inBody); +} + +bool BodyInterface::AssignBodyID(Body *ioBody) +{ + return mBodyManager->AddBody(ioBody); +} + +bool BodyInterface::AssignBodyID(Body *ioBody, const BodyID &inBodyID) +{ + return mBodyManager->AddBodyWithCustomID(ioBody, inBodyID); +} + +Body *BodyInterface::UnassignBodyID(const BodyID &inBodyID) +{ + Body *body = nullptr; + mBodyManager->RemoveBodies(&inBodyID, 1, &body); + return body; +} + +void BodyInterface::UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies) +{ + mBodyManager->RemoveBodies(inBodyIDs, inNumber, outBodies); +} + +void BodyInterface::DestroyBody(const BodyID &inBodyID) +{ + mBodyManager->DestroyBodies(&inBodyID, 1); +} + +void BodyInterface::DestroyBodies(const BodyID *inBodyIDs, int inNumber) +{ + mBodyManager->DestroyBodies(inBodyIDs, inNumber); +} + +void BodyInterface::AddBody(const BodyID &inBodyID, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Add to broadphase + BodyID id = inBodyID; + BroadPhase::AddState add_state = mBroadPhase->AddBodiesPrepare(&id, 1); + mBroadPhase->AddBodiesFinalize(&id, 1, add_state); + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::RemoveBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Deactivate body + if (body.IsActive()) + mBodyManager->DeactivateBodies(&inBodyID, 1); + + // Remove from broadphase + BodyID id = inBodyID; + mBroadPhase->RemoveBodies(&id, 1); + } +} + +bool BodyInterface::IsAdded(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + return lock.SucceededAndIsInBroadPhase(); +} + +BodyID BodyInterface::CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode) +{ + const Body *b = CreateBody(inSettings); + if (b == nullptr) + return BodyID(); // Out of bodies + AddBody(b->GetID(), inActivationMode); + return b->GetID(); +} + +BodyID BodyInterface::CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode) +{ + const Body *b = CreateSoftBody(inSettings); + if (b == nullptr) + return BodyID(); // Out of bodies + AddBody(b->GetID(), inActivationMode); + return b->GetID(); +} + +BodyInterface::AddState BodyInterface::AddBodiesPrepare(BodyID *ioBodies, int inNumber) +{ + return mBroadPhase->AddBodiesPrepare(ioBodies, inNumber); +} + +void BodyInterface::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, ioBodies, inNumber); + + // Add to broadphase + mBroadPhase->AddBodiesFinalize(ioBodies, inNumber, inAddState); + + // Optionally activate bodies + if (inActivationMode == EActivation::Activate) + mBodyManager->ActivateBodies(ioBodies, inNumber); +} + +void BodyInterface::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + mBroadPhase->AddBodiesAbort(ioBodies, inNumber, inAddState); +} + +void BodyInterface::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, ioBodies, inNumber); + + // Deactivate bodies + mBodyManager->DeactivateBodies(ioBodies, inNumber); + + // Remove from broadphase + mBroadPhase->RemoveBodies(ioBodies, inNumber); +} + +void BodyInterface::ActivateBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + ActivateBodyInternal(body); + } +} + +void BodyInterface::ActivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, inBodyIDs, inNumber); + + mBodyManager->ActivateBodies(inBodyIDs, inNumber); +} + +void BodyInterface::ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) +{ + AllHitCollisionCollector collector; + mBroadPhase->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); + ActivateBodies(collector.mHits.data(), (int)collector.mHits.size()); +} + +void BodyInterface::DeactivateBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + if (body.IsActive()) + mBodyManager->DeactivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::DeactivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, inBodyIDs, inNumber); + + mBodyManager->DeactivateBodies(inBodyIDs, inNumber); +} + +bool BodyInterface::IsActive(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + return lock.Succeeded() && lock.GetBody().IsActive(); +} + +void BodyInterface::ResetSleepTimer(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().ResetSleepTimer(); +} + +TwoBodyConstraint *BodyInterface::CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2) +{ + BodyID constraint_bodies[] = { inBodyID1, inBodyID2 }; + BodyLockMultiWrite lock(*mBodyLockInterface, constraint_bodies, 2); + + Body *body1 = lock.GetBody(0); + Body *body2 = lock.GetBody(1); + + JPH_ASSERT(body1 != body2); + JPH_ASSERT(body1 != nullptr || body2 != nullptr); + + return inSettings->Create(body1 != nullptr? *body1 : Body::sFixedToWorld, body2 != nullptr? *body2 : Body::sFixedToWorld); +} + +void BodyInterface::ActivateConstraint(const TwoBodyConstraint *inConstraint) +{ + BodyID bodies[] = { inConstraint->GetBody1()->GetID(), inConstraint->GetBody2()->GetID() }; + ActivateBodies(bodies, 2); +} + +RefConst BodyInterface::GetShape(const BodyID &inBodyID) const +{ + RefConst shape; + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + shape = lock.GetBody().GetShape(); + return shape; +} + +void BodyInterface::SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if shape actually changed + if (body.GetShape() != inShape) + { + // Update the shape + body.SetShapeInternal(inShape, inUpdateMassProperties); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update center of mass, mass and inertia + body.UpdateCenterOfMassInternal(inPreviousCenterOfMass, inUpdateMassProperties); + + // Recalculate bounding box + body.CalculateWorldSpaceBoundsInternal(); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +void BodyInterface::SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if layer actually changed, updating the broadphase is rather expensive + if (body.GetObjectLayer() != inLayer) + { + // Update the layer on the body + mBodyManager->SetBodyObjectLayerInternal(body, inLayer); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesLayerChanged(&id, 1); + } + } + } +} + +ObjectLayer BodyInterface::GetObjectLayer(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetObjectLayer(); + else + return cObjectLayerInvalid; +} + +void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if there is enough change + if (!body.GetPosition().IsClose(inPosition) + || !body.GetRotation().IsClose(inRotation)) + { + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + outPosition = body.GetPosition(); + outRotation = body.GetRotation(); + } + else + { + outPosition = RVec3::sZero(); + outRotation = Quat::sIdentity(); + } +} + +void BodyInterface::SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, body.GetRotation()); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +RVec3 BodyInterface::GetPosition(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetPosition(); + else + return RVec3::sZero(); +} + +RVec3 BodyInterface::GetCenterOfMassPosition(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetCenterOfMassPosition(); + else + return RVec3::sZero(); +} + +void BodyInterface::SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(body.GetPosition(), inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +Quat BodyInterface::GetRotation(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetRotation(); + else + return Quat::sIdentity(); +} + +RMat44 BodyInterface::GetWorldTransform(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetWorldTransform(); + else + return RMat44::sIdentity(); +} + +RMat44 BodyInterface::GetCenterOfMassTransform(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetCenterOfMassTransform(); + else + return RMat44::sIdentity(); +} + +void BodyInterface::MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + body.MoveKinematic(inTargetPosition, inTargetRotation, inDeltaTime); + + if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + body.SetAngularVelocityClamped(inAngularVelocity); + + if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + outLinearVelocity = body.GetLinearVelocity(); + outAngularVelocity = body.GetAngularVelocity(); + return; + } + } + + outLinearVelocity = outAngularVelocity = Vec3::sZero(); +} + +void BodyInterface::SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + + if (!body.IsActive() && !inLinearVelocity.IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +Vec3 BodyInterface::GetLinearVelocity(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetLinearVelocity(); + } + + return Vec3::sZero(); +} + +void BodyInterface::AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity); + + if (!body.IsActive() && !body.GetLinearVelocity().IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity); + body.SetAngularVelocityClamped(body.GetAngularVelocity() + inAngularVelocity); + + if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetAngularVelocityClamped(inAngularVelocity); + + if (!body.IsActive() && !inAngularVelocity.IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +Vec3 BodyInterface::GetAngularVelocity(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetAngularVelocity(); + } + + return Vec3::sZero(); +} + +Vec3 BodyInterface::GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetPointVelocity(inPoint); + } + + return Vec3::sZero(); +} + +void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce, inPoint); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddTorque(inTorque); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce); + body.AddTorque(inTorque); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddImpulse(inImpulse); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddImpulse(inImpulse, inPoint); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddAngularImpulse(inAngularImpulse); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +bool BodyInterface::ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() + && body.ApplyBuoyancyImpulse(inSurfacePosition, inSurfaceNormal, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime)) + { + ActivateBodyInternal(body); + return true; + } + } + + return false; +} + +void BodyInterface::SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + body.SetAngularVelocityClamped(inAngularVelocity); + + // Optionally activate body + if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Deactivate if we're making the body static + if (body.IsActive() && inMotionType == EMotionType::Static) + mBodyManager->DeactivateBodies(&inBodyID, 1); + + body.SetMotionType(inMotionType); + + // Activate body if requested + if (inMotionType != EMotionType::Static && inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } +} + +EBodyType BodyInterface::GetBodyType(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetBodyType(); + else + return EBodyType::RigidBody; +} + +EMotionType BodyInterface::GetMotionType(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetMotionType(); + else + return EMotionType::Static; +} + +void BodyInterface::SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + mBodyManager->SetMotionQuality(lock.GetBody(), inMotionQuality); +} + +EMotionQuality BodyInterface::GetMotionQuality(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && !lock.GetBody().IsStatic()) + return lock.GetBody().GetMotionProperties()->GetMotionQuality(); + else + return EMotionQuality::Discrete; +} + +Mat44 BodyInterface::GetInverseInertia(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetInverseInertia(); + else + return Mat44::sIdentity(); +} + +void BodyInterface::SetRestitution(const BodyID &inBodyID, float inRestitution) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetRestitution(inRestitution); +} + +float BodyInterface::GetRestitution(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetRestitution(); + else + return 0.0f; +} + +void BodyInterface::SetFriction(const BodyID &inBodyID, float inFriction) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetFriction(inFriction); +} + +float BodyInterface::GetFriction(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetFriction(); + else + return 0.0f; +} + +void BodyInterface::SetGravityFactor(const BodyID &inBodyID, float inGravityFactor) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + lock.GetBody().GetMotionPropertiesUnchecked()->SetGravityFactor(inGravityFactor); +} + +float BodyInterface::GetGravityFactor(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + return lock.GetBody().GetMotionPropertiesUnchecked()->GetGravityFactor(); + else + return 1.0f; +} + +void BodyInterface::SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.GetUseManifoldReduction() != inUseReduction) + { + body.SetUseManifoldReduction(inUseReduction); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + } + } +} + +bool BodyInterface::GetUseManifoldReduction(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetUseManifoldReduction(); + else + return true; +} + +TransformedShape BodyInterface::GetTransformedShape(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetTransformedShape(); + else + return TransformedShape(); +} + +uint64 BodyInterface::GetUserData(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetUserData(); + else + return 0; +} + +void BodyInterface::SetUserData(const BodyID &inBodyID, uint64 inUserData) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetUserData(inUserData); +} + +const PhysicsMaterial *BodyInterface::GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetShape()->GetMaterial(inSubShapeID); + else + return PhysicsMaterial::sDefault; +} + +void BodyInterface::InvalidateContactCache(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + mBodyManager->InvalidateContactCacheForBody(lock.GetBody()); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h new file mode 100644 index 000000000000..477b3564e404 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h @@ -0,0 +1,298 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class BodyCreationSettings; +class SoftBodyCreationSettings; +class BodyLockInterface; +class BroadPhase; +class BodyManager; +class TransformedShape; +class PhysicsMaterial; +class SubShapeID; +class Shape; +class TwoBodyConstraintSettings; +class TwoBodyConstraint; +class BroadPhaseLayerFilter; +class AABox; + +/// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations. +/// All quantities are in world space unless otherwise specified. +class JPH_EXPORT BodyInterface : public NonCopyable +{ +public: + /// Initialize the interface (should only be called by PhysicsSystem) + void Init(BodyLockInterface &inBodyLockInterface, BodyManager &inBodyManager, BroadPhase &inBroadPhase) { mBodyLockInterface = &inBodyLockInterface; mBodyManager = &inBodyManager; mBroadPhase = &inBroadPhase; } + + /// Create a rigid body + /// @return Created body or null when out of bodies + Body * CreateBody(const BodyCreationSettings &inSettings); + + /// Create a soft body + /// @return Created body or null when out of bodies + Body * CreateSoftBody(const SoftBodyCreationSettings &inSettings); + + /// Create a rigid body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly. + /// The ID created on the server can be replicated to the client and used to create a deterministic simulation. + /// @return Created body or null when the body ID is invalid or a body of the same ID already exists. + Body * CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings); + + /// Create a soft body with specified ID. See comments at CreateBodyWithID. + Body * CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings); + + /// Advanced use only. Creates a rigid body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID. + /// This can be used to decouple allocation from registering the body. A call to CreateBodyWithoutID followed by AssignBodyID is equivalent to calling CreateBodyWithID. + /// @return Created body + Body * CreateBodyWithoutID(const BodyCreationSettings &inSettings) const; + + /// Advanced use only. Creates a body without specifying an ID. See comments at CreateBodyWithoutID. + Body * CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const; + + /// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function, + /// or a body that has had its body ID unassigned through UnassignBodyIDs. Bodies that have an ID should be destroyed through DestroyBody. + void DestroyBodyWithoutID(Body *inBody) const; + + /// Advanced use only. Assigns the next available body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system. + /// @return false if the body already has an ID or out of body ids. + bool AssignBodyID(Body *ioBody); + + /// Advanced use only. Assigns a body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system. + /// @return false if the body already has an ID or if the ID is not valid. + bool AssignBodyID(Body *ioBody, const BodyID &inBodyID); + + /// Advanced use only. See UnassignBodyIDs. Unassigns the ID of a single body. + Body * UnassignBodyID(const BodyID &inBodyID); + + /// Advanced use only. Removes a number of body IDs from their bodies and returns the body pointers. Before calling this, the body should have been removed from the physics system. + /// The body can be destroyed through DestroyBodyWithoutID. This can be used to decouple deallocation. A call to UnassignBodyIDs followed by calls to DestroyBodyWithoutID is equivalent to calling DestroyBodies. + /// @param inBodyIDs A list of body IDs + /// @param inNumber Number of bodies in the list + /// @param outBodies If not null on input, this will contain a list of body pointers corresponding to inBodyIDs that can be destroyed afterwards (caller assumes ownership over these). + void UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies); + + /// Destroy a body. + /// Make sure that you remove the body from the physics system using BodyInterface::RemoveBody before calling this function. + void DestroyBody(const BodyID &inBodyID); + + /// Destroy multiple bodies + /// Make sure that you remove the bodies from the physics system using BodyInterface::RemoveBody before calling this function. + void DestroyBodies(const BodyID *inBodyIDs, int inNumber); + + /// Add body to the physics system. + /// Note that if you need to add multiple bodies, use the AddBodiesPrepare/AddBodiesFinalize function. + /// Adding many bodies, one at a time, results in a really inefficient broadphase until PhysicsSystem::OptimizeBroadPhase is called or when PhysicsSystem::Update rebuilds the tree! + /// After adding, to get a body by ID use the BodyLockRead or BodyLockWrite interface! + void AddBody(const BodyID &inBodyID, EActivation inActivationMode); + + /// Remove body from the physics system. + void RemoveBody(const BodyID &inBodyID); + + /// Check if a body has been added to the physics system. + bool IsAdded(const BodyID &inBodyID) const; + + /// Combines CreateBody and AddBody + /// @return Created body ID or an invalid ID when out of bodies + BodyID CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode); + + /// Combines CreateSoftBody and AddBody + /// @return Created body ID or an invalid ID when out of bodies + BodyID CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode); + + /// Add state handle, used to keep track of a batch of bodies while adding them to the PhysicsSystem. + using AddState = void *; + + ///@name Batch adding interface + ///@{ + + /// Prepare adding inNumber bodies at ioBodies to the PhysicsSystem, returns a handle that should be used in AddBodiesFinalize/Abort. + /// This can be done on a background thread without influencing the PhysicsSystem. + /// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called. + AddState AddBodiesPrepare(BodyID *ioBodies, int inNumber); + + /// Finalize adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode); + + /// Abort adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState. + /// This can be done on a background thread without influencing the PhysicsSystem. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + void AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState); + + /// Remove inNumber bodies in ioBodies from the PhysicsSystem. + /// ioBodies may be shuffled around by this function. + void RemoveBodies(BodyID *ioBodies, int inNumber); + ///@} + + ///@name Activate / deactivate a body + ///@{ + void ActivateBody(const BodyID &inBodyID); + void ActivateBodies(const BodyID *inBodyIDs, int inNumber); + void ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter); + void DeactivateBody(const BodyID &inBodyID); + void DeactivateBodies(const BodyID *inBodyIDs, int inNumber); + bool IsActive(const BodyID &inBodyID) const; + void ResetSleepTimer(const BodyID &inBodyID); + ///@} + + /// Create a two body constraint + TwoBodyConstraint * CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2); + + /// Activate non-static bodies attached to a constraint + void ActivateConstraint(const TwoBodyConstraint *inConstraint); + + ///@name Access to the shape of a body + ///@{ + + /// Get the current shape + RefConst GetShape(const BodyID &inBodyID) const; + + /// Set a new shape on the body + /// @param inBodyID Body ID of body that had its shape changed + /// @param inShape The new shape + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + /// @param inActivationMode Whether or not to activate the body + void SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const; + + /// Notify all systems to indicate that a shape has changed (usable for MutableCompoundShapes) + /// @param inBodyID Body ID of body that had its shape changed + /// @param inPreviousCenterOfMass Center of mass of the shape before the alterations + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + /// @param inActivationMode Whether or not to activate the body + void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const; + ///@} + + ///@name Object layer of a body + ///@{ + void SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer); + ObjectLayer GetObjectLayer(const BodyID &inBodyID) const; + ///@} + + ///@name Position and rotation of a body + ///@{ + void SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); + void SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); ///< Will only update the position/rotation and activate the body when the difference is larger than a very small number. This avoids updating the broadphase/waking up a body when the resulting position/orientation doesn't really change. + void GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const; + void SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode); + RVec3 GetPosition(const BodyID &inBodyID) const; + RVec3 GetCenterOfMassPosition(const BodyID &inBodyID) const; + void SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode); + Quat GetRotation(const BodyID &inBodyID) const; + RMat44 GetWorldTransform(const BodyID &inBodyID) const; + RMat44 GetCenterOfMassTransform(const BodyID &inBodyID) const; + ///@} + + /// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds (will activate body if needed) + void MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime); + + /// Linear or angular velocity (functions will activate body if needed). + /// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$ + void SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); + void GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const; + void SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); + Vec3 GetLinearVelocity(const BodyID &inBodyID) const; + void AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); ///< Add velocity to current velocity + void AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); ///< Add linear and angular to current velocities + void SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity); + Vec3 GetAngularVelocity(const BodyID &inBodyID) const; + Vec3 GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const; ///< Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body + + /// Set the complete motion state of a body. + /// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$ + void SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); + + ///@name Add forces to the body + ///@{ + void AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddForce + void AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode = EActivation::Activate); ///< Applied at inPoint + void AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddTorque + void AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< A combination of Body::AddForce and Body::AddTorque + ///@} + + ///@name Add an impulse to the body + ///@{ + void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass + void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at inPoint + void AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse); + bool ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); + ///@} + + ///@name Body type + ///@{ + EBodyType GetBodyType(const BodyID &inBodyID) const; + ///@} + + ///@name Body motion type + ///@{ + void SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode); + EMotionType GetMotionType(const BodyID &inBodyID) const; + ///@} + + ///@name Body motion quality + ///@{ + void SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality); + EMotionQuality GetMotionQuality(const BodyID &inBodyID) const; + ///@} + + /// Get inverse inertia tensor in world space + Mat44 GetInverseInertia(const BodyID &inBodyID) const; + + ///@name Restitution + ///@{ + void SetRestitution(const BodyID &inBodyID, float inRestitution); + float GetRestitution(const BodyID &inBodyID) const; + ///@} + + ///@name Friction + ///@{ + void SetFriction(const BodyID &inBodyID, float inFriction); + float GetFriction(const BodyID &inBodyID) const; + ///@} + + ///@name Gravity factor + ///@{ + void SetGravityFactor(const BodyID &inBodyID, float inGravityFactor); + float GetGravityFactor(const BodyID &inBodyID) const; + ///@} + + ///@name Manifold reduction + ///@{ + void SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction); + bool GetUseManifoldReduction(const BodyID &inBodyID) const; + ///@} + + /// Get transform and shape for this body, used to perform collision detection + TransformedShape GetTransformedShape(const BodyID &inBodyID) const; + + /// Get the user data for a body + uint64 GetUserData(const BodyID &inBodyID) const; + void SetUserData(const BodyID &inBodyID, uint64 inUserData) const; + + /// Get the material for a particular sub shape + const PhysicsMaterial * GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const; + + /// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step. + void InvalidateContactCache(const BodyID &inBodyID); + +private: + /// Helper function to activate a single body + JPH_INLINE void ActivateBodyInternal(Body &ioBody) const; + + BodyLockInterface * mBodyLockInterface = nullptr; + BodyManager * mBodyManager = nullptr; + BroadPhase * mBroadPhase = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h new file mode 100644 index 000000000000..eccfec4d1e8e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Base class for locking bodies for the duration of the scope of this class (do not use directly) +template +class BodyLockBase : public NonCopyable +{ +public: + /// Constructor will lock the body + BodyLockBase(const BodyLockInterface &inBodyLockInterface, const BodyID &inBodyID) : + mBodyLockInterface(inBodyLockInterface) + { + if (inBodyID == BodyID()) + { + // Invalid body id + mBodyLockMutex = nullptr; + mBody = nullptr; + } + else + { + // Get mutex + mBodyLockMutex = Write? inBodyLockInterface.LockWrite(inBodyID) : inBodyLockInterface.LockRead(inBodyID); + + // Get a reference to the body or nullptr when it is no longer valid + mBody = inBodyLockInterface.TryGetBody(inBodyID); + } + } + + /// Explicitly release the lock (normally this is done in the destructor) + inline void ReleaseLock() + { + if (mBodyLockMutex != nullptr) + { + if (Write) + mBodyLockInterface.UnlockWrite(mBodyLockMutex); + else + mBodyLockInterface.UnlockRead(mBodyLockMutex); + + mBodyLockMutex = nullptr; + mBody = nullptr; + } + } + + /// Destructor will unlock the body + ~BodyLockBase() + { + ReleaseLock(); + } + + /// Test if the lock was successful (if the body ID was valid) + inline bool Succeeded() const + { + return mBody != nullptr; + } + + /// Test if the lock was successful (if the body ID was valid) and the body is still in the broad phase + inline bool SucceededAndIsInBroadPhase() const + { + return mBody != nullptr && mBody->IsInBroadPhase(); + } + + /// Access the body + inline BodyType & GetBody() const + { + JPH_ASSERT(mBody != nullptr, "Should check Succeeded() first"); + return *mBody; + } + +private: + const BodyLockInterface & mBodyLockInterface; + SharedMutex * mBodyLockMutex; + BodyType * mBody; +}; + +/// A body lock takes a body ID and locks the underlying body so that other threads cannot access its members +/// +/// The common usage pattern is: +/// +/// BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken +/// BodyID body_id = ...; // Obtain ID to body +/// +/// // Scoped lock +/// { +/// BodyLockRead lock(lock_interface, body_id); +/// if (lock.Succeeded()) // body_id may no longer be valid +/// { +/// const Body &body = lock.GetBody(); +/// +/// // Do something with body +/// ... +/// } +/// } +class BodyLockRead : public BodyLockBase +{ + using BodyLockBase::BodyLockBase; +}; + +/// Specialization that locks a body for writing to. @see BodyLockRead for usage patterns. +class BodyLockWrite : public BodyLockBase +{ + using BodyLockBase::BodyLockBase; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h new file mode 100644 index 000000000000..b65951fc8ccd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h @@ -0,0 +1,134 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Base class interface for locking a body. Usually you will use BodyLockRead / BodyLockWrite / BodyLockMultiRead / BodyLockMultiWrite instead. +class BodyLockInterface : public NonCopyable +{ +public: + /// Redefine MutexMask + using MutexMask = BodyManager::MutexMask; + + /// Constructor + explicit BodyLockInterface(BodyManager &inBodyManager) : mBodyManager(inBodyManager) { } + virtual ~BodyLockInterface() = default; + + ///@name Locking functions + ///@{ + virtual SharedMutex * LockRead(const BodyID &inBodyID) const = 0; + virtual void UnlockRead(SharedMutex *inMutex) const = 0; + virtual SharedMutex * LockWrite(const BodyID &inBodyID) const = 0; + virtual void UnlockWrite(SharedMutex *inMutex) const = 0; + ///@} + + /// Get the mask needed to lock all bodies + inline MutexMask GetAllBodiesMutexMask() const + { + return mBodyManager.GetAllBodiesMutexMask(); + } + + ///@name Batch locking functions + ///@{ + virtual MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const = 0; + virtual void LockRead(MutexMask inMutexMask) const = 0; + virtual void UnlockRead(MutexMask inMutexMask) const = 0; + virtual void LockWrite(MutexMask inMutexMask) const = 0; + virtual void UnlockWrite(MutexMask inMutexMask) const = 0; + ///@} + + /// Convert body ID to body + inline Body * TryGetBody(const BodyID &inBodyID) const { return mBodyManager.TryGetBody(inBodyID); } + +protected: + BodyManager & mBodyManager; +}; + +/// Implementation that performs no locking (assumes the lock has already been taken) +class BodyLockInterfaceNoLock final : public BodyLockInterface +{ +public: + using BodyLockInterface::BodyLockInterface; + + ///@name Locking functions + virtual SharedMutex * LockRead([[maybe_unused]] const BodyID &inBodyID) const override { return nullptr; } + virtual void UnlockRead([[maybe_unused]] SharedMutex *inMutex) const override { /* Nothing to do */ } + virtual SharedMutex * LockWrite([[maybe_unused]] const BodyID &inBodyID) const override { return nullptr; } + virtual void UnlockWrite([[maybe_unused]] SharedMutex *inMutex) const override { /* Nothing to do */ } + + ///@name Batch locking functions + virtual MutexMask GetMutexMask([[maybe_unused]] const BodyID *inBodies, [[maybe_unused]] int inNumber) const override { return 0; } + virtual void LockRead([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void UnlockRead([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void LockWrite([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void UnlockWrite([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } +}; + +/// Implementation that uses the body manager to lock the correct mutex for a body +class BodyLockInterfaceLocking final : public BodyLockInterface +{ +public: + using BodyLockInterface::BodyLockInterface; + + ///@name Locking functions + virtual SharedMutex * LockRead(const BodyID &inBodyID) const override + { + SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID); + PhysicsLock::sLockShared(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + return &mutex; + } + + virtual void UnlockRead(SharedMutex *inMutex) const override + { + PhysicsLock::sUnlockShared(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + } + + virtual SharedMutex * LockWrite(const BodyID &inBodyID) const override + { + SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID); + PhysicsLock::sLock(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + return &mutex; + } + + virtual void UnlockWrite(SharedMutex *inMutex) const override + { + PhysicsLock::sUnlock(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + } + + ///@name Batch locking functions + virtual MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const override + { + return mBodyManager.GetMutexMask(inBodies, inNumber); + } + + virtual void LockRead(MutexMask inMutexMask) const override + { + mBodyManager.LockRead(inMutexMask); + } + + virtual void UnlockRead(MutexMask inMutexMask) const override + { + mBodyManager.UnlockRead(inMutexMask); + } + + virtual void LockWrite(MutexMask inMutexMask) const override + { + mBodyManager.LockWrite(inMutexMask); + } + + virtual void UnlockWrite(MutexMask inMutexMask) const override + { + mBodyManager.UnlockWrite(inMutexMask); + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h new file mode 100644 index 000000000000..5872729c029c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h @@ -0,0 +1,104 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Base class for locking multiple bodies for the duration of the scope of this class (do not use directly) +template +class BodyLockMultiBase : public NonCopyable +{ +public: + /// Redefine MutexMask + using MutexMask = BodyLockInterface::MutexMask; + + /// Constructor will lock the bodies + BodyLockMultiBase(const BodyLockInterface &inBodyLockInterface, const BodyID *inBodyIDs, int inNumber) : + mBodyLockInterface(inBodyLockInterface), + mMutexMask(inBodyLockInterface.GetMutexMask(inBodyIDs, inNumber)), + mBodyIDs(inBodyIDs), + mNumBodyIDs(inNumber) + { + if (mMutexMask != 0) + { + // Get mutex + if (Write) + inBodyLockInterface.LockWrite(mMutexMask); + else + inBodyLockInterface.LockRead(mMutexMask); + } + } + + /// Destructor will unlock the bodies + ~BodyLockMultiBase() + { + if (mMutexMask != 0) + { + if (Write) + mBodyLockInterface.UnlockWrite(mMutexMask); + else + mBodyLockInterface.UnlockRead(mMutexMask); + } + } + + /// Access the body (returns null if body was not properly locked) + inline BodyType * GetBody(int inBodyIndex) const + { + // Range check + JPH_ASSERT(inBodyIndex >= 0 && inBodyIndex < mNumBodyIDs); + + // Get body ID + const BodyID &body_id = mBodyIDs[inBodyIndex]; + if (body_id.IsInvalid()) + return nullptr; + + // Get a reference to the body or nullptr when it is no longer valid + return mBodyLockInterface.TryGetBody(body_id); + } + +private: + const BodyLockInterface & mBodyLockInterface; + MutexMask mMutexMask; + const BodyID * mBodyIDs; + int mNumBodyIDs; +}; + +/// A multi body lock takes a number of body IDs and locks the underlying bodies so that other threads cannot access its members +/// +/// The common usage pattern is: +/// +/// BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken +/// const BodyID *body_id = ...; // Obtain IDs to bodies +/// int num_body_ids = ...; +/// +/// // Scoped lock +/// { +/// BodyLockMultiRead lock(lock_interface, body_ids, num_body_ids); +/// for (int i = 0; i < num_body_ids; ++i) +/// { +/// const Body *body = lock.GetBody(i); +/// if (body != nullptr) +/// { +/// const Body &body = lock.Body(); +/// +/// // Do something with body +/// ... +/// } +/// } +/// } +class BodyLockMultiRead : public BodyLockMultiBase +{ + using BodyLockMultiBase::BodyLockMultiBase; +}; + +/// Specialization that locks multiple bodies for writing to. @see BodyLockMultiRead for usage patterns. +class BodyLockMultiWrite : public BodyLockMultiBase +{ + using BodyLockMultiBase::BodyLockMultiBase; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp new file mode 100644 index 000000000000..a48e4b0f82fa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp @@ -0,0 +1,1156 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_ENABLE_ASSERTS + static thread_local bool sOverrideAllowActivation = false; + static thread_local bool sOverrideAllowDeactivation = false; + + bool BodyManager::sGetOverrideAllowActivation() + { + return sOverrideAllowActivation; + } + + void BodyManager::sSetOverrideAllowActivation(bool inValue) + { + sOverrideAllowActivation = inValue; + } + + bool BodyManager::sGetOverrideAllowDeactivation() + { + return sOverrideAllowDeactivation; + } + + void BodyManager::sSetOverrideAllowDeactivation(bool inValue) + { + sOverrideAllowDeactivation = inValue; + } +#endif + +// Helper class that combines a body and its motion properties +class BodyWithMotionProperties : public Body +{ +public: + JPH_OVERRIDE_NEW_DELETE + + MotionProperties mMotionProperties; +}; + +// Helper class that combines a soft body its motion properties and shape +class SoftBodyWithMotionPropertiesAndShape : public Body +{ +public: + SoftBodyWithMotionPropertiesAndShape() + { + mShape.SetEmbedded(); + } + + SoftBodyMotionProperties mMotionProperties; + SoftBodyShape mShape; +}; + +inline void BodyManager::sDeleteBody(Body *inBody) +{ + if (inBody->mMotionProperties != nullptr) + { + JPH_IF_ENABLE_ASSERTS(inBody->mMotionProperties = nullptr;) + if (inBody->IsSoftBody()) + { + inBody->mShape = nullptr; // Release the shape to avoid assertion on shape destruction because of embedded object with refcount > 0 + delete static_cast(inBody); + } + else + delete static_cast(inBody); + } + else + delete inBody; +} + +BodyManager::~BodyManager() +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Destroy any bodies that are still alive + for (Body *b : mBodies) + if (sIsValidBodyPointer(b)) + sDeleteBody(b); + + for (BodyID *active_bodies : mActiveBodies) + delete [] active_bodies; +} + +void BodyManager::Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface) +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Num body mutexes must be a power of two and not bigger than our MutexMask + uint num_body_mutexes = Clamp(GetNextPowerOf2(inNumBodyMutexes == 0? 2 * thread::hardware_concurrency() : inNumBodyMutexes), 1, sizeof(MutexMask) * 8); +#ifdef JPH_TSAN_ENABLED + num_body_mutexes = min(num_body_mutexes, 32U); // TSAN errors out when locking too many mutexes on the same thread, see: https://github.com/google/sanitizers/issues/950 +#endif + + // Allocate the body mutexes + mBodyMutexes.Init(num_body_mutexes); + + // Allocate space for bodies + mBodies.reserve(inMaxBodies); + + // Allocate space for active bodies + for (BodyID *&active_bodies : mActiveBodies) + { + JPH_ASSERT(active_bodies == nullptr); + active_bodies = new BodyID [inMaxBodies]; + } + + // Allocate space for sequence numbers + mBodySequenceNumbers.resize(inMaxBodies, 0); + + // Keep layer interface + mBroadPhaseLayerInterface = &inLayerInterface; +} + +uint BodyManager::GetNumBodies() const +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + return mNumBodies; +} + +BodyManager::BodyStats BodyManager::GetBodyStats() const +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + BodyStats stats; + stats.mNumBodies = mNumBodies; + stats.mMaxBodies = uint(mBodies.capacity()); + + for (const Body *body : mBodies) + if (sIsValidBodyPointer(body)) + { + if (body->IsSoftBody()) + { + stats.mNumSoftBodies++; + if (body->IsActive()) + stats.mNumActiveSoftBodies++; + } + else + { + switch (body->GetMotionType()) + { + case EMotionType::Static: + stats.mNumBodiesStatic++; + break; + + case EMotionType::Dynamic: + stats.mNumBodiesDynamic++; + if (body->IsActive()) + stats.mNumActiveBodiesDynamic++; + break; + + case EMotionType::Kinematic: + stats.mNumBodiesKinematic++; + if (body->IsActive()) + stats.mNumActiveBodiesKinematic++; + break; + } + } + } + + return stats; +} + +Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const +{ + // Fill in basic properties + Body *body; + if (inBodyCreationSettings.HasMassProperties()) + { + BodyWithMotionProperties *bmp = new BodyWithMotionProperties; + body = bmp; + body->mMotionProperties = &bmp->mMotionProperties; + } + else + { + body = new Body; + } + body->mBodyType = EBodyType::RigidBody; + body->mShape = inBodyCreationSettings.GetShape(); + body->mUserData = inBodyCreationSettings.mUserData; + body->SetFriction(inBodyCreationSettings.mFriction); + body->SetRestitution(inBodyCreationSettings.mRestitution); + body->mMotionType = inBodyCreationSettings.mMotionType; + if (inBodyCreationSettings.mIsSensor) + body->SetIsSensor(true); + if (inBodyCreationSettings.mCollideKinematicVsNonDynamic) + body->SetCollideKinematicVsNonDynamic(true); + if (inBodyCreationSettings.mUseManifoldReduction) + body->SetUseManifoldReduction(true); + if (inBodyCreationSettings.mApplyGyroscopicForce) + body->SetApplyGyroscopicForce(true); + if (inBodyCreationSettings.mEnhancedInternalEdgeRemoval) + body->SetEnhancedInternalEdgeRemoval(true); + SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer); + body->mObjectLayer = inBodyCreationSettings.mObjectLayer; + body->mCollisionGroup = inBodyCreationSettings.mCollisionGroup; + + if (inBodyCreationSettings.HasMassProperties()) + { + MotionProperties *mp = body->mMotionProperties; + mp->SetLinearDamping(inBodyCreationSettings.mLinearDamping); + mp->SetAngularDamping(inBodyCreationSettings.mAngularDamping); + mp->SetMaxLinearVelocity(inBodyCreationSettings.mMaxLinearVelocity); + mp->SetMaxAngularVelocity(inBodyCreationSettings.mMaxAngularVelocity); + mp->SetMassProperties(inBodyCreationSettings.mAllowedDOFs, inBodyCreationSettings.GetMassProperties()); + mp->SetLinearVelocity(inBodyCreationSettings.mLinearVelocity); // Needs to happen after setting the max linear/angular velocity and setting allowed DOFs + mp->SetAngularVelocity(inBodyCreationSettings.mAngularVelocity); + mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor); + mp->SetNumVelocityStepsOverride(inBodyCreationSettings.mNumVelocityStepsOverride); + mp->SetNumPositionStepsOverride(inBodyCreationSettings.mNumPositionStepsOverride); + mp->mMotionQuality = inBodyCreationSettings.mMotionQuality; + mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping; + JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;) + JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;) + } + + // Position body + body->SetPositionAndRotationInternal(inBodyCreationSettings.mPosition, inBodyCreationSettings.mRotation); + + return body; +} + +/// Create a soft body using creation settings. The returned body will not be part of the body manager yet. +Body *BodyManager::AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const +{ + // Fill in basic properties + SoftBodyWithMotionPropertiesAndShape *bmp = new SoftBodyWithMotionPropertiesAndShape; + SoftBodyMotionProperties *mp = &bmp->mMotionProperties; + SoftBodyShape *shape = &bmp->mShape; + Body *body = bmp; + shape->mSoftBodyMotionProperties = mp; + body->mBodyType = EBodyType::SoftBody; + body->mMotionProperties = mp; + body->mShape = shape; + body->mUserData = inSoftBodyCreationSettings.mUserData; + body->SetFriction(inSoftBodyCreationSettings.mFriction); + body->SetRestitution(inSoftBodyCreationSettings.mRestitution); + body->mMotionType = EMotionType::Dynamic; + SetBodyObjectLayerInternal(*body, inSoftBodyCreationSettings.mObjectLayer); + body->mObjectLayer = inSoftBodyCreationSettings.mObjectLayer; + body->mCollisionGroup = inSoftBodyCreationSettings.mCollisionGroup; + mp->SetLinearDamping(inSoftBodyCreationSettings.mLinearDamping); + mp->SetAngularDamping(0); + mp->SetMaxLinearVelocity(inSoftBodyCreationSettings.mMaxLinearVelocity); + mp->SetMaxAngularVelocity(FLT_MAX); + mp->SetLinearVelocity(Vec3::sZero()); + mp->SetAngularVelocity(Vec3::sZero()); + mp->SetGravityFactor(inSoftBodyCreationSettings.mGravityFactor); + mp->mMotionQuality = EMotionQuality::Discrete; + mp->mAllowSleeping = inSoftBodyCreationSettings.mAllowSleeping; + JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;) + JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;) + mp->Initialize(inSoftBodyCreationSettings); + + body->SetPositionAndRotationInternal(inSoftBodyCreationSettings.mPosition, inSoftBodyCreationSettings.mMakeRotationIdentity? Quat::sIdentity() : inSoftBodyCreationSettings.mRotation); + + return body; +} + +void BodyManager::FreeBody(Body *inBody) const +{ + JPH_ASSERT(inBody->GetID().IsInvalid(), "This function should only be called on a body that doesn't have an ID yet, use DestroyBody otherwise"); + + sDeleteBody(inBody); +} + +bool BodyManager::AddBody(Body *ioBody) +{ + // Return error when body was already added + if (!ioBody->GetID().IsInvalid()) + return false; + + // Determine next free index + uint32 idx; + { + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + if (mBodyIDFreeListStart != cBodyIDFreeListEnd) + { + // Pop an item from the freelist + JPH_ASSERT(mBodyIDFreeListStart & cIsFreedBody); + idx = uint32(mBodyIDFreeListStart >> cFreedBodyIndexShift); + JPH_ASSERT(!sIsValidBodyPointer(mBodies[idx])); + mBodyIDFreeListStart = uintptr_t(mBodies[idx]); + mBodies[idx] = ioBody; + } + else + { + if (mBodies.size() < mBodies.capacity()) + { + // Allocate a new entry, note that the array should not actually resize since we've reserved it at init time + idx = uint32(mBodies.size()); + mBodies.push_back(ioBody); + } + else + { + // Out of bodies + return false; + } + } + + // Update cached number of bodies + mNumBodies++; + } + + // Get next sequence number and assign the ID + uint8 seq_no = GetNextSequenceNumber(idx); + ioBody->mID = BodyID(idx, seq_no); + return true; +} + +bool BodyManager::AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID) +{ + // Return error when body was already added + if (!ioBody->GetID().IsInvalid()) + return false; + + { + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Check if index is beyond the max body ID + uint32 idx = inBodyID.GetIndex(); + if (idx >= mBodies.capacity()) + return false; // Return error + + if (idx < mBodies.size()) + { + // Body array entry has already been allocated, check if there's a free body here + if (sIsValidBodyPointer(mBodies[idx])) + return false; // Return error + + // Remove the entry from the freelist + uintptr_t idx_start = mBodyIDFreeListStart >> cFreedBodyIndexShift; + if (idx == idx_start) + { + // First entry, easy to remove, the start of the list is our next + mBodyIDFreeListStart = uintptr_t(mBodies[idx]); + } + else + { + // Loop over the freelist and find the entry in the freelist pointing to our index + // TODO: This is O(N), see if this becomes a performance problem (don't want to put the freed bodies in a double linked list) + uintptr_t cur, next; + for (cur = idx_start; cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift; cur = next) + { + next = uintptr_t(mBodies[cur]) >> cFreedBodyIndexShift; + if (next == idx) + { + mBodies[cur] = mBodies[idx]; + break; + } + } + JPH_ASSERT(cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift); + } + + // Put the body in the slot + mBodies[idx] = ioBody; + } + else + { + // Ensure that all body IDs up to this body ID have been allocated and added to the free list + while (idx > mBodies.size()) + { + // Push the id onto the freelist + mBodies.push_back((Body *)mBodyIDFreeListStart); + mBodyIDFreeListStart = (uintptr_t(mBodies.size() - 1) << cFreedBodyIndexShift) | cIsFreedBody; + } + + // Add the element to the list + mBodies.push_back(ioBody); + } + + // Update cached number of bodies + mNumBodies++; + } + + // Assign the ID + ioBody->mID = inBodyID; + return true; +} + +Body *BodyManager::RemoveBodyInternal(const BodyID &inBodyID) +{ + // Get body + uint32 idx = inBodyID.GetIndex(); + Body *body = mBodies[idx]; + + // Validate that it can be removed + JPH_ASSERT(body->GetID() == inBodyID); + JPH_ASSERT(!body->IsActive()); + JPH_ASSERT(!body->IsInBroadPhase(), "Use BodyInterface::RemoveBody to remove this body first!"); + + // Push the id onto the freelist + mBodies[idx] = (Body *)mBodyIDFreeListStart; + mBodyIDFreeListStart = (uintptr_t(idx) << cFreedBodyIndexShift) | cIsFreedBody; + + return body; +} + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + +void BodyManager::ValidateFreeList() const +{ + // Check that the freelist is correct + size_t num_freed = 0; + for (uintptr_t start = mBodyIDFreeListStart; start != cBodyIDFreeListEnd; start = uintptr_t(mBodies[start >> cFreedBodyIndexShift])) + { + JPH_ASSERT(start & cIsFreedBody); + num_freed++; + } + JPH_ASSERT(mNumBodies == mBodies.size() - num_freed); +} + +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) + +void BodyManager::RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies) +{ + // Don't take lock if no bodies are to be destroyed + if (inNumber <= 0) + return; + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Update cached number of bodies + JPH_ASSERT(mNumBodies >= (uint)inNumber); + mNumBodies -= inNumber; + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + { + // Remove body + Body *body = RemoveBodyInternal(*b); + + // Clear the ID + body->mID = BodyID(); + + // Return the body to the caller + if (outBodies != nullptr) + { + *outBodies = body; + ++outBodies; + } + } + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + ValidateFreeList(); +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) +} + +void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be destroyed + if (inNumber <= 0) + return; + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Update cached number of bodies + JPH_ASSERT(mNumBodies >= (uint)inNumber); + mNumBodies -= inNumber; + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + { + // Remove body + Body *body = RemoveBodyInternal(*b); + + // Free the body + sDeleteBody(body); + } + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + ValidateFreeList(); +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) +} + +void BodyManager::AddBodyToActiveBodies(Body &ioBody) +{ + // Select the correct array to use + int type = (int)ioBody.GetBodyType(); + atomic &num_active_bodies = mNumActiveBodies[type]; + BodyID *active_bodies = mActiveBodies[type]; + + MotionProperties *mp = ioBody.mMotionProperties; + uint32 num_active_bodies_val = num_active_bodies.load(memory_order_relaxed); + mp->mIndexInActiveBodies = num_active_bodies_val; + JPH_ASSERT(num_active_bodies_val < GetMaxBodies()); + active_bodies[num_active_bodies_val] = ioBody.GetID(); + num_active_bodies.fetch_add(1, memory_order_release); // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs + + // Count CCD bodies + if (mp->GetMotionQuality() == EMotionQuality::LinearCast) + mNumActiveCCDBodies++; +} + +void BodyManager::RemoveBodyFromActiveBodies(Body &ioBody) +{ + // Select the correct array to use + int type = (int)ioBody.GetBodyType(); + atomic &num_active_bodies = mNumActiveBodies[type]; + BodyID *active_bodies = mActiveBodies[type]; + + uint32 last_body_index = num_active_bodies.load(memory_order_relaxed) - 1; + MotionProperties *mp = ioBody.mMotionProperties; + if (mp->mIndexInActiveBodies != last_body_index) + { + // This is not the last body, use the last body to fill the hole + BodyID last_body_id = active_bodies[last_body_index]; + active_bodies[mp->mIndexInActiveBodies] = last_body_id; + + // Update that body's index in the active list + Body &last_body = *mBodies[last_body_id.GetIndex()]; + JPH_ASSERT(last_body.mMotionProperties->mIndexInActiveBodies == last_body_index); + last_body.mMotionProperties->mIndexInActiveBodies = mp->mIndexInActiveBodies; + } + + // Mark this body as no longer active + mp->mIndexInActiveBodies = Body::cInactiveIndex; + + // Remove unused element from active bodies list + num_active_bodies.fetch_sub(1, memory_order_release); + + // Count CCD bodies + if (mp->GetMotionQuality() == EMotionQuality::LinearCast) + mNumActiveCCDBodies--; +} + +void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be activated + if (inNumber <= 0) + return; + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation); + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + if (!b->IsInvalid()) + { + BodyID body_id = *b; + Body &body = *mBodies[body_id.GetIndex()]; + + JPH_ASSERT(body.GetID() == body_id); + JPH_ASSERT(body.IsInBroadPhase(), "Use BodyInterface::AddBody to add the body first!"); + + if (!body.IsStatic()) + { + // Reset sleeping timer so that we don't immediately go to sleep again + body.ResetSleepTimer(); + + // Check if we're sleeping + if (body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex) + { + AddBodyToActiveBodies(body); + + // Call activation listener + if (mActivationListener != nullptr) + mActivationListener->OnBodyActivated(body_id, body.GetUserData()); + } + } + } +} + +void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be deactivated + if (inNumber <= 0) + return; + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowDeactivation); + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + if (!b->IsInvalid()) + { + BodyID body_id = *b; + Body &body = *mBodies[body_id.GetIndex()]; + + JPH_ASSERT(body.GetID() == body_id); + JPH_ASSERT(body.IsInBroadPhase(), "Use BodyInterface::AddBody to add the body first!"); + + if (body.mMotionProperties != nullptr + && body.mMotionProperties->mIndexInActiveBodies != Body::cInactiveIndex) + { + // Remove the body from the active bodies list + RemoveBodyFromActiveBodies(body); + + // Mark this body as no longer active + body.mMotionProperties->mIslandIndex = Body::cInactiveIndex; + + // Reset velocity + body.mMotionProperties->mLinearVelocity = Vec3::sZero(); + body.mMotionProperties->mAngularVelocity = Vec3::sZero(); + + // Call activation listener + if (mActivationListener != nullptr) + mActivationListener->OnBodyDeactivated(body_id, body.GetUserData()); + } + } +} + +void BodyManager::SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality) +{ + MotionProperties *mp = ioBody.GetMotionPropertiesUnchecked(); + if (mp != nullptr && mp->GetMotionQuality() != inMotionQuality) + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked); + + bool is_active = ioBody.IsActive(); + if (is_active && mp->GetMotionQuality() == EMotionQuality::LinearCast) + --mNumActiveCCDBodies; + + mp->mMotionQuality = inMotionQuality; + + if (is_active && mp->GetMotionQuality() == EMotionQuality::LinearCast) + ++mNumActiveCCDBodies; + } +} + +void BodyManager::GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + const BodyID *active_bodies = mActiveBodies[(int)inType]; + outBodyIDs.assign(active_bodies, active_bodies + mNumActiveBodies[(int)inType].load(memory_order_relaxed)); +} + +void BodyManager::GetBodyIDs(BodyIDVector &outBodies) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Reserve space for all bodies + outBodies.clear(); + outBodies.reserve(mNumBodies); + + // Iterate the list and find the bodies that are not null + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b)) + outBodies.push_back(b->GetID()); + + // Validate that our reservation was correct + JPH_ASSERT(outBodies.size() == mNumBodies); +} + +void BodyManager::SetBodyActivationListener(BodyActivationListener *inListener) +{ + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + mActivationListener = inListener; +} + +BodyManager::MutexMask BodyManager::GetMutexMask(const BodyID *inBodies, int inNumber) const +{ + JPH_ASSERT(sizeof(MutexMask) * 8 >= mBodyMutexes.GetNumMutexes(), "MutexMask must have enough bits"); + + if (inNumber >= (int)mBodyMutexes.GetNumMutexes()) + { + // Just lock everything if there are too many bodies + return GetAllBodiesMutexMask(); + } + else + { + MutexMask mask = 0; + for (const BodyID *b = inBodies, *b_end = inBodies + inNumber; b < b_end; ++b) + if (!b->IsInvalid()) + { + uint32 index = mBodyMutexes.GetMutexIndex(b->GetIndex()); + mask |= (MutexMask(1) << index); + } + return mask; + } +} + +void BodyManager::LockRead(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).lock_shared(); +} + +void BodyManager::UnlockRead(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).unlock_shared(); +} + +void BodyManager::LockWrite(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).lock(); +} + +void BodyManager::UnlockWrite(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).unlock(); +} + +void BodyManager::LockAllBodies() const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + mBodyMutexes.LockAll(); + + PhysicsLock::sLock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); +} + +void BodyManager::UnlockAllBodies() const +{ + PhysicsLock::sUnlock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + mBodyMutexes.UnlockAll(); +} + +void BodyManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + { + LockAllBodies(); + + // Determine which bodies to save + Array bodies; + bodies.reserve(mNumBodies); + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase() && (inFilter == nullptr || inFilter->ShouldSaveBody(*b))) + bodies.push_back(b); + + // Write state of bodies + uint32 num_bodies = (uint32)bodies.size(); + inStream.Write(num_bodies); + for (const Body *b : bodies) + { + inStream.Write(b->GetID()); + inStream.Write(b->IsActive()); + b->SaveState(inStream); + } + + UnlockAllBodies(); + } +} + +bool BodyManager::RestoreState(StateRecorder &inStream) +{ + BodyIDVector bodies_to_activate, bodies_to_deactivate; + + { + LockAllBodies(); + + if (inStream.IsValidating()) + { + // Read state of bodies, note this reads it in a way to be consistent with validation + uint32 old_num_bodies = 0; + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase()) + ++old_num_bodies; + uint32 num_bodies = old_num_bodies; // Initialize to current value for validation + inStream.Read(num_bodies); + if (num_bodies != old_num_bodies) + { + JPH_ASSERT(false, "Cannot handle adding/removing bodies"); + UnlockAllBodies(); + return false; + } + + for (Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase()) + { + BodyID body_id = b->GetID(); // Initialize to current value for validation + inStream.Read(body_id); + if (body_id != b->GetID()) + { + JPH_ASSERT(false, "Cannot handle adding/removing bodies"); + UnlockAllBodies(); + return false; + } + bool is_active = b->IsActive(); // Initialize to current value for validation + inStream.Read(is_active); + if (is_active != b->IsActive()) + { + if (is_active) + bodies_to_activate.push_back(body_id); + else + bodies_to_deactivate.push_back(body_id); + } + b->RestoreState(inStream); + } + } + else + { + // Not validating, we can be a bit more loose, read number of bodies + uint32 num_bodies = 0; + inStream.Read(num_bodies); + + // Iterate over the stored bodies and restore their state + for (uint32 idx = 0; idx < num_bodies; ++idx) + { + BodyID body_id; + inStream.Read(body_id); + Body *b = TryGetBody(body_id); + if (b == nullptr) + { + JPH_ASSERT(false, "Restoring state for non-existing body"); + UnlockAllBodies(); + return false; + } + bool is_active; + inStream.Read(is_active); + if (is_active != b->IsActive()) + { + if (is_active) + bodies_to_activate.push_back(body_id); + else + bodies_to_deactivate.push_back(body_id); + } + b->RestoreState(inStream); + } + } + + UnlockAllBodies(); + } + + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + for (BodyID body_id : bodies_to_activate) + { + Body *body = TryGetBody(body_id); + AddBodyToActiveBodies(*body); + } + + for (BodyID body_id : bodies_to_deactivate) + { + Body *body = TryGetBody(body_id); + RemoveBodyFromActiveBodies(*body); + } + } + + return true; +} + +void BodyManager::SaveBodyState(const Body &inBody, StateRecorder &inStream) const +{ + inStream.Write(inBody.IsActive()); + + inBody.SaveState(inStream); +} + +void BodyManager::RestoreBodyState(Body &ioBody, StateRecorder &inStream) +{ + bool is_active = ioBody.IsActive(); + inStream.Read(is_active); + + ioBody.RestoreState(inStream); + + if (is_active != ioBody.IsActive()) + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation); + + if (is_active) + AddBodyToActiveBodies(ioBody); + else + RemoveBodyFromActiveBodies(ioBody); + } +} + +#ifdef JPH_DEBUG_RENDERER +void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter) +{ + JPH_PROFILE_FUNCTION(); + + LockAllBodies(); + + for (const Body *body : mBodies) + if (sIsValidBodyPointer(body) && body->IsInBroadPhase() && (!inBodyFilter || inBodyFilter->ShouldDraw(*body))) + { + JPH_ASSERT(mBodies[body->GetID().GetIndex()] == body); + + bool is_sensor = body->IsSensor(); + + // Determine drawing mode + Color color; + if (is_sensor) + color = Color::sYellow; + else + switch (inDrawSettings.mDrawShapeColor) + { + case EShapeColor::InstanceColor: + // Each instance has own color + color = Color::sGetDistinctColor(body->mID.GetIndex()); + break; + + case EShapeColor::ShapeTypeColor: + color = ShapeFunctions::sGet(body->GetShape()->GetSubType()).mColor; + break; + + case EShapeColor::MotionTypeColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + color = Color::sGreen; + break; + + case EMotionType::Dynamic: + color = Color::sGetDistinctColor(body->mID.GetIndex()); + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::SleepColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + color = body->IsActive()? Color::sGreen : Color::sRed; + break; + + case EMotionType::Dynamic: + color = body->IsActive()? Color::sYellow : Color::sRed; + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::IslandColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + case EMotionType::Dynamic: + { + uint32 idx = body->GetMotionProperties()->GetIslandIndexInternal(); + color = idx != Body::cInactiveIndex? Color::sGetDistinctColor(idx) : Color::sLightGrey; + } + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::MaterialColor: + color = Color::sWhite; + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + + // Draw the results of GetSupportFunction + if (inDrawSettings.mDrawGetSupportFunction) + body->mShape->DrawGetSupportFunction(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawSupportDirection); + + // Draw the results of GetSupportingFace + if (inDrawSettings.mDrawGetSupportingFace) + body->mShape->DrawGetSupportingFace(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + + // Draw the shape + if (inDrawSettings.mDrawShape) + body->mShape->Draw(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawShapeColor == EShapeColor::MaterialColor, inDrawSettings.mDrawShapeWireframe || is_sensor); + + // Draw bounding box + if (inDrawSettings.mDrawBoundingBox) + inRenderer->DrawWireBox(body->mBounds, color); + + // Draw center of mass transform + if (inDrawSettings.mDrawCenterOfMassTransform) + inRenderer->DrawCoordinateSystem(body->GetCenterOfMassTransform(), 0.2f); + + // Draw world transform + if (inDrawSettings.mDrawWorldTransform) + inRenderer->DrawCoordinateSystem(body->GetWorldTransform(), 0.2f); + + // Draw world space linear and angular velocity + if (inDrawSettings.mDrawVelocity) + { + RVec3 pos = body->GetCenterOfMassPosition(); + inRenderer->DrawArrow(pos, pos + body->GetLinearVelocity(), Color::sGreen, 0.1f); + inRenderer->DrawArrow(pos, pos + body->GetAngularVelocity(), Color::sRed, 0.1f); + } + + if (inDrawSettings.mDrawMassAndInertia && body->IsDynamic()) + { + const MotionProperties *mp = body->GetMotionProperties(); + if (mp->GetInverseMass() > 0.0f + && !Vec3::sEquals(mp->GetInverseInertiaDiagonal(), Vec3::sZero()).TestAnyXYZTrue()) + { + // Invert mass again + float mass = 1.0f / mp->GetInverseMass(); + + // Invert diagonal again + Vec3 diagonal = mp->GetInverseInertiaDiagonal().Reciprocal(); + + // Determine how big of a box has the equivalent inertia + Vec3 box_size = MassProperties::sGetEquivalentSolidBoxSize(mass, diagonal); + + // Draw box with equivalent inertia + inRenderer->DrawWireBox(body->GetCenterOfMassTransform() * Mat44::sRotation(mp->GetInertiaRotation()), AABox(-0.5f * box_size, 0.5f * box_size), Color::sOrange); + + // Draw mass + inRenderer->DrawText3D(body->GetCenterOfMassPosition(), StringFormat("%.2f", (double)mass), Color::sOrange, 0.2f); + } + } + + if (inDrawSettings.mDrawSleepStats && body->IsDynamic() && body->IsActive()) + { + // Draw stats to know which bodies could go to sleep + String text = StringFormat("t: %.1f", (double)body->mMotionProperties->mSleepTestTimer); + uint8 g = uint8(Clamp(255.0f * body->mMotionProperties->mSleepTestTimer / inPhysicsSettings.mTimeBeforeSleep, 0.0f, 255.0f)); + Color sleep_color = Color(0, 255 - g, g); + inRenderer->DrawText3D(body->GetCenterOfMassPosition(), text, sleep_color, 0.2f); + for (int i = 0; i < 3; ++i) + inRenderer->DrawWireSphere(JPH_IF_DOUBLE_PRECISION(body->mMotionProperties->GetSleepTestOffset() +) body->mMotionProperties->mSleepTestSpheres[i].GetCenter(), body->mMotionProperties->mSleepTestSpheres[i].GetRadius(), sleep_color); + } + + if (body->IsSoftBody()) + { + const SoftBodyMotionProperties *mp = static_cast(body->GetMotionProperties()); + RMat44 com = body->GetCenterOfMassTransform(); + + if (inDrawSettings.mDrawSoftBodyVertices) + mp->DrawVertices(inRenderer, com); + + if (inDrawSettings.mDrawSoftBodyVertexVelocities) + mp->DrawVertexVelocities(inRenderer, com); + + if (inDrawSettings.mDrawSoftBodyEdgeConstraints) + mp->DrawEdgeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyBendConstraints) + mp->DrawBendConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyVolumeConstraints) + mp->DrawVolumeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodySkinConstraints) + mp->DrawSkinConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyLRAConstraints) + mp->DrawLRAConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyPredictedBounds) + mp->DrawPredictedBounds(inRenderer, com); + } + } + + UnlockAllBodies(); +} +#endif // JPH_DEBUG_RENDERER + +void BodyManager::InvalidateContactCacheForBody(Body &ioBody) +{ + // If this is the first time we flip the collision cache invalid flag, we need to add it to an internal list to ensure we reset the flag at the end of the physics update + if (ioBody.InvalidateContactCacheInternal()) + { + lock_guard lock(mBodiesCacheInvalidMutex); + mBodiesCacheInvalid.push_back(ioBody.GetID()); + } +} + +void BodyManager::ValidateContactCacheForAllBodies() +{ + lock_guard lock(mBodiesCacheInvalidMutex); + + for (const BodyID &b : mBodiesCacheInvalid) + { + // The body may have been removed between the call to InvalidateContactCacheForBody and this call, so check if it still exists + Body *body = TryGetBody(b); + if (body != nullptr) + body->ValidateContactCacheInternal(); + } + mBodiesCacheInvalid.clear(); +} + +#ifdef JPH_DEBUG +void BodyManager::ValidateActiveBodyBounds() +{ + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + for (uint type = 0; type < cBodyTypeCount; ++type) + for (BodyID *id = mActiveBodies[type], *id_end = mActiveBodies[type] + mNumActiveBodies[type].load(memory_order_relaxed); id < id_end; ++id) + { + const Body *body = mBodies[id->GetIndex()]; + AABox cached = body->GetWorldSpaceBounds(); + AABox calculated = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + JPH_ASSERT(cached == calculated); + } +} +#endif // JPH_DEBUG + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h new file mode 100644 index 000000000000..518574707a00 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h @@ -0,0 +1,377 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +// Classes +class BodyCreationSettings; +class SoftBodyCreationSettings; +class BodyActivationListener; +class StateRecorderFilter; +struct PhysicsSettings; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +class BodyDrawFilter; +#endif // JPH_DEBUG_RENDERER + +#ifdef JPH_DEBUG_RENDERER + +/// Defines how to color soft body constraints +enum class ESoftBodyConstraintColor +{ + ConstraintType, /// Draw different types of constraints in different colors + ConstraintGroup, /// Draw constraints in the same group in the same color, non-parallel group will be red + ConstraintOrder, /// Draw constraints in the same group in the same color, non-parallel group will be red, and order within each group will be indicated with gradient +}; + +#endif // JPH_DEBUG_RENDERER + +/// Array of bodies +using BodyVector = Array; + +/// Array of body ID's +using BodyIDVector = Array; + +/// Class that contains all bodies +class JPH_EXPORT BodyManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + ~BodyManager(); + + /// Initialize the manager + void Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface); + + /// Gets the current amount of bodies that are in the body manager + uint GetNumBodies() const; + + /// Gets the max bodies that we can support + uint GetMaxBodies() const { return uint(mBodies.capacity()); } + + /// Helper struct that counts the number of bodies of each type + struct BodyStats + { + uint mNumBodies = 0; ///< Total number of bodies in the body manager + uint mMaxBodies = 0; ///< Max allowed number of bodies in the body manager (as configured in Init(...)) + + uint mNumBodiesStatic = 0; ///< Number of static bodies + + uint mNumBodiesDynamic = 0; ///< Number of dynamic bodies + uint mNumActiveBodiesDynamic = 0; ///< Number of dynamic bodies that are currently active + + uint mNumBodiesKinematic = 0; ///< Number of kinematic bodies + uint mNumActiveBodiesKinematic = 0; ///< Number of kinematic bodies that are currently active + + uint mNumSoftBodies = 0; ///< Number of soft bodies + uint mNumActiveSoftBodies = 0; ///< Number of soft bodies that are currently active + }; + + /// Get stats about the bodies in the body manager (slow, iterates through all bodies) + BodyStats GetBodyStats() const; + + /// Create a body using creation settings. The returned body will not be part of the body manager yet. + Body * AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const; + + /// Create a soft body using creation settings. The returned body will not be part of the body manager yet. + Body * AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const; + + /// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies). + void FreeBody(Body *inBody) const; + + /// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available. + bool AddBody(Body *ioBody); + + /// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid. + bool AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID); + + /// Remove a list of bodies from the body manager + void RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies); + + /// Remove a set of bodies from the body manager and frees them. + void DestroyBodies(const BodyID *inBodyIDs, int inNumber); + + /// Activate a list of bodies. + /// This function should only be called when an exclusive lock for the bodies are held. + void ActivateBodies(const BodyID *inBodyIDs, int inNumber); + + /// Deactivate a list of bodies. + /// This function should only be called when an exclusive lock for the bodies are held. + void DeactivateBodies(const BodyID *inBodyIDs, int inNumber); + + /// Update the motion quality for a body + void SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality); + + /// Get copy of the list of active bodies under protection of a lock. + void GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const; + + /// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment. + const BodyID * GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[int(inType)]; } + + /// Get the number of active bodies. + uint32 GetNumActiveBodies(EBodyType inType) const { return mNumActiveBodies[int(inType)].load(memory_order_acquire); } + + /// Get the number of active bodies that are using continuous collision detection + uint32 GetNumActiveCCDBodies() const { return mNumActiveCCDBodies; } + + /// Listener that is notified whenever a body is activated/deactivated + void SetBodyActivationListener(BodyActivationListener *inListener); + BodyActivationListener * GetBodyActivationListener() const { return mActivationListener; } + + /// Check if this is a valid body pointer. When a body is freed the memory that the pointer occupies is reused to store a freelist. + static inline bool sIsValidBodyPointer(const Body *inBody) { return (uintptr_t(inBody) & cIsFreedBody) == 0; } + + /// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check. + const BodyVector & GetBodies() const { return mBodies; } + + /// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check. + BodyVector & GetBodies() { return mBodies; } + + /// Get all body IDs under the protection of a lock + void GetBodyIDs(BodyIDVector &outBodies) const; + + /// Access a body (not protected by lock) + const Body & GetBody(const BodyID &inID) const { return *mBodies[inID.GetIndex()]; } + + /// Access a body (not protected by lock) + Body & GetBody(const BodyID &inID) { return *mBodies[inID.GetIndex()]; } + + /// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock) + const Body * TryGetBody(const BodyID &inID) const + { + uint32 idx = inID.GetIndex(); + if (idx >= mBodies.size()) + return nullptr; + + const Body *body = mBodies[idx]; + if (sIsValidBodyPointer(body) && body->GetID() == inID) + return body; + + return nullptr; + } + + /// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock) + Body * TryGetBody(const BodyID &inID) + { + uint32 idx = inID.GetIndex(); + if (idx >= mBodies.size()) + return nullptr; + + Body *body = mBodies[idx]; + if (sIsValidBodyPointer(body) && body->GetID() == inID) + return body; + + return nullptr; + } + + /// Access the mutex for a single body + SharedMutex & GetMutexForBody(const BodyID &inID) const { return mBodyMutexes.GetMutexByObjectIndex(inID.GetIndex()); } + + /// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex. + using MutexMask = uint64; + + ///@name Batch body mutex access (do not use directly) + ///@{ + MutexMask GetAllBodiesMutexMask() const { return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; } + MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const; + void LockRead(MutexMask inMutexMask) const; + void UnlockRead(MutexMask inMutexMask) const; + void LockWrite(MutexMask inMutexMask) const; + void UnlockWrite(MutexMask inMutexMask) const; + ///@} + + /// Lock all bodies. This should only be done during PhysicsSystem::Update(). + void LockAllBodies() const; + + /// Unlock all bodies. This should only be done during PhysicsSystem::Update(). + void UnlockAllBodies() const; + + /// Function to update body's layer (should only be called by the BodyInterface since it also requires updating the broadphase) + inline void SetBodyObjectLayerInternal(Body &ioBody, ObjectLayer inLayer) const { ioBody.mObjectLayer = inLayer; ioBody.mBroadPhaseLayer = mBroadPhaseLayerInterface->GetBroadPhaseLayer(inLayer); } + + /// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step. + void InvalidateContactCacheForBody(Body &ioBody); + + /// Reset the Body::EFlags::InvalidateContactCache flag for all bodies. All contact pairs in the contact cache will now by valid again. + void ValidateContactCacheForAllBodies(); + + /// Saving state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restoring state for replay. Returns false if failed. + bool RestoreState(StateRecorder &inStream); + + /// Save the state of a single body for replay + void SaveBodyState(const Body &inBody, StateRecorder &inStream) const; + + /// Save the state of a single body for replay + void RestoreBodyState(Body &inBody, StateRecorder &inStream); + +#ifdef JPH_DEBUG_RENDERER + enum class EShapeColor + { + InstanceColor, ///< Random color per instance + ShapeTypeColor, ///< Convex = green, scaled = yellow, compound = orange, mesh = red + MotionTypeColor, ///< Static = grey, keyframed = green, dynamic = random color per instance + SleepColor, ///< Static = grey, keyframed = green, dynamic = yellow, sleeping = red + IslandColor, ///< Static = grey, active = random color per island, sleeping = light grey + MaterialColor, ///< Color as defined by the PhysicsMaterial of the shape + }; + + /// Draw settings + struct DrawSettings + { + bool mDrawGetSupportFunction = false; ///< Draw the GetSupport() function, used for convex collision detection + bool mDrawSupportDirection = false; ///< When drawing the support function, also draw which direction mapped to a specific support point + bool mDrawGetSupportingFace = false; ///< Draw the faces that were found colliding during collision detection + bool mDrawShape = true; ///< Draw the shapes of all bodies + bool mDrawShapeWireframe = false; ///< When mDrawShape is true and this is true, the shapes will be drawn in wireframe instead of solid. + EShapeColor mDrawShapeColor = EShapeColor::MotionTypeColor; ///< Coloring scheme to use for shapes + bool mDrawBoundingBox = false; ///< Draw a bounding box per body + bool mDrawCenterOfMassTransform = false; ///< Draw the center of mass for each body + bool mDrawWorldTransform = false; ///< Draw the world transform (which can be different than the center of mass) for each body + bool mDrawVelocity = false; ///< Draw the velocity vector for each body + bool mDrawMassAndInertia = false; ///< Draw the mass and inertia (as the box equivalent) for each body + bool mDrawSleepStats = false; ///< Draw stats regarding the sleeping algorithm of each body + bool mDrawSoftBodyVertices = false; ///< Draw the vertices of soft bodies + bool mDrawSoftBodyVertexVelocities = false; ///< Draw the velocities of the vertices of soft bodies + bool mDrawSoftBodyEdgeConstraints = false; ///< Draw the edge constraints of soft bodies + bool mDrawSoftBodyBendConstraints = false; ///< Draw the bend constraints of soft bodies + bool mDrawSoftBodyVolumeConstraints = false; ///< Draw the volume constraints of soft bodies + bool mDrawSoftBodySkinConstraints = false; ///< Draw the skin constraints of soft bodies + bool mDrawSoftBodyLRAConstraints = false; ///< Draw the LRA constraints of soft bodies + bool mDrawSoftBodyPredictedBounds = false; ///< Draw the predicted bounds of soft bodies + ESoftBodyConstraintColor mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints + }; + + /// Draw the state of the bodies (debugging purposes) + void Draw(const DrawSettings &inSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr); +#endif // JPH_DEBUG_RENDERER + +#ifdef JPH_ENABLE_ASSERTS + /// Lock the active body list, asserts when Activate/DeactivateBody is called. + void SetActiveBodiesLocked(bool inLocked) { mActiveBodiesLocked = inLocked; } + + /// Per thread override of the locked state, to be used by the PhysicsSystem only! + class GrantActiveBodiesAccess + { + public: + inline GrantActiveBodiesAccess(bool inAllowActivation, bool inAllowDeactivation) + { + JPH_ASSERT(!sGetOverrideAllowActivation()); + sSetOverrideAllowActivation(inAllowActivation); + + JPH_ASSERT(!sGetOverrideAllowDeactivation()); + sSetOverrideAllowDeactivation(inAllowDeactivation); + } + + inline ~GrantActiveBodiesAccess() + { + sSetOverrideAllowActivation(false); + sSetOverrideAllowDeactivation(false); + } + }; +#endif + +#ifdef JPH_DEBUG + /// Validate if the cached bounding boxes are correct for all active bodies + void ValidateActiveBodyBounds(); +#endif // JPH_DEBUG + +private: + /// Increment and get the sequence number of the body +#ifdef JPH_COMPILER_CLANG + __attribute__((no_sanitize("implicit-conversion"))) // We intentionally overflow the uint8 sequence number +#endif + inline uint8 GetNextSequenceNumber(int inBodyIndex) { return ++mBodySequenceNumbers[inBodyIndex]; } + + /// Add a single body to mActiveBodies, note doesn't lock the active body mutex! + inline void AddBodyToActiveBodies(Body &ioBody); + + /// Remove a single body from mActiveBodies, note doesn't lock the active body mutex! + inline void RemoveBodyFromActiveBodies(Body &ioBody); + + /// Helper function to remove a body from the manager + JPH_INLINE Body * RemoveBodyInternal(const BodyID &inBodyID); + + /// Helper function to delete a body (which could actually be a BodyWithMotionProperties) + inline static void sDeleteBody(Body *inBody); + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + /// Function to check that the free list is not corrupted + void ValidateFreeList() const; +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) + + /// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array. + BodyVector mBodies; + + /// Current number of allocated bodies + uint mNumBodies = 0; + + /// Indicates that there are no more freed body IDs + static constexpr uintptr_t cBodyIDFreeListEnd = ~uintptr_t(0); + + /// Bit that indicates a pointer in mBodies is actually the index of the next freed body. We use the lowest bit because we know that Bodies need to be 16 byte aligned so addresses can never end in a 1 bit. + static constexpr uintptr_t cIsFreedBody = uintptr_t(1); + + /// Amount of bits to shift to get an index to the next freed body + static constexpr uint cFreedBodyIndexShift = 1; + + /// Index of first entry in mBodies that is unused + uintptr_t mBodyIDFreeListStart = cBodyIDFreeListEnd; + + /// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart + mutable Mutex mBodiesMutex; + + /// An array of mutexes protecting the bodies in the mBodies array + using BodyMutexes = MutexArray; + mutable BodyMutexes mBodyMutexes; + + /// List of next sequence number for a body ID + Array mBodySequenceNumbers; + + /// Mutex that protects the mActiveBodies array + mutable Mutex mActiveBodiesMutex; + + /// List of all active dynamic bodies (size is equal to max amount of bodies) + BodyID * mActiveBodies[cBodyTypeCount] = { }; + + /// How many bodies there are in the list of active bodies + atomic mNumActiveBodies[cBodyTypeCount] = { }; + + /// How many of the active bodies have continuous collision detection enabled + uint32 mNumActiveCCDBodies = 0; + + /// Mutex that protects the mBodiesCacheInvalid array + mutable Mutex mBodiesCacheInvalidMutex; + + /// List of all bodies that should have their cache invalidated + BodyIDVector mBodiesCacheInvalid; + + /// Listener that is notified whenever a body is activated/deactivated + BodyActivationListener * mActivationListener = nullptr; + + /// Cached broadphase layer interface + const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr; + +#ifdef JPH_ENABLE_ASSERTS + static bool sGetOverrideAllowActivation(); + static void sSetOverrideAllowActivation(bool inValue); + + static bool sGetOverrideAllowDeactivation(); + static void sSetOverrideAllowDeactivation(bool inValue); + + /// Debug system that tries to limit changes to active bodies during the PhysicsSystem::Update() + bool mActiveBodiesLocked = false; +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h new file mode 100644 index 000000000000..8ac849a49a83 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a body pair +struct alignas(uint64) BodyPair +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + BodyPair() = default; + BodyPair(BodyID inA, BodyID inB) : mBodyA(inA), mBodyB(inB) { } + + /// Equals operator + bool operator == (const BodyPair &inRHS) const { return *reinterpret_cast(this) == *reinterpret_cast(&inRHS); } + + /// Smaller than operator, used for consistently ordering body pairs + bool operator < (const BodyPair &inRHS) const { return *reinterpret_cast(this) < *reinterpret_cast(&inRHS); } + + /// Get the hash value of this object + uint64 GetHash() const { return Hash64(*reinterpret_cast(this)); } + + BodyID mBodyA; + BodyID mBodyB; +}; + +static_assert(sizeof(BodyPair) == sizeof(uint64), "Mismatch in class size"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h new file mode 100644 index 000000000000..984af06ad23a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Type of body +enum class EBodyType : uint8 +{ + RigidBody, ///< Rigid body consisting of a rigid shape + SoftBody, ///< Soft body consisting of a deformable shape +}; + +/// How many types of bodies there are +static constexpr uint cBodyTypeCount = 2; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp new file mode 100644 index 000000000000..91df6b0df3a2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp @@ -0,0 +1,185 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MassProperties) +{ + JPH_ADD_ATTRIBUTE(MassProperties, mMass) + JPH_ADD_ATTRIBUTE(MassProperties, mInertia) +} + +bool MassProperties::DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const +{ + // Using eigendecomposition to get the principal components of the inertia tensor + // See: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix + Matrix<3, 3> inertia; + inertia.CopyPart(mInertia, 0, 0, 3, 3, 0, 0); + Matrix<3, 3> eigen_vec = Matrix<3, 3>::sIdentity(); + Vector<3> eigen_val; + if (!EigenValueSymmetric(inertia, eigen_vec, eigen_val)) + return false; + + // Sort so that the biggest value goes first + int indices[] = { 0, 1, 2 }; + InsertionSort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; }); + + // Convert to a regular Mat44 and Vec3 + outRotation = Mat44::sIdentity(); + for (int i = 0; i < 3; ++i) + { + outRotation.SetColumn3(i, Vec3(reinterpret_cast(eigen_vec.GetColumn(indices[i])))); + outDiagonal.SetComponent(i, eigen_val[indices[i]]); + } + + // Make sure that the rotation matrix is a right handed matrix + if (outRotation.GetAxisX().Cross(outRotation.GetAxisY()).Dot(outRotation.GetAxisZ()) < 0.0f) + outRotation.SetAxisZ(-outRotation.GetAxisZ()); + +#ifdef JPH_ENABLE_ASSERTS + // Validate that the solution is correct, for each axis we want to make sure that the difference in inertia is + // smaller than some fraction of the inertia itself in that axis + Mat44 new_inertia = outRotation * Mat44::sScale(outDiagonal) * outRotation.Inversed(); + for (int i = 0; i < 3; ++i) + JPH_ASSERT(new_inertia.GetColumn3(i).IsClose(mInertia.GetColumn3(i), mInertia.GetColumn3(i).LengthSq() * 1.0e-10f)); +#endif + + return true; +} + +void MassProperties::SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity) +{ + // Calculate mass + mMass = inBoxSize.GetX() * inBoxSize.GetY() * inBoxSize.GetZ() * inDensity; + + // Calculate inertia + Vec3 size_sq = inBoxSize * inBoxSize; + Vec3 scale = (size_sq.Swizzle() + size_sq.Swizzle()) * (mMass / 12.0f); + mInertia = Mat44::sScale(scale); +} + +void MassProperties::ScaleToMass(float inMass) +{ + if (mMass > 0.0f) + { + // Calculate how much we have to scale the inertia tensor + float mass_scale = inMass / mMass; + + // Update mass + mMass = inMass; + + // Update inertia tensor + for (int i = 0; i < 3; ++i) + mInertia.SetColumn4(i, mInertia.GetColumn4(i) * mass_scale); + } + else + { + // Just set the mass + mMass = inMass; + } +} + +Vec3 MassProperties::sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal) +{ + // Moment of inertia of a solid box has diagonal: + // mass / 12 * [size_y^2 + size_z^2, size_x^2 + size_z^2, size_x^2 + size_y^2] + // Solving for size_x, size_y and size_y (diagonal and mass are known): + Vec3 diagonal = inInertiaDiagonal * (12.0f / inMass); + return Vec3(sqrt(0.5f * (-diagonal[0] + diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] - diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] + diagonal[1] - diagonal[2]))); +} + +void MassProperties::Scale(Vec3Arg inScale) +{ + // See: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor + // The diagonal of the inertia tensor can be calculated like this: + // Ixx = sum_{k = 1 to n}(m_k * (y_k^2 + z_k^2)) + // Iyy = sum_{k = 1 to n}(m_k * (x_k^2 + z_k^2)) + // Izz = sum_{k = 1 to n}(m_k * (x_k^2 + y_k^2)) + // + // We want to isolate the terms x_k, y_k and z_k: + // d = [0.5, 0.5, 0.5].[Ixx, Iyy, Izz] + // [sum_{k = 1 to n}(m_k * x_k^2), sum_{k = 1 to n}(m_k * y_k^2), sum_{k = 1 to n}(m_k * z_k^2)] = [d, d, d] - [Ixx, Iyy, Izz] + Vec3 diagonal = mInertia.GetDiagonal3(); + Vec3 xyz_sq = Vec3::sReplicate(Vec3::sReplicate(0.5f).Dot(diagonal)) - diagonal; + + // When scaling a shape these terms change like this: + // sum_{k = 1 to n}(m_k * (scale_x * x_k)^2) = scale_x^2 * sum_{k = 1 to n}(m_k * x_k^2) + // Same for y_k and z_k + // Using these terms we can calculate the new diagonal of the inertia tensor: + Vec3 xyz_scaled_sq = inScale * inScale * xyz_sq; + float i_xx = xyz_scaled_sq.GetY() + xyz_scaled_sq.GetZ(); + float i_yy = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetZ(); + float i_zz = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetY(); + + // The off diagonal elements are calculated like: + // Ixy = -sum_{k = 1 to n}(x_k y_k) + // Ixz = -sum_{k = 1 to n}(x_k z_k) + // Iyz = -sum_{k = 1 to n}(y_k z_k) + // Scaling these is simple: + float i_xy = inScale.GetX() * inScale.GetY() * mInertia(0, 1); + float i_xz = inScale.GetX() * inScale.GetZ() * mInertia(0, 2); + float i_yz = inScale.GetY() * inScale.GetZ() * mInertia(1, 2); + + // Update inertia tensor + mInertia(0, 0) = i_xx; + mInertia(0, 1) = i_xy; + mInertia(1, 0) = i_xy; + mInertia(1, 1) = i_yy; + mInertia(0, 2) = i_xz; + mInertia(2, 0) = i_xz; + mInertia(1, 2) = i_yz; + mInertia(2, 1) = i_yz; + mInertia(2, 2) = i_zz; + + // Mass scales linear with volume (note that the scaling can be negative and we don't want the mass to become negative) + float mass_scale = abs(inScale.GetX() * inScale.GetY() * inScale.GetZ()); + mMass *= mass_scale; + + // Inertia scales linear with mass. This updates the m_k terms above. + mInertia *= mass_scale; + + // Ensure that the bottom right element is a 1 again + mInertia(3, 3) = 1.0f; +} + +void MassProperties::Rotate(Mat44Arg inRotation) +{ + mInertia = inRotation.Multiply3x3(mInertia).Multiply3x3RightTransposed(inRotation); +} + +void MassProperties::Translate(Vec3Arg inTranslation) +{ + // Transform the inertia using the parallel axis theorem: I' = I + m * (translation^2 E - translation translation^T) + // Where I is the original body's inertia and E the identity matrix + // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem + mInertia += mMass * (Mat44::sScale(inTranslation.Dot(inTranslation)) - Mat44::sOuterProduct(inTranslation, inTranslation)); + + // Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change + mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); +} + +void MassProperties::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMass); + inStream.Write(mInertia); +} + +void MassProperties::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMass); + inStream.Read(mInertia); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h new file mode 100644 index 000000000000..c2bfbffb2e10 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h @@ -0,0 +1,58 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Describes the mass and inertia properties of a body. Used during body construction only. +class JPH_EXPORT MassProperties +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MassProperties) + +public: + /// Using eigendecomposition, decompose the inertia tensor into a diagonal matrix D and a right-handed rotation matrix R so that the inertia tensor is \f$R \: D \: R^{-1}\f$. + /// @see https://en.wikipedia.org/wiki/Moment_of_inertia section 'Principal axes' + /// @param outRotation The rotation matrix R + /// @param outDiagonal The diagonal of the diagonal matrix D + /// @return True if successful, false if failed + bool DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const; + + /// Set the mass and inertia of a box with edge size inBoxSize and density inDensity + void SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity); + + /// Set the mass and scale the inertia tensor to match the mass + void ScaleToMass(float inMass); + + /// Calculates the size of the solid box that has an inertia tensor diagonal inInertiaDiagonal + static Vec3 sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal); + + /// Rotate the inertia by 3x3 matrix inRotation + void Rotate(Mat44Arg inRotation); + + /// Translate the inertia by a vector inTranslation + void Translate(Vec3Arg inTranslation); + + /// Scale the mass and inertia by inScale, note that elements can be < 0 to flip the shape + void Scale(Vec3Arg inScale); + + /// Saves the state of this object in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Mass of the shape (kg) + float mMass = 0.0f; + + /// Inertia tensor of the shape (kg m^2) + Mat44 mInertia = Mat44::sZero(); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp new file mode 100644 index 000000000000..96aba179e119 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp @@ -0,0 +1,92 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties) +{ + // Store allowed DOFs + mAllowedDOFs = inAllowedDOFs; + + // Decompose DOFs + uint allowed_translation_axis = uint(inAllowedDOFs) & 0b111; + uint allowed_rotation_axis = (uint(inAllowedDOFs) >> 3) & 0b111; + + // Set inverse mass + if (allowed_translation_axis == 0) + { + // No translation possible + mInvMass = 0.0f; + } + else + { + JPH_ASSERT(inMassProperties.mMass > 0.0f, "Invalid mass. " + "Some shapes like MeshShape or TriangleShape cannot calculate mass automatically, " + "in this case you need to provide it by setting BodyCreationSettings::mOverrideMassProperties and mMassPropertiesOverride."); + mInvMass = 1.0f / inMassProperties.mMass; + } + + if (allowed_rotation_axis == 0) + { + // No rotation possible + mInvInertiaDiagonal = Vec3::sZero(); + mInertiaRotation = Quat::sIdentity(); + } + else + { + // Set inverse inertia + Mat44 rotation; + Vec3 diagonal; + if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal) + && !diagonal.IsNearZero()) + { + mInvInertiaDiagonal = diagonal.Reciprocal(); + mInertiaRotation = rotation.GetQuaternion(); + } + else + { + // Failed! Fall back to inertia tensor of sphere with radius 1. + mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass); + mInertiaRotation = Quat::sIdentity(); + } + } + + JPH_ASSERT(mInvMass != 0.0f || mInvInertiaDiagonal != Vec3::sZero(), "Can't lock all axes, use a static body for this. This will crash with a division by zero later!"); +} + +void MotionProperties::SaveState(StateRecorder &inStream) const +{ + // Only write properties that can change at runtime + inStream.Write(mLinearVelocity); + inStream.Write(mAngularVelocity); + inStream.Write(mForce); + inStream.Write(mTorque); +#ifdef JPH_DOUBLE_PRECISION + inStream.Write(mSleepTestOffset); +#endif // JPH_DOUBLE_PRECISION + inStream.Write(mSleepTestSpheres); + inStream.Write(mSleepTestTimer); + inStream.Write(mAllowSleeping); +} + +void MotionProperties::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mLinearVelocity); + inStream.Read(mAngularVelocity); + inStream.Read(mForce); + inStream.Read(mTorque); +#ifdef JPH_DOUBLE_PRECISION + inStream.Read(mSleepTestOffset); +#endif // JPH_DOUBLE_PRECISION + inStream.Read(mSleepTestSpheres); + inStream.Read(mSleepTestTimer); + inStream.Read(mAllowSleeping); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h new file mode 100644 index 000000000000..d8a485b61129 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h @@ -0,0 +1,282 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StateRecorder; + +/// Enum that determines if an object can go to sleep +enum class ECanSleep +{ + CannotSleep = 0, ///< Object cannot go to sleep + CanSleep = 1, ///< Object can go to sleep +}; + +/// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body. +class JPH_EXPORT MotionProperties +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Motion quality, or how well it detects collisions when it has a high velocity + EMotionQuality GetMotionQuality() const { return mMotionQuality; } + + /// Get the allowed degrees of freedom that this body has (this can be changed by calling SetMassProperties) + inline EAllowedDOFs GetAllowedDOFs() const { return mAllowedDOFs; } + + /// If this body can go to sleep. + inline bool GetAllowSleeping() const { return mAllowSleeping; } + + /// Get world space linear velocity of the center of mass + inline Vec3 GetLinearVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mLinearVelocity; } + + /// Set world space linear velocity of the center of mass + void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = LockTranslation(inLinearVelocity); } + + /// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity + void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { mLinearVelocity = LockTranslation(inLinearVelocity); ClampLinearVelocity(); } + + /// Get world space angular velocity of the center of mass + inline Vec3 GetAngularVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mAngularVelocity; } + + /// Set world space angular velocity of the center of mass + void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); } + + /// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity + void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); } + + /// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds. + inline void MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime); + + ///@name Velocity limits + ///@{ + + /// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding. + inline float GetMaxLinearVelocity() const { return mMaxLinearVelocity; } + inline void SetMaxLinearVelocity(float inLinearVelocity) { JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; } + + /// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding. + inline float GetMaxAngularVelocity() const { return mMaxAngularVelocity; } + inline void SetMaxAngularVelocity(float inAngularVelocity) { JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; } + ///@} + + /// Clamp velocity according to limit + inline void ClampLinearVelocity(); + inline void ClampAngularVelocity(); + + /// Get linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + inline float GetLinearDamping() const { return mLinearDamping; } + void SetLinearDamping(float inLinearDamping) { JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; } + + /// Get angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + inline float GetAngularDamping() const { return mAngularDamping; } + void SetAngularDamping(float inAngularDamping) { JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; } + + /// Get gravity factor (1 = normal gravity, 0 = no gravity) + inline float GetGravityFactor() const { return mGravityFactor; } + void SetGravityFactor(float inGravityFactor) { mGravityFactor = inGravityFactor; } + + /// Set the mass and inertia tensor + void SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties); + + /// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0) + inline float GetInverseMass() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; } + inline float GetInverseMassUnchecked() const { return mInvMass; } + + /// Set the inverse mass (1 / mass). + /// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$). + /// If you change mass, inertia should probably change as well. You can use ScaleToMass to update mass and inertia at the same time. + /// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs). + void SetInverseMass(float inInverseMass) { mInvMass = inInverseMass; } + + /// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0) + inline Vec3 GetInverseInertiaDiagonal() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; } + + /// Rotation (R) that takes inverse inertia diagonal to local space: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$ + inline Quat GetInertiaRotation() const { return mInertiaRotation; } + + /// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$. + /// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$). + /// If you change inertia, mass should probably change as well. You can use ScaleToMass to update mass and inertia at the same time. + /// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs). + void SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot) { mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; } + + /// Sets the mass to inMass and scale the inertia tensor based on the ratio between the old and new mass. + /// Note that this only works when the current mass is finite (i.e. the body is dynamic and translational degrees of freedom are not restricted). + void ScaleToMass(float inMass); + + /// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object. + inline Mat44 GetLocalSpaceInverseInertia() const; + + /// Same as GetLocalSpaceInverseInertia() but doesn't check if the body is dynamic + inline Mat44 GetLocalSpaceInverseInertiaUnchecked() const; + + /// Get inverse inertia matrix (\f$I^{-1}\f$) for a given object rotation (translation will be ignored). Zero if object is static or kinematic. + inline Mat44 GetInverseInertiaForRotation(Mat44Arg inRotation) const; + + /// Multiply a vector with the inverse world space inertia tensor (\f$I_{world}^{-1}\f$). Zero if object is static or kinematic. + JPH_INLINE Vec3 MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const; + + /// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s) + JPH_INLINE Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); } + + // Get the total amount of force applied to the center of mass this time step (through Body::AddForce calls). Note that it will reset to zero after PhysicsSystem::Update. + JPH_INLINE Vec3 GetAccumulatedForce() const { return Vec3::sLoadFloat3Unsafe(mForce); } + + // Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update. + JPH_INLINE Vec3 GetAccumulatedTorque() const { return Vec3::sLoadFloat3Unsafe(mTorque); } + + // Reset the total accumulated force, note that this will be done automatically after every time step. + JPH_INLINE void ResetForce() { mForce = Float3(0, 0, 0); } + + // Reset the total accumulated torque, note that this will be done automatically after every time step. + JPH_INLINE void ResetTorque() { mTorque = Float3(0, 0, 0); } + + // Reset the current velocity and accumulated force and torque. + JPH_INLINE void ResetMotion() + { + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + mLinearVelocity = mAngularVelocity = Vec3::sZero(); + mForce = mTorque = Float3(0, 0, 0); + } + + /// Returns a vector where the linear components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff + JPH_INLINE UVec4 GetLinearDOFsMask() const + { + UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0); + return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask); + } + + /// Takes a translation vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0 + JPH_INLINE Vec3 LockTranslation(Vec3Arg inV) const + { + return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat())); + } + + /// Returns a vector where the angular components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff + JPH_INLINE UVec4 GetAngularDOFsMask() const + { + UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0); + return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask); + } + + /// Takes an angular velocity / torque vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0 + JPH_INLINE Vec3 LockAngular(Vec3Arg inV) const + { + return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat())); + } + + /// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumVelocityStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); } + uint GetNumVelocityStepsOverride() const { return mNumVelocityStepsOverride; } + + /// Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumPositionStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); } + uint GetNumPositionStepsOverride() const { return mNumPositionStepsOverride; } + + //////////////////////////////////////////////////////////// + // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY + //////////////////////////////////////////////////////////// + + ///@name Update linear and angular velocity (used during constraint solving) + ///@{ + inline void AddLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity + inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); } + inline void SubLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity - inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); } + inline void AddAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); } + inline void SubAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); } + ///@} + + /// Apply the gyroscopic force (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + inline void ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime); + + /// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem) + inline void ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime); + + /// Access to the island index + uint32 GetIslandIndexInternal() const { return mIslandIndex; } + void SetIslandIndexInternal(uint32 inIndex) { mIslandIndex = inIndex; } + + /// Access to the index in the active bodies array + uint32 GetIndexInActiveBodiesInternal() const { return mIndexInActiveBodies; } + +#ifdef JPH_DOUBLE_PRECISION + inline DVec3 GetSleepTestOffset() const { return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); } +#endif // JPH_DOUBLE_PRECISION + + /// Reset spheres to center around inPoints with radius 0 + inline void ResetSleepTestSpheres(const RVec3 *inPoints); + + /// Reset the sleep test timer without resetting the sleep test spheres + inline void ResetSleepTestTimer() { mSleepTestTimer = 0.0f; } + + /// Accumulate sleep time and return if a body can go to sleep + inline ECanSleep AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep); + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + static constexpr uint32 cInactiveIndex = uint32(-1); ///< Constant indicating that body is not active + +private: + friend class BodyManager; + friend class Body; + + // 1st cache line + // 16 byte aligned + Vec3 mLinearVelocity { Vec3::sZero() }; ///< World space linear velocity of the center of mass (m/s) + Vec3 mAngularVelocity { Vec3::sZero() }; ///< World space angular velocity (rad/s) + Vec3 mInvInertiaDiagonal; ///< Diagonal of inverse inertia matrix: D + Quat mInertiaRotation; ///< Rotation (R) that takes inverse inertia diagonal to local space: Ibody^-1 = R * D * R^-1 + + // 2nd cache line + // 4 byte aligned + Float3 mForce { 0, 0, 0 }; ///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable! + Float3 mTorque { 0, 0, 0 }; ///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable! + float mInvMass; ///< Inverse mass of the object (1/kg) + float mLinearDamping; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + float mAngularDamping; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + float mMaxLinearVelocity; ///< Maximum linear velocity that this body can reach (m/s) + float mMaxAngularVelocity; ///< Maximum angular velocity that this body can reach (rad/s) + float mGravityFactor; ///< Factor to multiply gravity with + uint32 mIndexInActiveBodies = cInactiveIndex; ///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active (note that there are 2 lists, one for rigid and one for soft bodies) + uint32 mIslandIndex = cInactiveIndex; ///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex + + // 1 byte aligned + EMotionQuality mMotionQuality; ///< Motion quality, or how well it detects collisions when it has a high velocity + bool mAllowSleeping; ///< If this body can go to sleep + EAllowedDOFs mAllowedDOFs = EAllowedDOFs::All; ///< Allowed degrees of freedom for this body + uint8 mNumVelocityStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumPositionStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + + // 3rd cache line (least frequently used) + // 4 byte aligned (or 8 byte if running in double precision) +#ifdef JPH_DOUBLE_PRECISION + Double3 mSleepTestOffset; ///< mSleepTestSpheres are relative to this offset to prevent floating point inaccuracies. Warning: Loaded using sLoadDouble3Unsafe which will read 8 extra bytes. +#endif // JPH_DOUBLE_PRECISION + Sphere mSleepTestSpheres[3]; ///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis + float mSleepTestTimer; ///< How long this body has been within the movement tolerance + +#ifdef JPH_ENABLE_ASSERTS + EBodyType mCachedBodyType; ///< Copied from Body::mBodyType and cached for asserting purposes + EMotionType mCachedMotionType; ///< Copied from Body::mMotionType and cached for asserting purposes +#endif +}; + +JPH_NAMESPACE_END + +#include "MotionProperties.inl" diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl new file mode 100644 index 000000000000..e9521f7444de --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl @@ -0,0 +1,178 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType != EMotionType::Static); + + // Calculate required linear velocity + mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime); + + // Calculate required angular velocity + Vec3 axis; + float angle; + inDeltaRotation.GetAxisAngle(axis, angle); + mAngularVelocity = LockAngular(axis * (angle / inDeltaTime)); +} + +void MotionProperties::ClampLinearVelocity() +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + + float len_sq = mLinearVelocity.LengthSq(); + JPH_ASSERT(isfinite(len_sq)); + if (len_sq > Square(mMaxLinearVelocity)) + mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq); +} + +void MotionProperties::ClampAngularVelocity() +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + + float len_sq = mAngularVelocity.LengthSq(); + JPH_ASSERT(isfinite(len_sq)); + if (len_sq > Square(mMaxAngularVelocity)) + mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq); +} + +inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const +{ + Mat44 rotation = Mat44::sRotation(mInertiaRotation); + Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1)); + return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed); +} + +inline void MotionProperties::ScaleToMass(float inMass) +{ + JPH_ASSERT(mInvMass > 0.0f, "Body must have finite mass"); + JPH_ASSERT(inMass > 0.0f, "New mass cannot be zero"); + + float new_inv_mass = 1.0f / inMass; + mInvInertiaDiagonal *= new_inv_mass / mInvMass; + mInvMass = new_inv_mass; +} + +inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + return GetLocalSpaceInverseInertiaUnchecked(); +} + +Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + Mat44 rotation = inRotation.Multiply3x3(Mat44::sRotation(mInertiaRotation)); + Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1)); + Mat44 inverse_inertia = rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed); + + // We need to mask out both the rows and columns of DOFs that are not allowed + Vec4 angular_dofs_mask = GetAngularDOFsMask().ReinterpretAsFloat(); + inverse_inertia.SetColumn4(0, Vec4::sAnd(inverse_inertia.GetColumn4(0), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatX()))); + inverse_inertia.SetColumn4(1, Vec4::sAnd(inverse_inertia.GetColumn4(1), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatY()))); + inverse_inertia.SetColumn4(2, Vec4::sAnd(inverse_inertia.GetColumn4(2), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatZ()))); + + return inverse_inertia; +} + +Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Mask out columns of DOFs that are not allowed + Vec3 angular_dofs_mask = Vec3(GetAngularDOFsMask().ReinterpretAsFloat()); + Vec3 v = Vec3::sAnd(inV, angular_dofs_mask); + + // Multiply vector by inverse inertia + Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation); + Vec3 result = rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(v)); + + // Mask out rows of DOFs that are not allowed + return Vec3::sAnd(result, angular_dofs_mask); +} + +void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Calculate local space inertia tensor (a diagonal in local space) + UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero()); + Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sReplicate(1.0f), is_zero); + Vec3 nominator = Vec3::sSelect(Vec3::sReplicate(1.0f), Vec3::sZero(), is_zero); + Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero + + // Calculate local space angular momentum + Quat inertia_space_to_world_space = inBodyRotation * mInertiaRotation; + Vec3 local_angular_velocity = inertia_space_to_world_space.Conjugated() * mAngularVelocity; + Vec3 local_momentum = local_inertia * local_angular_velocity; + + // The gyroscopic force applies a torque: T = -w x I w where w is angular velocity and I the inertia tensor + // Calculate the new angular momentum by applying the gyroscopic force and make sure the new magnitude is the same as the old one + // to avoid introducing energy into the system due to the Euler step + Vec3 new_local_momentum = local_momentum - inDeltaTime * local_angular_velocity.Cross(local_momentum); + float new_local_momentum_len_sq = new_local_momentum.LengthSq(); + new_local_momentum = new_local_momentum_len_sq > 0.0f? new_local_momentum * sqrt(local_momentum.LengthSq() / new_local_momentum_len_sq) : Vec3::sZero(); + + // Convert back to world space angular velocity + mAngularVelocity = inertia_space_to_world_space * (mInvInertiaDiagonal * new_local_momentum); +} + +void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Update linear velocity + mLinearVelocity = LockTranslation(mLinearVelocity + inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce())); + + // Update angular velocity + mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque()); + + // Linear damping: dv/dt = -c * v + // Solution: v(t) = v(0) * e^(-c * t) or v2 = v1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mLinearVelocity *= max(0.0f, 1.0f - mLinearDamping * inDeltaTime); + mAngularVelocity *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime); + + // Clamp velocities + ClampLinearVelocity(); + ClampAngularVelocity(); +} + +void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints) +{ +#ifdef JPH_DOUBLE_PRECISION + // Make spheres relative to the first point and initialize them to zero radius + DVec3 offset = inPoints[0]; + offset.StoreDouble3(&mSleepTestOffset); + mSleepTestSpheres[0] = Sphere(Vec3::sZero(), 0.0f); + for (int i = 1; i < 3; ++i) + mSleepTestSpheres[i] = Sphere(Vec3(inPoints[i] - offset), 0.0f); +#else + // Initialize the spheres to zero radius around the supplied points + for (int i = 0; i < 3; ++i) + mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f); +#endif + + mSleepTestTimer = 0.0f; +} + +ECanSleep MotionProperties::AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep) +{ + mSleepTestTimer += inDeltaTime; + return mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h new file mode 100644 index 000000000000..b1ba343c06ac --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Motion quality, or how well it detects collisions when it has a high velocity +enum class EMotionQuality : uint8 +{ + /// Update the body in discrete steps. Body will tunnel throuh thin objects if its velocity is high enough. + /// This is the cheapest way of simulating a body. + Discrete, + + /// Update the body using linear casting. When stepping the body, its collision shape is cast from + /// start to destination using the starting rotation. The body will not be able to tunnel through thin + /// objects at high velocity, but tunneling is still possible if the body is long and thin and has high + /// angular velocity. Time is stolen from the object (which means it will move up to the first collision + /// and will not bounce off the surface until the next integration step). This will make the body appear + /// to go slower when it collides with high velocity. In order to not get stuck, the body is always + /// allowed to move by a fraction of it's inner radius, which may eventually lead it to pass through geometry. + /// + /// Note that if you're using a collision listener, you can receive contact added/persisted notifications of contacts + /// that may in the end not happen. This happens between bodies that are using casting: If bodies A and B collide at t1 + /// and B and C collide at t2 where t2 < t1 and A and C don't collide. In this case you may receive an incorrect contact + /// point added callback between A and B (which will be removed the next frame). + LinearCast, +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h new file mode 100644 index 000000000000..6de0d8c8ec73 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Motion type of a physics body +enum class EMotionType : uint8 +{ + Static, ///< Non movable + Kinematic, ///< Movable using velocities only, does not respond to forces + Dynamic, ///< Responds to forces as a normal physics object +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp new file mode 100644 index 000000000000..14b2312870cd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp @@ -0,0 +1,323 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? static_cast(inSystem->GetBodyLockInterface()) : static_cast(inSystem->GetBodyLockInterfaceNoLock()); +} + +static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock(); +} + +static inline const NarrowPhaseQuery &sGetNarrowPhaseQuery(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetNarrowPhaseQuery() : inSystem->GetNarrowPhaseQueryNoLock(); +} + +Character::Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) : + CharacterBase(inSettings, inSystem), + mLayer(inSettings->mLayer) +{ + // Construct rigid body + BodyCreationSettings settings(mShape, inPosition, inRotation, EMotionType::Dynamic, mLayer); + settings.mAllowedDOFs = EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ; + settings.mEnhancedInternalEdgeRemoval = inSettings->mEnhancedInternalEdgeRemoval; + settings.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + settings.mMassPropertiesOverride.mMass = inSettings->mMass; + settings.mFriction = inSettings->mFriction; + settings.mGravityFactor = inSettings->mGravityFactor; + settings.mUserData = inUserData; + const Body *body = mSystem->GetBodyInterface().CreateBody(settings); + if (body != nullptr) + mBodyID = body->GetID(); +} + +Character::~Character() +{ + // Destroy the body + mSystem->GetBodyInterface().DestroyBody(mBodyID); +} + +void Character::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddBody(mBodyID, inActivationMode); +} + +void Character::RemoveFromPhysicsSystem(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).RemoveBody(mBodyID); +} + +void Character::Activate(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).ActivateBody(mBodyID); +} + +void Character::CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Create query broadphase layer filter + DefaultBroadPhaseLayerFilter broadphase_layer_filter = mSystem->GetDefaultBroadPhaseLayerFilter(mLayer); + + // Create query object layer filter + DefaultObjectLayerFilter object_layer_filter = mSystem->GetDefaultLayerFilter(mLayer); + + // Ignore my own body + IgnoreSingleBodyFilter body_filter(mBodyID); + + // Settings for collide shape + CollideShapeSettings settings; + settings.mMaxSeparationDistance = inMaxSeparationDistance; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + settings.mActiveEdgeMovementDirection = inMovementDirection; + settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces; + + sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sReplicate(1.0f), inCenterOfMassTransform, settings, inBaseOffset, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter); +} + +void Character::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Calculate center of mass transform + RMat44 center_of_mass = RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass()); + + CheckCollision(center_of_mass, inMovementDirection, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies); +} + +void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Determine position and velocity of body + RMat44 query_transform; + Vec3 velocity; + { + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID); + if (!lock.Succeeded()) + return; + + const Body &body = lock.GetBody(); + + // Correct the center of mass transform for the difference between the old and new center of mass shape + query_transform = body.GetCenterOfMassTransform().PreTranslated(inShape->GetCenterOfMass() - mShape->GetCenterOfMass()); + velocity = body.GetLinearVelocity(); + } + + CheckCollision(query_transform, velocity, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies); +} + +void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies) +{ + // Get character position, rotation and velocity + RVec3 char_pos; + Quat char_rot; + Vec3 char_vel; + { + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID); + if (!lock.Succeeded()) + return; + const Body &body = lock.GetBody(); + char_pos = body.GetPosition(); + char_rot = body.GetRotation(); + char_vel = body.GetLinearVelocity(); + } + + // Collector that finds the hit with the normal that is the most 'up' + class MyCollector : public CollideShapeCollector + { + public: + // Constructor + explicit MyCollector(Vec3Arg inUp, RVec3 inBaseOffset) : mBaseOffset(inBaseOffset), mUp(inUp) { } + + // See: CollectorType::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + Vec3 normal = -inResult.mPenetrationAxis.Normalized(); + float dot = normal.Dot(mUp); + if (dot > mBestDot) // Find the hit that is most aligned with the up vector + { + mGroundBodyID = inResult.mBodyID2; + mGroundBodySubShapeID = inResult.mSubShapeID2; + mGroundPosition = mBaseOffset + inResult.mContactPointOn2; + mGroundNormal = normal; + mBestDot = dot; + } + } + + BodyID mGroundBodyID; + SubShapeID mGroundBodySubShapeID; + RVec3 mGroundPosition = RVec3::sZero(); + Vec3 mGroundNormal = Vec3::sZero(); + + private: + RVec3 mBaseOffset; + Vec3 mUp; + float mBestDot = -FLT_MAX; + }; + + // Collide shape + MyCollector collector(mUp, char_pos); + CheckCollision(char_pos, char_rot, char_vel, inMaxSeparationDistance, mShape, char_pos, collector, inLockBodies); + + // Copy results + mGroundBodyID = collector.mGroundBodyID; + mGroundBodySubShapeID = collector.mGroundBodySubShapeID; + mGroundPosition = collector.mGroundPosition; + mGroundNormal = collector.mGroundNormal; + + // Get additional data from body + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mGroundBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Update ground state + RMat44 inv_transform = RMat44::sInverseRotationTranslation(char_rot, char_pos); + if (mSupportingVolume.SignedDistance(Vec3(inv_transform * mGroundPosition)) > 0.0f) + mGroundState = EGroundState::NotSupported; + else if (IsSlopeTooSteep(mGroundNormal)) + mGroundState = EGroundState::OnSteepGround; + else + mGroundState = EGroundState::OnGround; + + // Copy other body properties + mGroundMaterial = body.GetShape()->GetMaterial(mGroundBodySubShapeID); + mGroundVelocity = body.GetPointVelocity(mGroundPosition); + mGroundUserData = body.GetUserData(); + } + else + { + mGroundState = EGroundState::InAir; + mGroundMaterial = PhysicsMaterial::sDefault; + mGroundVelocity = Vec3::sZero(); + mGroundUserData = 0; + } +} + +void Character::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetLinearAndAngularVelocity(mBodyID, inLinearVelocity, inAngularVelocity); +} + +Vec3 Character::GetLinearVelocity(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetLinearVelocity(mBodyID); +} + +void Character::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetLinearVelocity(mBodyID, inLinearVelocity); +} + +void Character::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddLinearVelocity(mBodyID, inLinearVelocity); +} + +void Character::AddImpulse(Vec3Arg inImpulse, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddImpulse(mBodyID, inImpulse); +} + +void Character::GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const +{ + sGetBodyInterface(mSystem, inLockBodies).GetPositionAndRotation(mBodyID, outPosition, outRotation); +} + +void Character::SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) const +{ + sGetBodyInterface(mSystem, inLockBodies).SetPositionAndRotation(mBodyID, inPosition, inRotation, inActivationMode); +} + +RVec3 Character::GetPosition(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetPosition(mBodyID); +} + +void Character::SetPosition(RVec3Arg inPosition, EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetPosition(mBodyID, inPosition, inActivationMode); +} + +Quat Character::GetRotation(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetRotation(mBodyID); +} + +void Character::SetRotation(QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetRotation(mBodyID, inRotation, inActivationMode); +} + +RVec3 Character::GetCenterOfMassPosition(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetCenterOfMassPosition(mBodyID); +} + +RMat44 Character::GetWorldTransform(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetWorldTransform(mBodyID); +} + +void Character::SetLayer(ObjectLayer inLayer, bool inLockBodies) +{ + mLayer = inLayer; + + sGetBodyInterface(mSystem, inLockBodies).SetObjectLayer(mBodyID, inLayer); +} + +bool Character::SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies) +{ + if (inMaxPenetrationDepth < FLT_MAX) + { + // Collector that checks if there is anything in the way while switching to inShape + class MyCollector : public CollideShapeCollector + { + public: + // Constructor + explicit MyCollector(float inMaxPenetrationDepth) : mMaxPenetrationDepth(inMaxPenetrationDepth) { } + + // See: CollectorType::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + if (inResult.mPenetrationDepth > mMaxPenetrationDepth) + { + mHadCollision = true; + ForceEarlyOut(); + } + } + + float mMaxPenetrationDepth; + bool mHadCollision = false; + }; + + // Test if anything is in the way of switching + RVec3 char_pos = GetPosition(inLockBodies); + MyCollector collector(inMaxPenetrationDepth); + CheckCollision(inShape, 0.0f, char_pos, collector, inLockBodies); + if (collector.mHadCollision) + return false; + } + + // Switch the shape + mShape = inShape; + sGetBodyInterface(mSystem, inLockBodies).SetShape(mBodyID, mShape, false, EActivation::Activate); + return true; +} + +TransformedShape Character::GetTransformedShape(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetTransformedShape(mBodyID); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h new file mode 100644 index 000000000000..db67f8b3b6b7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h @@ -0,0 +1,147 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Contains the configuration of a character +class JPH_EXPORT CharacterSettings : public CharacterBaseSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Layer that this character will be added to + ObjectLayer mLayer = 0; + + /// Mass of the character + float mMass = 80.0f; + + /// Friction for the character + float mFriction = 0.2f; + + /// Value to multiply gravity with for this character + float mGravityFactor = 1.0f; +}; + +/// Runtime character object. +/// This object usually represents the player or a humanoid AI. It uses a single rigid body, +/// usually with a capsule shape to simulate movement and collision for the character. +/// The character is a keyframed object, the application controls it by setting the velocity. +class JPH_EXPORT Character : public CharacterBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inSettings The settings for the character + /// @param inPosition Initial position for the character + /// @param inRotation Initial rotation for the character (usually only around Y) + /// @param inUserData Application specific value + /// @param inSystem Physics system that this character will be added to later + Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem); + + /// Destructor + virtual ~Character() override; + + /// Add bodies and constraints to the system and optionally activate the bodies + void AddToPhysicsSystem(EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Remove bodies and constraints from the system + void RemoveFromPhysicsSystem(bool inLockBodies = true); + + /// Wake up the character + void Activate(bool inLockBodies = true); + + /// Needs to be called after every PhysicsSystem::Update + /// @param inMaxSeparationDistance Max distance between the floor and the character to still consider the character standing on the floor + /// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false) + void PostSimulation(float inMaxSeparationDistance, bool inLockBodies = true); + + /// Control the velocity of the character + void SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true); + + /// Get the linear velocity of the character (m / s) + Vec3 GetLinearVelocity(bool inLockBodies = true) const; + + /// Set the linear velocity of the character (m / s) + void SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add world space linear velocity to current velocity (m / s) + void AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add impulse to the center of mass of the character + void AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true); + + /// Get the body associated with this character + BodyID GetBodyID() const { return mBodyID; } + + /// Get position / rotation of the body + void GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const; + + /// Set the position / rotation of the body, optionally activating it. + void SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true) const; + + /// Get the position of the character + RVec3 GetPosition(bool inLockBodies = true) const; + + /// Set the position of the character, optionally activating it. + void SetPosition(RVec3Arg inPostion, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Get the rotation of the character + Quat GetRotation(bool inLockBodies = true) const; + + /// Set the rotation of the character, optionally activating it. + void SetRotation(QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Position of the center of mass of the underlying rigid body + RVec3 GetCenterOfMassPosition(bool inLockBodies = true) const; + + /// Calculate the world transform of the character + RMat44 GetWorldTransform(bool inLockBodies = true) const; + + /// Get the layer of the character + ObjectLayer GetLayer() const { return mLayer; } + + /// Update the layer of the character + void SetLayer(ObjectLayer inLayer, bool inLockBodies = true); + + /// Switch the shape of the character (e.g. for stance). When inMaxPenetrationDepth is not FLT_MAX, it checks + /// if the new shape collides before switching shape. Returns true if the switch succeeded. + bool SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies = true); + + /// Get the transformed shape that represents the volume of the character, can be used for collision checks. + TransformedShape GetTransformedShape(bool inLockBodies = true) const; + + /// @brief Get all contacts for the character at a particular location + /// @param inPosition Position to test. + /// @param inRotation Rotation at which to test the shape. + /// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal. + /// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly). + /// @param inShape Shape to test collision with. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + /// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false) + void CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies = true) const; + +private: + /// Check collisions between inShape and the world using the center of mass transform + void CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const; + + /// Check collisions between inShape and the world using the current position / rotation of the character + void CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const; + + /// The body of this character + BodyID mBodyID; + + /// The layer the body is in + ObjectLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp new file mode 100644 index 000000000000..d31425065097 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp @@ -0,0 +1,59 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +CharacterBase::CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem) : + mSystem(inSystem), + mShape(inSettings->mShape), + mUp(inSettings->mUp), + mSupportingVolume(inSettings->mSupportingVolume) +{ + // Initialize max slope angle + SetMaxSlopeAngle(inSettings->mMaxSlopeAngle); +} + +const char *CharacterBase::sToString(EGroundState inState) +{ + switch (inState) + { + case EGroundState::OnGround: return "OnGround"; + case EGroundState::OnSteepGround: return "OnSteepGround"; + case EGroundState::NotSupported: return "NotSupported"; + case EGroundState::InAir: return "InAir"; + } + + JPH_ASSERT(false); + return "Unknown"; +} + +void CharacterBase::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mGroundState); + inStream.Write(mGroundBodyID); + inStream.Write(mGroundBodySubShapeID); + inStream.Write(mGroundPosition); + inStream.Write(mGroundNormal); + inStream.Write(mGroundVelocity); + // Can't save user data (may be a pointer) and material +} + +void CharacterBase::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mGroundState); + inStream.Read(mGroundBodyID); + inStream.Read(mGroundBodySubShapeID); + inStream.Read(mGroundPosition); + inStream.Read(mGroundNormal); + inStream.Read(mGroundVelocity); + mGroundUserData = 0; // Cannot restore user data + mGroundMaterial = PhysicsMaterial::sDefault; // Cannot restore material +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h new file mode 100644 index 000000000000..fc19c3d9949b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h @@ -0,0 +1,157 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class StateRecorder; + +/// Base class for configuration of a character +class JPH_EXPORT CharacterBaseSettings : public RefTarget +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CharacterBaseSettings() = default; + CharacterBaseSettings(const CharacterBaseSettings &inSettings) = default; + CharacterBaseSettings & operator = (const CharacterBaseSettings &inSettings) = default; + + /// Virtual destructor + virtual ~CharacterBaseSettings() = default; + + /// Vector indicating the up direction of the character + Vec3 mUp = Vec3::sAxisY(); + + /// Plane, defined in local space relative to the character. Every contact behind this plane can support the + /// character, every contact in front of this plane is treated as only colliding with the player. + /// Default: Accept any contact. + Plane mSupportingVolume { Vec3::sAxisY(), -1.0e10f }; + + /// Maximum angle of slope that character can still walk on (radians). + float mMaxSlopeAngle = DegreesToRadians(50.0f); + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool mEnhancedInternalEdgeRemoval = false; + + /// Initial shape that represents the character's volume. + /// Usually this is a capsule, make sure the shape is made so that the bottom of the shape is at (0, 0, 0). + RefConst mShape; +}; + +/// Base class for character class +class JPH_EXPORT CharacterBase : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem); + + /// Destructor + virtual ~CharacterBase() = default; + + /// Set the maximum angle of slope that character can still walk on (radians) + void SetMaxSlopeAngle(float inMaxSlopeAngle) { mCosMaxSlopeAngle = Cos(inMaxSlopeAngle); } + float GetCosMaxSlopeAngle() const { return mCosMaxSlopeAngle; } + + /// Set the up vector for the character + void SetUp(Vec3Arg inUp) { mUp = inUp; } + Vec3 GetUp() const { return mUp; } + + /// Check if the normal of the ground surface is too steep to walk on + bool IsSlopeTooSteep(Vec3Arg inNormal) const + { + // If cos max slope angle is close to one the system is turned off, + // otherwise check the angle between the up and normal vector + return mCosMaxSlopeAngle < cNoMaxSlopeAngle && inNormal.Dot(mUp) < mCosMaxSlopeAngle; + } + + /// Get the current shape that the character is using. + const Shape * GetShape() const { return mShape; } + + enum class EGroundState + { + OnGround, ///< Character is on the ground and can move freely. + OnSteepGround, ///< Character is on a slope that is too steep and can't climb up any further. The caller should start applying downward velocity if sliding from the slope is desired. + NotSupported, ///< Character is touching an object, but is not supported by it and should fall. The GetGroundXXX functions will return information about the touched object. + InAir, ///< Character is in the air and is not touching anything. + }; + + /// Debug function to convert enum values to string + static const char * sToString(EGroundState inState); + + ///@name Properties of the ground this character is standing on + + /// Current ground state + EGroundState GetGroundState() const { return mGroundState; } + + /// Returns true if the player is supported by normal or steep ground + bool IsSupported() const { return mGroundState == EGroundState::OnGround || mGroundState == EGroundState::OnSteepGround; } + + /// Get the contact point with the ground + RVec3 GetGroundPosition() const { return mGroundPosition; } + + /// Get the contact normal with the ground + Vec3 GetGroundNormal() const { return mGroundNormal; } + + /// Velocity in world space of ground + Vec3 GetGroundVelocity() const { return mGroundVelocity; } + + /// Material that the character is standing on + const PhysicsMaterial * GetGroundMaterial() const { return mGroundMaterial; } + + /// BodyID of the object the character is standing on. Note may have been removed! + BodyID GetGroundBodyID() const { return mGroundBodyID; } + + /// Sub part of the body that we're standing on. + SubShapeID GetGroundSubShapeID() const { return mGroundBodySubShapeID; } + + /// User data value of the body that we're standing on + uint64 GetGroundUserData() const { return mGroundUserData; } + + // Saving / restoring state for replay + virtual void SaveState(StateRecorder &inStream) const; + virtual void RestoreState(StateRecorder &inStream); + +protected: + // Cached physics system + PhysicsSystem * mSystem; + + // The shape that the body currently has + RefConst mShape; + + // The character's world space up axis + Vec3 mUp; + + // Every contact behind this plane can support the character + Plane mSupportingVolume; + + // Beyond this value there is no max slope + static constexpr float cNoMaxSlopeAngle = 0.9999f; + + // Cosine of the maximum angle of slope that character can still walk on + float mCosMaxSlopeAngle; + + // Ground properties + EGroundState mGroundState = EGroundState::InAir; + BodyID mGroundBodyID; + SubShapeID mGroundBodySubShapeID; + RVec3 mGroundPosition = RVec3::sZero(); + Vec3 mGroundNormal = Vec3::sZero(); + Vec3 mGroundVelocity = Vec3::sZero(); + RefConst mGroundMaterial = PhysicsMaterial::sDefault; + uint64 mGroundUserData = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp new file mode 100644 index 000000000000..6c16dc83be18 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp @@ -0,0 +1,1734 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +void CharacterVsCharacterCollisionSimple::Remove(const CharacterVirtual *inCharacter) +{ + Array::iterator i = std::find(mCharacters.begin(), mCharacters.end(), inCharacter); + if (i != mCharacters.end()) + mCharacters.erase(i); +} + +void CharacterVsCharacterCollisionSimple::CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const +{ + // Make shape 1 relative to inBaseOffset + Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44(); + + const Shape *shape = inCharacter->GetShape(); + CollideShapeSettings settings = inCollideShapeSettings; + + // Iterate over all characters + for (const CharacterVirtual *c : mCharacters) + if (c != inCharacter + && !ioCollector.ShouldEarlyOut()) + { + // Collector needs to know which character we're colliding with + ioCollector.SetUserData(reinterpret_cast(c)); + + // Make shape 2 relative to inBaseOffset + Mat44 transform2 = c->GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + + // We need to add the padding of character 2 so that we will detect collision with its outer shell + settings.mMaxSeparationDistance = inCollideShapeSettings.mMaxSeparationDistance + c->GetCharacterPadding(); + + // Note that this collides against the character's shape without padding, this will be corrected for in CharacterVirtual::GetContactsAtPosition + CollisionDispatch::sCollideShapeVsShape(shape, c->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, SubShapeIDCreator(), SubShapeIDCreator(), settings, ioCollector); + } + + // Reset the user data + ioCollector.SetUserData(0); +} + +void CharacterVsCharacterCollisionSimple::CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const +{ + // Convert shape cast relative to inBaseOffset + Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44(); + ShapeCast shape_cast(inCharacter->GetShape(), Vec3::sReplicate(1.0f), transform1, inDirection); + + // Iterate over all characters + for (const CharacterVirtual *c : mCharacters) + if (c != inCharacter + && !ioCollector.ShouldEarlyOut()) + { + // Collector needs to know which character we're colliding with + ioCollector.SetUserData(reinterpret_cast(c)); + + // Make shape 2 relative to inBaseOffset + Mat44 transform2 = c->GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + + // Note that this collides against the character's shape without padding, this will be corrected for in CharacterVirtual::GetFirstContactForSweep + CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, c->GetShape(), Vec3::sReplicate(1.0f), { }, transform2, SubShapeIDCreator(), SubShapeIDCreator(), ioCollector); + } + + // Reset the user data + ioCollector.SetUserData(0); +} + +CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) : + CharacterBase(inSettings, inSystem), + mBackFaceMode(inSettings->mBackFaceMode), + mPredictiveContactDistance(inSettings->mPredictiveContactDistance), + mMaxCollisionIterations(inSettings->mMaxCollisionIterations), + mMaxConstraintIterations(inSettings->mMaxConstraintIterations), + mMinTimeRemaining(inSettings->mMinTimeRemaining), + mCollisionTolerance(inSettings->mCollisionTolerance), + mCharacterPadding(inSettings->mCharacterPadding), + mMaxNumHits(inSettings->mMaxNumHits), + mHitReductionCosMaxAngle(inSettings->mHitReductionCosMaxAngle), + mPenetrationRecoverySpeed(inSettings->mPenetrationRecoverySpeed), + mEnhancedInternalEdgeRemoval(inSettings->mEnhancedInternalEdgeRemoval), + mShapeOffset(inSettings->mShapeOffset), + mPosition(inPosition), + mRotation(inRotation), + mUserData(inUserData) +{ + // Copy settings + SetMaxStrength(inSettings->mMaxStrength); + SetMass(inSettings->mMass); + + // Create an inner rigid body if requested + if (inSettings->mInnerBodyShape != nullptr) + { + BodyCreationSettings settings(inSettings->mInnerBodyShape, GetInnerBodyPosition(), mRotation, EMotionType::Kinematic, inSettings->mInnerBodyLayer); + settings.mAllowSleeping = false; // Disable sleeping so that we will receive sensor callbacks + settings.mUserData = inUserData; + mInnerBodyID = inSystem->GetBodyInterface().CreateAndAddBody(settings, EActivation::Activate); + } +} + +CharacterVirtual::~CharacterVirtual() +{ + if (!mInnerBodyID.IsInvalid()) + { + mSystem->GetBodyInterface().RemoveBody(mInnerBodyID); + mSystem->GetBodyInterface().DestroyBody(mInnerBodyID); + } +} + +void CharacterVirtual::UpdateInnerBodyTransform() +{ + if (!mInnerBodyID.IsInvalid()) + mSystem->GetBodyInterface().SetPositionAndRotation(mInnerBodyID, GetInnerBodyPosition(), mRotation, EActivation::DontActivate); +} + +void CharacterVirtual::GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const +{ + // Get real velocity of body + if (!inBody.IsStatic()) + { + const MotionProperties *mp = inBody.GetMotionPropertiesUnchecked(); + outLinearVelocity = mp->GetLinearVelocity(); + outAngularVelocity = mp->GetAngularVelocity(); + } + else + { + outLinearVelocity = outAngularVelocity = Vec3::sZero(); + } + + // Allow application to override + if (mListener != nullptr) + mListener->OnAdjustBodyVelocity(this, inBody, outLinearVelocity, outAngularVelocity); +} + +Vec3 CharacterVirtual::CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const +{ + // Get angular velocity + float angular_velocity_len_sq = inAngularVelocity.LengthSq(); + if (angular_velocity_len_sq < 1.0e-12f) + return inLinearVelocity; + float angular_velocity_len = sqrt(angular_velocity_len_sq); + + // Calculate the rotation that the object will make in the time step + Quat rotation = Quat::sRotation(inAngularVelocity / angular_velocity_len, angular_velocity_len * inDeltaTime); + + // Calculate where the new character position will be + RVec3 new_position = inCenterOfMass + rotation * Vec3(mPosition - inCenterOfMass); + + // Calculate the velocity + return inLinearVelocity + Vec3(new_position - mPosition) / inDeltaTime; +} + +template +void CharacterVirtual::sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult) +{ + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + inCharacter->GetAdjustedBodyVelocity(inBody, linear_velocity, angular_velocity); + + outContact.mPosition = inBaseOffset + inResult.mContactPointOn2; + outContact.mLinearVelocity = linear_velocity + angular_velocity.Cross(Vec3(outContact.mPosition - inBody.GetCenterOfMassPosition())); // Calculate point velocity + outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()); + outContact.mSurfaceNormal = inCollector.GetContext()->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, outContact.mPosition); + if (outContact.mContactNormal.Dot(outContact.mSurfaceNormal) < 0.0f) + outContact.mSurfaceNormal = -outContact.mSurfaceNormal; // Flip surface normal if we're hitting a back face + if (outContact.mContactNormal.Dot(inUp) > outContact.mSurfaceNormal.Dot(inUp)) + outContact.mSurfaceNormal = outContact.mContactNormal; // Replace surface normal with contact normal if the contact normal is pointing more upwards + outContact.mDistance = -inResult.mPenetrationDepth; + outContact.mBodyB = inResult.mBodyID2; + outContact.mSubShapeIDB = inResult.mSubShapeID2; + outContact.mMotionTypeB = inBody.GetMotionType(); + outContact.mIsSensorB = inBody.IsSensor(); + outContact.mUserData = inBody.GetUserData(); + outContact.mMaterial = inCollector.GetContext()->GetMaterial(inResult.mSubShapeID2); +} + +void CharacterVirtual::sFillCharacterContactProperties(Contact &outContact, CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult) +{ + outContact.mPosition = inBaseOffset + inResult.mContactPointOn2; + outContact.mLinearVelocity = inOtherCharacter->GetLinearVelocity(); + outContact.mSurfaceNormal = outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()); + outContact.mDistance = -inResult.mPenetrationDepth; + outContact.mCharacterB = inOtherCharacter; + outContact.mSubShapeIDB = inResult.mSubShapeID2; + outContact.mMotionTypeB = EMotionType::Kinematic; // Other character is kinematic, we can't directly move it + outContact.mIsSensorB = false; + outContact.mUserData = inOtherCharacter->GetUserData(); + outContact.mMaterial = PhysicsMaterial::sDefault; +} + +void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResult) +{ + // If we exceed our contact limit, try to clean up near-duplicate contacts + if (mContacts.size() == mMaxHits) + { + // Flag that we hit this code path + mMaxHitsExceeded = true; + + // Check if we can do reduction + if (mHitReductionCosMaxAngle > -1.0f) + { + // Loop all contacts and find similar contacts + for (int i = (int)mContacts.size() - 1; i >= 0; --i) + { + Contact &contact_i = mContacts[i]; + for (int j = i - 1; j >= 0; --j) + { + Contact &contact_j = mContacts[j]; + if (contact_i.IsSameBody(contact_j) + && contact_i.mContactNormal.Dot(contact_j.mContactNormal) > mHitReductionCosMaxAngle) // Very similar contact normals + { + // Remove the contact with the biggest distance + bool i_is_last = i == (int)mContacts.size() - 1; + if (contact_i.mDistance > contact_j.mDistance) + { + // Remove i + if (!i_is_last) + contact_i = mContacts.back(); + mContacts.pop_back(); + + // Break out of the loop, i is now an element that we already processed + break; + } + else + { + // Remove j + contact_j = mContacts.back(); + mContacts.pop_back(); + + // If i was the last element, we just moved it into position j. Break out of the loop, we'll see it again later. + if (i_is_last) + break; + } + } + } + } + } + + if (mContacts.size() == mMaxHits) + { + // There are still too many hits, give up! + ForceEarlyOut(); + return; + } + } + + if (inResult.mBodyID2.IsInvalid()) + { + // Assuming this is a hit against another character + JPH_ASSERT(mOtherCharacter != nullptr); + + // Create contact with other character + mContacts.emplace_back(); + Contact &contact = mContacts.back(); + sFillCharacterContactProperties(contact, mOtherCharacter, mBaseOffset, inResult); + contact.mFraction = 0.0f; + } + else + { + // Create contact with other body + BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2); + if (lock.SucceededAndIsInBroadPhase()) + { + mContacts.emplace_back(); + Contact &contact = mContacts.back(); + sFillContactProperties(mCharacter, contact, lock.GetBody(), mUp, mBaseOffset, *this, inResult); + contact.mFraction = 0.0f; + } + } +} + +void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inResult) +{ + if (inResult.mFraction < mContact.mFraction // Since we're doing checks against the world and against characters, we may get a hit with a higher fraction than the previous hit + && inResult.mFraction > 0.0f // Ignore collisions at fraction = 0 + && inResult.mPenetrationAxis.Dot(mDisplacement) > 0.0f) // Ignore penetrations that we're moving away from + { + // Test if this contact should be ignored + for (const IgnoredContact &c : mIgnoredContacts) + if (c.mBodyID == inResult.mBodyID2 && c.mSubShapeID == inResult.mSubShapeID2) + return; + + Contact contact; + + if (inResult.mBodyID2.IsInvalid()) + { + // Assuming this is a hit against another character + JPH_ASSERT(mOtherCharacter != nullptr); + + // Create contact with other character + sFillCharacterContactProperties(contact, mOtherCharacter, mBaseOffset, inResult); + } + else + { + // Lock body only while we fetch contact properties + BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2); + if (!lock.SucceededAndIsInBroadPhase()) + return; + + // Sweeps don't result in OnContactAdded callbacks so we can ignore sensors here + const Body &body = lock.GetBody(); + if (body.IsSensor()) + return; + + // Convert the hit result into a contact + sFillContactProperties(mCharacter, contact, body, mUp, mBaseOffset, *this, inResult); + } + + contact.mFraction = inResult.mFraction; + + // Check if the contact that will make us penetrate more than the allowed tolerance + if (contact.mDistance + contact.mContactNormal.Dot(mDisplacement) < -mCharacter->mCollisionTolerance + && mCharacter->ValidateContact(contact)) + { + mContact = contact; + UpdateEarlyOutFraction(contact.mFraction); + } + } +} + +void CharacterVirtual::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + // Query shape transform + RMat44 transform = GetCenterOfMassTransform(inPosition, inRotation, inShape); + + // Settings for collide shape + CollideShapeSettings settings; + settings.mBackFaceMode = mBackFaceMode; + settings.mActiveEdgeMovementDirection = inMovementDirection; + settings.mMaxSeparationDistance = mCharacterPadding + inMaxSeparationDistance; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + + // Body filter + IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter); + + // Select the right function + auto collide_shape_function = mEnhancedInternalEdgeRemoval? &NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval : &NarrowPhaseQuery::CollideShape; + + // Collide shape + (mSystem->GetNarrowPhaseQuery().*collide_shape_function)(inShape, Vec3::sReplicate(1.0f), transform, settings, inBaseOffset, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter); + + // Also collide with other characters + if (mCharacterVsCharacterCollision != nullptr) + { + ioCollector.SetContext(nullptr); // We're no longer colliding with a transformed shape, reset + mCharacterVsCharacterCollision->CollideCharacter(this, transform, settings, inBaseOffset, ioCollector); + } +} + +void CharacterVirtual::GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + // Remove previous results + outContacts.clear(); + + // Body filter + IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter); + + // Collide shape + ContactCollector collector(mSystem, this, mMaxNumHits, mHitReductionCosMaxAngle, mUp, mPosition, outContacts); + CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, mPosition, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter); + + // The broadphase bounding boxes will not be deterministic, which means that the order in which the contacts are received by the collector is not deterministic. + // Therefore we need to sort the contacts to preserve determinism. Note that currently this will fail if we exceed mMaxNumHits hits. + QuickSort(outContacts.begin(), outContacts.end(), ContactOrderingPredicate()); + + // Flag if we exceeded the max number of hits + mMaxHitsExceeded = collector.mMaxHitsExceeded; + + // Reduce distance to contact by padding to ensure we stay away from the object by a little margin + // (this will make collision detection cheaper - especially for sweep tests as they won't hit the surface if we're properly sliding) + for (Contact &c : outContacts) + { + c.mDistance -= mCharacterPadding; + + if (c.mCharacterB != nullptr) + c.mDistance -= c.mCharacterB->mCharacterPadding; + } +} + +void CharacterVirtual::RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const +{ + // Only use this algorithm if we're penetrating further than this (due to numerical precision issues we can always penetrate a little bit and we don't want to discard contacts if they just have a tiny penetration) + // We do need to account for padding (see GetContactsAtPosition) that is removed from the contact distances, to compensate we add it to the cMinRequiredPenetration + const float cMinRequiredPenetration = 1.25f * mCharacterPadding; + + // Discard conflicting penetrating contacts + for (size_t c1 = 0; c1 < ioContacts.size(); c1++) + { + Contact &contact1 = ioContacts[c1]; + if (contact1.mDistance <= -cMinRequiredPenetration) // Only for penetrations + for (size_t c2 = c1 + 1; c2 < ioContacts.size(); c2++) + { + Contact &contact2 = ioContacts[c2]; + if (contact1.IsSameBody(contact2) + && contact2.mDistance <= -cMinRequiredPenetration // Only for penetrations + && contact1.mContactNormal.Dot(contact2.mContactNormal) < 0.0f) // Only opposing normals + { + // Discard contacts with the least amount of penetration + if (contact1.mDistance < contact2.mDistance) + { + // Discard the 2nd contact + outIgnoredContacts.emplace_back(contact2.mBodyB, contact2.mSubShapeIDB); + ioContacts.erase(ioContacts.begin() + c2); + c2--; + } + else + { + // Discard the first contact + outIgnoredContacts.emplace_back(contact1.mBodyB, contact1.mSubShapeIDB); + ioContacts.erase(ioContacts.begin() + c1); + c1--; + break; + } + } + } + } +} + +bool CharacterVirtual::ValidateContact(const Contact &inContact) const +{ + if (mListener == nullptr) + return true; + + if (inContact.mCharacterB != nullptr) + return mListener->OnCharacterContactValidate(this, inContact.mCharacterB, inContact.mSubShapeIDB); + else + return mListener->OnContactValidate(this, inContact.mBodyB, inContact.mSubShapeIDB); +} + +void CharacterVirtual::ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings) const +{ + if (mListener != nullptr) + { + if (inContact.mCharacterB != nullptr) + mListener->OnCharacterContactAdded(this, inContact.mCharacterB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings); + else + mListener->OnContactAdded(this, inContact.mBodyB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings); + } +} + +template +inline static bool sCorrectFractionForCharacterPadding(const Shape *inShape, Mat44Arg inStart, Vec3Arg inDisplacement, Vec3Arg inScale, const T &inPolygon, float &ioFraction) +{ + if (inShape->GetType() == EShapeType::Convex) + { + // Get the support function for the shape we're casting + const ConvexShape *convex_shape = static_cast(inShape); + ConvexShape::SupportBuffer buffer; + const ConvexShape::Support *support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, inScale); + + // Cast the shape against the polygon + GJKClosestPoint gjk; + return gjk.CastShape(inStart, inDisplacement, cDefaultCollisionTolerance, *support, inPolygon, ioFraction); + } + else if (inShape->GetSubType() == EShapeSubType::RotatedTranslated) + { + const RotatedTranslatedShape *rt_shape = static_cast(inShape); + return sCorrectFractionForCharacterPadding(rt_shape->GetInnerShape(), inStart * Mat44::sRotation(rt_shape->GetRotation()), inDisplacement, rt_shape->TransformScale(inScale), inPolygon, ioFraction); + } + else if (inShape->GetSubType() == EShapeSubType::Scaled) + { + const ScaledShape *scaled_shape = static_cast(inShape); + return sCorrectFractionForCharacterPadding(scaled_shape->GetInnerShape(), inStart, inDisplacement, inScale * scaled_shape->GetScale(), inPolygon, ioFraction); + } + else + { + JPH_ASSERT(false, "Not supported yet!"); + return false; + } +} + +bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + // Too small distance -> skip checking + float displacement_len_sq = inDisplacement.LengthSq(); + if (displacement_len_sq < 1.0e-8f) + return false; + + // Calculate start transform + RMat44 start = GetCenterOfMassTransform(inPosition, mRotation, mShape); + + // Settings for the cast + ShapeCastSettings settings; + settings.mBackFaceModeTriangles = mBackFaceMode; + settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = false; + + // Calculate how much extra fraction we need to add to the cast to account for the character padding + float character_padding_fraction = mCharacterPadding / sqrt(displacement_len_sq); + + // Body filter + IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter); + + // Cast shape + Contact contact; + contact.mFraction = 1.0f + character_padding_fraction; + RVec3 base_offset = start.GetTranslation(); + ContactCastCollector collector(mSystem, this, inDisplacement, mUp, inIgnoredContacts, base_offset, contact); + collector.ResetEarlyOutFraction(contact.mFraction); + RShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement); + mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, base_offset, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter); + + // Also collide with other characters + if (mCharacterVsCharacterCollision != nullptr) + { + collector.SetContext(nullptr); // We're no longer colliding with a transformed shape, reset + mCharacterVsCharacterCollision->CastCharacter(this, start, inDisplacement, settings, base_offset, collector); + } + + if (contact.mBodyB.IsInvalid() && contact.mCharacterB == nullptr) + return false; + + // Store contact + outContact = contact; + + TransformedShape ts; + float character_padding = mCharacterPadding; + if (outContact.mCharacterB != nullptr) + { + // Create a transformed shape for the character + RMat44 com = outContact.mCharacterB->GetCenterOfMassTransform(); + ts = TransformedShape(com.GetTranslation(), com.GetQuaternion(), outContact.mCharacterB->GetShape(), BodyID(), SubShapeIDCreator()); + + // We need to take the other character's padding into account as well + character_padding += outContact.mCharacterB->mCharacterPadding; + } + else + { + // Create a transformed shape for the body + ts = mSystem->GetBodyInterface().GetTransformedShape(outContact.mBodyB); + } + + // Fetch the face we're colliding with + Shape::SupportingFace face; + ts.GetSupportingFace(outContact.mSubShapeIDB, -outContact.mContactNormal, base_offset, face); + + bool corrected = false; + if (face.size() >= 2) + { + // Inflate the colliding face by the character padding + PolygonConvexSupport polygon(face); + AddConvexRadius add_cvx(polygon, character_padding); + + // Correct fraction to hit this inflated face instead of the inner shape + corrected = sCorrectFractionForCharacterPadding(mShape, start.GetRotation(), inDisplacement, Vec3::sReplicate(1.0f), add_cvx, outContact.mFraction); + } + if (!corrected) + { + // When there's only a single contact point or when we were unable to correct the fraction, + // we can just move the fraction back so that the character and its padding don't hit the contact point anymore + outContact.mFraction = max(0.0f, outContact.mFraction - character_padding_fraction); + } + + // Ensure that we never return a fraction that's bigger than 1 (which could happen due to float precision issues). + outContact.mFraction = min(outContact.mFraction, 1.0f); + + return true; +} + +void CharacterVirtual::DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const +{ + for (Contact &c : inContacts) + { + Vec3 contact_velocity = c.mLinearVelocity; + + // Penetrating contact: Add a contact velocity that pushes the character out at the desired speed + if (c.mDistance < 0.0f) + contact_velocity -= c.mContactNormal * c.mDistance * mPenetrationRecoverySpeed / inDeltaTime; + + // Convert to a constraint + outConstraints.emplace_back(); + Constraint &constraint = outConstraints.back(); + constraint.mContact = &c; + constraint.mLinearVelocity = contact_velocity; + constraint.mPlane = Plane(c.mContactNormal, c.mDistance); + + // Next check if the angle is too steep and if it is add an additional constraint that holds the character back + if (IsSlopeTooSteep(c.mSurfaceNormal)) + { + // Only take planes that point up. + // Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement. + float dot = c.mContactNormal.Dot(mUp); + if (dot > 1.0e-3f) // Add a little slack, if the normal is perfectly horizontal we already have our vertical plane. + { + // Mark the slope constraint as steep + constraint.mIsSteepSlope = true; + + // Make horizontal normal + Vec3 normal = (c.mContactNormal - dot * mUp).Normalized(); + + // Create a secondary constraint that blocks horizontal movement + outConstraints.emplace_back(); + Constraint &vertical_constraint = outConstraints.back(); + vertical_constraint.mContact = &c; + vertical_constraint.mLinearVelocity = contact_velocity.Dot(normal) * normal; // Project the contact velocity on the new normal so that both planes push at an equal rate + vertical_constraint.mPlane = Plane(normal, c.mDistance / normal.Dot(c.mContactNormal)); // Calculate the distance we have to travel horizontally to hit the contact plane + } + } + } +} + +bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const +{ + Contact &contact = *ioConstraint.mContact; + + // Validate the contact point + if (!ValidateContact(contact)) + return false; + + // Send contact added event + CharacterContactSettings settings; + ContactAdded(contact, settings); + contact.mCanPushCharacter = settings.mCanPushCharacter; + + // We don't have any further interaction with sensors beyond an OnContactAdded notification + if (contact.mIsSensorB) + return false; + + // If body B cannot receive an impulse, we're done + if (!settings.mCanReceiveImpulses || contact.mMotionTypeB != EMotionType::Dynamic) + return true; + + // Lock the body we're colliding with + BodyLockWrite lock(mSystem->GetBodyLockInterface(), contact.mBodyB); + if (!lock.SucceededAndIsInBroadPhase()) + return false; // Body has been removed, we should not collide with it anymore + const Body &body = lock.GetBody(); + + // Calculate the velocity that we want to apply at B so that it will start moving at the character's speed at the contact point + constexpr float cDamping = 0.9f; + constexpr float cPenetrationResolution = 0.4f; + Vec3 relative_velocity = inVelocity - contact.mLinearVelocity; + float projected_velocity = relative_velocity.Dot(contact.mContactNormal); + float delta_velocity = -projected_velocity * cDamping - min(contact.mDistance, 0.0f) * cPenetrationResolution / inDeltaTime; + + // Don't apply impulses if we're separating + if (delta_velocity < 0.0f) + return true; + + // Determine mass properties of the body we're colliding with + const MotionProperties *motion_properties = body.GetMotionProperties(); + RVec3 center_of_mass = body.GetCenterOfMassPosition(); + Mat44 inverse_inertia = body.GetInverseInertia(); + float inverse_mass = motion_properties->GetInverseMass(); + + // Calculate the inverse of the mass of body B as seen at the contact point in the direction of the contact normal + Vec3 jacobian = Vec3(contact.mPosition - center_of_mass).Cross(contact.mContactNormal); + float inv_effective_mass = inverse_inertia.Multiply3x3(jacobian).Dot(jacobian) + inverse_mass; + + // Impulse P = M dv + float impulse = delta_velocity / inv_effective_mass; + + // Clamp the impulse according to the character strength, character strength is a force in newtons, P = F dt + float max_impulse = mMaxStrength * inDeltaTime; + impulse = min(impulse, max_impulse); + + // Calculate the world space impulse to apply + Vec3 world_impulse = -impulse * contact.mContactNormal; + + // Cancel impulse in down direction (we apply gravity later) + float impulse_dot_up = world_impulse.Dot(mUp); + if (impulse_dot_up < 0.0f) + world_impulse -= impulse_dot_up * mUp; + + // Now apply the impulse (body is already locked so we use the no-lock interface) + mSystem->GetBodyInterfaceNoLock().AddImpulse(contact.mBodyB, world_impulse, contact.mPosition); + return true; +} + +void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator +#ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints +#endif // JPH_DEBUG_RENDERER + ) const +{ + // If there are no constraints we can immediately move to our target + if (ioConstraints.empty()) + { + outDisplacement = inVelocity * inTimeRemaining; + outTimeSimulated = inTimeRemaining; + return; + } + + // Create array that holds the constraints in order of time of impact (sort will happen later) + Array> sorted_constraints(inAllocator); + sorted_constraints.resize(ioConstraints.size()); + for (size_t index = 0; index < sorted_constraints.size(); index++) + sorted_constraints[index] = &ioConstraints[index]; + + // This is the velocity we use for the displacement, if we hit something it will be shortened + Vec3 velocity = inVelocity; + + // Keep track of the last velocity that was applied to the character so that we can detect when the velocity reverses + Vec3 last_velocity = inVelocity; + + // Start with no displacement + outDisplacement = Vec3::sZero(); + outTimeSimulated = 0.0f; + + // These are the contacts that we hit previously without moving a significant distance + Array> previous_contacts(inAllocator); + previous_contacts.resize(mMaxConstraintIterations); + int num_previous_contacts = 0; + + // Loop for a max amount of iterations + for (uint iteration = 0; iteration < mMaxConstraintIterations; iteration++) + { + // Calculate time of impact for all constraints + for (Constraint &c : ioConstraints) + { + // Project velocity on plane direction + c.mProjectedVelocity = c.mPlane.GetNormal().Dot(c.mLinearVelocity - velocity); + if (c.mProjectedVelocity < 1.0e-6f) + { + c.mTOI = FLT_MAX; + } + else + { + // Distance to plane + float dist = c.mPlane.SignedDistance(outDisplacement); + + if (dist - c.mProjectedVelocity * inTimeRemaining > -1.0e-4f) + { + // Too little penetration, accept the movement + c.mTOI = FLT_MAX; + } + else + { + // Calculate time of impact + c.mTOI = max(0.0f, dist / c.mProjectedVelocity); + } + } + } + + // Sort constraints on proximity + QuickSort(sorted_constraints.begin(), sorted_constraints.end(), [](const Constraint *inLHS, const Constraint *inRHS) { + // If both constraints hit at t = 0 then order the one that will push the character furthest first + // Note that because we add velocity to penetrating contacts, this will also resolve contacts that penetrate the most + if (inLHS->mTOI <= 0.0f && inRHS->mTOI <= 0.0f) + return inLHS->mProjectedVelocity > inRHS->mProjectedVelocity; + + // Then sort on time of impact + if (inLHS->mTOI != inRHS->mTOI) + return inLHS->mTOI < inRHS->mTOI; + + // As a tie breaker sort static first so it has the most influence + return inLHS->mContact->mMotionTypeB > inRHS->mContact->mMotionTypeB; + }); + + // Find the first valid constraint + Constraint *constraint = nullptr; + for (Constraint *c : sorted_constraints) + { + // Take the first contact and see if we can reach it + if (c->mTOI >= inTimeRemaining) + { + // We can reach our goal! + outDisplacement += velocity * inTimeRemaining; + outTimeSimulated += inTimeRemaining; + return; + } + + // Test if this contact was discarded by the contact callback before + if (c->mContact->mWasDiscarded) + continue; + + // Check if we made contact with this before + if (!c->mContact->mHadCollision) + { + // Handle the contact + if (!HandleContact(velocity, *c, inDeltaTime)) + { + // Constraint should be ignored, remove it from the list + c->mContact->mWasDiscarded = true; + + // Mark it as ignored for GetFirstContactForSweep + ioIgnoredContacts.emplace_back(c->mContact->mBodyB, c->mContact->mSubShapeIDB); + continue; + } + + c->mContact->mHadCollision = true; + } + + // Cancel velocity of constraint if it cannot push the character + if (!c->mContact->mCanPushCharacter) + c->mLinearVelocity = Vec3::sZero(); + + // We found the first constraint that we want to collide with + constraint = c; + break; + } + + if (constraint == nullptr) + { + // All constraints were discarded, we can reach our goal! + outDisplacement += velocity * inTimeRemaining; + outTimeSimulated += inTimeRemaining; + return; + } + + // Move to the contact + outDisplacement += velocity * constraint->mTOI; + inTimeRemaining -= constraint->mTOI; + outTimeSimulated += constraint->mTOI; + + // If there's not enough time left to be simulated, bail + if (inTimeRemaining < mMinTimeRemaining) + return; + + // If we've moved significantly, clear all previous contacts + if (constraint->mTOI > 1.0e-4f) + num_previous_contacts = 0; + + // Get the normal of the plane we're hitting + Vec3 plane_normal = constraint->mPlane.GetNormal(); + + // If we're hitting a steep slope we cancel the velocity towards the slope first so that we don't end up sliding up the slope + // (we may hit the slope before the vertical wall constraint we added which will result in a small movement up causing jitter in the character movement) + if (constraint->mIsSteepSlope) + { + // We're hitting a steep slope, create a vertical plane that blocks any further movement up the slope (note: not normalized) + Vec3 vertical_plane_normal = plane_normal - plane_normal.Dot(mUp) * mUp; + + // Get the relative velocity between the character and the constraint + Vec3 relative_velocity = velocity - constraint->mLinearVelocity; + + // Remove velocity towards the slope + velocity = velocity - min(0.0f, relative_velocity.Dot(vertical_plane_normal)) * vertical_plane_normal / vertical_plane_normal.LengthSq(); + } + + // Get the relative velocity between the character and the constraint + Vec3 relative_velocity = velocity - constraint->mLinearVelocity; + + // Calculate new velocity if we cancel the relative velocity in the normal direction + Vec3 new_velocity = velocity - relative_velocity.Dot(plane_normal) * plane_normal; + + // Find the normal of the previous contact that we will violate the most if we move in this new direction + float highest_penetration = 0.0f; + Constraint *other_constraint = nullptr; + for (Constraint **c = previous_contacts.data(); c < previous_contacts.data() + num_previous_contacts; ++c) + if (*c != constraint) + { + // Calculate how much we will penetrate if we move in this direction + Vec3 other_normal = (*c)->mPlane.GetNormal(); + float penetration = ((*c)->mLinearVelocity - new_velocity).Dot(other_normal); + if (penetration > highest_penetration) + { + // We don't want parallel or anti-parallel normals as that will cause our cross product below to become zero. Slack is approx 10 degrees. + float dot = other_normal.Dot(plane_normal); + if (dot < 0.984f && dot > -0.984f) + { + highest_penetration = penetration; + other_constraint = *c; + } + } + } + + // Check if we found a 2nd constraint + if (other_constraint != nullptr) + { + // Calculate the sliding direction and project the new velocity onto that sliding direction + Vec3 other_normal = other_constraint->mPlane.GetNormal(); + Vec3 slide_dir = plane_normal.Cross(other_normal).Normalized(); + Vec3 velocity_in_slide_dir = new_velocity.Dot(slide_dir) * slide_dir; + + // Cancel the constraint velocity in the other constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes + constraint->mLinearVelocity -= min(0.0f, constraint->mLinearVelocity.Dot(other_normal)) * other_normal; + + // Cancel the other constraints velocity in this constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes + other_constraint->mLinearVelocity -= min(0.0f, other_constraint->mLinearVelocity.Dot(plane_normal)) * plane_normal; + + // Calculate the velocity of this constraint perpendicular to the slide direction + Vec3 perpendicular_velocity = constraint->mLinearVelocity - constraint->mLinearVelocity.Dot(slide_dir) * slide_dir; + + // Calculate the velocity of the other constraint perpendicular to the slide direction + Vec3 other_perpendicular_velocity = other_constraint->mLinearVelocity - other_constraint->mLinearVelocity.Dot(slide_dir) * slide_dir; + + // Add all components together + new_velocity = velocity_in_slide_dir + perpendicular_velocity + other_perpendicular_velocity; + } + + // Allow application to modify calculated velocity + if (mListener != nullptr) + { + if (constraint->mContact->mCharacterB != nullptr) + mListener->OnCharacterContactSolve(this, constraint->mContact->mCharacterB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity); + else + mListener->OnContactSolve(this, constraint->mContact->mBodyB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity); + } + +#ifdef JPH_DEBUG_RENDERER + if (inDrawConstraints) + { + // Calculate where to draw + RVec3 offset = mPosition + Vec3(0, 0, 2.5f * (iteration + 1)); + + // Draw constraint plane + DebugRenderer::sInstance->DrawPlane(offset, constraint->mPlane.GetNormal(), Color::sCyan, 1.0f); + + // Draw 2nd constraint plane + if (other_constraint != nullptr) + DebugRenderer::sInstance->DrawPlane(offset, other_constraint->mPlane.GetNormal(), Color::sBlue, 1.0f); + + // Draw starting velocity + DebugRenderer::sInstance->DrawArrow(offset, offset + velocity, Color::sGreen, 0.05f); + + // Draw resulting velocity + DebugRenderer::sInstance->DrawArrow(offset, offset + new_velocity, Color::sRed, 0.05f); + } +#endif // JPH_DEBUG_RENDERER + + // Update the velocity + velocity = new_velocity; + + // Add the contact to the list so that next iteration we can avoid violating it again + previous_contacts[num_previous_contacts] = constraint; + num_previous_contacts++; + + // Check early out + if (constraint->mProjectedVelocity < 1.0e-8f // Constraint should not be pushing, otherwise there may be other constraints that are pushing us + && velocity.LengthSq() < 1.0e-8f) // There's not enough velocity left + return; + + // If the constraint has velocity we accept the new velocity, otherwise check that we didn't reverse velocity + if (!constraint->mLinearVelocity.IsNearZero(1.0e-8f)) + last_velocity = constraint->mLinearVelocity; + else if (velocity.Dot(last_velocity) < 0.0f) + return; + } +} + +void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator) +{ + // Flag contacts as having a collision if they're close enough but ignore contacts we're moving away from. + // Note that if we did MoveShape before we want to preserve any contacts that it marked as colliding + for (Contact &c : mActiveContacts) + if (!c.mWasDiscarded + && !c.mHadCollision + && c.mDistance < mCollisionTolerance + && (inSkipContactVelocityCheck || c.mSurfaceNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 1.0e-4f)) + { + if (ValidateContact(c) && !c.mIsSensorB) + c.mHadCollision = true; + else + c.mWasDiscarded = true; + } + + // Calculate transform that takes us to character local space + RMat44 inv_transform = RMat44::sInverseRotationTranslation(mRotation, mPosition); + + // Determine if we're supported or not + int num_supported = 0; + int num_sliding = 0; + int num_avg_normal = 0; + Vec3 avg_normal = Vec3::sZero(); + Vec3 avg_velocity = Vec3::sZero(); + const Contact *supporting_contact = nullptr; + float max_cos_angle = -FLT_MAX; + const Contact *deepest_contact = nullptr; + float smallest_distance = FLT_MAX; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + { + // Calculate the angle between the plane normal and the up direction + float cos_angle = c.mSurfaceNormal.Dot(mUp); + + // Find the deepest contact + if (c.mDistance < smallest_distance) + { + deepest_contact = &c; + smallest_distance = c.mDistance; + } + + // If this contact is in front of our plane, we cannot be supported by it + if (mSupportingVolume.SignedDistance(Vec3(inv_transform * c.mPosition)) > 0.0f) + continue; + + // Find the contact with the normal that is pointing most upwards and store it + if (max_cos_angle < cos_angle) + { + supporting_contact = &c; + max_cos_angle = cos_angle; + } + + // Check if this is a sliding or supported contact + bool is_supported = mCosMaxSlopeAngle > cNoMaxSlopeAngle || cos_angle >= mCosMaxSlopeAngle; + if (is_supported) + num_supported++; + else + num_sliding++; + + // If the angle between the two is less than 85 degrees we also use it to calculate the average normal + if (cos_angle >= 0.08f) + { + avg_normal += c.mSurfaceNormal; + num_avg_normal++; + + // For static or dynamic objects or for contacts that don't support us just take the contact velocity + if (c.mMotionTypeB != EMotionType::Kinematic || !is_supported) + avg_velocity += c.mLinearVelocity; + else + { + // For keyframed objects that support us calculate the velocity at our position rather than at the contact position so that we properly follow the object + BodyLockRead lock(mSystem->GetBodyLockInterface(), c.mBodyB); + if (lock.SucceededAndIsInBroadPhase()) + { + const Body &body = lock.GetBody(); + + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity); + + // Calculate the ground velocity + avg_velocity += CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime); + } + else + { + // Fall back to contact velocity + avg_velocity += c.mLinearVelocity; + } + } + } + } + + // Take either the most supporting contact or the deepest contact + const Contact *best_contact = supporting_contact != nullptr? supporting_contact : deepest_contact; + + // Calculate average normal and velocity + if (num_avg_normal >= 1) + { + mGroundNormal = avg_normal.Normalized(); + mGroundVelocity = avg_velocity / float(num_avg_normal); + } + else if (best_contact != nullptr) + { + mGroundNormal = best_contact->mSurfaceNormal; + mGroundVelocity = best_contact->mLinearVelocity; + } + else + { + mGroundNormal = Vec3::sZero(); + mGroundVelocity = Vec3::sZero(); + } + + // Copy contact properties + if (best_contact != nullptr) + { + mGroundBodyID = best_contact->mBodyB; + mGroundBodySubShapeID = best_contact->mSubShapeIDB; + mGroundPosition = best_contact->mPosition; + mGroundMaterial = best_contact->mMaterial; + mGroundUserData = best_contact->mUserData; + } + else + { + mGroundBodyID = BodyID(); + mGroundBodySubShapeID = SubShapeID(); + mGroundPosition = RVec3::sZero(); + mGroundMaterial = PhysicsMaterial::sDefault; + mGroundUserData = 0; + } + + // Determine ground state + if (num_supported > 0) + { + // We made contact with something that supports us + mGroundState = EGroundState::OnGround; + } + else if (num_sliding > 0) + { + if ((mLinearVelocity - deepest_contact->mLinearVelocity).Dot(mUp) > 1.0e-4f) + { + // We cannot be on ground if we're moving upwards relative to the ground + mGroundState = EGroundState::OnSteepGround; + } + else + { + // If we're sliding down, we may actually be standing on multiple sliding contacts in such a way that we can't slide off, in this case we're also supported + + // Convert the contacts into constraints + TempContactList contacts(mActiveContacts.begin(), mActiveContacts.end(), inAllocator); + ConstraintList constraints(inAllocator); + constraints.reserve(contacts.size() * 2); + DetermineConstraints(contacts, mLastDeltaTime, constraints); + + // Solve the displacement using these constraints, this is used to check if we didn't move at all because we are supported + Vec3 displacement; + float time_simulated; + IgnoredContactList ignored_contacts(inAllocator); + ignored_contacts.reserve(contacts.size()); + SolveConstraints(-mUp, 1.0f, 1.0f, constraints, ignored_contacts, time_simulated, displacement, inAllocator); + + // If we're blocked then we're supported, otherwise we're sliding + float min_required_displacement_sq = Square(0.6f * mLastDeltaTime); + if (time_simulated < 0.001f || displacement.LengthSq() < min_required_displacement_sq) + mGroundState = EGroundState::OnGround; + else + mGroundState = EGroundState::OnSteepGround; + } + } + else + { + // Not supported by anything + mGroundState = best_contact != nullptr? EGroundState::NotSupported : EGroundState::InAir; + } +} + +void CharacterVirtual::StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator) +{ + mActiveContacts.assign(inContacts.begin(), inContacts.end()); + + UpdateSupportingContact(true, inAllocator); +} + +void CharacterVirtual::MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator +#ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints +#endif // JPH_DEBUG_RENDERER + ) const +{ + JPH_DET_LOG("CharacterVirtual::MoveShape: pos: " << ioPosition << " vel: " << inVelocity << " dt: " << inDeltaTime); + + Vec3 movement_direction = inVelocity.NormalizedOr(Vec3::sZero()); + + float time_remaining = inDeltaTime; + for (uint iteration = 0; iteration < mMaxCollisionIterations && time_remaining >= mMinTimeRemaining; iteration++) + { + JPH_DET_LOG("iter: " << iteration << " time: " << time_remaining); + + // Determine contacts in the neighborhood + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(ioPosition, movement_direction, mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + +#ifdef JPH_ENABLE_DETERMINISM_LOG + for (const Contact &c : contacts) + JPH_DET_LOG("contact: " << c.mPosition << " vel: " << c.mLinearVelocity << " cnormal: " << c.mContactNormal << " snormal: " << c.mSurfaceNormal << " dist: " << c.mDistance << " fraction: " << c.mFraction << " body: " << c.mBodyB << " subshape: " << c.mSubShapeIDB); +#endif // JPH_ENABLE_DETERMINISM_LOG + + // Remove contacts with the same body that have conflicting normals + IgnoredContactList ignored_contacts(inAllocator); + ignored_contacts.reserve(contacts.size()); + RemoveConflictingContacts(contacts, ignored_contacts); + + // Convert contacts into constraints + ConstraintList constraints(inAllocator); + constraints.reserve(contacts.size() * 2); + DetermineConstraints(contacts, inDeltaTime, constraints); + +#ifdef JPH_DEBUG_RENDERER + bool draw_constraints = inDrawConstraints && iteration == 0; + if (draw_constraints) + { + for (const Constraint &c : constraints) + { + // Draw contact point + DebugRenderer::sInstance->DrawMarker(c.mContact->mPosition, Color::sYellow, 0.05f); + Vec3 dist_to_plane = -c.mPlane.GetConstant() * c.mPlane.GetNormal(); + + // Draw arrow towards surface that we're hitting + DebugRenderer::sInstance->DrawArrow(c.mContact->mPosition, c.mContact->mPosition - dist_to_plane, Color::sYellow, 0.05f); + + // Draw plane around the player position indicating the space that we can move + DebugRenderer::sInstance->DrawPlane(mPosition + dist_to_plane, c.mPlane.GetNormal(), Color::sCyan, 1.0f); + DebugRenderer::sInstance->DrawArrow(mPosition + dist_to_plane, mPosition + dist_to_plane + c.mContact->mSurfaceNormal, Color::sRed, 0.05f); + } + } +#endif // JPH_DEBUG_RENDERER + + // Solve the displacement using these constraints + Vec3 displacement; + float time_simulated; + SolveConstraints(inVelocity, inDeltaTime, time_remaining, constraints, ignored_contacts, time_simulated, displacement, inAllocator + #ifdef JPH_DEBUG_RENDERER + , draw_constraints + #endif // JPH_DEBUG_RENDERER + ); + + // Store the contacts now that the colliding ones have been marked + if (outActiveContacts != nullptr) + outActiveContacts->assign(contacts.begin(), contacts.end()); + + // Do a sweep to test if the path is really unobstructed + Contact cast_contact; + if (GetFirstContactForSweep(ioPosition, displacement, cast_contact, ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + { + displacement *= cast_contact.mFraction; + time_simulated *= cast_contact.mFraction; + } + + // Update the position + ioPosition += displacement; + time_remaining -= time_simulated; + + // If the displacement during this iteration was too small we assume we cannot further progress this update + if (displacement.LengthSq() < 1.0e-8f) + break; + } +} + +void CharacterVirtual::SetUserData(uint64 inUserData) +{ + mUserData = inUserData; + + if (!mInnerBodyID.IsInvalid()) + mSystem->GetBodyInterface().SetUserData(mInnerBodyID, inUserData); +} + +Vec3 CharacterVirtual::CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const +{ + // If we're not pushing against a steep slope, return the desired velocity + // Note: This is important as WalkStairs overrides the ground state to OnGround when its first check fails but the second succeeds + if (mGroundState == CharacterVirtual::EGroundState::OnGround + || mGroundState == CharacterVirtual::EGroundState::InAir) + return inDesiredVelocity; + + Vec3 desired_velocity = inDesiredVelocity; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && IsSlopeTooSteep(c.mSurfaceNormal)) + { + // Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement. + Vec3 normal = c.mContactNormal; + + // Remove normal vertical component + normal -= normal.Dot(mUp) * mUp; + + // Cancel horizontal movement in opposite direction + float dot = normal.Dot(desired_velocity); + if (dot < 0.0f) + desired_velocity -= (dot * normal) / normal.LengthSq(); + } + return desired_velocity; +} + +void CharacterVirtual::Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // If there's no delta time, we don't need to do anything + if (inDeltaTime <= 0.0f) + return; + + // Remember delta time for checking if we're supported by the ground + mLastDeltaTime = inDeltaTime; + + // Slide the shape through the world + MoveShape(mPosition, mLinearVelocity, inDeltaTime, &mActiveContacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator + #ifdef JPH_DEBUG_RENDERER + , sDrawConstraints + #endif // JPH_DEBUG_RENDERER + ); + + // Determine the object that we're standing on + UpdateSupportingContact(false, inAllocator); + + // Ensure that the rigid body ends up at the new position + UpdateInnerBodyTransform(); + + // If we're on the ground + if (!mGroundBodyID.IsInvalid() && mMass > 0.0f) + { + // Add the impulse to the ground due to gravity: P = F dt = M g dt + float normal_dot_gravity = mGroundNormal.Dot(inGravity); + if (normal_dot_gravity < 0.0f) + { + Vec3 world_impulse = -(mMass * normal_dot_gravity / inGravity.Length() * inDeltaTime) * inGravity; + mSystem->GetBodyInterface().AddImpulse(mGroundBodyID, world_impulse, mGroundPosition); + } + } +} + +void CharacterVirtual::RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Determine the contacts + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + StoreActiveContacts(contacts, inAllocator); +} + +void CharacterVirtual::UpdateGroundVelocity() +{ + BodyLockRead lock(mSystem->GetBodyLockInterface(), mGroundBodyID); + if (lock.SucceededAndIsInBroadPhase()) + { + const Body &body = lock.GetBody(); + + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity); + + // Calculate the ground velocity + mGroundVelocity = CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime); + } +} + +void CharacterVirtual::MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Set the new position + SetPosition(inPosition); + + // Trigger contact added callback + CharacterContactSettings dummy; + ContactAdded(inContact, dummy); + + // Determine the contacts + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits + 1); // +1 because we can add one extra below + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + // Ensure that we mark inContact as colliding + bool found_contact = false; + for (Contact &c : contacts) + if (c.mBodyB == inContact.mBodyB + && c.mSubShapeIDB == inContact.mSubShapeIDB) + { + c.mHadCollision = true; + found_contact = true; + } + if (!found_contact) + { + contacts.push_back(inContact); + + Contact © = contacts.back(); + copy.mHadCollision = true; + } + + StoreActiveContacts(contacts, inAllocator); + JPH_ASSERT(mGroundState != EGroundState::InAir); + + // Ensure that the rigid body ends up at the new position + UpdateInnerBodyTransform(); +} + +bool CharacterVirtual::SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + if (mShape == nullptr || mSystem == nullptr) + { + // It hasn't been initialized yet + mShape = inShape; + return true; + } + + if (inShape != mShape && inShape != nullptr) + { + if (inMaxPenetrationDepth < FLT_MAX) + { + // Check collision around the new shape + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), inShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + // Test if this results in penetration, if so cancel the transition + for (const Contact &c : contacts) + if (c.mDistance < -inMaxPenetrationDepth + && !c.mIsSensorB) + return false; + + StoreActiveContacts(contacts, inAllocator); + } + + // Set new shape + mShape = inShape; + } + + return mShape == inShape; +} + +void CharacterVirtual::SetInnerBodyShape(const Shape *inShape) +{ + mSystem->GetBodyInterface().SetShape(mInnerBodyID, inShape, false, EActivation::DontActivate); +} + +bool CharacterVirtual::CanWalkStairs(Vec3Arg inLinearVelocity) const +{ + // We can only walk stairs if we're supported + if (!IsSupported()) + return false; + + // Check if there's enough horizontal velocity to trigger a stair walk + Vec3 horizontal_velocity = inLinearVelocity - inLinearVelocity.Dot(mUp) * mUp; + if (horizontal_velocity.IsNearZero(1.0e-6f)) + return false; + + // Check contacts for steep slopes + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact + && IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep + return true; + + return false; +} + +bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Move up + Vec3 up = inStepUp; + Contact contact; + IgnoredContactList dummy_ignored_contacts(inAllocator); + if (GetFirstContactForSweep(mPosition, up, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + { + if (contact.mFraction < 1.0e-6f) + return false; // No movement, cancel + + // Limit up movement to the first contact point + up *= contact.mFraction; + } + RVec3 up_position = mPosition + up; + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep up + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(mPosition, up_position, Color::sWhite, 0.01f); +#endif // JPH_DEBUG_RENDERER + + // Collect normals of steep slopes that we would like to walk stairs on. + // We need to do this before calling MoveShape because it will update mActiveContacts. + Vec3 character_velocity = inStepForward / inDeltaTime; + Vec3 horizontal_velocity = character_velocity - character_velocity.Dot(mUp) * mUp; + Array> steep_slope_normals(inAllocator); + steep_slope_normals.reserve(mActiveContacts.size()); + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact + && IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep + steep_slope_normals.push_back(c.mSurfaceNormal); + if (steep_slope_normals.empty()) + return false; // No steep slopes, cancel + + // Horizontal movement + RVec3 new_position = up_position; + MoveShape(new_position, character_velocity, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + Vec3 horizontal_movement = Vec3(new_position - up_position); + float horizontal_movement_sq = horizontal_movement.LengthSq(); + if (horizontal_movement_sq < 1.0e-8f) + return false; // No movement, cancel + + // Check if we made any progress towards any of the steep slopes, if not we just slid along the slope + // so we need to cancel the stair walk or else we will move faster than we should as we've done + // normal movement first and then stair walk. + bool made_progress = false; + float max_dot = -0.05f * inStepForward.Length(); + for (const Vec3 &normal : steep_slope_normals) + if (normal.Dot(horizontal_movement) < max_dot) + { + // We moved more than 5% of the forward step against a steep slope, accept this as progress + made_progress = true; + break; + } + if (!made_progress) + return false; + +#ifdef JPH_DEBUG_RENDERER + // Draw horizontal sweep + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(up_position, new_position, Color::sWhite, 0.01f); +#endif // JPH_DEBUG_RENDERER + + // Move down towards the floor. + // Note that we travel the same amount down as we traveled up with the specified extra + Vec3 down = -up + inStepDownExtra; + if (!GetFirstContactForSweep(new_position, down, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; // No floor found, we're in mid air, cancel stair walk + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep down + if (sDrawWalkStairs) + { + RVec3 debug_pos = new_position + contact.mFraction * down; + DebugRenderer::sInstance->DrawArrow(new_position, debug_pos, Color::sWhite, 0.01f); + DebugRenderer::sInstance->DrawArrow(contact.mPosition, contact.mPosition + contact.mSurfaceNormal, Color::sWhite, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sWhite, false, true); + } +#endif // JPH_DEBUG_RENDERER + + // Test for floor that will support the character + if (IsSlopeTooSteep(contact.mSurfaceNormal)) + { + // If no test position was provided, we cancel the stair walk + if (inStepForwardTest.IsNearZero()) + return false; + + // Delta time may be very small, so it may be that we hit the edge of a step and the normal is too horizontal. + // In order to judge if the floor is flat further along the sweep, we test again for a floor at inStepForwardTest + // and check if the normal is valid there. + RVec3 test_position = up_position; + MoveShape(test_position, inStepForwardTest / inDeltaTime, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + float test_horizontal_movement_sq = Vec3(test_position - up_position).LengthSq(); + if (test_horizontal_movement_sq <= horizontal_movement_sq + 1.0e-8f) + return false; // We didn't move any further than in the previous test + + #ifdef JPH_DEBUG_RENDERER + // Draw 2nd sweep horizontal + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(up_position, test_position, Color::sCyan, 0.01f); + #endif // JPH_DEBUG_RENDERER + + // Then sweep down + Contact test_contact; + if (!GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; + + #ifdef JPH_DEBUG_RENDERER + // Draw 2nd sweep down + if (sDrawWalkStairs) + { + RVec3 debug_pos = test_position + test_contact.mFraction * down; + DebugRenderer::sInstance->DrawArrow(test_position, debug_pos, Color::sCyan, 0.01f); + DebugRenderer::sInstance->DrawArrow(test_contact.mPosition, test_contact.mPosition + test_contact.mSurfaceNormal, Color::sCyan, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sCyan, false, true); + } + #endif // JPH_DEBUG_RENDERER + + if (IsSlopeTooSteep(test_contact.mSurfaceNormal)) + return false; + } + + // Calculate new down position + down *= contact.mFraction; + new_position += down; + + // Move the character to the new location + MoveToContact(new_position, contact, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + + // Override ground state to 'on ground', it is possible that the contact normal is too steep, but in this case the inStepForwardTest has found a contact normal that is not too steep + mGroundState = EGroundState::OnGround; + + return true; +} + +bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Try to find the floor + Contact contact; + IgnoredContactList dummy_ignored_contacts(inAllocator); + if (!GetFirstContactForSweep(mPosition, inStepDown, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; // If no floor found, don't update our position + + // Calculate new position + RVec3 new_position = mPosition + contact.mFraction * inStepDown; + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep down + if (sDrawStickToFloor) + { + DebugRenderer::sInstance->DrawArrow(mPosition, new_position, Color::sOrange, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(new_position, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sOrange, false, true); + } +#endif // JPH_DEBUG_RENDERER + + // Move the character to the new location + MoveToContact(new_position, contact, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + return true; +} + +void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Update the velocity + Vec3 desired_velocity = mLinearVelocity; + mLinearVelocity = CancelVelocityTowardsSteepSlopes(desired_velocity); + + // Remember old position + RVec3 old_position = mPosition; + + // Track if on ground before the update + bool ground_to_air = IsSupported(); + + // Update the character position (instant, do not have to wait for physics update) + Update(inDeltaTime, inGravity, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + + // ... and that we got into air after + if (IsSupported()) + ground_to_air = false; + + // If stick to floor enabled and we're going from supported to not supported + if (ground_to_air && !inSettings.mStickToFloorStepDown.IsNearZero()) + { + // If we're not moving up, stick to the floor + float velocity = Vec3(mPosition - old_position).Dot(mUp) / inDeltaTime; + if (velocity <= 1.0e-6f) + StickToFloor(inSettings.mStickToFloorStepDown, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + } + + // If walk stairs enabled + if (!inSettings.mWalkStairsStepUp.IsNearZero()) + { + // Calculate how much we wanted to move horizontally + Vec3 desired_horizontal_step = desired_velocity * inDeltaTime; + desired_horizontal_step -= desired_horizontal_step.Dot(mUp) * mUp; + float desired_horizontal_step_len = desired_horizontal_step.Length(); + if (desired_horizontal_step_len > 0.0f) + { + // Calculate how much we moved horizontally + Vec3 achieved_horizontal_step = Vec3(mPosition - old_position); + achieved_horizontal_step -= achieved_horizontal_step.Dot(mUp) * mUp; + + // Only count movement in the direction of the desired movement + // (otherwise we find it ok if we're sliding downhill while we're trying to climb uphill) + Vec3 step_forward_normalized = desired_horizontal_step / desired_horizontal_step_len; + achieved_horizontal_step = max(0.0f, achieved_horizontal_step.Dot(step_forward_normalized)) * step_forward_normalized; + float achieved_horizontal_step_len = achieved_horizontal_step.Length(); + + // If we didn't move as far as we wanted and we're against a slope that's too steep + if (achieved_horizontal_step_len + 1.0e-4f < desired_horizontal_step_len + && CanWalkStairs(desired_velocity)) + { + // Calculate how much we should step forward + // Note that we clamp the step forward to a minimum distance. This is done because at very high frame rates the delta time + // may be very small, causing a very small step forward. If the step becomes small enough, we may not move far enough + // horizontally to actually end up at the top of the step. + Vec3 step_forward = step_forward_normalized * max(inSettings.mWalkStairsMinStepForward, desired_horizontal_step_len - achieved_horizontal_step_len); + + // Calculate how far to scan ahead for a floor. This is only used in case the floor normal at step_forward is too steep. + // In that case an additional check will be performed at this distance to check if that normal is not too steep. + // Start with the ground normal in the horizontal plane and normalizing it + Vec3 step_forward_test = -mGroundNormal; + step_forward_test -= step_forward_test.Dot(mUp) * mUp; + step_forward_test = step_forward_test.NormalizedOr(step_forward_normalized); + + // If this normalized vector and the character forward vector is bigger than a preset angle, we use the character forward vector instead of the ground normal + // to do our forward test + if (step_forward_test.Dot(step_forward_normalized) < inSettings.mWalkStairsCosAngleForwardContact) + step_forward_test = step_forward_normalized; + + // Calculate the correct magnitude for the test vector + step_forward_test *= inSettings.mWalkStairsStepForwardTest; + + WalkStairs(inDeltaTime, inSettings.mWalkStairsStepUp, step_forward, step_forward_test, inSettings.mWalkStairsStepDownExtra, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + } + } + } +} + +void CharacterVirtual::Contact::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mLinearVelocity); + inStream.Write(mContactNormal); + inStream.Write(mSurfaceNormal); + inStream.Write(mDistance); + inStream.Write(mFraction); + inStream.Write(mBodyB); + inStream.Write(mSubShapeIDB); + inStream.Write(mMotionTypeB); + inStream.Write(mHadCollision); + inStream.Write(mWasDiscarded); + inStream.Write(mCanPushCharacter); + // Cannot store user data (may be a pointer) and material +} + +void CharacterVirtual::Contact::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mLinearVelocity); + inStream.Read(mContactNormal); + inStream.Read(mSurfaceNormal); + inStream.Read(mDistance); + inStream.Read(mFraction); + inStream.Read(mBodyB); + inStream.Read(mSubShapeIDB); + inStream.Read(mMotionTypeB); + inStream.Read(mHadCollision); + inStream.Read(mWasDiscarded); + inStream.Read(mCanPushCharacter); + mUserData = 0; // Cannot restore user data + mMaterial = PhysicsMaterial::sDefault; // Cannot restore material +} + +void CharacterVirtual::SaveState(StateRecorder &inStream) const +{ + CharacterBase::SaveState(inStream); + + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mLinearVelocity); + inStream.Write(mLastDeltaTime); + inStream.Write(mMaxHitsExceeded); + + // Store contacts that had collision, we're using it at the beginning of the step in CancelVelocityTowardsSteepSlopes + uint32 num_contacts = 0; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + ++num_contacts; + inStream.Write(num_contacts); + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + c.SaveState(inStream); +} + +void CharacterVirtual::RestoreState(StateRecorder &inStream) +{ + CharacterBase::RestoreState(inStream); + + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mLinearVelocity); + inStream.Read(mLastDeltaTime); + inStream.Read(mMaxHitsExceeded); + + // When validating remove contacts that don't have collision since we didn't save them + if (inStream.IsValidating()) + for (int i = (int)mActiveContacts.size() - 1; i >= 0; --i) + if (!mActiveContacts[i].mHadCollision) + mActiveContacts.erase(mActiveContacts.begin() + i); + + uint32 num_contacts = (uint32)mActiveContacts.size(); + inStream.Read(num_contacts); + mActiveContacts.resize(num_contacts); + for (Contact &c : mActiveContacts) + c.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h new file mode 100644 index 000000000000..65b8f89df560 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h @@ -0,0 +1,642 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CharacterVirtual; +class CollideShapeSettings; + +/// Contains the configuration of a character +class JPH_EXPORT CharacterVirtualSettings : public CharacterBaseSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Character mass (kg). Used to push down objects with gravity when the character is standing on top. + float mMass = 70.0f; + + /// Maximum force with which the character can push other bodies (N). + float mMaxStrength = 100.0f; + + /// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. + Vec3 mShapeOffset = Vec3::sZero(); + + ///@name Movement settings + EBackFaceMode mBackFaceMode = EBackFaceMode::CollideWithBackFaces; ///< When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides. + float mPredictiveContactDistance = 0.1f; ///< How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions. + uint mMaxCollisionIterations = 5; ///< Max amount of collision loops + uint mMaxConstraintIterations = 15; ///< How often to try stepping in the constraint solving + float mMinTimeRemaining = 1.0e-4f; ///< Early out condition: If this much time is left to simulate we are done + float mCollisionTolerance = 1.0e-3f; ///< How far we're willing to penetrate geometry + float mCharacterPadding = 0.02f; ///< How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck + uint mMaxNumHits = 256; ///< Max num hits to collect in order to avoid excess of contact points collection + float mHitReductionCosMaxAngle = 0.999f; ///< Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float mPenetrationRecoverySpeed = 1.0f; ///< This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update + + /// This character can optionally have an inner rigid body. This rigid body can be used to give the character presence in the world. When set it means that: + /// - Regular collision checks (e.g. NarrowPhaseQuery::CastRay) will collide with the rigid body (they cannot collide with CharacterVirtual since it is not added to the broad phase) + /// - Regular contact callbacks will be called through the ContactListener (next to the ones that will be passed to the CharacterContactListener) + /// - Fast moving objects of motion quality LinearCast will not be able to pass through the CharacterVirtual in 1 time step + RefConst mInnerBodyShape; + + /// Layer that the inner rigid body will be added to + ObjectLayer mInnerBodyLayer = 0; +}; + +/// This class contains settings that allow you to override the behavior of a character's collision response +class CharacterContactSettings +{ +public: + /// True when the object can push the virtual character. + bool mCanPushCharacter = true; + + /// True when the virtual character can apply impulses (push) the body. + /// Note that this only works against rigid bodies. Other CharacterVirtual objects can only be moved in their own update, + /// so you must ensure that in their OnCharacterContactAdded mCanPushCharacter is true. + bool mCanReceiveImpulses = true; +}; + +/// This class receives callbacks when a virtual character hits something. +class JPH_EXPORT CharacterContactListener +{ +public: + /// Destructor + virtual ~CharacterContactListener() = default; + + /// Callback to adjust the velocity of a body as seen by the character. Can be adjusted to e.g. implement a conveyor belt or an inertial dampener system of a sci-fi space ship. + /// Note that inBody2 is locked during the callback so you can read its properties freely. + virtual void OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity) { /* Do nothing, the linear and angular velocity are already filled in */ } + + /// Checks if a character can collide with specified body. Return true if the contact is valid. + virtual bool OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) { return true; } + + /// Same as OnContactValidate but when colliding with a CharacterVirtual + virtual bool OnCharacterContactValidate(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2) { return true; } + + /// Called whenever the character collides with a body. + /// @param inCharacter Character that is being solved + /// @param inBodyID2 Body ID of body that is being hit + /// @param inSubShapeID2 Sub shape ID of shape that is being hit + /// @param inContactPosition World space contact position + /// @param inContactNormal World space contact normal + /// @param ioSettings Settings returned by the contact callback to indicate how the character should behave + virtual void OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ } + + /// Same as OnContactAdded but when colliding with a CharacterVirtual + virtual void OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ } + + /// Called whenever a contact is being used by the solver. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces). + /// @param inCharacter Character that is being solved + /// @param inBodyID2 Body ID of body that is being hit + /// @param inSubShapeID2 Sub shape ID of shape that is being hit + /// @param inContactPosition World space contact position + /// @param inContactNormal World space contact normal + /// @param inContactVelocity World space velocity of contact point (e.g. for a moving platform) + /// @param inContactMaterial Material of contact point + /// @param inCharacterVelocity World space velocity of the character prior to hitting this contact + /// @param ioNewCharacterVelocity Contains the calculated world space velocity of the character after hitting this contact, this velocity slides along the surface of the contact. Can be modified by the listener to provide an alternative velocity. + virtual void OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ } + + /// Same as OnContactSolve but when colliding with a CharacterVirtual + virtual void OnCharacterContactSolve(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ } +}; + +/// Interface class that allows a CharacterVirtual to check collision with other CharacterVirtual instances. +/// Since CharacterVirtual instances are not registered anywhere, it is up to the application to test collision against relevant characters. +/// The characters could be stored in a tree structure to make this more efficient. +class JPH_EXPORT CharacterVsCharacterCollision : public NonCopyable +{ +public: + virtual ~CharacterVsCharacterCollision() = default; + + /// Collide a character against other CharacterVirtuals. + /// @param inCharacter The character to collide. + /// @param inCenterOfMassTransform Center of mass transform for this character. + /// @param inCollideShapeSettings Settings for the collision check. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + virtual void CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const = 0; + + /// Cast a character against other CharacterVirtuals. + /// @param inCharacter The character to cast. + /// @param inCenterOfMassTransform Center of mass transform for this character. + /// @param inDirection Direction and length to cast in. + /// @param inShapeCastSettings Settings for the shape cast. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + virtual void CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const = 0; +}; + +/// Simple collision checker that loops over all registered characters. +/// Note that this is not thread safe, so make sure that only one CharacterVirtual is checking collision at a time. +class JPH_EXPORT CharacterVsCharacterCollisionSimple : public CharacterVsCharacterCollision +{ +public: + /// Add a character to the list of characters to check collision against. + void Add(CharacterVirtual *inCharacter) { mCharacters.push_back(inCharacter); } + + /// Remove a character from the list of characters to check collision against. + void Remove(const CharacterVirtual *inCharacter); + + // See: CharacterVsCharacterCollision + virtual void CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const override; + virtual void CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const override; + + Array mCharacters; ///< The list of characters to check collision against +}; + +/// Runtime character object. +/// This object usually represents the player. Contrary to the Character class it doesn't use a rigid body but moves doing collision checks only (hence the name virtual). +/// The advantage of this is that you can determine when the character moves in the frame (usually this has to happen at a very particular point in the frame) +/// but the downside is that other objects don't see this virtual character. In order to make this work it is recommended to pair a CharacterVirtual with a Character that +/// moves along. This Character should be keyframed (or at least have no gravity) and move along with the CharacterVirtual so that other rigid bodies can collide with it. +class JPH_EXPORT CharacterVirtual : public CharacterBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inSettings The settings for the character + /// @param inPosition Initial position for the character + /// @param inRotation Initial rotation for the character (usually only around the up-axis) + /// @param inUserData Application specific value + /// @param inSystem Physics system that this character will be added to + CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem); + + /// Constructor without user data + CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem) : CharacterVirtual(inSettings, inPosition, inRotation, 0, inSystem) { } + + /// Destructor + virtual ~CharacterVirtual() override; + + /// Set the contact listener + void SetListener(CharacterContactListener *inListener) { mListener = inListener; } + + /// Get the current contact listener + CharacterContactListener * GetListener() const { return mListener; } + + /// Set the character vs character collision interface + void SetCharacterVsCharacterCollision(CharacterVsCharacterCollision *inCharacterVsCharacterCollision) { mCharacterVsCharacterCollision = inCharacterVsCharacterCollision; } + + /// Get the linear velocity of the character (m / s) + Vec3 GetLinearVelocity() const { return mLinearVelocity; } + + /// Set the linear velocity of the character (m / s) + void SetLinearVelocity(Vec3Arg inLinearVelocity) { mLinearVelocity = inLinearVelocity; } + + /// Get the position of the character + RVec3 GetPosition() const { return mPosition; } + + /// Set the position of the character + void SetPosition(RVec3Arg inPosition) { mPosition = inPosition; UpdateInnerBodyTransform(); } + + /// Get the rotation of the character + Quat GetRotation() const { return mRotation; } + + /// Set the rotation of the character + void SetRotation(QuatArg inRotation) { mRotation = inRotation; UpdateInnerBodyTransform(); } + + // Get the center of mass position of the shape + inline RVec3 GetCenterOfMassPosition() const { return mPosition + (mRotation * (mShapeOffset + mShape->GetCenterOfMass()) + mCharacterPadding * mUp); } + + /// Calculate the world transform of the character + RMat44 GetWorldTransform() const { return RMat44::sRotationTranslation(mRotation, mPosition); } + + /// Calculates the transform for this character's center of mass + RMat44 GetCenterOfMassTransform() const { return GetCenterOfMassTransform(mPosition, mRotation, mShape); } + + /// Character mass (kg) + float GetMass() const { return mMass; } + void SetMass(float inMass) { mMass = inMass; } + + /// Maximum force with which the character can push other bodies (N) + float GetMaxStrength() const { return mMaxStrength; } + void SetMaxStrength(float inMaxStrength) { mMaxStrength = inMaxStrength; } + + /// This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update + float GetPenetrationRecoverySpeed() const { return mPenetrationRecoverySpeed; } + void SetPenetrationRecoverySpeed(float inSpeed) { mPenetrationRecoverySpeed = inSpeed; } + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool GetEnhancedInternalEdgeRemoval() const { return mEnhancedInternalEdgeRemoval; } + void SetEnhancedInternalEdgeRemoval(bool inApply) { mEnhancedInternalEdgeRemoval = inApply; } + + /// Character padding + float GetCharacterPadding() const { return mCharacterPadding; } + + /// Max num hits to collect in order to avoid excess of contact points collection + uint GetMaxNumHits() const { return mMaxNumHits; } + void SetMaxNumHits(uint inMaxHits) { mMaxNumHits = inMaxHits; } + + /// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float GetHitReductionCosMaxAngle() const { return mHitReductionCosMaxAngle; } + void SetHitReductionCosMaxAngle(float inCosMaxAngle) { mHitReductionCosMaxAngle = inCosMaxAngle; } + + /// Returns if we exceeded the maximum number of hits during the last collision check and had to discard hits based on distance. + /// This can be used to find areas that have too complex geometry for the character to navigate properly. + /// To solve you can either increase the max number of hits or simplify the geometry. Note that the character simulation will + /// try to do its best to select the most relevant contacts to avoid the character from getting stuck. + bool GetMaxHitsExceeded() const { return mMaxHitsExceeded; } + + /// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. Note that setting it on the fly can cause the shape to teleport into collision. + Vec3 GetShapeOffset() const { return mShapeOffset; } + void SetShapeOffset(Vec3Arg inShapeOffset) { mShapeOffset = inShapeOffset; UpdateInnerBodyTransform(); } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData); + + /// Optional inner rigid body that proxies the character in the world. Can be used to update body properties. + BodyID GetInnerBodyID() const { return mInnerBodyID; } + + /// This function can be called prior to calling Update() to convert a desired velocity into a velocity that won't make the character move further onto steep slopes. + /// This velocity can then be set on the character using SetLinearVelocity() + /// @param inDesiredVelocity Velocity to clamp against steep walls + /// @return A new velocity vector that won't make the character move up steep slopes + Vec3 CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const; + + /// This is the main update function. It moves the character according to its current velocity (the character is similar to a kinematic body in the sense + /// that you set the velocity and the character will follow unless collision is blocking the way). Note it's your own responsibility to apply gravity to the character velocity! + /// Different surface materials (like ice) can be emulated by getting the ground material and adjusting the velocity and/or the max slope angle accordingly every frame. + /// @param inDeltaTime Time step to simulate. + /// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + void Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function will return true if the character has moved into a slope that is too steep (e.g. a vertical wall). + /// You would call WalkStairs to attempt to step up stairs. + /// @param inLinearVelocity The linear velocity that the player desired. This is used to determine if we're pushing into a step. + bool CanWalkStairs(Vec3Arg inLinearVelocity) const; + + /// When stair walking is needed, you can call the WalkStairs function to cast up, forward and down again to try to find a valid position + /// @param inDeltaTime Time step to simulate. + /// @param inStepUp The direction and distance to step up (this corresponds to the max step height) + /// @param inStepForward The direction and distance to step forward after the step up + /// @param inStepForwardTest When running at a high frequency, inStepForward can be very small and it's likely that you hit the side of the stairs on the way down. This could produce a normal that violates the max slope angle. If this happens, we test again using this distance from the up position to see if we find a valid slope. + /// @param inStepDownExtra An additional translation that is added when stepping down at the end. Allows you to step further down than up. Set to zero if you don't want this. Should be in the opposite direction of up. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return true if the stair walk was successful + bool WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function can be used to artificially keep the character to the floor. Normally when a character is on a small step and starts moving horizontally, the character will + /// lose contact with the floor because the initial vertical velocity is zero while the horizontal velocity is quite high. To prevent the character from losing contact with the floor, + /// we do an additional collision check downwards and if we find the floor within a certain distance, we project the character onto the floor. + /// @param inStepDown Max amount to project the character downwards (if no floor is found within this distance, the function will return false) + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return True if the character was successfully projected onto the floor. + bool StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// Settings struct with settings for ExtendedUpdate + struct ExtendedUpdateSettings + { + Vec3 mStickToFloorStepDown { 0, -0.5f, 0 }; ///< See StickToFloor inStepDown parameter. Can be zero to turn off. + Vec3 mWalkStairsStepUp { 0, 0.4f, 0 }; ///< See WalkStairs inStepUp parameter. Can be zero to turn off. + float mWalkStairsMinStepForward { 0.02f }; ///< See WalkStairs inStepForward parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity. + float mWalkStairsStepForwardTest { 0.15f }; ///< See WalkStairs inStepForwardTest parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity. + float mWalkStairsCosAngleForwardContact { Cos(DegreesToRadians(75.0f)) }; ///< Cos(angle) where angle is the maximum angle between the ground normal in the horizontal plane and the character forward vector where we're willing to adjust the step forward test towards the contact normal. + Vec3 mWalkStairsStepDownExtra { Vec3::sZero() }; ///< See WalkStairs inStepDownExtra + }; + + /// This function combines Update, StickToFloor and WalkStairs. This function serves as an example of how these functions could be combined. + /// Before calling, call SetLinearVelocity to update the horizontal/vertical speed of the character, typically this is: + /// - When on OnGround and not moving away from ground: velocity = GetGroundVelocity() + horizontal speed as input by player + optional vertical jump velocity + delta time * gravity + /// - Else: velocity = current vertical velocity + horizontal speed as input by player + delta time * gravity + /// @param inDeltaTime Time step to simulate. + /// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force. + /// @param inSettings A structure containing settings for the algorithm. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + void ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function can be used after a character has teleported to determine the new contacts with the world. + void RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// Use the ground body ID to get an updated estimate of the ground velocity. This function can be used if the ground body has moved / changed velocity and you want a new estimate of the ground velocity. + /// It will not perform collision detection, so is less accurate than RefreshContacts but a lot faster. + void UpdateGroundVelocity(); + + /// Switch the shape of the character (e.g. for stance). + /// @param inShape The shape to switch to. + /// @param inMaxPenetrationDepth When inMaxPenetrationDepth is not FLT_MAX, it checks if the new shape collides before switching shape. This is the max penetration we're willing to accept after the switch. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return Returns true if the switch succeeded. + bool SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// Updates the shape of the inner rigid body. Should be called after a successful call to SetShape. + void SetInnerBodyShape(const Shape *inShape); + + /// Get the transformed shape that represents the volume of the character, can be used for collision checks. + TransformedShape GetTransformedShape() const { return TransformedShape(GetCenterOfMassPosition(), mRotation, mShape, mInnerBodyID); } + + /// @brief Get all contacts for the character at a particular location. + /// When colliding with another character virtual, this pointer will be provided through CollideShapeCollector::SetUserContext before adding a hit. + /// @param inPosition Position to test, note that this position will be corrected for the character padding. + /// @param inRotation Rotation at which to test the shape. + /// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal. + /// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly). + /// @param inShape Shape to test collision with. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + void CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + + // Saving / restoring state for replay + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + +#ifdef JPH_DEBUG_RENDERER + static inline bool sDrawConstraints = false; ///< Draw the current state of the constraints for iteration 0 when creating them + static inline bool sDrawWalkStairs = false; ///< Draw the state of the walk stairs algorithm + static inline bool sDrawStickToFloor = false; ///< Draw the state of the stick to floor algorithm +#endif + + // Encapsulates a collision contact + struct Contact + { + // Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + // Checks if two contacts refer to the same body (or virtual character) + inline bool IsSameBody(const Contact &inOther) const { return mBodyB == inOther.mBodyB && mCharacterB == inOther.mCharacterB; } + + RVec3 mPosition; ///< Position where the character makes contact + Vec3 mLinearVelocity; ///< Velocity of the contact point + Vec3 mContactNormal; ///< Contact normal, pointing towards the character + Vec3 mSurfaceNormal; ///< Surface normal of the contact + float mDistance; ///< Distance to the contact <= 0 means that it is an actual contact, > 0 means predictive + float mFraction; ///< Fraction along the path where this contact takes place + BodyID mBodyB; ///< ID of body we're colliding with (if not invalid) + CharacterVirtual * mCharacterB = nullptr; ///< Character we're colliding with (if not null) + SubShapeID mSubShapeIDB; ///< Sub shape ID of body we're colliding with + EMotionType mMotionTypeB; ///< Motion type of B, used to determine the priority of the contact + bool mIsSensorB; ///< If B is a sensor + uint64 mUserData; ///< User data of B + const PhysicsMaterial * mMaterial; ///< Material of B + bool mHadCollision = false; ///< If the character actually collided with the contact (can be false if a predictive contact never becomes a real one) + bool mWasDiscarded = false; ///< If the contact validate callback chose to discard this contact + bool mCanPushCharacter = true; ///< When true, the velocity of the contact point can push the character + }; + + using TempContactList = Array>; + using ContactList = Array; + + /// Access to the internal list of contacts that the character has found. + const ContactList & GetActiveContacts() const { return mActiveContacts; } + + /// Check if the character is currently in contact with or has collided with another body in the last time step + bool HasCollidedWith(const BodyID &inBody) const + { + for (const CharacterVirtual::Contact &c : mActiveContacts) + if (c.mHadCollision && c.mBodyB == inBody) + return true; + return false; + } + + /// Check if the character is currently in contact with or has collided with another character in the last time step + bool HasCollidedWith(const CharacterVirtual *inCharacter) const + { + for (const CharacterVirtual::Contact &c : mActiveContacts) + if (c.mHadCollision && c.mCharacterB == inCharacter) + return true; + return false; + } + +private: + // Sorting predicate for making contact order deterministic + struct ContactOrderingPredicate + { + inline bool operator () (const Contact &inLHS, const Contact &inRHS) const + { + if (inLHS.mBodyB != inRHS.mBodyB) + return inLHS.mBodyB < inRHS.mBodyB; + + return inLHS.mSubShapeIDB.GetValue() < inRHS.mSubShapeIDB.GetValue(); + } + }; + + // A contact that needs to be ignored + struct IgnoredContact + { + IgnoredContact() = default; + IgnoredContact(const BodyID &inBodyID, const SubShapeID &inSubShapeID) : mBodyID(inBodyID), mSubShapeID(inSubShapeID) { } + + BodyID mBodyID; ///< ID of body we're colliding with + SubShapeID mSubShapeID; ///< Sub shape of body we're colliding with + }; + + using IgnoredContactList = Array>; + + // A constraint that limits the movement of the character + struct Constraint + { + Contact * mContact; ///< Contact that this constraint was generated from + float mTOI; ///< Calculated time of impact (can be negative if penetrating) + float mProjectedVelocity; ///< Velocity of the contact projected on the contact normal (negative if separating) + Vec3 mLinearVelocity; ///< Velocity of the contact (can contain a corrective velocity to resolve penetration) + Plane mPlane; ///< Plane around the origin that describes how far we can displace (from the origin) + bool mIsSteepSlope = false; ///< If this constraint belongs to a steep slope + }; + + using ConstraintList = Array>; + + // Collision collector that collects hits for CollideShape + class ContactCollector : public CollideShapeCollector + { + public: + ContactCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, uint inMaxHits, float inHitReductionCosMaxAngle, Vec3Arg inUp, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mContacts(outContacts), mMaxHits(inMaxHits), mHitReductionCosMaxAngle(inHitReductionCosMaxAngle) { } + + virtual void SetUserData(uint64 inUserData) override { mOtherCharacter = reinterpret_cast(inUserData); } + + virtual void AddHit(const CollideShapeResult &inResult) override; + + RVec3 mBaseOffset; + Vec3 mUp; + PhysicsSystem * mSystem; + const CharacterVirtual * mCharacter; + CharacterVirtual * mOtherCharacter = nullptr; + TempContactList & mContacts; + uint mMaxHits; + float mHitReductionCosMaxAngle; + bool mMaxHitsExceeded = false; + }; + + // A collision collector that collects hits for CastShape + class ContactCastCollector : public CastShapeCollector + { + public: + ContactCastCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, Vec3Arg inDisplacement, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, RVec3Arg inBaseOffset, Contact &outContact) : mBaseOffset(inBaseOffset), mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mIgnoredContacts(inIgnoredContacts), mContact(outContact) { } + + virtual void SetUserData(uint64 inUserData) override { mOtherCharacter = reinterpret_cast(inUserData); } + + virtual void AddHit(const ShapeCastResult &inResult) override; + + RVec3 mBaseOffset; + Vec3 mDisplacement; + Vec3 mUp; + PhysicsSystem * mSystem; + const CharacterVirtual * mCharacter; + CharacterVirtual * mOtherCharacter = nullptr; + const IgnoredContactList & mIgnoredContacts; + Contact & mContact; + }; + + // Helper function to convert a Jolt collision result into a contact + template + inline static void sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult); + inline static void sFillCharacterContactProperties(Contact &outContact, CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult); + + // Move the shape from ioPosition and try to displace it by inVelocity * inDeltaTime, this will try to slide the shape along the world geometry + void MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator + #ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints = false + #endif // JPH_DEBUG_RENDERER + ) const; + + // Ask the callback if inContact is a valid contact point + bool ValidateContact(const Contact &inContact) const; + + // Trigger the contact callback for inContact and get the contact settings + void ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings) const; + + // Tests the shape for collision around inPosition + void GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + + // Remove penetrating contacts with the same body that have conflicting normals, leaving these will make the character mover get stuck + void RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const; + + // Convert contacts into constraints. The character is assumed to start at the origin and the constraints are planes around the origin that confine the movement of the character. + void DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const; + + // Use the constraints to solve the displacement of the character. This will slide the character on the planes around the origin for as far as possible. + void SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator + #ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints = false + #endif // JPH_DEBUG_RENDERER + ) const; + + // Get the velocity of a body adjusted by the contact listener + void GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const; + + // Calculate the ground velocity of the character assuming it's standing on an object with specified linear and angular velocity and with specified center of mass. + // Note that we don't just take the point velocity because a point on an object with angular velocity traces an arc, + // so if you just take point velocity * delta time you get an error that accumulates over time + Vec3 CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const; + + // Handle contact with physics object that we're colliding against + bool HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const; + + // Does a swept test of the shape from inPosition with displacement inDisplacement, returns true if there was a collision + bool GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + + // Store contacts so that we have proper ground information + void StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator); + + // This function will determine which contacts are touching the character and will calculate the one that is supporting us + void UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator); + + /// This function can be called after moving the character to a new colliding position + void MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + // This function returns the actual center of mass of the shape, not corrected for the character padding + inline RMat44 GetCenterOfMassTransform(RVec3Arg inPosition, QuatArg inRotation, const Shape *inShape) const + { + return RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(mShapeOffset + inShape->GetCenterOfMass()).PostTranslated(mCharacterPadding * mUp); + } + + // This function returns the position of the inner rigid body + inline RVec3 GetInnerBodyPosition() const + { + return mPosition + (mRotation * mShapeOffset + mCharacterPadding * mUp); + } + + // Move the inner rigid body to the current position + void UpdateInnerBodyTransform(); + + // Our main listener for contacts + CharacterContactListener * mListener = nullptr; + + // Interface to detect collision between characters + CharacterVsCharacterCollision * mCharacterVsCharacterCollision = nullptr; + + // Movement settings + EBackFaceMode mBackFaceMode; // When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides. + float mPredictiveContactDistance; // How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions. + uint mMaxCollisionIterations; // Max amount of collision loops + uint mMaxConstraintIterations; // How often to try stepping in the constraint solving + float mMinTimeRemaining; // Early out condition: If this much time is left to simulate we are done + float mCollisionTolerance; // How far we're willing to penetrate geometry + float mCharacterPadding; // How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck + uint mMaxNumHits; // Max num hits to collect in order to avoid excess of contact points collection + float mHitReductionCosMaxAngle; // Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float mPenetrationRecoverySpeed; // This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update + bool mEnhancedInternalEdgeRemoval; // Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + + // Character mass (kg) + float mMass; + + // Maximum force with which the character can push other bodies (N) + float mMaxStrength; + + // An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. + Vec3 mShapeOffset = Vec3::sZero(); + + // Current position (of the base, not the center of mass) + RVec3 mPosition = RVec3::sZero(); + + // Current rotation (of the base, not of the center of mass) + Quat mRotation = Quat::sIdentity(); + + // Current linear velocity + Vec3 mLinearVelocity = Vec3::sZero(); + + // List of contacts that were active in the last frame + ContactList mActiveContacts; + + // Remembers the delta time of the last update + float mLastDeltaTime = 1.0f / 60.0f; + + // Remember if we exceeded the maximum number of hits and had to remove similar contacts + mutable bool mMaxHitsExceeded = false; + + // User data, can be used for anything by the application + uint64 mUserData = 0; + + // The inner rigid body that proxies the character in the world + BodyID mInnerBodyID; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h new file mode 100644 index 000000000000..a1cedf1ad925 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h @@ -0,0 +1,20 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds AABox moving linearly through 3d space +struct AABoxCast +{ + JPH_OVERRIDE_NEW_DELETE + + AABox mBox; ///< Axis aligned box at starting location + Vec3 mDirection; ///< Direction and length of the cast (anything beyond this length will not be reported as a hit) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h new file mode 100644 index 000000000000..30f96aeb123e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// How to treat active/inactive edges. +/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large, see: ActiveEdges +enum class EActiveEdgeMode : uint8 +{ + CollideOnlyWithActive, ///< Do not collide with inactive edges. For physics simulation, this gives less ghost collisions. + CollideWithAll, ///< Collide with all edges. Use this when you're interested in all collisions. +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h new file mode 100644 index 000000000000..7e51d2a63b72 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h @@ -0,0 +1,114 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large. +namespace ActiveEdges +{ + /// Helper function to check if an edge is active or not + /// @param inNormal1 Triangle normal of triangle on the left side of the edge (when looking along the edge from the top) + /// @param inNormal2 Triangle normal of triangle on the right side of the edge + /// @param inEdgeDirection Vector that points along the edge + /// @param inCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive) + inline static bool IsEdgeActive(Vec3Arg inNormal1, Vec3Arg inNormal2, Vec3Arg inEdgeDirection, float inCosThresholdAngle) + { + // If normals are opposite the edges are active (the triangles are back to back) + float cos_angle_normals = inNormal1.Dot(inNormal2); + if (cos_angle_normals < -0.999848f) // cos(179 degrees) + return true; + + // Check if concave edge, if so we are not active + if (inNormal1.Cross(inNormal2).Dot(inEdgeDirection) < 0.0f) + return false; + + // Convex edge, active when angle bigger than threshold + return cos_angle_normals < inCosThresholdAngle; + } + + /// Replace normal by triangle normal if a hit is hitting an inactive edge + /// @param inV0 , inV1 , inV2 form the triangle + /// @param inTriangleNormal is the normal of the provided triangle (does not need to be normalized) + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// @param inPoint Collision point on the triangle + /// @param inNormal Collision normal on the triangle (does not need to be normalized) + /// @param inMovementDirection Can be zero. This gives an indication of in which direction the motion is to determine if when we hit an inactive edge/triangle we should return the triangle normal. + /// @return Returns inNormal if an active edge was hit, otherwise returns inTriangleNormal + inline static Vec3 FixNormal(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inTriangleNormal, uint8 inActiveEdges, Vec3Arg inPoint, Vec3Arg inNormal, Vec3Arg inMovementDirection) + { + // Check: All of the edges are active, we have the correct normal already. No need to call this function! + JPH_ASSERT(inActiveEdges != 0b111); + + // If inNormal would affect movement less than inTriangleNormal use inNormal + // This is done since it is really hard to make a distinction between sliding over a horizontal triangulated grid and hitting an edge (in this case you want to use the triangle normal) + // and sliding over a triangulated grid and grazing a vertical triangle with an inactive edge (in this case using the triangle normal will cause the object to bounce back so we want to use the calculated normal). + // To solve this we take a movement hint to give an indication of what direction our object is moving. If the edge normal results in less motion difference than the triangle normal we use the edge normal. + float normal_length = inNormal.Length(); + float triangle_normal_length = inTriangleNormal.Length(); + if (inMovementDirection.Dot(inNormal) * triangle_normal_length < inMovementDirection.Dot(inTriangleNormal) * normal_length) + return inNormal; + + // Check: None of the edges are active, we need to use the triangle normal + if (inActiveEdges == 0) + return inTriangleNormal; + + // Some edges are active. + // If normal is parallel to the triangle normal we don't need to check the active edges. + if (inTriangleNormal.Dot(inNormal) > 0.999848f * normal_length * triangle_normal_length) // cos(1 degree) + return inNormal; + + const float cEpsilon = 1.0e-4f; + const float cOneMinusEpsilon = 1.0f - cEpsilon; + + uint colliding_edge; + + // Test where the contact point is in the triangle + float u, v, w; + ClosestPoint::GetBaryCentricCoordinates(inV0 - inPoint, inV1 - inPoint, inV2 - inPoint, u, v, w); + if (u > cOneMinusEpsilon) + { + // Colliding with v0, edge 0 or 2 needs to be active + colliding_edge = 0b101; + } + else if (v > cOneMinusEpsilon) + { + // Colliding with v1, edge 0 or 1 needs to be active + colliding_edge = 0b011; + } + else if (w > cOneMinusEpsilon) + { + // Colliding with v2, edge 1 or 2 needs to be active + colliding_edge = 0b110; + } + else if (u < cEpsilon) + { + // Colliding with edge v1, v2, edge 1 needs to be active + colliding_edge = 0b010; + } + else if (v < cEpsilon) + { + // Colliding with edge v0, v2, edge 2 needs to be active + colliding_edge = 0b100; + } + else if (w < cEpsilon) + { + // Colliding with edge v0, v1, edge 0 needs to be active + colliding_edge = 0b001; + } + else + { + // Interior hit + return inTriangleNormal; + } + + // If this edge is active, use the provided normal instead of the triangle normal + return (inActiveEdges & colliding_edge) != 0? inNormal : inTriangleNormal; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h new file mode 100644 index 000000000000..441dcd89a5fd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// How collision detection functions will treat back facing triangles +enum class EBackFaceMode : uint8 +{ + IgnoreBackFaces, ///< Ignore collision with back facing surfaces/triangles + CollideWithBackFaces, ///< Collide with back facing surfaces/triangles +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp new file mode 100644 index 000000000000..1317d13393c7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void BroadPhase::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) +{ + mBodyManager = inBodyManager; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h new file mode 100644 index 000000000000..8b6506e901d7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h @@ -0,0 +1,112 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +// Shorthand function to ifdef out code if broadphase stats tracking is off +#ifdef JPH_TRACK_BROADPHASE_STATS + #define JPH_IF_TRACK_BROADPHASE_STATS(...) __VA_ARGS__ +#else + #define JPH_IF_TRACK_BROADPHASE_STATS(...) +#endif // JPH_TRACK_BROADPHASE_STATS + +class BodyManager; +struct BodyPair; + +using BodyPairCollector = CollisionCollector; + +/// Used to do coarse collision detection operations to quickly prune out bodies that will not collide. +class JPH_EXPORT BroadPhase : public BroadPhaseQuery +{ +public: + /// Initialize the broadphase. + /// @param inBodyManager The body manager singleton + /// @param inLayerInterface Interface that maps object layers to broadphase layers. + /// Note that the broadphase takes a pointer to the data inside inObjectToBroadPhaseLayer so this object should remain static. + virtual void Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface); + + /// Should be called after many objects have been inserted to make the broadphase more efficient, usually done on startup only + virtual void Optimize() { /* Optionally overridden by implementation */ } + + /// Must be called just before updating the broadphase when none of the body mutexes are locked + virtual void FrameSync() { /* Optionally overridden by implementation */ } + + /// Must be called before UpdatePrepare to prevent modifications from being made to the tree + virtual void LockModifications() { /* Optionally overridden by implementation */ } + + /// Context used during broadphase update + struct UpdateState { void *mData[4]; }; + + /// Update the broadphase, needs to be called frequently to update the internal state when bodies have been modified. + /// The UpdatePrepare() function can run in a background thread without influencing the broadphase + virtual UpdateState UpdatePrepare() { return UpdateState(); } + + /// Finalizing the update will quickly apply the changes + virtual void UpdateFinalize([[maybe_unused]] const UpdateState &inUpdateState) { /* Optionally overridden by implementation */ } + + /// Must be called after UpdateFinalize to allow modifications to the broadphase + virtual void UnlockModifications() { /* Optionally overridden by implementation */ } + + /// Handle used during adding bodies to the broadphase + using AddState = void *; + + /// Prepare adding inNumber bodies at ioBodies to the broadphase, returns a handle that should be used in AddBodiesFinalize/Abort. + /// This can be done on a background thread without influencing the broadphase. + /// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called. + virtual AddState AddBodiesPrepare([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber) { return nullptr; } // By default the broadphase doesn't support this + + /// Finalize adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) = 0; + + /// Abort adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState. + /// This can be done on a background thread without influencing the broadphase. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + virtual void AddBodiesAbort([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber, [[maybe_unused]] AddState inAddState) { /* By default nothing needs to be done */ } + + /// Remove inNumber bodies in ioBodies from the broadphase. + /// ioBodies may be shuffled around by this function. + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) = 0; + + /// Call whenever the aabb of a body changes (can change order of ioBodies array) + /// inTakeLock should be false if we're between LockModifications/UnlockModificiations in which case care needs to be taken to not call this between UpdatePrepare/UpdateFinalize + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock = true) = 0; + + /// Call whenever the layer (and optionally the aabb as well) of a body changes (can change order of ioBodies array) + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) = 0; + + /// Find all colliding pairs between dynamic bodies + /// Note that this function is very specifically tailored for the PhysicsSystem::Update function, hence it is not part of the BroadPhaseQuery interface. + /// One of the assumptions it can make is that no locking is needed during the query as it will only be called during a very particular part of the update. + /// @param ioActiveBodies is a list of bodies for which we need to find colliding pairs (this function can change the order of the ioActiveBodies array). This can be a subset of the set of active bodies in the system. + /// @param inNumActiveBodies is the size of the ioActiveBodies array. + /// @param inSpeculativeContactDistance Distance at which speculative contact points will be created. + /// @param inObjectVsBroadPhaseLayerFilter is the filter that determines if an object can collide with a broadphase layer. + /// @param inObjectLayerPairFilter is the filter that determines if two objects can collide. + /// @param ioPairCollector receives callbacks for every body pair found. + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0; + + /// Same as BroadPhaseQuery::CastAABox but can be implemented in a way to take no broad phase locks. + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const = 0; + + /// Get the bounding box of all objects in the broadphase + virtual AABox GetBounds() const = 0; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Trace the collected broadphase stats in CSV form. + /// This report can be used to judge and tweak the efficiency of the broadphase. + virtual void ReportStats() { /* Can be implemented by derived classes */ } +#endif // JPH_TRACK_BROADPHASE_STATS + +protected: + /// Link to the body manager that manages the bodies in this broadphase + BodyManager * mBodyManager = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp new file mode 100644 index 000000000000..fc2332182d15 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp @@ -0,0 +1,313 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void BroadPhaseBruteForce::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + lock_guard lock(mMutex); + + BodyVector &bodies = mBodyManager->GetBodies(); + + // Allocate space + uint32 idx = (uint32)mBodyIDs.size(); + mBodyIDs.resize(idx + inNumber); + + // Add bodies + for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b) + { + Body &body = *bodies[b->GetIndex()]; + + // Validate that body ID is consistent with array index + JPH_ASSERT(body.GetID() == *b); + JPH_ASSERT(!body.IsInBroadPhase()); + + // Add it to the list + mBodyIDs[idx] = body.GetID(); + ++idx; + + // Indicate body is in the broadphase + body.SetInBroadPhaseInternal(true); + } + + // Resort + QuickSort(mBodyIDs.begin(), mBodyIDs.end()); +} + +void BroadPhaseBruteForce::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + lock_guard lock(mMutex); + + BodyVector &bodies = mBodyManager->GetBodies(); + + JPH_ASSERT((int)mBodyIDs.size() >= inNumber); + + // Remove bodies + for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b) + { + Body &body = *bodies[b->GetIndex()]; + + // Validate that body ID is consistent with array index + JPH_ASSERT(body.GetID() == *b); + JPH_ASSERT(body.IsInBroadPhase()); + + // Find body id + Array::const_iterator it = std::lower_bound(mBodyIDs.begin(), mBodyIDs.end(), body.GetID()); + JPH_ASSERT(it != mBodyIDs.end()); + + // Remove element + mBodyIDs.erase(it); + + // Indicate body is no longer in the broadphase + body.SetInBroadPhaseInternal(false); + } +} + +void BroadPhaseBruteForce::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) +{ + // Do nothing, we directly reference the body +} + +void BroadPhaseBruteForce::NotifyBodiesLayerChanged(BodyID * ioBodies, int inNumber) +{ + // Do nothing, we directly reference the body +} + +void BroadPhaseBruteForce::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // Load ray + Vec3 origin(inRay.mOrigin); + RayInvDirection inv_direction(inRay.mDirection); + + // For all bodies + float early_out_fraction = ioCollector.GetEarlyOutFraction(); + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with ray + const AABox &bounds = body.GetWorldSpaceBounds(); + float fraction = RayAABox(origin, inv_direction, bounds.mMin, bounds.mMax); + if (fraction < early_out_fraction) + { + // Store hit + BroadPhaseCastResult result { b, fraction }; + ioCollector.AddHit(result); + if (ioCollector.ShouldEarlyOut()) + break; + early_out_fraction = ioCollector.GetEarlyOutFraction(); + } + } + } +} + +void BroadPhaseBruteForce::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.Overlaps(inBox)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + float radius_sq = Square(inRadius); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.GetSqDistanceTo(inCenter) <= radius_sq) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.Contains(inPoint)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (inBox.Overlaps(bounds)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // Load box + Vec3 origin(inBox.mBox.GetCenter()); + Vec3 extent(inBox.mBox.GetExtent()); + RayInvDirection inv_direction(inBox.mDirection); + + // For all bodies + float early_out_fraction = ioCollector.GetPositiveEarlyOutFraction(); + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with ray + const AABox &bounds = body.GetWorldSpaceBounds(); + float fraction = RayAABox(origin, inv_direction, bounds.mMin - extent, bounds.mMax + extent); + if (fraction < early_out_fraction) + { + // Store hit + BroadPhaseCastResult result { b, fraction }; + ioCollector.AddHit(result); + if (ioCollector.ShouldEarlyOut()) + break; + early_out_fraction = ioCollector.GetPositiveEarlyOutFraction(); + } + } + } +} + +void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const +{ + shared_lock lock(mMutex); + + // Loop through all active bodies + size_t num_bodies = mBodyIDs.size(); + for (int b1 = 0; b1 < inNumActiveBodies; ++b1) + { + BodyID b1_id = ioActiveBodies[b1]; + const Body &body1 = mBodyManager->GetBody(b1_id); + const ObjectLayer layer1 = body1.GetObjectLayer(); + + // Expand the bounding box by the speculative contact distance + AABox bounds1 = body1.GetWorldSpaceBounds(); + bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance)); + + // For all other bodies + for (size_t b2 = 0; b2 < num_bodies; ++b2) + { + // Check if bodies can collide + BodyID b2_id = mBodyIDs[b2]; + const Body &body2 = mBodyManager->GetBody(b2_id); + if (!Body::sFindCollidingPairsCanCollide(body1, body2)) + continue; + + // Check if layers can collide + const ObjectLayer layer2 = body2.GetObjectLayer(); + if (!inObjectLayerPairFilter.ShouldCollide(layer1, layer2)) + continue; + + // Check if bounds overlap + const AABox &bounds2 = body2.GetWorldSpaceBounds(); + if (!bounds1.Overlaps(bounds2)) + continue; + + // Store overlapping pair + ioPairCollector.AddHit({ b1_id, b2_id }); + } + } +} + +AABox BroadPhaseBruteForce::GetBounds() const +{ + shared_lock lock(mMutex); + + AABox bounds; + for (BodyID b : mBodyIDs) + bounds.Encapsulate(mBodyManager->GetBody(b).GetWorldSpaceBounds()); + return bounds; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h new file mode 100644 index 000000000000..c3e20f5c8b84 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Test BroadPhase implementation that does not do anything to speed up the operations. Can be used as a reference implementation. +class JPH_EXPORT BroadPhaseBruteForce final : public BroadPhase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Implementing interface of BroadPhase (see BroadPhase for documentation) + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) override; + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override; + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override; + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override; + virtual AABox GetBounds() const override; + +private: + Array mBodyIDs; + mutable SharedMutex mMutex; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h new file mode 100644 index 000000000000..bc591e733b2f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// An object layer can be mapped to a broadphase layer. Objects with the same broadphase layer will end up in the same sub structure (usually a tree) of the broadphase. +/// When there are many layers, this reduces the total amount of sub structures the broad phase needs to manage. Usually you want objects that don't collide with each other +/// in different broad phase layers, but there could be exceptions if objects layers only contain a minor amount of objects so it is not beneficial to give each layer its +/// own sub structure in the broadphase. +/// Note: This class requires explicit casting from and to Type to avoid confusion with ObjectLayer +class BroadPhaseLayer +{ +public: + using Type = uint8; + + JPH_INLINE BroadPhaseLayer() = default; + JPH_INLINE explicit constexpr BroadPhaseLayer(Type inValue) : mValue(inValue) { } + JPH_INLINE constexpr BroadPhaseLayer(const BroadPhaseLayer &) = default; + JPH_INLINE BroadPhaseLayer & operator = (const BroadPhaseLayer &) = default; + + JPH_INLINE constexpr bool operator == (const BroadPhaseLayer &inRHS) const + { + return mValue == inRHS.mValue; + } + + JPH_INLINE constexpr bool operator != (const BroadPhaseLayer &inRHS) const + { + return mValue != inRHS.mValue; + } + + JPH_INLINE constexpr bool operator < (const BroadPhaseLayer &inRHS) const + { + return mValue < inRHS.mValue; + } + + JPH_INLINE explicit constexpr operator Type() const + { + return mValue; + } + + JPH_INLINE Type GetValue() const + { + return mValue; + } + +private: + Type mValue; +}; + +/// Constant value used to indicate an invalid broad phase layer +static constexpr BroadPhaseLayer cBroadPhaseLayerInvalid(0xff); + +/// Interface that the application should implement to allow mapping object layers to broadphase layers +class JPH_EXPORT BroadPhaseLayerInterface : public NonCopyable +{ +public: + /// Destructor + virtual ~BroadPhaseLayerInterface() = default; + + /// Return the number of broadphase layers there are + virtual uint GetNumBroadPhaseLayers() const = 0; + + /// Convert an object layer to the corresponding broadphase layer + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const = 0; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Get the user readable name of a broadphase layer (debugging purposes) + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const = 0; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +/// Class to test if an object can collide with a broadphase layer. Used while finding collision pairs. +class JPH_EXPORT ObjectVsBroadPhaseLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectVsBroadPhaseLayerFilter() = default; + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] BroadPhaseLayer inLayer2) const + { + return true; + } +}; + +/// Filter class for broadphase layers +class JPH_EXPORT BroadPhaseLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BroadPhaseLayerFilter() = default; + + /// Function to filter out broadphase layers when doing collision query test (return true to allow testing against objects with this layer) + virtual bool ShouldCollide([[maybe_unused]] BroadPhaseLayer inLayer) const + { + return true; + } +}; + +/// Default filter class that uses the pair filter in combination with a specified layer to filter layers +class JPH_EXPORT DefaultBroadPhaseLayerFilter : public BroadPhaseLayerFilter +{ +public: + /// Constructor + DefaultBroadPhaseLayerFilter(const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, ObjectLayer inLayer) : + mObjectVsBroadPhaseLayerFilter(inObjectVsBroadPhaseLayerFilter), + mLayer(inLayer) + { + } + + // See BroadPhaseLayerFilter::ShouldCollide + virtual bool ShouldCollide(BroadPhaseLayer inLayer) const override + { + return mObjectVsBroadPhaseLayerFilter.ShouldCollide(mLayer, inLayer); + } + +private: + const ObjectVsBroadPhaseLayerFilter &mObjectVsBroadPhaseLayerFilter; + ObjectLayer mLayer; +}; + +/// Allows objects from a specific broad phase layer only +class JPH_EXPORT SpecifiedBroadPhaseLayerFilter : public BroadPhaseLayerFilter +{ +public: + /// Constructor + explicit SpecifiedBroadPhaseLayerFilter(BroadPhaseLayer inLayer) : + mLayer(inLayer) + { + } + + // See BroadPhaseLayerFilter::ShouldCollide + virtual bool ShouldCollide(BroadPhaseLayer inLayer) const override + { + return mLayer == inLayer; + } + +private: + BroadPhaseLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h new file mode 100644 index 000000000000..15a894bb12c6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h @@ -0,0 +1,92 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// BroadPhaseLayerInterface implementation. +/// This defines a mapping between object and broadphase layers. +/// This implementation works together with ObjectLayerPairFilterMask and ObjectVsBroadPhaseLayerFilterMask. +/// A broadphase layer is suitable for an object if its group & inGroupsToInclude is not zero and its group & inGroupsToExclude is zero. +/// The broadphase layers are iterated from lowest to highest value and the first one that matches is taken. If none match then it takes the last layer. +class BroadPhaseLayerInterfaceMask : public BroadPhaseLayerInterface +{ +public: + JPH_OVERRIDE_NEW_DELETE + + explicit BroadPhaseLayerInterfaceMask(uint inNumBroadPhaseLayers) + { + JPH_ASSERT(inNumBroadPhaseLayers > 0); + mMapping.resize(inNumBroadPhaseLayers); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined"); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } + + // Configures a broadphase layer. + void ConfigureLayer(BroadPhaseLayer inBroadPhaseLayer, uint32 inGroupsToInclude, uint32 inGroupsToExclude) + { + JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < (uint)mMapping.size()); + Mapping &m = mMapping[(BroadPhaseLayer::Type)inBroadPhaseLayer]; + m.mGroupsToInclude = inGroupsToInclude; + m.mGroupsToExclude = inGroupsToExclude; + } + + virtual uint GetNumBroadPhaseLayers() const override + { + return (uint)mMapping.size(); + } + + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override + { + // Try to find the first broadphase layer that matches + uint32 group = ObjectLayerPairFilterMask::sGetGroup(inLayer); + for (const Mapping &m : mMapping) + if ((group & m.mGroupsToInclude) != 0 && (group & m.mGroupsToExclude) == 0) + return BroadPhaseLayer(BroadPhaseLayer::Type(&m - mMapping.data())); + + // Fall back to the last broadphase layer + return BroadPhaseLayer(BroadPhaseLayer::Type(mMapping.size() - 1)); + } + + /// Returns true if an object layer should collide with a broadphase layer, this function is being called from ObjectVsBroadPhaseLayerFilterMask + inline bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const + { + uint32 mask = ObjectLayerPairFilterMask::sGetMask(inLayer1); + const Mapping &m = mMapping[(BroadPhaseLayer::Type)inLayer2]; + return &m == &mMapping.back() // Last layer may collide with anything + || (m.mGroupsToInclude & mask) != 0; // Mask allows it to collide with objects that could reside in this layer + } + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + void SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName) + { + mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName; + } + + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override + { + return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer]; + } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + +private: + struct Mapping + { + uint32 mGroupsToInclude = 0; + uint32 mGroupsToExclude = ~uint32(0); + }; + Array mMapping; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + Array mBroadPhaseLayerNames; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h new file mode 100644 index 000000000000..e777a08589bb --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h @@ -0,0 +1,64 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// BroadPhaseLayerInterface implementation. +/// This defines a mapping between object and broadphase layers. +/// This implementation uses a simple table +class BroadPhaseLayerInterfaceTable : public BroadPhaseLayerInterface +{ +public: + JPH_OVERRIDE_NEW_DELETE + + BroadPhaseLayerInterfaceTable(uint inNumObjectLayers, uint inNumBroadPhaseLayers) : + mNumBroadPhaseLayers(inNumBroadPhaseLayers) + { + mObjectToBroadPhase.resize(inNumObjectLayers, BroadPhaseLayer(0)); +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined"); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } + + void MapObjectToBroadPhaseLayer(ObjectLayer inObjectLayer, BroadPhaseLayer inBroadPhaseLayer) + { + JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < mNumBroadPhaseLayers); + mObjectToBroadPhase[inObjectLayer] = inBroadPhaseLayer; + } + + virtual uint GetNumBroadPhaseLayers() const override + { + return mNumBroadPhaseLayers; + } + + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override + { + return mObjectToBroadPhase[inLayer]; + } + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + void SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName) + { + mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName; + } + + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override + { + return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer]; + } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + +private: + uint mNumBroadPhaseLayers; + Array mObjectToBroadPhase; +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + Array mBroadPhaseLayerNames; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp new file mode 100644 index 000000000000..717244c02381 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp @@ -0,0 +1,609 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +BroadPhaseQuadTree::~BroadPhaseQuadTree() +{ + delete [] mLayers; +} + +void BroadPhaseQuadTree::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) +{ + BroadPhase::Init(inBodyManager, inLayerInterface); + + // Store input parameters + mBroadPhaseLayerInterface = &inLayerInterface; + mNumLayers = inLayerInterface.GetNumBroadPhaseLayers(); + JPH_ASSERT(mNumLayers < (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + +#ifdef JPH_ENABLE_ASSERTS + // Store lock context + mLockContext = inBodyManager; +#endif // JPH_ENABLE_ASSERTS + + // Store max bodies + mMaxBodies = inBodyManager->GetMaxBodies(); + + // Initialize tracking data + mTracking.resize(mMaxBodies); + + // Init allocator + // Estimate the amount of nodes we're going to need + uint32 num_leaves = (uint32)(mMaxBodies + 1) / 2; // Assume 50% fill + uint32 num_leaves_plus_internal_nodes = num_leaves + (num_leaves + 2) / 3; // = Sum(num_leaves * 4^-i) with i = [0, Inf]. + mAllocator.Init(2 * num_leaves_plus_internal_nodes, 256); // We use double the amount of nodes while rebuilding the tree during Update() + + // Init sub trees + mLayers = new QuadTree [mNumLayers]; + for (uint l = 0; l < mNumLayers; ++l) + { + mLayers[l].Init(mAllocator); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + // Set the name of the layer + mLayers[l].SetName(inLayerInterface.GetBroadPhaseLayerName(BroadPhaseLayer(BroadPhaseLayer::Type(l)))); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } +} + +void BroadPhaseQuadTree::FrameSync() +{ + JPH_PROFILE_FUNCTION(); + + // Take a unique lock on the old query lock so that we know no one is using the old nodes anymore. + // Note that nothing should be locked at this point to avoid risking a lock inversion deadlock. + // Note that in other places where we lock this mutex we don't use SharedLock to detect lock inversions. As long as + // nothing else is locked this is safe. This is why BroadPhaseQuery should be the highest priority lock. + UniqueLock root_lock(mQueryLocks[mQueryLockIdx ^ 1] JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseQuery)); + + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + mLayers[l].DiscardOldTree(); +} + +void BroadPhaseQuadTree::Optimize() +{ + JPH_PROFILE_FUNCTION(); + + FrameSync(); + + LockModifications(); + + for (uint l = 0; l < mNumLayers; ++l) + { + QuadTree &tree = mLayers[l]; + if (tree.HasBodies()) + { + QuadTree::UpdateState update_state; + tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state, true); + tree.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state); + } + } + + UnlockModifications(); + + mNextLayerToUpdate = 0; +} + +void BroadPhaseQuadTree::LockModifications() +{ + // From this point on we prevent modifications to the tree + PhysicsLock::sLock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare() +{ + // LockModifications should have been called + JPH_ASSERT(mUpdateMutex.is_locked()); + + // Create update state + UpdateState update_state; + UpdateStateImpl *update_state_impl = reinterpret_cast(&update_state); + + // Loop until we've seen all layers + for (uint iteration = 0; iteration < mNumLayers; ++iteration) + { + // Get the layer + QuadTree &tree = mLayers[mNextLayerToUpdate]; + mNextLayerToUpdate = (mNextLayerToUpdate + 1) % mNumLayers; + + // If it is dirty we update this one + if (tree.HasBodies() && tree.IsDirty() && tree.CanBeUpdated()) + { + update_state_impl->mTree = &tree; + tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState, false); + return update_state; + } + } + + // Nothing to update + update_state_impl->mTree = nullptr; + return update_state; +} + +void BroadPhaseQuadTree::UpdateFinalize(const UpdateState &inUpdateState) +{ + // LockModifications should have been called + JPH_ASSERT(mUpdateMutex.is_locked()); + + // Test if a tree was updated + const UpdateStateImpl *update_state_impl = reinterpret_cast(&inUpdateState); + if (update_state_impl->mTree == nullptr) + return; + + update_state_impl->mTree->UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState); + + // Make all queries from now on use the new lock + mQueryLockIdx = mQueryLockIdx ^ 1; +} + +void BroadPhaseQuadTree::UnlockModifications() +{ + // From this point on we allow modifications to the tree again + PhysicsLock::sUnlock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = new LayerState [mNumLayers]; + + // Sort bodies on layer + Body * const * const bodies_ptr = bodies.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [bodies_ptr](BodyID inLHS, BodyID inRHS) { return bodies_ptr[inLHS.GetIndex()]->GetBroadPhaseLayer() < bodies_ptr[inRHS.GetIndex()]->GetBroadPhaseLayer(); }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broadphase layer + BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)bodies[b_start->GetIndex()]->GetBroadPhaseLayer(); + JPH_ASSERT(broadphase_layer < mNumLayers); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [bodies_ptr](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < (BroadPhaseLayer::Type)bodies_ptr[inBodyID.GetIndex()]->GetBroadPhaseLayer(); }); + + // Keep track of state for this layer + LayerState &layer_state = state[broadphase_layer]; + layer_state.mBodyStart = b_start; + layer_state.mBodyEnd = b_mid; + + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesPrepare(bodies, mTracking, b_start, int(b_mid - b_start), layer_state.mAddState); + + // Keep track in which tree we placed the object + for (const BodyID *b = b_start; b < b_mid; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + Tracking &t = mTracking[index]; + JPH_ASSERT(t.mBroadPhaseLayer == (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + t.mBroadPhaseLayer = broadphase_layer; + JPH_ASSERT(t.mObjectLayer == cObjectLayerInvalid); + t.mObjectLayer = bodies[index]->GetObjectLayer(); + } + + // Repeat + b_start = b_mid; + } + + return state; +} + +void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + JPH_PROFILE_FUNCTION(); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + + BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = (LayerState *)inAddState; + + for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++) + { + const LayerState &l = state[broadphase_layer]; + if (l.mBodyStart != nullptr) + { + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesFinalize(mTracking, int(l.mBodyEnd - l.mBodyStart), l.mAddState); + + // Mark added to broadphase + for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(mTracking[index].mBroadPhaseLayer == broadphase_layer); + JPH_ASSERT(mTracking[index].mObjectLayer == bodies[index]->GetObjectLayer()); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + bodies[index]->SetInBroadPhaseInternal(true); + } + } + } + + delete [] state; +} + +void BroadPhaseQuadTree::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + JPH_PROFILE_FUNCTION(); + + JPH_IF_ENABLE_ASSERTS(const BodyVector &bodies = mBodyManager->GetBodies();) + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = (LayerState *)inAddState; + + for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++) + { + const LayerState &l = state[broadphase_layer]; + if (l.mBodyStart != nullptr) + { + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesAbort(mTracking, l.mAddState); + + // Reset bookkeeping + for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + Tracking &t = mTracking[index]; + JPH_ASSERT(t.mBroadPhaseLayer == broadphase_layer); + t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + t.mObjectLayer = cObjectLayerInvalid; + } + } + } + + delete [] state; +} + +void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + + JPH_ASSERT(inNumber > 0); + + BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Sort bodies on layer + Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broad phase layer + BroadPhaseLayer::Type broadphase_layer = mTracking[b_start->GetIndex()].mBroadPhaseLayer; + JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; }); + + // Remove all bodies of the same layer + mLayers[broadphase_layer].RemoveBodies(bodies, mTracking, b_start, int(b_mid - b_start)); + + for (const BodyID *b = b_start; b < b_mid; ++b) + { + // Reset bookkeeping + uint32 index = b->GetIndex(); + Tracking &t = tracking[index]; + t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + t.mObjectLayer = cObjectLayerInvalid; + + // Mark removed from broadphase + JPH_ASSERT(bodies[index]->IsInBroadPhase()); + bodies[index]->SetInBroadPhaseInternal(false); + } + + // Repeat + b_start = b_mid; + } +} + +void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + if (inTakeLock) + PhysicsLock::sLockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + else + JPH_ASSERT(mUpdateMutex.is_locked()); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Sort bodies on layer + const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broadphase layer + BroadPhaseLayer::Type broadphase_layer = tracking[b_start->GetIndex()].mBroadPhaseLayer; + JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; }); + + // Nodify all bodies of the same layer changed + mLayers[broadphase_layer].NotifyBodiesAABBChanged(bodies, mTracking, b_start, int(b_mid - b_start)); + + // Repeat + b_start = b_mid; + } + + if (inTakeLock) + PhysicsLock::sUnlockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + // First sort the bodies that actually changed layer to beginning of the array + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + for (BodyID *body_id = ioBodies + inNumber - 1; body_id >= ioBodies; --body_id) + { + uint32 index = body_id->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *body_id, "Provided BodyID doesn't match BodyID in body manager"); + const Body *body = bodies[index]; + BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)body->GetBroadPhaseLayer(); + JPH_ASSERT(broadphase_layer < mNumLayers); + if (mTracking[index].mBroadPhaseLayer == broadphase_layer) + { + // Update tracking information + mTracking[index].mObjectLayer = body->GetObjectLayer(); + + // Move the body to the end, layer didn't change + std::swap(*body_id, ioBodies[inNumber - 1]); + --inNumber; + } + } + + if (inNumber > 0) + { + // Changing layer requires us to remove from one tree and add to another, so this is equivalent to removing all bodies first and then adding them again + RemoveBodies(ioBodies, inNumber); + AddState add_state = AddBodiesPrepare(ioBodies, inNumber); + AddBodiesFinalize(ioBodies, inNumber, add_state); + } +} + +void BroadPhaseQuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CastRay(inRay, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideAABox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideSphere(inCenter, inRadius, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollidePoint(inPoint, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const +{ + JPH_PROFILE_FUNCTION(); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Note that we don't take any locks at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update. + + // Sort bodies on layer + const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mObjectLayer < tracking[inRHS.GetIndex()].mObjectLayer; }); + + BodyID *b_start = ioActiveBodies, *b_end = ioActiveBodies + inNumActiveBodies; + while (b_start < b_end) + { + // Get broadphase layer + ObjectLayer object_layer = tracking[b_start->GetIndex()].mObjectLayer; + JPH_ASSERT(object_layer != cObjectLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, object_layer, [tracking](ObjectLayer inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mObjectLayer; }); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inObjectVsBroadPhaseLayerFilter.ShouldCollide(object_layer, BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter); + } + } + + // Repeat + b_start = b_mid; + } +} + +AABox BroadPhaseQuadTree::GetBounds() const +{ + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + AABox bounds; + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + bounds.Encapsulate(mLayers[l].GetBounds()); + return bounds; +} + +#ifdef JPH_TRACK_BROADPHASE_STATS + +void BroadPhaseQuadTree::ReportStats() +{ + Trace("Query Type, Filter Description, Tree Name, Num Queries, Total Time (%%), Total Time Excl. Collector (%%), Nodes Visited, Bodies Visited, Hits Reported, Hits Reported vs Bodies Visited (%%), Hits Reported vs Nodes Visited"); + + uint64 total_ticks = 0; + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + total_ticks += mLayers[l].GetTicks100Pct(); + + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + mLayers[l].ReportStats(total_ticks); +} + +#endif // JPH_TRACK_BROADPHASE_STATS + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h new file mode 100644 index 000000000000..ae97c10f0e33 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h @@ -0,0 +1,108 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Fast SIMD based quad tree BroadPhase that is multithreading aware and tries to do a minimal amount of locking. +class JPH_EXPORT BroadPhaseQuadTree final : public BroadPhase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + virtual ~BroadPhaseQuadTree() override; + + // Implementing interface of BroadPhase (see BroadPhase for documentation) + virtual void Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) override; + virtual void Optimize() override; + virtual void FrameSync() override; + virtual void LockModifications() override; + virtual UpdateState UpdatePrepare() override; + virtual void UpdateFinalize(const UpdateState &inUpdateState) override; + virtual void UnlockModifications() override; + virtual AddState AddBodiesPrepare(BodyID *ioBodies, int inNumber) override; + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) override; + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override; + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override; + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override; + virtual AABox GetBounds() const override; +#ifdef JPH_TRACK_BROADPHASE_STATS + virtual void ReportStats() override; +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + /// Helper struct for AddBodies handle + struct LayerState + { + JPH_OVERRIDE_NEW_DELETE + + BodyID * mBodyStart = nullptr; + BodyID * mBodyEnd; + QuadTree::AddState mAddState; + }; + + using Tracking = QuadTree::Tracking; + using TrackingVector = QuadTree::TrackingVector; + +#ifdef JPH_ENABLE_ASSERTS + /// Context used to lock a physics lock + PhysicsLockContext mLockContext = nullptr; +#endif // JPH_ENABLE_ASSERTS + + /// Max amount of bodies we support + size_t mMaxBodies = 0; + + /// Array that for each BodyID keeps track of where it is located in which tree + TrackingVector mTracking; + + /// Node allocator for all trees + QuadTree::Allocator mAllocator; + + /// Information about broad phase layers + const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr; + + /// One tree per object layer + QuadTree * mLayers; + uint mNumLayers; + + /// UpdateState implementation for this tree used during UpdatePrepare/Finalize() + struct UpdateStateImpl + { + QuadTree * mTree; + QuadTree::UpdateState mUpdateState; + }; + + static_assert(sizeof(UpdateStateImpl) <= sizeof(UpdateState)); + static_assert(alignof(UpdateStateImpl) <= alignof(UpdateState)); + + /// Mutex that prevents object modification during UpdatePrepare/Finalize() + SharedMutex mUpdateMutex; + + /// We double buffer all trees so that we can query while building the next one and we destroy the old tree the next physics update. + /// This structure ensures that we wait for queries that are still using the old tree. + mutable SharedMutex mQueryLocks[2]; + + /// This index indicates which lock is currently active, it alternates between 0 and 1 + atomic mQueryLockIdx { 0 }; + + /// This is the next tree to update in UpdatePrepare() + uint32 mNextLayerToUpdate = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h new file mode 100644 index 000000000000..10085e66fe8d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h @@ -0,0 +1,53 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RayCast; +class BroadPhaseCastResult; +class AABox; +class OrientedBox; +struct AABoxCast; + +// Various collector configurations +using RayCastBodyCollector = CollisionCollector; +using CastShapeBodyCollector = CollisionCollector; +using CollideShapeBodyCollector = CollisionCollector; + +/// Interface to the broadphase that can perform collision queries. These queries will only test the bounding box of the body to quickly determine a potential set of colliding bodies. +/// The shapes of the bodies are not tested, if you want this then you should use the NarrowPhaseQuery interface. +class JPH_EXPORT BroadPhaseQuery : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~BroadPhaseQuery() = default; + + /// Cast a ray and add any hits to ioCollector + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with inBox and any hits to ioCollector + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with a sphere and any hits to ioCollector + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with a point and any hits to ioCollector + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with an oriented box and any hits to ioCollector + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Cast a box and add any hits to ioCollector + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h new file mode 100644 index 000000000000..20305a5f0ca8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that determines if an object layer can collide with a broadphase layer. +/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectLayerPairFilterMask +class ObjectVsBroadPhaseLayerFilterMask : public ObjectVsBroadPhaseLayerFilter +{ +public: + JPH_OVERRIDE_NEW_DELETE + +/// Constructor + ObjectVsBroadPhaseLayerFilterMask(const BroadPhaseLayerInterfaceMask &inBroadPhaseLayerInterface) : + mBroadPhaseLayerInterface(inBroadPhaseLayerInterface) + { + } + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override + { + // Just defer to BroadPhaseLayerInterface + return mBroadPhaseLayerInterface.ShouldCollide(inLayer1, inLayer2); + } + +private: + const BroadPhaseLayerInterfaceMask &mBroadPhaseLayerInterface; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h new file mode 100644 index 000000000000..532ce6da0edb --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that determines if an object layer can collide with a broadphase layer. +/// This implementation uses a table and constructs itself from an ObjectLayerPairFilter and a BroadPhaseLayerInterface. +class ObjectVsBroadPhaseLayerFilterTable : public ObjectVsBroadPhaseLayerFilter +{ +private: + /// Get which bit corresponds to the pair (inLayer1, inLayer2) + uint GetBit(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const + { + // Calculate at which bit the entry for this pair resides + return inLayer1 * mNumBroadPhaseLayers + (BroadPhaseLayer::Type)inLayer2; + } + +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct the table + /// @param inBroadPhaseLayerInterface The broad phase layer interface that maps object layers to broad phase layers + /// @param inNumBroadPhaseLayers Number of broad phase layers + /// @param inObjectLayerPairFilter The object layer pair filter that determines which object layers can collide + /// @param inNumObjectLayers Number of object layers + ObjectVsBroadPhaseLayerFilterTable(const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, uint inNumBroadPhaseLayers, const ObjectLayerPairFilter &inObjectLayerPairFilter, uint inNumObjectLayers) : + mNumBroadPhaseLayers(inNumBroadPhaseLayers) + { + // Resize table and set all entries to false + mTable.resize((inNumBroadPhaseLayers * inNumObjectLayers + 7) / 8, 0); + + // Loop over all object layer pairs + for (ObjectLayer o1 = 0; o1 < inNumObjectLayers; ++o1) + for (ObjectLayer o2 = 0; o2 < inNumObjectLayers; ++o2) + { + // Get the broad phase layer for the second object layer + BroadPhaseLayer b2 = inBroadPhaseLayerInterface.GetBroadPhaseLayer(o2); + JPH_ASSERT((BroadPhaseLayer::Type)b2 < inNumBroadPhaseLayers); + + // If the object layers collide then so should the object and broadphase layer + if (inObjectLayerPairFilter.ShouldCollide(o1, o2)) + { + uint bit = GetBit(o1, b2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + } + } + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override + { + uint bit = GetBit(inLayer1, inLayer2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + +private: + uint mNumBroadPhaseLayers; ///< The total number of broadphase layers + Array mTable; ///< The table of bits that indicates which layers collide +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp new file mode 100644 index 000000000000..cfcf8ba30091 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp @@ -0,0 +1,1692 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef JPH_DUMP_BROADPHASE_TREE +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END +#endif // JPH_DUMP_BROADPHASE_TREE + +JPH_NAMESPACE_BEGIN + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// QuadTree::Node +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +QuadTree::Node::Node(bool inIsChanged) : + mIsChanged(inIsChanged) +{ + // First reset bounds + Vec4 val = Vec4::sReplicate(cLargeFloat); + val.StoreFloat4((Float4 *)&mBoundsMinX); + val.StoreFloat4((Float4 *)&mBoundsMinY); + val.StoreFloat4((Float4 *)&mBoundsMinZ); + val = Vec4::sReplicate(-cLargeFloat); + val.StoreFloat4((Float4 *)&mBoundsMaxX); + val.StoreFloat4((Float4 *)&mBoundsMaxY); + val.StoreFloat4((Float4 *)&mBoundsMaxZ); + + // Reset child node ids + mChildNodeID[0] = NodeID::sInvalid(); + mChildNodeID[1] = NodeID::sInvalid(); + mChildNodeID[2] = NodeID::sInvalid(); + mChildNodeID[3] = NodeID::sInvalid(); +} + +void QuadTree::Node::GetChildBounds(int inChildIndex, AABox &outBounds) const +{ + // Read bounding box in order min -> max + outBounds.mMin = Vec3(mBoundsMinX[inChildIndex], mBoundsMinY[inChildIndex], mBoundsMinZ[inChildIndex]); + outBounds.mMax = Vec3(mBoundsMaxX[inChildIndex], mBoundsMaxY[inChildIndex], mBoundsMaxZ[inChildIndex]); +} + +void QuadTree::Node::SetChildBounds(int inChildIndex, const AABox &inBounds) +{ + // Set max first (this keeps the bounding box invalid for reading threads) + mBoundsMaxZ[inChildIndex] = inBounds.mMax.GetZ(); + mBoundsMaxY[inChildIndex] = inBounds.mMax.GetY(); + mBoundsMaxX[inChildIndex] = inBounds.mMax.GetX(); + + // Then set min (and make box valid) + mBoundsMinZ[inChildIndex] = inBounds.mMin.GetZ(); + mBoundsMinY[inChildIndex] = inBounds.mMin.GetY(); + mBoundsMinX[inChildIndex] = inBounds.mMin.GetX(); // Min X becomes valid last +} + +void QuadTree::Node::InvalidateChildBounds(int inChildIndex) +{ + // First we make the box invalid by setting the min to cLargeFloat + mBoundsMinX[inChildIndex] = cLargeFloat; // Min X becomes invalid first + mBoundsMinY[inChildIndex] = cLargeFloat; + mBoundsMinZ[inChildIndex] = cLargeFloat; + + // Then we reset the max values too + mBoundsMaxX[inChildIndex] = -cLargeFloat; + mBoundsMaxY[inChildIndex] = -cLargeFloat; + mBoundsMaxZ[inChildIndex] = -cLargeFloat; +} + +void QuadTree::Node::GetNodeBounds(AABox &outBounds) const +{ + // Get first child bounds + GetChildBounds(0, outBounds); + + // Encapsulate other child bounds + for (int child_idx = 1; child_idx < 4; ++child_idx) + { + AABox tmp; + GetChildBounds(child_idx, tmp); + outBounds.Encapsulate(tmp); + } +} + +bool QuadTree::Node::EncapsulateChildBounds(int inChildIndex, const AABox &inBounds) +{ + bool changed = AtomicMin(mBoundsMinX[inChildIndex], inBounds.mMin.GetX()); + changed |= AtomicMin(mBoundsMinY[inChildIndex], inBounds.mMin.GetY()); + changed |= AtomicMin(mBoundsMinZ[inChildIndex], inBounds.mMin.GetZ()); + changed |= AtomicMax(mBoundsMaxX[inChildIndex], inBounds.mMax.GetX()); + changed |= AtomicMax(mBoundsMaxY[inChildIndex], inBounds.mMax.GetY()); + changed |= AtomicMax(mBoundsMaxZ[inChildIndex], inBounds.mMax.GetZ()); + return changed; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// QuadTree +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +const float QuadTree::cLargeFloat = 1.0e30f; +const AABox QuadTree::cInvalidBounds(Vec3::sReplicate(cLargeFloat), Vec3::sReplicate(-cLargeFloat)); + +void QuadTree::GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const +{ + uint32 body_location = inTracking[inBodyID.GetIndex()].mBodyLocation; + JPH_ASSERT(body_location != Tracking::cInvalidBodyLocation); + outNodeIdx = body_location & 0x3fffffff; + outChildIdx = body_location >> 30; + JPH_ASSERT(mAllocator->Get(outNodeIdx).mChildNodeID[outChildIdx] == inBodyID, "Make sure that the body is in the node where it should be"); +} + +void QuadTree::SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const +{ + JPH_ASSERT(inNodeIdx <= 0x3fffffff); + JPH_ASSERT(inChildIdx < 4); + JPH_ASSERT(mAllocator->Get(inNodeIdx).mChildNodeID[inChildIdx] == inBodyID, "Make sure that the body is in the node where it should be"); + ioTracking[inBodyID.GetIndex()].mBodyLocation = inNodeIdx + (inChildIdx << 30); + +#ifdef JPH_ENABLE_ASSERTS + uint32 v1, v2; + GetBodyLocation(ioTracking, inBodyID, v1, v2); + JPH_ASSERT(v1 == inNodeIdx); + JPH_ASSERT(v2 == inChildIdx); +#endif +} + +void QuadTree::sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID) +{ + ioTracking[inBodyID.GetIndex()].mBodyLocation = Tracking::cInvalidBodyLocation; +} + +QuadTree::~QuadTree() +{ + // Get rid of any nodes that are still to be freed + DiscardOldTree(); + + // Get the current root node + const RootNode &root_node = GetCurrentRoot(); + + // Collect all bodies + Allocator::Batch free_batch; + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + JPH_ASSERT(node_stack[0].IsValid()); + if (node_stack[0].IsNode()) + { + int top = 0; + do + { + // Process node + NodeID node_id = node_stack[top]; + JPH_ASSERT(!node_id.IsBody()); + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + // Recurse and get all child nodes + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid() && child_node_id.IsNode()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = child_node_id; + top++; + } + + // Mark node to be freed + mAllocator->AddObjectToBatch(free_batch, node_idx); + --top; + } + while (top >= 0); + } + + // Now free all nodes + mAllocator->DestructObjectBatch(free_batch); +} + +uint32 QuadTree::AllocateNode(bool inIsChanged) +{ + uint32 index = mAllocator->ConstructObject(inIsChanged); + if (index == Allocator::cInvalidObjectIndex) + { + Trace("QuadTree: Out of nodes!"); + std::abort(); + } + return index; +} + +void QuadTree::Init(Allocator &inAllocator) +{ + // Store allocator + mAllocator = &inAllocator; + + // Allocate root node + mRootNode[mRootNodeIndex].mIndex = AllocateNode(false); +} + +void QuadTree::DiscardOldTree() +{ + // Check if there is an old tree + RootNode &old_root_node = mRootNode[mRootNodeIndex ^ 1]; + if (old_root_node.mIndex != cInvalidNodeIndex) + { + // Clear the root + old_root_node.mIndex = cInvalidNodeIndex; + + // Now free all old nodes + mAllocator->DestructObjectBatch(mFreeNodeBatch); + + // Clear the batch + mFreeNodeBatch = Allocator::Batch(); + } +} + +AABox QuadTree::GetBounds() const +{ + uint32 node_idx = GetCurrentRoot().mIndex; + JPH_ASSERT(node_idx != cInvalidNodeIndex); + const Node &node = mAllocator->Get(node_idx); + + AABox bounds; + node.GetNodeBounds(bounds); + return bounds; +} + +void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); +#endif + + // Assert we have no nodes pending deletion, this means DiscardOldTree wasn't called yet + JPH_ASSERT(mFreeNodeBatch.mNumObjects == 0); + + // Mark tree non-dirty + mIsDirty = false; + + // Get the current root node + const RootNode &root_node = GetCurrentRoot(); + +#ifdef JPH_DUMP_BROADPHASE_TREE + DumpTree(root_node.GetNodeID(), StringFormat("%s_PRE", mName).c_str()); +#endif + + // Assert sane data +#ifdef JPH_DEBUG + ValidateTree(inBodies, ioTracking, root_node.mIndex, mNumBodies); +#endif + + // Create space for all body ID's + NodeID *node_ids = new NodeID [mNumBodies]; + NodeID *cur_node_id = node_ids; + + // Collect all bodies + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID node_id = node_stack[top]; + if (node_id.IsBody()) + { + // Validate that we're still in the right layer + #ifdef JPH_ENABLE_ASSERTS + uint32 body_index = node_id.GetBodyID().GetIndex(); + JPH_ASSERT(ioTracking[body_index].mObjectLayer == inBodies[body_index]->GetObjectLayer()); + #endif + + // Store body + *cur_node_id = node_id; + ++cur_node_id; + } + else + { + // Process normal node + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + if (!node.mIsChanged && !inFullRebuild) + { + // Node is unchanged, treat it as a whole + *cur_node_id = node_id; + ++cur_node_id; + } + else + { + // Node is changed, recurse and get all children + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid()) + { + if (top < cStackSize) + { + node_stack[top] = child_node_id; + top++; + } + else + { + JPH_ASSERT(false, "Stack full!\n" + "This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n" + "If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree."); + + // Falling back to adding the node as a whole + *cur_node_id = child_node_id; + ++cur_node_id; + } + } + + // Mark node to be freed + mAllocator->AddObjectToBatch(mFreeNodeBatch, node_idx); + } + } + --top; + } + while (top >= 0); + + // Check that our book keeping matches + uint32 num_node_ids = uint32(cur_node_id - node_ids); + JPH_ASSERT(inFullRebuild? num_node_ids == mNumBodies : num_node_ids <= mNumBodies); + + // This will be the new root node id + NodeID root_node_id; + + if (num_node_ids > 0) + { + // We mark the first 5 levels (max 1024 nodes) of the newly built tree as 'changed' so that + // those nodes get recreated every time when we rebuild the tree. This balances the amount of + // time we spend on rebuilding the tree ('unchanged' nodes will be put in the new tree as a whole) + // vs the quality of the built tree. + constexpr uint cMaxDepthMarkChanged = 5; + + // Build new tree + AABox root_bounds; + root_node_id = BuildTree(inBodies, ioTracking, node_ids, num_node_ids, cMaxDepthMarkChanged, root_bounds); + + if (root_node_id.IsBody()) + { + // For a single body we need to allocate a new root node + uint32 root_idx = AllocateNode(false); + Node &root = mAllocator->Get(root_idx); + root.SetChildBounds(0, root_bounds); + root.mChildNodeID[0] = root_node_id; + SetBodyLocation(ioTracking, root_node_id.GetBodyID(), root_idx, 0); + root_node_id = NodeID::sFromNodeIndex(root_idx); + } + } + else + { + // Empty tree, create root node + uint32 root_idx = AllocateNode(false); + root_node_id = NodeID::sFromNodeIndex(root_idx); + } + + // Delete temporary data + delete [] node_ids; + + outUpdateState.mRootNodeID = root_node_id; +} + +void QuadTree::UpdateFinalize([[maybe_unused]] const BodyVector &inBodies, [[maybe_unused]] const TrackingVector &inTracking, const UpdateState &inUpdateState) +{ + // Tree building is complete, now we switch the old with the new tree + uint32 new_root_idx = mRootNodeIndex ^ 1; + RootNode &new_root_node = mRootNode[new_root_idx]; + { + // Note: We don't need to lock here as the old tree stays available so any queries + // that use it can continue using it until DiscardOldTree is called. This slot + // should be empty and unused at this moment. + JPH_ASSERT(new_root_node.mIndex == cInvalidNodeIndex); + new_root_node.mIndex = inUpdateState.mRootNodeID.GetNodeIndex(); + } + + // All queries that start from now on will use this new tree + mRootNodeIndex = new_root_idx; + +#ifdef JPH_DUMP_BROADPHASE_TREE + DumpTree(new_root_node.GetNodeID(), StringFormat("%s_POST", mName).c_str()); +#endif + +#ifdef JPH_DEBUG + ValidateTree(inBodies, inTracking, new_root_node.mIndex, mNumBodies); +#endif +} + +void QuadTree::sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint) +{ + // Handle trivial case + if (inNumber <= 4) + { + outMidPoint = inNumber / 2; + return; + } + + // Calculate bounding box of box centers + Vec3 center_min = Vec3::sReplicate(cLargeFloat); + Vec3 center_max = Vec3::sReplicate(-cLargeFloat); + for (const Vec3 *c = ioNodeCenters, *c_end = ioNodeCenters + inNumber; c < c_end; ++c) + { + Vec3 center = *c; + center_min = Vec3::sMin(center_min, center); + center_max = Vec3::sMax(center_max, center); + } + + // Calculate split plane + int dimension = (center_max - center_min).GetHighestComponentIndex(); + float split = 0.5f * (center_min + center_max)[dimension]; + + // Divide bodies + int start = 0, end = inNumber; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && ioNodeCenters[start][dimension] < split) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && ioNodeCenters[end - 1][dimension] >= split) + --end; + + if (start < end) + { + // Swap the two elements + std::swap(ioNodeIDs[start], ioNodeIDs[end - 1]); + std::swap(ioNodeCenters[start], ioNodeCenters[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + + if (start > 0 && start < inNumber) + { + // Success! + outMidPoint = start; + } + else + { + // Failed to divide bodies + outMidPoint = inNumber / 2; + } +} + +void QuadTree::sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit) +{ + NodeID *node_ids = ioNodeIDs + inBegin; + Vec3 *node_centers = ioNodeCenters + inBegin; + int number = inEnd - inBegin; + + // Partition entire range + sPartition(node_ids, node_centers, number, outSplit[2]); + + // Partition lower half + sPartition(node_ids, node_centers, outSplit[2], outSplit[1]); + + // Partition upper half + sPartition(node_ids + outSplit[2], node_centers + outSplit[2], number - outSplit[2], outSplit[3]); + + // Convert to proper range + outSplit[0] = inBegin; + outSplit[1] += inBegin; + outSplit[2] += inBegin; + outSplit[3] += outSplit[2]; + outSplit[4] = inEnd; +} + +AABox QuadTree::GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const +{ + if (inNodeID.IsNode()) + { + // It is a node + uint32 node_idx = inNodeID.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + AABox bounds; + node.GetNodeBounds(bounds); + return bounds; + } + else + { + // It is a body + return inBodies[inNodeID.GetBodyID().GetIndex()]->GetWorldSpaceBounds(); + } +} + +QuadTree::NodeID QuadTree::BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds) +{ + // Trivial case: No bodies in tree + if (inNumber == 0) + { + outBounds = cInvalidBounds; + return NodeID::sInvalid(); + } + + // Trivial case: When we have 1 body or node, return it + if (inNumber == 1) + { + if (ioNodeIDs->IsNode()) + { + // When returning an existing node as root, ensure that no parent has been set + Node &node = mAllocator->Get(ioNodeIDs->GetNodeIndex()); + node.mParentNodeIndex = cInvalidNodeIndex; + } + outBounds = GetNodeOrBodyBounds(inBodies, *ioNodeIDs); + return *ioNodeIDs; + } + + // Calculate centers of all bodies that are to be inserted + Vec3 *centers = new Vec3 [inNumber]; + JPH_ASSERT(IsAligned(centers, JPH_VECTOR_ALIGNMENT)); + Vec3 *c = centers; + for (const NodeID *n = ioNodeIDs, *n_end = ioNodeIDs + inNumber; n < n_end; ++n, ++c) + *c = GetNodeOrBodyBounds(inBodies, *n).GetCenter(); + + // The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here + struct StackEntry + { + uint32 mNodeIdx; // Node index of node that is generated + int mChildIdx; // Index of child that we're currently processing + int mSplit[5]; // Indices where the node ID's have been split to form 4 partitions + uint32 mDepth; // Depth of this node in the tree + Vec3 mNodeBoundsMin; // Bounding box of this node, accumulated while iterating over children + Vec3 mNodeBoundsMax; + }; + static_assert(sizeof(StackEntry) == 64); + StackEntry stack[cStackSize / 4]; // We don't process 4 at a time in this loop but 1, so the stack can be 4x as small + int top = 0; + + // Create root node + stack[0].mNodeIdx = AllocateNode(inMaxDepthMarkChanged > 0); + stack[0].mChildIdx = -1; + stack[0].mDepth = 0; + stack[0].mNodeBoundsMin = Vec3::sReplicate(cLargeFloat); + stack[0].mNodeBoundsMax = Vec3::sReplicate(-cLargeFloat); + sPartition4(ioNodeIDs, centers, 0, inNumber, stack[0].mSplit); + + for (;;) + { + StackEntry &cur_stack = stack[top]; + + // Next child + cur_stack.mChildIdx++; + + // Check if all children processed + if (cur_stack.mChildIdx >= 4) + { + // Terminate if there's nothing left to pop + if (top <= 0) + break; + + // Add our bounds to our parents bounds + StackEntry &prev_stack = stack[top - 1]; + prev_stack.mNodeBoundsMin = Vec3::sMin(prev_stack.mNodeBoundsMin, cur_stack.mNodeBoundsMin); + prev_stack.mNodeBoundsMax = Vec3::sMax(prev_stack.mNodeBoundsMax, cur_stack.mNodeBoundsMax); + + // Store parent node + Node &node = mAllocator->Get(cur_stack.mNodeIdx); + node.mParentNodeIndex = prev_stack.mNodeIdx; + + // Store this node's properties in the parent node + Node &parent_node = mAllocator->Get(prev_stack.mNodeIdx); + parent_node.mChildNodeID[prev_stack.mChildIdx] = NodeID::sFromNodeIndex(cur_stack.mNodeIdx); + parent_node.SetChildBounds(prev_stack.mChildIdx, AABox(cur_stack.mNodeBoundsMin, cur_stack.mNodeBoundsMax)); + + // Pop entry from stack + --top; + } + else + { + // Get low and high index to bodies to process + int low = cur_stack.mSplit[cur_stack.mChildIdx]; + int high = cur_stack.mSplit[cur_stack.mChildIdx + 1]; + int num_bodies = high - low; + + if (num_bodies == 1) + { + // Get body info + NodeID child_node_id = ioNodeIDs[low]; + AABox bounds = GetNodeOrBodyBounds(inBodies, child_node_id); + + // Update node + Node &node = mAllocator->Get(cur_stack.mNodeIdx); + node.mChildNodeID[cur_stack.mChildIdx] = child_node_id; + node.SetChildBounds(cur_stack.mChildIdx, bounds); + + if (child_node_id.IsNode()) + { + // Update parent for this node + Node &child_node = mAllocator->Get(child_node_id.GetNodeIndex()); + child_node.mParentNodeIndex = cur_stack.mNodeIdx; + } + else + { + // Set location in tracking + SetBodyLocation(ioTracking, child_node_id.GetBodyID(), cur_stack.mNodeIdx, cur_stack.mChildIdx); + } + + // Encapsulate bounding box in parent + cur_stack.mNodeBoundsMin = Vec3::sMin(cur_stack.mNodeBoundsMin, bounds.mMin); + cur_stack.mNodeBoundsMax = Vec3::sMax(cur_stack.mNodeBoundsMax, bounds.mMax); + } + else if (num_bodies > 1) + { + // Allocate new node + StackEntry &new_stack = stack[++top]; + JPH_ASSERT(top < cStackSize / 4); + uint32 next_depth = cur_stack.mDepth + 1; + new_stack.mNodeIdx = AllocateNode(inMaxDepthMarkChanged > next_depth); + new_stack.mChildIdx = -1; + new_stack.mDepth = next_depth; + new_stack.mNodeBoundsMin = Vec3::sReplicate(cLargeFloat); + new_stack.mNodeBoundsMax = Vec3::sReplicate(-cLargeFloat); + sPartition4(ioNodeIDs, centers, low, high, new_stack.mSplit); + } + } + } + + // Delete temporary data + delete [] centers; + + // Store bounding box of root + outBounds.mMin = stack[0].mNodeBoundsMin; + outBounds.mMax = stack[0].mNodeBoundsMax; + + // Return root + return NodeID::sFromNodeIndex(stack[0].mNodeIdx); +} + +void QuadTree::MarkNodeAndParentsChanged(uint32 inNodeIndex) +{ + uint32 node_idx = inNodeIndex; + + do + { + // If node has changed, parent will be too + Node &node = mAllocator->Get(node_idx); + if (node.mIsChanged) + break; + + // Mark node as changed + node.mIsChanged = true; + + // Get our parent + node_idx = node.mParentNodeIndex; + } + while (node_idx != cInvalidNodeIndex); +} + +void QuadTree::WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds) +{ + uint32 node_idx = inNodeIndex; + + for (;;) + { + // Mark node as changed + Node &node = mAllocator->Get(node_idx); + node.mIsChanged = true; + + // Get our parent + uint32 parent_idx = node.mParentNodeIndex; + if (parent_idx == cInvalidNodeIndex) + break; + + // Find which child of the parent we're in + Node &parent_node = mAllocator->Get(parent_idx); + NodeID node_id = NodeID::sFromNodeIndex(node_idx); + int child_idx = -1; + for (int i = 0; i < 4; ++i) + if (parent_node.mChildNodeID[i] == node_id) + { + // Found one, set the node index and child index and update the bounding box too + child_idx = i; + break; + } + JPH_ASSERT(child_idx != -1, "Nodes don't get removed from the tree, we must have found it"); + + // To avoid any race conditions with other threads we only enlarge bounding boxes + if (!parent_node.EncapsulateChildBounds(child_idx, inNewBounds)) + { + // No changes to bounding box, only marking as changed remains to be done + if (!parent_node.mIsChanged) + MarkNodeAndParentsChanged(parent_idx); + break; + } + + // Update node index + node_idx = parent_idx; + } +} + +bool QuadTree::TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies) +{ + // Tentively assign the node as parent + bool leaf_is_node = inLeafID.IsNode(); + if (leaf_is_node) + { + uint32 leaf_idx = inLeafID.GetNodeIndex(); + mAllocator->Get(leaf_idx).mParentNodeIndex = inNodeIndex; + } + + // Fetch node that we're adding to + Node &node = mAllocator->Get(inNodeIndex); + + // Find an empty child + for (uint32 child_idx = 0; child_idx < 4; ++child_idx) + if (node.mChildNodeID[child_idx].CompareExchange(NodeID::sInvalid(), inLeafID)) // Check if we can claim it + { + // We managed to add it to the node + + // If leaf was a body, we need to update its bookkeeping + if (!leaf_is_node) + SetBodyLocation(ioTracking, inLeafID.GetBodyID(), inNodeIndex, child_idx); + + // Now set the bounding box making the child valid for queries + node.SetChildBounds(child_idx, inLeafBounds); + + // Widen the bounds for our parents too + WidenAndMarkNodeAndParentsChanged(inNodeIndex, inLeafBounds); + + // Update body counter + mNumBodies += inLeafNumBodies; + + // And we're done + return true; + } + + return false; +} + +bool QuadTree::TryCreateNewRoot(TrackingVector &ioTracking, atomic &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies) +{ + // Fetch old root + uint32 root_idx = ioRootNodeIndex; + Node &root = mAllocator->Get(root_idx); + + // Create new root, mark this new root as changed as we're not creating a very efficient tree at this point + uint32 new_root_idx = AllocateNode(true); + Node &new_root = mAllocator->Get(new_root_idx); + + // First child is current root, note that since the tree may be modified concurrently we cannot assume that the bounds of our child will be correct so we set a very large bounding box + new_root.mChildNodeID[0] = NodeID::sFromNodeIndex(root_idx); + new_root.SetChildBounds(0, AABox(Vec3::sReplicate(-cLargeFloat), Vec3::sReplicate(cLargeFloat))); + + // Second child is new leaf + new_root.mChildNodeID[1] = inLeafID; + new_root.SetChildBounds(1, inLeafBounds); + + // Tentatively assign new root as parent + bool leaf_is_node = inLeafID.IsNode(); + if (leaf_is_node) + { + uint32 leaf_idx = inLeafID.GetNodeIndex(); + mAllocator->Get(leaf_idx).mParentNodeIndex = new_root_idx; + } + + // Try to swap it + if (ioRootNodeIndex.compare_exchange_strong(root_idx, new_root_idx)) + { + // We managed to set the new root + + // If leaf was a body, we need to update its bookkeeping + if (!leaf_is_node) + SetBodyLocation(ioTracking, inLeafID.GetBodyID(), new_root_idx, 1); + + // Store parent node for old root + root.mParentNodeIndex = new_root_idx; + + // Update body counter + mNumBodies += inLeafNumBodies; + + // And we're done + return true; + } + + // Failed to swap, someone else must have created a new root, try again + mAllocator->DestructObject(new_root_idx); + return false; +} + +void QuadTree::AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_ENABLE_ASSERTS + // Below we just cast the body ID's to node ID's, check here that that is valid + for (const BodyID *b = ioBodyIDs, *b_end = ioBodyIDs + inNumber; b < b_end; ++b) + NodeID::sFromBodyID(*b); +#endif + + // Build subtree for the new bodies, note that we mark all nodes as 'not changed' + // so they will stay together as a batch and will make the tree rebuild cheaper + outState.mLeafID = BuildTree(inBodies, ioTracking, (NodeID *)ioBodyIDs, inNumber, 0, outState.mLeafBounds); + +#ifdef JPH_DEBUG + if (outState.mLeafID.IsNode()) + ValidateTree(inBodies, ioTracking, outState.mLeafID.GetNodeIndex(), inNumber); +#endif +} + +void QuadTree::AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState) +{ + // Assert sane input + JPH_ASSERT(inNumberBodies > 0); + + // Mark tree dirty + mIsDirty = true; + + // Get the current root node + RootNode &root_node = GetCurrentRoot(); + + for (;;) + { + // Check if we can insert the body in the root + if (TryInsertLeaf(ioTracking, root_node.mIndex, inState.mLeafID, inState.mLeafBounds, inNumberBodies)) + return; + + // Check if we can create a new root + if (TryCreateNewRoot(ioTracking, root_node.mIndex, inState.mLeafID, inState.mLeafBounds, inNumberBodies)) + return; + } +} + +void QuadTree::AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState) +{ + // Collect all bodies + Allocator::Batch free_batch; + NodeID node_stack[cStackSize]; + node_stack[0] = inState.mLeafID; + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Reset location of body + sInvalidateBodyLocation(ioTracking, child_node_id.GetBodyID()); + } + else + { + // Process normal node + uint32 node_idx = child_node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + for (NodeID sub_child_node_id : node.mChildNodeID) + if (sub_child_node_id.IsValid()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = sub_child_node_id; + top++; + } + + // Mark it to be freed + mAllocator->AddObjectToBatch(free_batch, node_idx); + } + --top; + } + while (top >= 0); + + // Now free all nodes as a single batch + mAllocator->DestructObjectBatch(free_batch); +} + +void QuadTree::RemoveBodies([[maybe_unused]] const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + + // Mark tree dirty + mIsDirty = true; + + for (const BodyID *cur = ioBodyIDs, *end = ioBodyIDs + inNumber; cur < end; ++cur) + { + // Check if BodyID is correct + JPH_ASSERT(inBodies[cur->GetIndex()]->GetID() == *cur, "Provided BodyID doesn't match BodyID in body manager"); + + // Get location of body + uint32 node_idx, child_idx; + GetBodyLocation(ioTracking, *cur, node_idx, child_idx); + + // First we reset our internal bookkeeping + sInvalidateBodyLocation(ioTracking, *cur); + + // Then we make the bounding box invalid, no queries can find this node anymore + Node &node = mAllocator->Get(node_idx); + node.InvalidateChildBounds(child_idx); + + // Finally we reset the child id, this makes the node available for adds again + node.mChildNodeID[child_idx] = NodeID::sInvalid(); + + // We don't need to bubble up our bounding box changes to our parents since we never make volumes smaller, only bigger + // But we do need to mark the nodes as changed so that the tree can be rebuilt + MarkNodeAndParentsChanged(node_idx); + } + + mNumBodies -= inNumber; +} + +void QuadTree::NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + + for (const BodyID *cur = ioBodyIDs, *end = ioBodyIDs + inNumber; cur < end; ++cur) + { + // Check if BodyID is correct + const Body *body = inBodies[cur->GetIndex()]; + JPH_ASSERT(body->GetID() == *cur, "Provided BodyID doesn't match BodyID in body manager"); + + // Get the new bounding box + const AABox &new_bounds = body->GetWorldSpaceBounds(); + + // Get location of body + uint32 node_idx, child_idx; + GetBodyLocation(inTracking, *cur, node_idx, child_idx); + + // Widen bounds for node + Node &node = mAllocator->Get(node_idx); + if (node.EncapsulateChildBounds(child_idx, new_bounds)) + { + // Mark tree dirty + mIsDirty = true; + + // If bounds changed, widen the bounds for our parents too + WidenAndMarkNodeAndParentsChanged(node_idx, new_bounds); + } + } +} + +template +JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const +{ + // Get the root + const RootNode &root_node = GetCurrentRoot(); + +#ifdef JPH_TRACK_BROADPHASE_STATS + // Start tracking stats + int bodies_visited = 0; + int hits_collected = 0; + int nodes_visited = 0; + uint64 collector_ticks = 0; + + uint64 start = GetProcessorTickCount(); +#endif // JPH_TRACK_BROADPHASE_STATS + + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Track amount of bodies visited + JPH_IF_TRACK_BROADPHASE_STATS(++bodies_visited;) + + BodyID body_id = child_node_id.GetBodyID(); + ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid + if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer)) + { + JPH_PROFILE("VisitBody"); + + // Track amount of hits + JPH_IF_TRACK_BROADPHASE_STATS(++hits_collected;) + + // Start track time the collector takes + JPH_IF_TRACK_BROADPHASE_STATS(uint64 collector_start = GetProcessorTickCount();) + + // We found a body we collide with, call our visitor + ioVisitor.VisitBody(body_id, top); + + // End track time the collector takes + JPH_IF_TRACK_BROADPHASE_STATS(collector_ticks += GetProcessorTickCount() - collector_start;) + + // Check if we're done + if (ioVisitor.ShouldAbort()) + break; + } + } + else if (child_node_id.IsValid()) + { + JPH_IF_TRACK_BROADPHASE_STATS(++nodes_visited;) + + // Check if stack can hold more nodes + if (top + 4 < cStackSize) + { + // Process normal node + const Node &node = mAllocator->Get(child_node_id.GetNodeIndex()); + JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE)); + + // Load bounds of 4 children + Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX); + Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY); + Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ); + Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX); + Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY); + Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ); + + // Load ids for 4 children + UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]); + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, child_ids, top); + child_ids.StoreInt4((uint32 *)&node_stack[top]); + top += num_results; + } + else + JPH_ASSERT(false, "Stack full!\n" + "This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n" + "If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree."); + } + + // Fetch next node until we find one that the visitor wants to see + do + --top; + while (top >= 0 && !ioVisitor.ShouldVisitNode(top)); + } + while (top >= 0); + +#ifdef JPH_TRACK_BROADPHASE_STATS + // Calculate total time the broadphase walk took + uint64 total_ticks = GetProcessorTickCount() - start; + + // Update stats under lock protection (slow!) + { + unique_lock lock(mStatsMutex); + Stat &s = ioStats[inObjectLayerFilter.GetDescription()]; + s.mNumQueries++; + s.mNodesVisited += nodes_visited; + s.mBodiesVisited += bodies_visited; + s.mHitsReported += hits_collected; + s.mTotalTicks += total_ticks; + s.mCollectorTicks += collector_ticks; + } +#endif // JPH_TRACK_BROADPHASE_STATS +} + +void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const RayCast &inRay, RayCastBodyCollector &ioCollector) : + mOrigin(inRay.mOrigin), + mInvDirection(inRay.mDirection), + mCollector(ioCollector) + { + mFractionStack[0] = -1; + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mFractionStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) + { + // Test the ray against 4 bounding boxes + Vec4 fraction = RayAABox4(mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(fraction, mCollector.GetEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] }; + mCollector.AddHit(result); + } + + private: + Vec3 mOrigin; + RayInvDirection mInvDirection; + RayCastBodyCollector & mCollector; + float mFractionStack[cStackSize]; + }; + + Visitor visitor(inRay, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastRayStats)); +} + +void QuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const AABox &inBox, CollideShapeBodyCollector &ioCollector) : + mBox(inBox), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test the box vs 4 boxes + UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + const AABox & mBox; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideAABoxStats)); +} + +void QuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector) : + mCenterX(inCenter.SplatX()), + mCenterY(inCenter.SplatY()), + mCenterZ(inCenter.SplatZ()), + mRadiusSq(Vec4::sReplicate(Square(inRadius))), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test 4 boxes vs sphere + UVec4 hitting = AABox4VsSphere(mCenterX, mCenterY, mCenterZ, mRadiusSq, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + Vec4 mCenterX; + Vec4 mCenterY; + Vec4 mCenterZ; + Vec4 mRadiusSq; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inCenter, inRadius, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideSphereStats)); +} + +void QuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector) : + mPoint(inPoint), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test if point overlaps with box + UVec4 hitting = AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + Vec3 mPoint; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inPoint, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollidePointStats)); +} + +void QuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector) : + mBox(inBox), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test if point overlaps with box + UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + OrientedBox mBox; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideOrientedBoxStats)); +} + +void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector) : + mOrigin(inBox.mBox.GetCenter()), + mExtent(inBox.mBox.GetExtent()), + mInvDirection(inBox.mDirection), + mCollector(ioCollector) + { + mFractionStack[0] = -1; + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mFractionStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) + { + // Enlarge them by the casted aabox extents + Vec4 bounds_min_x = inBoundsMinX, bounds_min_y = inBoundsMinY, bounds_min_z = inBoundsMinZ, bounds_max_x = inBoundsMaxX, bounds_max_y = inBoundsMaxY, bounds_max_z = inBoundsMaxZ; + AABox4EnlargeWithExtent(mExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test 4 children + Vec4 fraction = RayAABox4(mOrigin, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(fraction, mCollector.GetPositiveEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] }; + mCollector.AddHit(result); + } + + private: + Vec3 mOrigin; + Vec3 mExtent; + RayInvDirection mInvDirection; + CastShapeBodyCollector & mCollector; + float mFractionStack[cStackSize]; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastAABoxStats)); +} + +void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const +{ + // Note that we don't lock the tree at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update. + // We double check this at the end of the function. + const RootNode &root_node = GetCurrentRoot(); + JPH_ASSERT(root_node.mIndex != cInvalidNodeIndex); + + // Assert sane input + JPH_ASSERT(inActiveBodies != nullptr); + JPH_ASSERT(inNumActiveBodies > 0); + + NodeID node_stack[cStackSize]; + + // Loop over all active bodies + for (int b1 = 0; b1 < inNumActiveBodies; ++b1) + { + BodyID b1_id = inActiveBodies[b1]; + const Body &body1 = *inBodies[b1_id.GetIndex()]; + JPH_ASSERT(!body1.IsStatic()); + + // Expand the bounding box by the speculative contact distance + AABox bounds1 = body1.GetWorldSpaceBounds(); + bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance)); + + // Test each body with the tree + node_stack[0] = root_node.GetNodeID(); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Don't collide with self + BodyID b2_id = child_node_id.GetBodyID(); + if (b1_id != b2_id) + { + // Collision between dynamic pairs need to be picked up only once + const Body &body2 = *inBodies[b2_id.GetIndex()]; + if (inObjectLayerPairFilter.ShouldCollide(body1.GetObjectLayer(), body2.GetObjectLayer()) + && Body::sFindCollidingPairsCanCollide(body1, body2) + && bounds1.Overlaps(body2.GetWorldSpaceBounds())) // In the broadphase we widen the bounding box when a body moves, do a final check to see if the bounding boxes actually overlap + { + // Store potential hit between bodies + ioPairCollector.AddHit({ b1_id, b2_id }); + } + } + } + else if (child_node_id.IsValid()) + { + // Process normal node + const Node &node = mAllocator->Get(child_node_id.GetNodeIndex()); + JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE)); + + // Get bounds of 4 children + Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX); + Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY); + Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ); + Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX); + Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY); + Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ); + + // Test overlap + UVec4 overlap = AABox4VsBox(bounds1, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz); + int num_results = overlap.CountTrues(); + if (num_results > 0) + { + // Load ids for 4 children + UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]); + + // Sort so that overlaps are first + child_ids = UVec4::sSort4True(overlap, child_ids); + + // Push them onto the stack + if (top + 4 < cStackSize) + { + child_ids.StoreInt4((uint32 *)&node_stack[top]); + top += num_results; + } + else + JPH_ASSERT(false, "Stack full!\n" + "This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n" + "If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree."); + } + } + --top; + } + while (top >= 0); + } + + // Test that the root node was not swapped while finding collision pairs. + // This would mean that UpdateFinalize/DiscardOldTree ran during collision detection which should not be possible due to the way the jobs are scheduled. + JPH_ASSERT(root_node.mIndex != cInvalidNodeIndex); + JPH_ASSERT(&root_node == &GetCurrentRoot()); +} + +#ifdef JPH_DEBUG + +void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const +{ + JPH_PROFILE_FUNCTION(); + + // Root should be valid + JPH_ASSERT(inNodeIndex != cInvalidNodeIndex); + + // To avoid call overhead, create a stack in place + struct StackEntry + { + uint32 mNodeIndex; + uint32 mParentNodeIndex; + }; + StackEntry stack[cStackSize]; + stack[0].mNodeIndex = inNodeIndex; + stack[0].mParentNodeIndex = cInvalidNodeIndex; + int top = 0; + + uint32 num_bodies = 0; + + do + { + // Copy entry from the stack + StackEntry cur_stack = stack[top]; + + // Validate parent + const Node &node = mAllocator->Get(cur_stack.mNodeIndex); + JPH_ASSERT(node.mParentNodeIndex == cur_stack.mParentNodeIndex); + + // Validate that when a parent is not-changed that all of its children are also + JPH_ASSERT(cur_stack.mParentNodeIndex == cInvalidNodeIndex || mAllocator->Get(cur_stack.mParentNodeIndex).mIsChanged || !node.mIsChanged); + + // Loop children + for (uint32 i = 0; i < 4; ++i) + { + NodeID child_node_id = node.mChildNodeID[i]; + if (child_node_id.IsValid()) + { + if (child_node_id.IsNode()) + { + // Child is a node, recurse + uint32 child_idx = child_node_id.GetNodeIndex(); + JPH_ASSERT(top < cStackSize); + StackEntry &new_entry = stack[top++]; + new_entry.mNodeIndex = child_idx; + new_entry.mParentNodeIndex = cur_stack.mNodeIndex; + + // Validate that the bounding box is bigger or equal to the bounds in the tree + // Bounding box could also be invalid if all children of our child were removed + AABox child_bounds; + node.GetChildBounds(i, child_bounds); + AABox real_child_bounds; + mAllocator->Get(child_idx).GetNodeBounds(real_child_bounds); + JPH_ASSERT(child_bounds.Contains(real_child_bounds) || !real_child_bounds.IsValid()); + } + else + { + // Increment number of bodies found + ++num_bodies; + + // Check if tracker matches position of body + uint32 node_idx, child_idx; + GetBodyLocation(inTracking, child_node_id.GetBodyID(), node_idx, child_idx); + JPH_ASSERT(node_idx == cur_stack.mNodeIndex); + JPH_ASSERT(child_idx == i); + + // Validate that the body bounds are bigger or equal to the bounds in the tree + AABox body_bounds; + node.GetChildBounds(i, body_bounds); + const Body *body = inBodies[child_node_id.GetBodyID().GetIndex()]; + AABox cached_body_bounds = body->GetWorldSpaceBounds(); + AABox real_body_bounds = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + JPH_ASSERT(cached_body_bounds == real_body_bounds); // Check that cached body bounds are up to date + JPH_ASSERT(body_bounds.Contains(real_body_bounds)); + } + } + } + --top; + } + while (top >= 0); + + // Check that the amount of bodies in the tree matches our counter + JPH_ASSERT(num_bodies == inNumExpectedBodies); +} + +#endif + +#ifdef JPH_DUMP_BROADPHASE_TREE + +void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const +{ + // Open DOT file + std::ofstream f; + f.open(StringFormat("%s.dot", inFileNamePrefix).c_str(), std::ofstream::out | std::ofstream::trunc); + if (!f.is_open()) + return; + + // Write header + f << "digraph {\n"; + + // Iterate the entire tree + NodeID node_stack[cStackSize]; + node_stack[0] = inRoot; + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID node_id = node_stack[top]; + if (node_id.IsBody()) + { + // Output body + String body_id = ConvertToString(node_id.GetBodyID().GetIndex()); + f << "body" << body_id << "[label = \"Body " << body_id << "\"]\n"; + } + else + { + // Process normal node + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + // Get bounding box + AABox bounds; + node.GetNodeBounds(bounds); + + // Output node + String node_str = ConvertToString(node_idx); + f << "node" << node_str << "[label = \"Node " << node_str << "\nVolume: " << ConvertToString(bounds.GetVolume()) << "\" color=" << (node.mIsChanged? "red" : "black") << "]\n"; + + // Recurse and get all children + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = child_node_id; + top++; + + // Output link + f << "node" << node_str << " -> "; + if (child_node_id.IsBody()) + f << "body" << ConvertToString(child_node_id.GetBodyID().GetIndex()); + else + f << "node" << ConvertToString(child_node_id.GetNodeIndex()); + f << "\n"; + } + } + --top; + } + while (top >= 0); + + // Finish DOT file + f << "}\n"; + f.close(); + + // Convert to svg file + String cmd = StringFormat("dot %s.dot -Tsvg -o %s.svg", inFileNamePrefix, inFileNamePrefix); + system(cmd.c_str()); +} + +#endif // JPH_DUMP_BROADPHASE_TREE + +#ifdef JPH_TRACK_BROADPHASE_STATS + +uint64 QuadTree::GetTicks100Pct(const LayerToStats &inLayer) const +{ + uint64 total_ticks = 0; + for (const LayerToStats::value_type &kv : inLayer) + total_ticks += kv.second.mTotalTicks; + return total_ticks; +} + +void QuadTree::ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const +{ + for (const LayerToStats::value_type &kv : inLayer) + { + double total_pct = 100.0 * double(kv.second.mTotalTicks) / double(inTicks100Pct); + double total_pct_excl_collector = 100.0 * double(kv.second.mTotalTicks - kv.second.mCollectorTicks) / double(inTicks100Pct); + double hits_reported_vs_bodies_visited = kv.second.mBodiesVisited > 0? 100.0 * double(kv.second.mHitsReported) / double(kv.second.mBodiesVisited) : 100.0; + double hits_reported_vs_nodes_visited = kv.second.mNodesVisited > 0? double(kv.second.mHitsReported) / double(kv.second.mNodesVisited) : -1.0; + + std::stringstream str; + str << inName << ", " << kv.first << ", " << mName << ", " << kv.second.mNumQueries << ", " << total_pct << ", " << total_pct_excl_collector << ", " << kv.second.mNodesVisited << ", " << kv.second.mBodiesVisited << ", " << kv.second.mHitsReported << ", " << hits_reported_vs_bodies_visited << ", " << hits_reported_vs_nodes_visited; + Trace(str.str().c_str()); + } +} + +uint64 QuadTree::GetTicks100Pct() const +{ + uint64 total_ticks = 0; + total_ticks += GetTicks100Pct(mCastRayStats); + total_ticks += GetTicks100Pct(mCollideAABoxStats); + total_ticks += GetTicks100Pct(mCollideSphereStats); + total_ticks += GetTicks100Pct(mCollidePointStats); + total_ticks += GetTicks100Pct(mCollideOrientedBoxStats); + total_ticks += GetTicks100Pct(mCastAABoxStats); + return total_ticks; +} + +void QuadTree::ReportStats(uint64 inTicks100Pct) const +{ + unique_lock lock(mStatsMutex); + ReportStats("RayCast", mCastRayStats, inTicks100Pct); + ReportStats("CollideAABox", mCollideAABoxStats, inTicks100Pct); + ReportStats("CollideSphere", mCollideSphereStats, inTicks100Pct); + ReportStats("CollidePoint", mCollidePointStats, inTicks100Pct); + ReportStats("CollideOrientedBox", mCollideOrientedBoxStats, inTicks100Pct); + ReportStats("CastAABox", mCastAABoxStats, inTicks100Pct); +} + +#endif // JPH_TRACK_BROADPHASE_STATS + +uint QuadTree::GetMaxTreeDepth(const NodeID &inNodeID) const +{ + // Reached a leaf? + if (!inNodeID.IsValid() || inNodeID.IsBody()) + return 0; + + // Recurse to children + uint max_depth = 0; + const Node &node = mAllocator->Get(inNodeID.GetNodeIndex()); + for (NodeID child_node_id : node.mChildNodeID) + max_depth = max(max_depth, GetMaxTreeDepth(child_node_id)); + return max_depth + 1; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h new file mode 100644 index 000000000000..d698beb5f372 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h @@ -0,0 +1,390 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +//#define JPH_DUMP_BROADPHASE_TREE + +JPH_NAMESPACE_BEGIN + +/// Internal tree structure in broadphase, is essentially a quad AABB tree. +/// Tree is lockless (except for UpdatePrepare/Finalize() function), modifying objects in the tree will widen the aabbs of parent nodes to make the node fit. +/// During the UpdatePrepare/Finalize() call the tree is rebuilt to achieve a tight fit again. +class JPH_EXPORT QuadTree : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + +private: + // Forward declare + class AtomicNodeID; + + /// Class that points to either a body or a node in the tree + class NodeID + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor does not initialize + inline NodeID() = default; + + /// Construct a node ID + static inline NodeID sInvalid() { return NodeID(cInvalidNodeIndex); } + static inline NodeID sFromBodyID(BodyID inID) { NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; } + static inline NodeID sFromNodeIndex(uint32 inIdx) { NodeID node_id(inIdx | cIsNode); JPH_ASSERT(node_id.IsNode()); return node_id; } + + /// Check what type of ID it is + inline bool IsValid() const { return mID != cInvalidNodeIndex; } + inline bool IsBody() const { return (mID & cIsNode) == 0; } + inline bool IsNode() const { return (mID & cIsNode) != 0; } + + /// Get body or node index + inline BodyID GetBodyID() const { JPH_ASSERT(IsBody()); return BodyID(mID); } + inline uint32 GetNodeIndex() const { JPH_ASSERT(IsNode()); return mID & ~cIsNode; } + + /// Comparison + inline bool operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); } + inline bool operator == (const NodeID &inRHS) const { return mID == inRHS.mID; } + + private: + friend class AtomicNodeID; + + inline explicit NodeID(uint32 inID) : mID(inID) { } + + static const uint32 cIsNode = BodyID::cBroadPhaseBit; ///< If this bit is set it means that the ID refers to a node, otherwise it refers to a body + + uint32 mID; + }; + + static_assert(sizeof(NodeID) == sizeof(BodyID), "Body id's should have the same size as NodeIDs"); + + /// A NodeID that uses atomics to store the value + class AtomicNodeID + { + public: + /// Constructor + AtomicNodeID() = default; + explicit AtomicNodeID(const NodeID &inRHS) : mID(inRHS.mID) { } + + /// Assignment + inline void operator = (const NodeID &inRHS) { mID = inRHS.mID; } + + /// Getting the value + inline operator NodeID () const { return NodeID(mID); } + + /// Check if the ID is valid + inline bool IsValid() const { return mID != cInvalidNodeIndex; } + + /// Comparison + inline bool operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); } + inline bool operator == (const NodeID &inRHS) const { return mID == inRHS.mID; } + + /// Atomically compare and swap value. Expects inOld value, replaces with inNew value or returns false + inline bool CompareExchange(NodeID inOld, NodeID inNew) { return mID.compare_exchange_strong(inOld.mID, inNew.mID); } + + private: + atomic mID; + }; + + /// Class that represents a node in the tree + class Node + { + public: + /// Construct node + explicit Node(bool inIsChanged); + + /// Get bounding box encapsulating all children + void GetNodeBounds(AABox &outBounds) const; + + /// Get bounding box in a consistent way with the functions below (check outBounds.IsValid() before using the box) + void GetChildBounds(int inChildIndex, AABox &outBounds) const; + + /// Set the bounds in such a way that other threads will either see a fully correct bounding box or a bounding box with no volume + void SetChildBounds(int inChildIndex, const AABox &inBounds); + + /// Invalidate bounding box in such a way that other threads will not temporarily see a very large bounding box + void InvalidateChildBounds(int inChildIndex); + + /// Encapsulate inBounds in node bounds, returns true if there were changes + bool EncapsulateChildBounds(int inChildIndex, const AABox &inBounds); + + /// Bounding box for child nodes or bodies (all initially set to invalid so no collision test will ever traverse to the leaf) + atomic mBoundsMinX[4]; + atomic mBoundsMinY[4]; + atomic mBoundsMinZ[4]; + atomic mBoundsMaxX[4]; + atomic mBoundsMaxY[4]; + atomic mBoundsMaxZ[4]; + + /// Index of child node or body ID. + AtomicNodeID mChildNodeID[4]; + + /// Index of the parent node. + /// Note: This value is unreliable during the UpdatePrepare/Finalize() function as a node may be relinked to the newly built tree. + atomic mParentNodeIndex = cInvalidNodeIndex; + + /// If this part of the tree has changed, if not, we will treat this sub tree as a single body during the UpdatePrepare/Finalize(). + /// If any changes are made to an object inside this sub tree then the direct path from the body to the top of the tree will become changed. + atomic mIsChanged; + + // Padding to align to 124 bytes + uint32 mPadding = 0; + }; + + // Maximum size of the stack during tree walk + static constexpr int cStackSize = 128; + + static_assert(sizeof(atomic) == 4, "Assuming that an atomic doesn't add any additional storage"); + static_assert(sizeof(atomic) == 4, "Assuming that an atomic doesn't add any additional storage"); + static_assert(std::is_trivially_destructible(), "Assuming that we don't have a destructor"); + +public: + /// Class that allocates tree nodes, can be shared between multiple trees + using Allocator = FixedSizeFreeList; + + static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes"); + + /// Data to track location of a Body in the tree + struct Tracking + { + /// Constructor to satisfy the vector class + Tracking() = default; + Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { } + + /// Invalid body location identifier + static const uint32 cInvalidBodyLocation = 0xffffffff; + + atomic mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + atomic mObjectLayer = cObjectLayerInvalid; + atomic mBodyLocation { cInvalidBodyLocation }; + }; + + using TrackingVector = Array; + + /// Destructor + ~QuadTree(); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Name of the tree for debugging purposes + void SetName(const char *inName) { mName = inName; } + inline const char * GetName() const { return mName; } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + + /// Check if there is anything in the tree + inline bool HasBodies() const { return mNumBodies != 0; } + + /// Check if the tree needs an UpdatePrepare/Finalize() + inline bool IsDirty() const { return mIsDirty; } + + /// Check if this tree can get an UpdatePrepare/Finalize() or if it needs a DiscardOldTree() first + inline bool CanBeUpdated() const { return mFreeNodeBatch.mNumObjects == 0; } + + /// Initialization + void Init(Allocator &inAllocator); + + struct UpdateState + { + NodeID mRootNodeID; ///< This will be the new root node id + }; + + /// Will throw away the previous frame's nodes so that we can start building a new tree in the background + void DiscardOldTree(); + + /// Get the bounding box for this tree + AABox GetBounds() const; + + /// Update the broadphase, needs to be called regularly to achieve a tight fit of the tree when bodies have been modified. + /// UpdatePrepare() will build the tree, UpdateFinalize() will lock the root of the tree shortly and swap the trees and afterwards clean up temporary data structures. + void UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild); + void UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState); + + /// Temporary data structure to pass information between AddBodiesPrepare and AddBodiesFinalize/Abort + struct AddState + { + NodeID mLeafID = NodeID::sInvalid(); + AABox mLeafBounds; + }; + + /// Prepare adding inNumber bodies at ioBodyIDs to the quad tree, returns the state in outState that should be used in AddBodiesFinalize. + /// This can be done on a background thread without influencing the broadphase. + /// ioBodyIDs may be shuffled around by this function. + void AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState); + + /// Finalize adding bodies to the quadtree, supply the same number of bodies as in AddBodiesPrepare. + void AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState); + + /// Abort adding bodies to the quadtree, supply the same bodies and state as in AddBodiesPrepare. + /// This can be done on a background thread without influencing the broadphase. + void AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState); + + /// Remove inNumber bodies in ioBodyIDs from the quadtree. + void RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber); + + /// Call whenever the aabb of a body changes. + void NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber); + + /// Cast a ray and get the intersecting bodies in ioCollector. + void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with inBox in ioCollector + void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with a sphere in ioCollector + void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with a point and any hits to ioCollector + void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with an oriented box and any hits to ioCollector + void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Cast a box and get intersecting bodies in ioCollector + void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Find all colliding pairs between dynamic bodies, calls ioPairCollector for every pair found + void FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Sum up all the ticks spent in the various layers + uint64 GetTicks100Pct() const; + + /// Trace the stats of this tree to the TTY + void ReportStats(uint64 inTicks100Pct) const; +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + /// Constants + static const uint32 cInvalidNodeIndex = 0xffffffff; ///< Value used to indicate node index is invalid + static const float cLargeFloat; ///< A large floating point number that is small enough to not cause any overflows + static const AABox cInvalidBounds; ///< Invalid bounding box using cLargeFloat + + /// We alternate between two trees in order to let collision queries complete in parallel to adding/removing objects to the tree + struct RootNode + { + /// Get the ID of the root node + inline NodeID GetNodeID() const { return NodeID::sFromNodeIndex(mIndex); } + + /// Index of the root node of the tree (this is always a node, never a body id) + atomic mIndex { cInvalidNodeIndex }; + }; + + /// Caches location of body inBodyID in the tracker, body can be found in mNodes[inNodeIdx].mChildNodeID[inChildIdx] + void GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const; + void SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const; + static void sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID); + + /// Get the current root of the tree + JPH_INLINE const RootNode & GetCurrentRoot() const { return mRootNode[mRootNodeIndex]; } + JPH_INLINE RootNode & GetCurrentRoot() { return mRootNode[mRootNodeIndex]; } + + /// Depending on if inNodeID is a body or tree node return the bounding box + inline AABox GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const; + + /// Mark node and all of its parents as changed + inline void MarkNodeAndParentsChanged(uint32 inNodeIndex); + + /// Widen parent bounds of node inNodeIndex to encapsulate inNewBounds, also mark node and all of its parents as changed + inline void WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds); + + /// Allocate a new node + inline uint32 AllocateNode(bool inIsChanged); + + /// Try to insert a new leaf to the tree at inNodeIndex + inline bool TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies); + + /// Try to replace the existing root with a new root that contains both the existing root and the new leaf + inline bool TryCreateNewRoot(TrackingVector &ioTracking, atomic &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies); + + /// Build a tree for ioBodyIDs, returns the NodeID of the root (which will be the ID of a single body if inNumber = 1). All tree levels up to inMaxDepthMarkChanged will be marked as 'changed'. + NodeID BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds); + + /// Sorts ioNodeIDs spatially into 2 groups. Second groups starts at ioNodeIDs + outMidPoint. + /// After the function returns ioNodeIDs and ioNodeCenters will be shuffled + static void sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint); + + /// Sorts ioNodeIDs from inBegin to (but excluding) inEnd spatially into 4 groups. + /// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1] + /// After the function returns ioNodeIDs and ioNodeCenters will be shuffled + static void sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit); + +#ifdef JPH_DEBUG + /// Validate that the tree is consistent. + /// Note: This function only works if the tree is not modified while we're traversing it. + void ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const; +#endif + +#ifdef JPH_DUMP_BROADPHASE_TREE + /// Dump the tree in DOT format (see: https://graphviz.org/) + void DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const; +#endif + + /// Allocator that controls adding / freeing nodes + Allocator * mAllocator = nullptr; + + /// This is a list of nodes that must be deleted after the trees are swapped and the old tree is no longer in use + Allocator::Batch mFreeNodeBatch; + + /// Number of bodies currently in the tree + /// This is aligned to be in a different cache line from the `Allocator` pointer to prevent cross-thread syncs + /// when reading nodes. + alignas(JPH_CACHE_LINE_SIZE) atomic mNumBodies { 0 }; + + /// We alternate between two tree root nodes. When updating, we activate the new tree and we keep the old tree alive. + /// for queries that are in progress until the next time DiscardOldTree() is called. + RootNode mRootNode[2]; + atomic mRootNodeIndex { 0 }; + + /// Flag to keep track of changes to the broadphase, if false, we don't need to UpdatePrepare/Finalize() + atomic mIsDirty = false; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Mutex protecting the various LayerToStats members + mutable Mutex mStatsMutex; + + struct Stat + { + uint64 mNumQueries = 0; + uint64 mNodesVisited = 0; + uint64 mBodiesVisited = 0; + uint64 mHitsReported = 0; + uint64 mTotalTicks = 0; + uint64 mCollectorTicks = 0; + }; + + using LayerToStats = UnorderedMap; + + /// Sum up all the ticks in a layer + uint64 GetTicks100Pct(const LayerToStats &inLayer) const; + + /// Trace the stats of a single query type to the TTY + void ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const; + + mutable LayerToStats mCastRayStats; + mutable LayerToStats mCollideAABoxStats; + mutable LayerToStats mCollideSphereStats; + mutable LayerToStats mCollidePointStats; + mutable LayerToStats mCollideOrientedBoxStats; + mutable LayerToStats mCastAABoxStats; +#endif // JPH_TRACK_BROADPHASE_STATS + + /// Debug function to get the depth of the tree from node inNodeID + uint GetMaxTreeDepth(const NodeID &inNodeID) const; + + /// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitBody for each body encountered + template + JPH_INLINE void WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Name of this tree for debugging purposes + const char * mName = "Layer"; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp new file mode 100644 index 000000000000..a69208a04dd7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp @@ -0,0 +1,109 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CastConvexVsTriangles::CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) : + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mScale(inScale), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mCollector(ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); + + // Determine if shape is inside out or not + mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; +} + +void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) +{ + JPH_PROFILE_FUNCTION(); + + // Scale triangle + Vec3 v0 = mScale * inV0; + Vec3 v1 = mScale * inV1; + Vec3 v2 = mScale * inV2; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(mShapeCast.mDirection) > 0.0f; + if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Create triangle support function + TriangleConvexSupport triangle { v0, v1, v2 }; + + // Check if we already created the cast shape support function + if (mSupport == nullptr) + { + // Determine if we want to use the actual shape or a shrunken shape with convex radius + ConvexShape::ESupportMode support_mode = mShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default; + + // Create support function + mSupport = static_cast(mShapeCast.mShape)->GetSupportFunction(support_mode, mSupportBuffer, mShapeCast.mScale); + } + + EPAPenetrationDepth epa; + float fraction = mCollector.GetEarlyOutFraction(); + Vec3 contact_point_a, contact_point_b, contact_normal; + if (epa.CastShape(mShapeCast.mCenterOfMassStart, mShapeCast.mDirection, mShapeCastSettings.mCollisionTolerance, mShapeCastSettings.mPenetrationTolerance, *mSupport, triangle, mSupport->GetConvexRadius(), 0.0f, mShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal)) + { + // Check if we have enabled active edge detection + if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection); + + // Update the contact normal to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + contact_normal = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, contact_point_b, contact_normal, active_edge_movement_direction); + } + + // Convert to world space + contact_point_a = mCenterOfMassTransform2 * contact_point_a; + contact_point_b = mCenterOfMassTransform2 * contact_point_b; + Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(contact_normal); + + // Its a hit, store the sub shape id's + ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, back_facing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Early out if this hit is deeper than the collector's early out value + if (fraction == 0.0f && -result.mPenetrationDepth >= mCollector.GetEarlyOutFraction()) + return; + + // Gather faces + if (mShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + Mat44 transform_1_to_2 = mShapeCast.mCenterOfMassStart; + transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * mShapeCast.mDirection); + static_cast(mShapeCast.mShape)->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, mCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face); + + // Get face of the triangle + triangle.GetSupportingFace(contact_normal, result.mShape2Face); + + // Convert to world space + for (Vec3 &p : result.mShape2Face) + p = mCenterOfMassTransform2 * p; + } + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h new file mode 100644 index 000000000000..6a6c161cd243 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h @@ -0,0 +1,46 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that casts a convex object vs one or more triangles +class JPH_EXPORT CastConvexVsTriangles +{ +public: + /// Constructor + /// @param inShapeCast The shape to cast against the triangles and its start and direction + /// @param inShapeCastSettings Settings for performing the cast + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param ioCollector The collector that receives the results. + CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector); + + /// Cast convex object with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const ShapeCast & mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + const Mat44 & mCenterOfMassTransform2; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator1; + CastShapeCollector & mCollector; + +private: + ConvexShape::SupportBuffer mSupportBuffer; ///< Buffer that holds the support function of the cast shape + const ConvexShape::Support * mSupport = nullptr; ///< Support function of the cast shape + float mScaleSign; ///< Sign of the scale, -1 if object is inside out, 1 if not +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h new file mode 100644 index 000000000000..6bb49feb7c8b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a ray cast or other object cast hit +class BroadPhaseCastResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction. + inline float GetEarlyOutFraction() const { return mFraction; } + + /// Reset this result so it can be reused for a new cast. + inline void Reset() { mBodyID = BodyID(); mFraction = 1.0f + FLT_EPSILON; } + + BodyID mBodyID; ///< Body that was hit + float mFraction = 1.0f + FLT_EPSILON; ///< Hit fraction of the ray/object [0, 1], HitPoint = Start + mFraction * (End - Start) +}; + +/// Specialization of cast result against a shape +class RayCastResult : public BroadPhaseCastResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + SubShapeID mSubShapeID2; ///< Sub shape ID of shape that we collided against +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp new file mode 100644 index 000000000000..166883be8da3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp @@ -0,0 +1,223 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CastSphereVsTriangles::CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) : + mStart(inShapeCast.mCenterOfMassStart.GetTranslation()), + mDirection(inShapeCast.mDirection), + mShapeCastSettings(inShapeCastSettings), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mScale(inScale), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mCollector(ioCollector) +{ + // Cast to sphere shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Sphere); + const SphereShape *sphere = static_cast(inShapeCast.mShape); + + // Scale the radius + mRadius = sphere->GetRadius() * abs(inShapeCast.mScale.GetX()); + + // Determine if shape is inside out or not + mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; +} + +void CastSphereVsTriangles::AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal) +{ + // Convert to world space + Vec3 contact_point_a = mCenterOfMassTransform2 * (mStart + inContactPointA); + Vec3 contact_point_b = mCenterOfMassTransform2 * (mStart + inContactPointB); + Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(inContactNormal); + + // Its a hit, store the sub shape id's + ShapeCastResult result(inFraction, contact_point_a, contact_point_b, contact_normal_world, inBackFacing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Note: We don't gather faces here because that's only useful if both shapes have a face. Since the sphere always has only 1 contact point, the manifold is always a point. + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +void CastSphereVsTriangles::AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal) +{ + // Check if we have enabled active edge detection + Vec3 contact_normal = inContactNormal; + if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection); + + // Update the contact normal to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + contact_normal = ActiveEdges::FixNormal(inV0, inV1, inV2, inBackFacing? inTriangleNormal : -inTriangleNormal, inActiveEdges, inContactPointB, inContactNormal, active_edge_movement_direction); + } + + AddHit(inBackFacing, inSubShapeID2, inFraction, inContactPointA, inContactPointB, contact_normal); +} + +// This is a simplified version of the ray cylinder test from: Real Time Collision Detection - Christer Ericson +// Chapter 5.3.7, page 194-197. Some conditions have been removed as we're not interested in hitting the caps of the cylinder. +// Note that the ray origin is assumed to be the origin here. +float CastSphereVsTriangles::RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const +{ + // Calculate cylinder axis + Vec3 axis = inCylinderB - inCylinderA; + + // Make ray start relative to cylinder side A (moving cylinder A to the origin) + Vec3 start = -inCylinderA; + + // Test if segment is fully on the A side of the cylinder + float start_dot_axis = start.Dot(axis); + float direction_dot_axis = inRayDirection.Dot(axis); + float end_dot_axis = start_dot_axis + direction_dot_axis; + if (start_dot_axis < 0.0f && end_dot_axis < 0.0f) + return FLT_MAX; + + // Test if segment is fully on the B side of the cylinder + float axis_len_sq = axis.LengthSq(); + if (start_dot_axis > axis_len_sq && end_dot_axis > axis_len_sq) + return FLT_MAX; + + // Calculate a, b and c, the factors for quadratic equation + // We're basically solving the ray: x = start + direction * t + // The closest point to x on the segment A B is: w = (x . axis) * axis / (axis . axis) + // The distance between x and w should be radius: (x - w) . (x - w) = radius^2 + // Solving this gives the following: + float a = axis_len_sq * inRayDirection.LengthSq() - Square(direction_dot_axis); + if (abs(a) < 1.0e-6f) + return FLT_MAX; // Segment runs parallel to cylinder axis, stop processing, we will either hit at fraction = 0 or we'll hit a vertex + float b = axis_len_sq * start.Dot(inRayDirection) - direction_dot_axis * start_dot_axis; // should be multiplied by 2, instead we'll divide a and c by 2 when we solve the quadratic equation + float c = axis_len_sq * (start.LengthSq() - Square(inRadius)) - Square(start_dot_axis); + float det = Square(b) - a * c; // normally 4 * a * c but since both a and c need to be divided by 2 we lose the 4 + if (det < 0.0f) + return FLT_MAX; // No solution to quadractic equation + + // Solve fraction t where the ray hits the cylinder + float t = -(b + sqrt(det)) / a; // normally divided by 2 * a but since a should be divided by 2 we lose the 2 + if (t < 0.0f || t > 1.0f) + return FLT_MAX; // Intersection lies outside segment + if (start_dot_axis + t * direction_dot_axis < 0.0f || start_dot_axis + t * direction_dot_axis > axis_len_sq) + return FLT_MAX; // Intersection outside the end point of the cylinder, stop processing, we will possibly hit a vertex + return t; +} + +void CastSphereVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) +{ + JPH_PROFILE_FUNCTION(); + + // Scale triangle and make it relative to the start of the cast + Vec3 v0 = mScale * inV0 - mStart; + Vec3 v1 = mScale * inV1 - mStart; + Vec3 v2 = mScale * inV2 - mStart; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0); + float triangle_normal_len = triangle_normal.Length(); + if (triangle_normal_len == 0.0f) + return; // Degenerate triangle + triangle_normal /= triangle_normal_len; + + // Backface check + float normal_dot_direction = triangle_normal.Dot(mDirection); + bool back_facing = normal_dot_direction > 0.0f; + if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Test if distance between the sphere and plane of triangle is smaller or equal than the radius + if (abs(v0.Dot(triangle_normal)) <= mRadius) + { + // Check if the sphere intersects at the start of the cast + uint32 closest_feature; + Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature); + float q_len_sq = q.LengthSq(); + if (q_len_sq <= Square(mRadius)) + { + // Early out if this hit is deeper than the collector's early out value + float q_len = sqrt(q_len_sq); + float penetration_depth = mRadius - q_len; + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Generate contact point + Vec3 contact_normal = q_len > 0.0f? q / q_len : Vec3::sAxisY(); + Vec3 contact_point_a = q + contact_normal * penetration_depth; + Vec3 contact_point_b = q; + AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, 0.0f, contact_point_a, contact_point_b, contact_normal); + return; + } + } + else + { + // Check if cast is not parallel to the plane of the triangle + float abs_normal_dot_direction = abs(normal_dot_direction); + if (abs_normal_dot_direction > 1.0e-6f) + { + // Calculate the point on the sphere that will hit the triangle's plane first and calculate a fraction where it will do so + Vec3 d = Sign(normal_dot_direction) * mRadius * triangle_normal; + float plane_intersection = (v0 - d).Dot(triangle_normal) / normal_dot_direction; + + // Check if sphere will hit in the interval that we're interested in + if (plane_intersection * abs_normal_dot_direction < -mRadius // Sphere hits the plane before the sweep, cannot intersect + || plane_intersection >= mCollector.GetEarlyOutFraction()) // Sphere hits the plane after the sweep / early out fraction, cannot intersect + return; + + // We can only report an interior hit if we're hitting the plane during our sweep and not before + if (plane_intersection >= 0.0f) + { + // Calculate the point of contact on the plane + Vec3 p = d + plane_intersection * mDirection; + + // Check if this is an interior point + float u, v, w; + if (ClosestPoint::GetBaryCentricCoordinates(v0 - p, v1 - p, v2 - p, u, v, w) + && u >= 0.0f && v >= 0.0f && w >= 0.0f) + { + // Interior point, we found the collision point. We don't need to check active edges. + AddHit(back_facing, inSubShapeID2, plane_intersection, p, p, back_facing? triangle_normal : -triangle_normal); + return; + } + } + } + } + + // Test 3 edges + float fraction = RayCylinder(mDirection, v0, v1, mRadius); + fraction = min(fraction, RayCylinder(mDirection, v1, v2, mRadius)); + fraction = min(fraction, RayCylinder(mDirection, v2, v0, mRadius)); + + // Test 3 vertices + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v0, mRadius)); + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v1, mRadius)); + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v2, mRadius)); + + // Check if we have a collision + JPH_ASSERT(fraction >= 0.0f); + if (fraction < mCollector.GetEarlyOutFraction()) + { + // Calculate the center of the sphere at the point of contact + Vec3 p = fraction * mDirection; + + // Get contact point and normal + uint32 closest_feature; + Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0 - p, v1 - p, v2 - p, closest_feature); + Vec3 contact_normal = q.Normalized(); + Vec3 contact_point_ab = p + q; + AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, fraction, contact_point_ab, contact_point_ab, contact_normal); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h new file mode 100644 index 000000000000..e37bf6829317 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that casts a sphere vs one or more triangles +class JPH_EXPORT CastSphereVsTriangles +{ +public: + /// Constructor + /// @param inShapeCast The sphere to cast against the triangles and its start and direction + /// @param inShapeCastSettings Settings for performing the cast + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param ioCollector The collector that receives the results. + CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector); + + /// Cast sphere with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + Vec3 mStart; ///< Starting location of the sphere + Vec3 mDirection; ///< Direction and length of movement of sphere + float mRadius; ///< Scaled radius of sphere + const ShapeCastSettings & mShapeCastSettings; + const Mat44 & mCenterOfMassTransform2; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator1; + CastShapeCollector & mCollector; + +private: + void AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal); + void AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal); + float RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const; + + float mScaleSign; ///< Sign of the scale, -1 if object is inside out, 1 if not +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h new file mode 100644 index 000000000000..4cac6256db1d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Whether or not to collect faces, used by CastShape and CollideShape +enum class ECollectFacesMode : uint8 +{ + CollectFaces, ///< mShape1/2Face is desired + NoFaces ///< mShape1/2Face is not desired +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp new file mode 100644 index 000000000000..f00b0023aa94 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp @@ -0,0 +1,150 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CollideConvexVsTriangles::CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mSubShapeID1(inSubShapeID1) +{ + // Get transforms + Mat44 inverse_transform2 = inCenterOfMassTransform2.InversedRotationTranslation(); + Mat44 transform1_to_2 = inverse_transform2 * inCenterOfMassTransform1; + mTransform2To1 = transform1_to_2.InversedRotationTranslation(); + + // Calculate bounds + mBoundsOf1 = inShape1->GetLocalBounds().Scaled(inScale1); + mBoundsOf1.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + mBoundsOf1InSpaceOf2 = mBoundsOf1.Transformed(transform1_to_2); // Convert bounding box of 1 into space of 2 + + // Determine if shape 2 is inside out or not + mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f; +} + +void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) +{ + JPH_PROFILE_FUNCTION(); + + // Scale triangle and transform it to the space of 1 + Vec3 v0 = mTransform2To1 * (mScale2 * inV0); + Vec3 v1 = mTransform2To1 * (mScale2 * inV1); + Vec3 v2 = mTransform2To1 * (mScale2 * inV2); + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(v0) > 0.0f; + if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Get bounding box for triangle + AABox triangle_bbox = AABox::sFromTwoPoints(v0, v1); + triangle_bbox.Encapsulate(v2); + + // Get intersection between triangle and shape box, if there is none, we're done + if (!triangle_bbox.Overlaps(mBoundsOf1)) + return; + + // Create triangle support function + TriangleConvexSupport triangle(v0, v1, v2); + + // Perform collision detection + // Note: As we don't remember the penetration axis from the last iteration, and it is likely that the shape (A) we're colliding the triangle (B) against is in front of the triangle, + // and the penetration axis is the shortest distance along to push B out of collision, we use the inverse of the triangle normal as an initial penetration axis. This has been seen + // to improve performance by approx. 5% over using a fixed axis like (1, 0, 0). + Vec3 penetration_axis = -triangle_normal, point1, point2; + EPAPenetrationDepth pen_depth; + EPAPenetrationDepth::EStatus status; + + // Get the support function + if (mShape1ExCvxRadius == nullptr) + mShape1ExCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, mBufferExCvxRadius, mScale1); + + // Perform GJK step + status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + mCollideShapeSettings.mMaxSeparationDistance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); + + // Check result of collision detection + if (status == EPAPenetrationDepth::EStatus::NotColliding) + return; + else if (status == EPAPenetrationDepth::EStatus::Indeterminate) + { + // Need to run expensive EPA algorithm + + // Get the support function + if (mShape1IncCvxRadius == nullptr) + mShape1IncCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, mBufferIncCvxRadius, mScale1); + + // Add convex radius + AddConvexRadius shape1_add_max_separation_distance(*mShape1IncCvxRadius, mCollideShapeSettings.mMaxSeparationDistance); + + // Perform EPA step + if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, triangle, mCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2)) + return; + } + + // Check if the penetration is bigger than the early out fraction + float penetration_depth = (point2 - point1).Length() - mCollideShapeSettings.mMaxSeparationDistance; + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Correct point1 for the added separation distance + float penetration_axis_len = penetration_axis.Length(); + if (penetration_axis_len > 0.0f) + point1 -= penetration_axis * (mCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len); + + // Check if we have enabled active edge detection + if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mTransform1.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection); + + // Update the penetration axis to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + penetration_axis = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, point2, penetration_axis, active_edge_movement_direction); + } + + // Convert to world space + point1 = mTransform1 * point1; + point2 = mTransform1 * point2; + Vec3 penetration_axis_world = mTransform1.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Gather faces + if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + mShape1->GetSupportingFace(SubShapeID(), -penetration_axis, mScale1, mTransform1, result.mShape1Face); + + // Get face of the triangle + result.mShape2Face.resize(3); + result.mShape2Face[0] = mTransform1 * v0; + result.mShape2Face[1] = mTransform1 * v1; + result.mShape2Face[2] = mTransform1 * v2; + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h new file mode 100644 index 000000000000..8adfa3b41080 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Collision detection helper that collides a convex object vs one or more triangles +class JPH_EXPORT CollideConvexVsTriangles +{ +public: + /// Constructor + /// @param inShape1 The convex shape to collide against triangles + /// @param inScale1 Local space scale for the convex object (scales relative to its center of mass) + /// @param inScale2 Local space scale for the triangles + /// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space + /// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space + /// @param inSubShapeID1 Sub shape ID of the convex object + /// @param inCollideShapeSettings Settings for the collide shape query + /// @param ioCollector The collector that will receive the results + CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector); + + /// Collide convex object with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const CollideShapeSettings & mCollideShapeSettings; ///< Settings for this collision operation + CollideShapeCollector & mCollector; ///< The collector that will receive the results + const ConvexShape * mShape1; ///< The shape that we're colliding with + Vec3 mScale1; ///< The scale of the shape (in shape local space) of the shape we're colliding with + Vec3 mScale2; ///< The scale of the shape (in shape local space) of the shape we're colliding against + Mat44 mTransform1; ///< Transform of the shape we're colliding with + Mat44 mTransform2To1; ///< Transform that takes a point in space of the colliding shape to the shape we're colliding with + AABox mBoundsOf1; ///< Bounds of the colliding shape in local space + AABox mBoundsOf1InSpaceOf2; ///< Bounds of the colliding shape in space of shape we're colliding with + SubShapeID mSubShapeID1; ///< Sub shape ID of colliding shape + float mScaleSign2; ///< Sign of the scale of object 2, -1 if object is inside out, 1 if not + ConvexShape::SupportBuffer mBufferExCvxRadius; ///< Buffer that holds the support function data excluding convex radius + ConvexShape::SupportBuffer mBufferIncCvxRadius; ///< Buffer that holds the support function data including convex radius + const ConvexShape::Support * mShape1ExCvxRadius = nullptr; ///< Actual support function object excluding convex radius + const ConvexShape::Support * mShape1IncCvxRadius = nullptr; ///< Actual support function object including convex radius +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h new file mode 100644 index 000000000000..8601b3c40a03 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h @@ -0,0 +1,25 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds the result of colliding a point against a shape +class CollidePointResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For point queries there is no sensible return value. + inline float GetEarlyOutFraction() const { return 0.0f; } + + BodyID mBodyID; ///< Body that was hit + SubShapeID mSubShapeID2; ///< Sub shape ID of shape that we collided against +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h new file mode 100644 index 000000000000..2c5f8f217ffa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h @@ -0,0 +1,105 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that contains all information of two colliding shapes +class CollideShapeResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor + CollideShapeResult() = default; + + /// Constructor + CollideShapeResult(Vec3Arg inContactPointOn1, Vec3Arg inContactPointOn2, Vec3Arg inPenetrationAxis, float inPenetrationDepth, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) : + mContactPointOn1(inContactPointOn1), + mContactPointOn2(inContactPointOn2), + mPenetrationAxis(inPenetrationAxis), + mPenetrationDepth(inPenetrationDepth), + mSubShapeID1(inSubShapeID1), + mSubShapeID2(inSubShapeID2), + mBodyID2(inBodyID2) + { + } + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. We use -penetration depth to get the hit with the biggest penetration depth + inline float GetEarlyOutFraction() const { return -mPenetrationDepth; } + + /// Reverses the hit result, swapping contact point 1 with contact point 2 etc. + inline CollideShapeResult Reversed() const + { + CollideShapeResult result; + result.mContactPointOn2 = mContactPointOn1; + result.mContactPointOn1 = mContactPointOn2; + result.mPenetrationAxis = -mPenetrationAxis; + result.mPenetrationDepth = mPenetrationDepth; + result.mSubShapeID2 = mSubShapeID1; + result.mSubShapeID1 = mSubShapeID2; + result.mBodyID2 = mBodyID2; + result.mShape2Face = mShape1Face; + result.mShape1Face = mShape2Face; + return result; + } + + using Face = StaticArray; + + Vec3 mContactPointOn1; ///< Contact point on the surface of shape 1 (in world space or relative to base offset) + Vec3 mContactPointOn2; ///< Contact point on the surface of shape 2 (in world space or relative to base offset). If the penetration depth is 0, this will be the same as mContactPointOn1. + Vec3 mPenetrationAxis; ///< Direction to move shape 2 out of collision along the shortest path (magnitude is meaningless, in world space). You can use -mPenetrationAxis.Normalized() as contact normal. + float mPenetrationDepth; ///< Penetration depth (move shape 2 by this distance to resolve the collision) + SubShapeID mSubShapeID1; ///< Sub shape ID that identifies the face on shape 1 + SubShapeID mSubShapeID2; ///< Sub shape ID that identifies the face on shape 2 + BodyID mBodyID2; ///< BodyID to which shape 2 belongs to + Face mShape1Face; ///< Colliding face on shape 1 (optional result, in world space or relative to base offset) + Face mShape2Face; ///< Colliding face on shape 2 (optional result, in world space or relative to base offset) +}; + +/// Settings to be passed with a collision query +class CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// How active edges (edges that a moving object should bump into) are handled + EActiveEdgeMode mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + + /// If colliding faces should be collected or only the collision point + ECollectFacesMode mCollectFacesMode = ECollectFacesMode::NoFaces; + + /// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter) + float mCollisionTolerance = cDefaultCollisionTolerance; + + /// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless) + float mPenetrationTolerance = cDefaultPenetrationTolerance; + + /// When mActiveEdgeMode is CollideOnlyWithActive a movement direction can be provided. When hitting an inactive edge, the system will select the triangle normal as penetration depth only if it impedes the movement less than with the calculated penetration depth. + Vec3 mActiveEdgeMovementDirection = Vec3::sZero(); +}; + +/// Settings to be passed with a collision query +class CollideShapeSettings : public CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// When > 0 contacts in the vicinity of the query shape can be found. All nearest contacts that are not further away than this distance will be found (unit: meter) + float mMaxSeparationDistance = 0.0f; + + /// How backfacing triangles should be treated + EBackFaceMode mBackFaceMode = EBackFaceMode::IgnoreBackFaces; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h new file mode 100644 index 000000000000..e976aa9de225 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that allows iterating over the vertices of a soft body. +/// It tracks the largest penetration and allows storing the resulting collision in a different structure than the soft body vertex itself. +class CollideSoftBodyVertexIterator +{ +public: + /// Default constructor + CollideSoftBodyVertexIterator() = default; + CollideSoftBodyVertexIterator(const CollideSoftBodyVertexIterator &) = default; + + /// Construct using (strided) pointers + CollideSoftBodyVertexIterator(const StridedPtr &inPosition, const StridedPtr &inInvMass, const StridedPtr &inCollisionPlane, const StridedPtr &inLargestPenetration, const StridedPtr &inCollidingShapeIndex) : + mPosition(inPosition), + mInvMass(inInvMass), + mCollisionPlane(inCollisionPlane), + mLargestPenetration(inLargestPenetration), + mCollidingShapeIndex(inCollidingShapeIndex) + { + } + + /// Construct using a soft body vertex + explicit CollideSoftBodyVertexIterator(SoftBodyVertex *inVertices) : + mPosition(&inVertices->mPosition, sizeof(SoftBodyVertex)), + mInvMass(&inVertices->mInvMass, sizeof(SoftBodyVertex)), + mCollisionPlane(&inVertices->mCollisionPlane, sizeof(SoftBodyVertex)), + mLargestPenetration(&inVertices->mLargestPenetration, sizeof(SoftBodyVertex)), + mCollidingShapeIndex(&inVertices->mCollidingShapeIndex, sizeof(SoftBodyVertex)) + { + } + + /// Default assignment + CollideSoftBodyVertexIterator & operator = (const CollideSoftBodyVertexIterator &) = default; + + /// Equality operator. + /// Note: Only used to determine end iterator, so we only compare position. + bool operator != (const CollideSoftBodyVertexIterator &inRHS) const + { + return mPosition != inRHS.mPosition; + } + + /// Next vertex + CollideSoftBodyVertexIterator & operator ++ () + { + ++mPosition; + ++mInvMass; + ++mCollisionPlane; + ++mLargestPenetration; + ++mCollidingShapeIndex; + return *this; + } + + /// Add an offset + /// Note: Only used to determine end iterator, so we only set position. + CollideSoftBodyVertexIterator operator + (int inOffset) const + { + return CollideSoftBodyVertexIterator(mPosition + inOffset, StridedPtr(), StridedPtr(), StridedPtr(), StridedPtr()); + } + + /// Get the position of the current vertex + Vec3 GetPosition() const + { + return *mPosition; + } + + /// Get the inverse mass of the current vertex + float GetInvMass() const + { + return *mInvMass; + } + + /// Update penetration of the current vertex + /// @return Returns true if the vertex has the largest penetration so far, this means you need to follow up by calling SetCollision + bool UpdatePenetration(float inLargestPenetration) const + { + float &penetration = *mLargestPenetration; + if (penetration >= inLargestPenetration) + return false; + penetration = inLargestPenetration; + return true; + } + + /// Update the collision of the current vertex + void SetCollision(const Plane &inCollisionPlane, int inCollidingShapeIndex) const + { + *mCollisionPlane = inCollisionPlane; + *mCollidingShapeIndex = inCollidingShapeIndex; + } + +private: + /// Input data + StridedPtr mPosition; + StridedPtr mInvMass; + + /// Output data + StridedPtr mCollisionPlane; + StridedPtr mLargestPenetration; + StridedPtr mCollidingShapeIndex; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h new file mode 100644 index 000000000000..c91becb64259 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h @@ -0,0 +1,90 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that collides soft body vertices vs triangles +class JPH_EXPORT CollideSoftBodyVerticesVsTriangles +{ +public: + CollideSoftBodyVerticesVsTriangles(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) : + mTransform(inCenterOfMassTransform * Mat44::sScale(inScale)), + mInvTransform(mTransform.Inversed()), + mNormalSign(ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f) + { + } + + JPH_INLINE void StartVertex(const CollideSoftBodyVertexIterator &inVertex) + { + mLocalPosition = mInvTransform * inVertex.GetPosition(); + mClosestDistanceSq = FLT_MAX; + } + + JPH_INLINE void ProcessTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Get the closest point from the vertex to the triangle + uint32 set; + Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(inV0 - mLocalPosition, inV1 - mLocalPosition, inV2 - mLocalPosition, set); + float dist_sq = closest_point.LengthSq(); + if (dist_sq < mClosestDistanceSq) + { + mV0 = inV0; + mV1 = inV1; + mV2 = inV2; + mClosestPoint = closest_point; + mClosestDistanceSq = dist_sq; + mSet = set; + } + } + + JPH_INLINE void FinishVertex(const CollideSoftBodyVertexIterator &ioVertex, int inCollidingShapeIndex) const + { + if (mClosestDistanceSq < FLT_MAX) + { + // Convert triangle to world space + Vec3 v0 = mTransform * mV0; + Vec3 v1 = mTransform * mV1; + Vec3 v2 = mTransform * mV2; + Vec3 triangle_normal = mNormalSign * (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sAxisY()); + + if (mSet == 0b111) + { + // Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m penetration + // because otherwise a triangle half a level a way will have a huge penetration if it is back facing + float penetration = min(triangle_normal.Dot(v0 - ioVertex.GetPosition()), 0.1f); + if (ioVertex.UpdatePenetration(penetration)) + ioVertex.SetCollision(Plane::sFromPointAndNormal(v0, triangle_normal), inCollidingShapeIndex); + } + else + { + // Closest point is on an edge or vertex, use closest point as collision plane + Vec3 closest_point = mTransform * (mLocalPosition + mClosestPoint); + Vec3 normal = ioVertex.GetPosition() - closest_point; + if (normal.Dot(triangle_normal) > 0.0f) // Ignore back facing edges + { + float normal_length = normal.Length(); + float penetration = -normal_length; + if (ioVertex.UpdatePenetration(penetration)) + ioVertex.SetCollision(Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal), inCollidingShapeIndex); + } + } + } + } + + Mat44 mTransform; + Mat44 mInvTransform; + Vec3 mLocalPosition; + Vec3 mV0, mV1, mV2; + Vec3 mClosestPoint; + float mNormalSign; + float mClosestDistanceSq; + uint32 mSet; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp new file mode 100644 index 000000000000..92ed28a7b495 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp @@ -0,0 +1,123 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +static constexpr uint8 sClosestFeatureToActiveEdgesMask[] = { + 0b000, // 0b000: Invalid, guarded by an assert + 0b101, // 0b001: Vertex 1 -> edge 1 or 3 + 0b011, // 0b010: Vertex 2 -> edge 1 or 2 + 0b001, // 0b011: Vertex 1 & 2 -> edge 1 + 0b110, // 0b100: Vertex 3 -> edge 2 or 3 + 0b100, // 0b101: Vertex 1 & 3 -> edge 3 + 0b010, // 0b110: Vertex 2 & 3 -> edge 2 + // 0b111: Vertex 1, 2 & 3 -> interior, guarded by an if +}; + +CollideSphereVsTriangles::CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale2(inScale2), + mTransform2(inCenterOfMassTransform2), + mSubShapeID1(inSubShapeID1) +{ + // Calculate the center of the sphere in the space of 2 + mSphereCenterIn2 = inCenterOfMassTransform2.Multiply3x3Transposed(inCenterOfMassTransform1.GetTranslation() - inCenterOfMassTransform2.GetTranslation()); + + // Determine if shape 2 is inside out or not + mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f; + + // Check that the sphere is uniformly scaled + JPH_ASSERT(ScaleHelpers::IsUniformScale(inScale1.Abs())); + mRadius = abs(inScale1.GetX()) * inShape1->GetRadius(); + mRadiusPlusMaxSeparationSq = Square(mRadius + inCollideShapeSettings.mMaxSeparationDistance); +} + +void CollideSphereVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) +{ + JPH_PROFILE_FUNCTION(); + + // Scale triangle and make it relative to the center of the sphere + Vec3 v0 = mScale2 * inV0 - mSphereCenterIn2; + Vec3 v1 = mScale2 * inV1 - mSphereCenterIn2; + Vec3 v2 = mScale2 * inV2 - mSphereCenterIn2; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(v0) > 0.0f; + if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Check if we collide with the sphere + uint32 closest_feature; + Vec3 point2 = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature); + float point2_len_sq = point2.LengthSq(); + if (point2_len_sq > mRadiusPlusMaxSeparationSq) + return; + + // Calculate penetration depth + float penetration_depth = mRadius - sqrt(point2_len_sq); + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Calculate penetration axis, direction along which to push 2 to move it out of collision (this is always away from the sphere center) + Vec3 penetration_axis = point2.NormalizedOr(Vec3::sAxisY()); + + // Calculate the point on the sphere + Vec3 point1 = mRadius * penetration_axis; + + // Check if we have enabled active edge detection + JPH_ASSERT(closest_feature != 0); + if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive + && closest_feature != 0b111 // For an interior hit we should already have the right normal + && (inActiveEdges & sClosestFeatureToActiveEdgesMask[closest_feature]) == 0) // If we didn't hit an active edge we should take the triangle normal + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mTransform2.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection); + + // See ActiveEdges::FixNormal. If penetration_axis affects the movement less than the triangle normal we keep penetration_axis. + Vec3 new_penetration_axis = back_facing? triangle_normal : -triangle_normal; + if (active_edge_movement_direction.Dot(penetration_axis) * new_penetration_axis.Length() >= active_edge_movement_direction.Dot(new_penetration_axis)) + penetration_axis = new_penetration_axis; + } + + // Convert to world space + point1 = mTransform2 * (mSphereCenterIn2 + point1); + point2 = mTransform2 * (mSphereCenterIn2 + point2); + Vec3 penetration_axis_world = mTransform2.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Gather faces + if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // The sphere doesn't have a supporting face + + // Get face of triangle 2 + result.mShape2Face.resize(3); + result.mShape2Face[0] = mTransform2 * (mSphereCenterIn2 + v0); + result.mShape2Face[1] = mTransform2 * (mSphereCenterIn2 + v1); + result.mShape2Face[2] = mTransform2 * (mSphereCenterIn2 + v2); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h new file mode 100644 index 000000000000..5d16d9a884fc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h @@ -0,0 +1,50 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Collision detection helper that collides a sphere vs one or more triangles +class JPH_EXPORT CollideSphereVsTriangles +{ +public: + /// Constructor + /// @param inShape1 The sphere to collide against triangles + /// @param inScale1 Local space scale for the sphere (scales relative to its center of mass) + /// @param inScale2 Local space scale for the triangles + /// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space + /// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space + /// @param inSubShapeID1 Sub shape ID of the convex object + /// @param inCollideShapeSettings Settings for the collide shape query + /// @param ioCollector The collector that will receive the results + CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector); + + /// Collide sphere with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const CollideShapeSettings & mCollideShapeSettings; ///< Settings for this collision operation + CollideShapeCollector & mCollector; ///< The collector that will receive the results + const SphereShape * mShape1; ///< The shape that we're colliding with + Vec3 mScale2; ///< The scale of the shape (in shape local space) of the shape we're colliding against + Mat44 mTransform2; ///< Transform of the shape we're colliding against + Vec3 mSphereCenterIn2; ///< The center of the sphere in the space of 2 + SubShapeID mSubShapeID1; ///< Sub shape ID of colliding shape + float mScaleSign2; ///< Sign of the scale of object 2, -1 if object is inside out, 1 if not + float mRadius; ///< Radius of the sphere + float mRadiusPlusMaxSeparationSq; ///< (Radius + Max SeparationDistance)^2 +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h new file mode 100644 index 000000000000..ec608502719d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h @@ -0,0 +1,105 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Body; +class TransformedShape; + +/// Traits to use for CastRay +class CollisionCollectorTraitsCastRay +{ +public: + /// For rays the early out fraction is the fraction along the line to order hits. + static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON; ///< Furthest hit: Fraction is 1 + epsilon + static constexpr float ShouldEarlyOutFraction = 0.0f; ///< Closest hit: Fraction is 0 +}; + +/// Traits to use for CastShape +class CollisionCollectorTraitsCastShape +{ +public: + /// For rays the early out fraction is the fraction along the line to order hits. + static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON; ///< Furthest hit: Fraction is 1 + epsilon + static constexpr float ShouldEarlyOutFraction = -FLT_MAX; ///< Deepest hit: Penetration is infinite +}; + +/// Traits to use for CollideShape +class CollisionCollectorTraitsCollideShape +{ +public: + /// For shape collisions we use -penetration depth to order hits. + static constexpr float InitialEarlyOutFraction = FLT_MAX; ///< Most shallow hit: Separation is infinite + static constexpr float ShouldEarlyOutFraction = -FLT_MAX; ///< Deepest hit: Penetration is infinite +}; + +/// Traits to use for CollidePoint +using CollisionCollectorTraitsCollidePoint = CollisionCollectorTraitsCollideShape; + +/// Virtual interface that allows collecting multiple collision results +template +class CollisionCollector +{ +public: + /// Declare ResultType so that derived classes can use it + using ResultType = ResultTypeArg; + + /// Default constructor + CollisionCollector() = default; + + /// Constructor to initialize from another collector + template + explicit CollisionCollector(const CollisionCollector &inRHS) : mEarlyOutFraction(inRHS.GetEarlyOutFraction()), mContext(inRHS.GetContext()) { } + CollisionCollector(const CollisionCollector &inRHS) = default; + + /// Destructor + virtual ~CollisionCollector() = default; + + /// If you want to reuse this collector, call Reset() + virtual void Reset() { mEarlyOutFraction = TraitsType::InitialEarlyOutFraction; } + + /// When running a query through the NarrowPhaseQuery class, this will be called for every body that is potentially colliding. + /// It allows collecting additional information needed by the collision collector implementation from the body under lock protection + /// before AddHit is called (e.g. the user data pointer or the velocity of the body). + virtual void OnBody([[maybe_unused]] const Body &inBody) { /* Collects nothing by default */ } + + /// Set by the collision detection functions to the current TransformedShape that we're colliding against before calling the AddHit function + void SetContext(const TransformedShape *inContext) { mContext = inContext; } + const TransformedShape *GetContext() const { return mContext; } + + /// This function can be used to set some user data on the collision collector + virtual void SetUserData(uint64 inUserData) { /* Does nothing by default */ } + + /// This function will be called for every hit found, it's up to the application to decide how to store the hit + virtual void AddHit(const ResultType &inResult) = 0; + + /// Update the early out fraction (should be lower than before) + inline void UpdateEarlyOutFraction(float inFraction) { JPH_ASSERT(inFraction <= mEarlyOutFraction); mEarlyOutFraction = inFraction; } + + /// Reset the early out fraction to a specific value + inline void ResetEarlyOutFraction(float inFraction = TraitsType::InitialEarlyOutFraction) { mEarlyOutFraction = inFraction; } + + /// Force the collision detection algorithm to terminate as soon as possible. Call this from the AddHit function when a satisfying hit is found. + inline void ForceEarlyOut() { mEarlyOutFraction = TraitsType::ShouldEarlyOutFraction; } + + /// When true, the collector will no longer accept any additional hits and the collision detection routine should early out as soon as possible + inline bool ShouldEarlyOut() const { return mEarlyOutFraction <= TraitsType::ShouldEarlyOutFraction; } + + /// Get the current early out value + inline float GetEarlyOutFraction() const { return mEarlyOutFraction; } + + /// Get the current early out value but make sure it's bigger than zero, this is used for shape casting as negative values are used for penetration + inline float GetPositiveEarlyOutFraction() const { return max(FLT_MIN, mEarlyOutFraction); } + +private: + /// The early out fraction determines the fraction below which the collector is still accepting a hit (can be used to reduce the amount of work) + float mEarlyOutFraction = TraitsType::InitialEarlyOutFraction; + + /// Set by the collision detection functions to the current TransformedShape of the body that we're colliding against before calling the AddHit function + const TransformedShape *mContext = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h new file mode 100644 index 000000000000..7bdd5da27972 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h @@ -0,0 +1,134 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Simple implementation that collects all hits and optionally sorts them on distance +template +class AllHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHits.clear(); + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + mHits.push_back(inResult); + } + + /// Order hits on closest first + void Sort() + { + QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); }); + } + + /// Check if any hits were collected + inline bool HadHit() const + { + return !mHits.empty(); + } + + Array mHits; +}; + +/// Simple implementation that collects the closest / deepest hit +template +class ClosestHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHadHit = false; + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + float early_out = inResult.GetEarlyOutFraction(); + if (!mHadHit || early_out < mHit.GetEarlyOutFraction()) + { + // Update early out fraction + CollectorType::UpdateEarlyOutFraction(early_out); + + // Store hit + mHit = inResult; + mHadHit = true; + } + } + + /// Check if this collector has had a hit + inline bool HadHit() const + { + return mHadHit; + } + + ResultType mHit; + +private: + bool mHadHit = false; +}; + +/// Simple implementation that collects any hit +template +class AnyHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHadHit = false; + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + // Test that the collector is not collecting more hits after forcing an early out + JPH_ASSERT(!mHadHit); + + // Abort any further testing + CollectorType::ForceEarlyOut(); + + // Store hit + mHit = inResult; + mHadHit = true; + } + + /// Check if this collector has had a hit + inline bool HadHit() const + { + return mHadHit; + } + + ResultType mHit; + +private: + bool mHadHit = false; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp new file mode 100644 index 000000000000..259a9526e8bc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp @@ -0,0 +1,107 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +CollisionDispatch::CollideShape CollisionDispatch::sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; +CollisionDispatch::CastShape CollisionDispatch::sCastShape[NumSubShapeTypes][NumSubShapeTypes]; + +void CollisionDispatch::sInit() +{ + for (uint i = 0; i < NumSubShapeTypes; ++i) + for (uint j = 0; j < NumSubShapeTypes; ++j) + { + if (sCollideShape[i][j] == nullptr) + sCollideShape[i][j] = [](const Shape *, const Shape *, Vec3Arg, Vec3Arg, Mat44Arg, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, const CollideShapeSettings &, CollideShapeCollector &, const ShapeFilter &) + { + JPH_ASSERT(false, "Unsupported shape pair"); + }; + + if (sCastShape[i][j] == nullptr) + sCastShape[i][j] = [](const ShapeCast &, const ShapeCastSettings &, const Shape *, Vec3Arg, const ShapeFilter &, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, CastShapeCollector &) + { + JPH_ASSERT(false, "Unsupported shape pair"); + }; + } +} + +void CollisionDispatch::sReversedCollideShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + // A collision collector that flips the collision results + class ReversedCollector : public CollideShapeCollector + { + public: + explicit ReversedCollector(CollideShapeCollector &ioCollector) : + CollideShapeCollector(ioCollector), + mCollector(ioCollector) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // Add the reversed hit + mCollector.AddHit(inResult.Reversed()); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + private: + CollideShapeCollector & mCollector; + }; + + ReversedShapeFilter shape_filter(inShapeFilter); + ReversedCollector collector(ioCollector); + sCollideShapeVsShape(inShape2, inShape1, inScale2, inScale1, inCenterOfMassTransform2, inCenterOfMassTransform1, inSubShapeIDCreator2, inSubShapeIDCreator1, inCollideShapeSettings, collector, shape_filter); +} + +void CollisionDispatch::sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // A collision collector that flips the collision results + class ReversedCollector : public CastShapeCollector + { + public: + explicit ReversedCollector(CastShapeCollector &ioCollector, Vec3Arg inWorldDirection) : + CastShapeCollector(ioCollector), + mCollector(ioCollector), + mWorldDirection(inWorldDirection) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Add the reversed hit + mCollector.AddHit(inResult.Reversed(mWorldDirection)); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + private: + CastShapeCollector & mCollector; + Vec3 mWorldDirection; + }; + + // Reverse the shape cast (shape cast is in local space to shape 2) + Mat44 com_start_inv = inShapeCast.mCenterOfMassStart.InversedRotationTranslation(); + ShapeCast local_shape_cast(inShape, inScale, com_start_inv, -com_start_inv.Multiply3x3(inShapeCast.mDirection)); + + // Calculate the center of mass of shape 1 at start of sweep + Mat44 shape1_com = inCenterOfMassTransform2 * inShapeCast.mCenterOfMassStart; + + // Calculate the world space direction vector of the shape cast + Vec3 world_direction = -inCenterOfMassTransform2.Multiply3x3(inShapeCast.mDirection); + + // Forward the cast + ReversedShapeFilter shape_filter(inShapeFilter); + ReversedCollector collector(ioCollector, world_direction); + sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShapeCast.mShape, inShapeCast.mScale, shape_filter, shape1_com, inSubShapeIDCreator2, inSubShapeIDCreator1, collector); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h new file mode 100644 index 000000000000..6842c8154a5c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Dispatch function, main function to handle collisions between shapes +class JPH_EXPORT CollisionDispatch +{ +public: + /// Collide 2 shapes and pass any collision on to ioCollector + /// @param inShape1 The first shape + /// @param inShape2 The second shape + /// @param inScale1 Local space scale of shape 1 (scales relative to its center of mass) + /// @param inScale2 Local space scale of shape 2 (scales relative to its center of mass) + /// @param inCenterOfMassTransform1 Transform to transform center of mass of shape 1 into world space + /// @param inCenterOfMassTransform2 Transform to transform center of mass of shape 2 into world space + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for shape 1 + /// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for shape 2 + /// @param inCollideShapeSettings Options for the CollideShape test + /// @param ioCollector The collector that receives the results. + /// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes. + static inline void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) + { + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()]);) + + // Only test shape if it passes the shape filter + if (inShapeFilter.ShouldCollide(inShape1, inSubShapeIDCreator1.GetID(), inShape2, inSubShapeIDCreator2.GetID())) + sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()](inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + } + + /// Cast a shape against this shape, passes any hits found to ioCollector. + /// Note: This version takes the shape cast in local space relative to the center of mass of inShape, take a look at sCastShapeVsShapeWorldSpace if you have a shape cast in world space. + /// @param inShapeCastLocal The shape to cast against the other shape and its start and direction. + /// @param inShapeCastSettings Settings for performing the cast + /// @param inShape The shape to cast against. + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes. + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local hit result quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for the shape we're casting against + /// @param ioCollector The collector that receives the results. + static inline void sCastShapeVsShapeLocalSpace(const ShapeCast &inShapeCastLocal, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) + { + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()]);) + + // Only test shape if it passes the shape filter + if (inShapeFilter.ShouldCollide(inShapeCastLocal.mShape, inSubShapeIDCreator1.GetID(), inShape, inSubShapeIDCreator2.GetID())) + sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()](inShapeCastLocal, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + } + + /// See: sCastShapeVsShapeLocalSpace. + /// The only difference is that the shape cast (inShapeCastWorld) is provided in world space. + /// Note: A shape cast contains the center of mass start of the shape, if you have the world transform of the shape you probably want to construct it using ShapeCast::sFromWorldTransform. + static inline void sCastShapeVsShapeWorldSpace(const ShapeCast &inShapeCastWorld, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) + { + ShapeCast local_shape_cast = inShapeCastWorld.PostTransformed(inCenterOfMassTransform2.InversedRotationTranslation()); + sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + } + + /// Function that collides 2 shapes (see sCollideShapeVsShape) + using CollideShape = void (*)(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + + /// Function that casts a shape vs another shape (see sCastShapeVsShapeLocalSpace) + using CastShape = void (*)(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Initialize all collision functions with a function that asserts and returns no collision + static void sInit(); + + /// Register a collide shape function in the collision table + static void sRegisterCollideShape(EShapeSubType inType1, EShapeSubType inType2, CollideShape inFunction) { sCollideShape[(int)inType1][(int)inType2] = inFunction; } + + /// Register a cast shape function in the collision table + static void sRegisterCastShape(EShapeSubType inType1, EShapeSubType inType2, CastShape inFunction) { sCastShape[(int)inType1][(int)inType2] = inFunction; } + + /// An implementation of CollideShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around + static void sReversedCollideShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + + /// An implementation of CastShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around + static void sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + +private: + static CollideShape sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; + static CastShape sCastShape[NumSubShapeTypes][NumSubShapeTypes]; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp new file mode 100644 index 000000000000..a8c98b6dc4ca --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CollisionGroup) +{ + JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupFilter) + JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupID) + JPH_ADD_ATTRIBUTE(CollisionGroup, mSubGroupID) +} + +void CollisionGroup::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mGroupID); + inStream.Write(mSubGroupID); +} + +void CollisionGroup::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mGroupID); + inStream.Read(mSubGroupID); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h new file mode 100644 index 000000000000..5e8c6cf1f5ee --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h @@ -0,0 +1,94 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Two objects collide with each other if: +/// - Both don't have a group filter +/// - The first group filter says that the objects can collide +/// - Or if there's no filter for the first object, the second group filter says the objects can collide +class JPH_EXPORT CollisionGroup +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, CollisionGroup) + +public: + using GroupID = uint32; + using SubGroupID = uint32; + + static const GroupID cInvalidGroup = ~GroupID(0); + static const SubGroupID cInvalidSubGroup = ~SubGroupID(0); + + /// Default constructor + CollisionGroup() = default; + + /// Construct with all properties + CollisionGroup(const GroupFilter *inFilter, GroupID inGroupID, SubGroupID inSubGroupID) : mGroupFilter(inFilter), mGroupID(inGroupID), mSubGroupID(inSubGroupID) { } + + /// Set the collision group filter + inline void SetGroupFilter(const GroupFilter *inFilter) + { + mGroupFilter = inFilter; + } + + /// Get the collision group filter + inline const GroupFilter *GetGroupFilter() const + { + return mGroupFilter; + } + + /// Set the main group id for this object + inline void SetGroupID(GroupID inID) + { + mGroupID = inID; + } + + inline GroupID GetGroupID() const + { + return mGroupID; + } + + /// Add this object to a sub group + inline void SetSubGroupID(SubGroupID inID) + { + mSubGroupID = inID; + } + + inline SubGroupID GetSubGroupID() const + { + return mSubGroupID; + } + + /// Check if this object collides with another object + bool CanCollide(const CollisionGroup &inOther) const + { + // Call the CanCollide function of the first group filter that's not null + if (mGroupFilter != nullptr) + return mGroupFilter->CanCollide(*this, inOther); + else if (inOther.mGroupFilter != nullptr) + return inOther.mGroupFilter->CanCollide(inOther, *this); + else + return true; + } + + /// Saves the state of this object in binary form to inStream. Does not save group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Does not save group filter. + void RestoreBinaryState(StreamIn &inStream); + +private: + RefConst mGroupFilter; + GroupID mGroupID = cInvalidGroup; + SubGroupID mSubGroupID = cInvalidSubGroup; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h new file mode 100644 index 000000000000..1a24b4fb88a5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h @@ -0,0 +1,119 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class CollideShapeResult; + +/// Array of contact points +using ContactPoints = StaticArray; + +/// Manifold class, describes the contact surface between two bodies +class ContactManifold +{ +public: + /// Swaps shape 1 and 2 + ContactManifold SwapShapes() const { return { mBaseOffset, -mWorldSpaceNormal, mPenetrationDepth, mSubShapeID2, mSubShapeID1, mRelativeContactPointsOn2, mRelativeContactPointsOn1 }; } + + /// Access to the world space contact positions + inline RVec3 GetWorldSpaceContactPointOn1(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn1[inIndex]; } + inline RVec3 GetWorldSpaceContactPointOn2(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn2[inIndex]; } + + RVec3 mBaseOffset; ///< Offset to which all the contact points are relative + Vec3 mWorldSpaceNormal; ///< Normal for this manifold, direction along which to move body 2 out of collision along the shortest path + float mPenetrationDepth; ///< Penetration depth (move shape 2 by this distance to resolve the collision). If this value is negative, this is a speculative contact point and may not actually result in a velocity change as during solving the bodies may not actually collide. + SubShapeID mSubShapeID1; ///< Sub shapes that formed this manifold (note that when multiple manifolds are combined because they're coplanar, we lose some information here because we only keep track of one sub shape pair that we encounter, see description at Body::SetUseManifoldReduction) + SubShapeID mSubShapeID2; + ContactPoints mRelativeContactPointsOn1; ///< Contact points on the surface of shape 1 relative to mBaseOffset. + ContactPoints mRelativeContactPointsOn2; ///< Contact points on the surface of shape 2 relative to mBaseOffset. If there's no penetration, this will be the same as mRelativeContactPointsOn1. If there is penetration they will be different. +}; + +/// When a contact point is added or persisted, the callback gets a chance to override certain properties of the contact constraint. +/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to. +class ContactSettings +{ +public: + float mCombinedFriction; ///< Combined friction for the body pair (see: PhysicsSystem::SetCombineFriction) + float mCombinedRestitution; ///< Combined restitution for the body pair (see: PhysicsSystem::SetCombineRestitution) + float mInvMassScale1 = 1.0f; ///< Scale factor for the inverse mass of body 1 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale1 = 1.0f; ///< Scale factor for the inverse inertia of body 1 (usually same as mInvMassScale1) + float mInvMassScale2 = 1.0f; ///< Scale factor for the inverse mass of body 2 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale2 = 1.0f; ///< Scale factor for the inverse inertia of body 2 (usually same as mInvMassScale2) + bool mIsSensor; ///< If the contact should be treated as a sensor vs body contact (no collision response) + Vec3 mRelativeLinearSurfaceVelocity = Vec3::sZero(); ///< Relative linear surface velocity between the bodies (world space surface velocity of body 2 - world space surface velocity of body 1), can be used to create a conveyor belt effect + Vec3 mRelativeAngularSurfaceVelocity = Vec3::sZero(); ///< Relative angular surface velocity between the bodies (world space angular surface velocity of body 2 - world space angular surface velocity of body 1). Note that this angular velocity is relative to the center of mass of body 1, so if you want it relative to body 2's center of mass you need to add body 2 angular velocity x (body 1 world space center of mass - body 2 world space center of mass) to mRelativeLinearSurfaceVelocity. +}; + +/// Return value for the OnContactValidate callback. Determines if the contact is being processed or not. +/// Results are ordered so that the strongest accept has the lowest number and the strongest reject the highest number (which allows for easy combining of results) +enum class ValidateResult +{ + AcceptAllContactsForThisBodyPair, ///< Accept this and any further contact points for this body pair + AcceptContact, ///< Accept this contact only (and continue calling this callback for every contact manifold for the same body pair) + RejectContact, ///< Reject this contact only (but process any other contact manifolds for the same body pair) + RejectAllContactsForThisBodyPair ///< Rejects this and any further contact points for this body pair +}; + +/// A listener class that receives collision contact events. +/// It can be registered with the ContactConstraintManager (or PhysicsSystem). +/// Note that contact listener callbacks are called from multiple threads at the same time when all bodies are locked, you're only allowed to read from the bodies and you can't change physics state. +/// During OnContactRemoved you cannot access the bodies at all, see the comments at that function. +class ContactListener +{ +public: + /// Ensure virtual destructor + virtual ~ContactListener() = default; + + /// Called after detecting a collision between a body pair, but before calling OnContactAdded and before adding the contact constraint. + /// If the function rejects the contact, the contact will not be added and any other contacts between this body pair will not be processed. + /// This function will only be called once per PhysicsSystem::Update per body pair and may not be called again the next update + /// if a contact persists and no new contact pairs between sub shapes are found. + /// This is a rather expensive time to reject a contact point since a lot of the collision detection has happened already, make sure you + /// filter out the majority of undesired body pairs through the ObjectLayerPairFilter that is registered on the PhysicsSystem. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// Body 1 will have a motion type that is larger or equal than body 2's motion type (order from large to small: dynamic -> kinematic -> static). When motion types are equal, they are ordered by BodyID. + /// The collision result (inCollisionResult) is reported relative to inBaseOffset. + virtual ValidateResult OnContactValidate([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] RVec3Arg inBaseOffset, [[maybe_unused]] const CollideShapeResult &inCollisionResult) { return ValidateResult::AcceptAllContactsForThisBodyPair; } + + /// Called whenever a new contact point is detected. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. + /// Note that only active bodies will report contacts, as soon as a body goes to sleep the contacts between that body and all other + /// bodies will receive an OnContactRemoved callback, if this is the case then Body::IsActive() will return false during the callback. + /// When contacts are added, the constraint solver has not run yet, so the collision impulse is unknown at that point. + /// The velocities of inBody1 and inBody2 are the velocities before the contact has been resolved, so you can use this to + /// estimate the collision impulse to e.g. determine the volume of the impact sound to play (see: EstimateCollisionResponse). + virtual void OnContactAdded([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ } + + /// Called whenever a contact is detected that was also detected last update. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. + /// If the structure of the shape of a body changes between simulation steps (e.g. by adding/removing a child shape of a compound shape), + /// it is possible that the same sub shape ID used to identify the removed child shape is now reused for a different child shape. The physics + /// system cannot detect this, so may send a 'contact persisted' callback even though the contact is now on a different child shape. You can + /// detect this by keeping the old shape (before adding/removing a part) around until the next PhysicsSystem::Update (when the OnContactPersisted + /// callbacks are triggered) and resolving the sub shape ID against both the old and new shape to see if they still refer to the same child shape. + virtual void OnContactPersisted([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ } + + /// Called whenever a contact was detected last update but is not detected anymore. + /// You cannot access the bodies at the time of this callback because: + /// - All bodies are locked at the time of this callback. + /// - Some properties of the bodies are being modified from another thread at the same time. + /// - The body may have been removed and destroyed (you'll receive an OnContactRemoved callback in the PhysicsSystem::Update after the body has been removed). + /// Cache what you need in the OnContactAdded and OnContactPersisted callbacks and store it in a separate structure to use during this callback. + /// Alternatively, you could just record that the contact was removed and process it after PhysicsSimulation::Update. + /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. + /// The sub shape ID were created in the previous simulation step too, so if the structure of a shape changes (e.g. by adding/removing a child shape of a compound shape), + /// the sub shape ID may not be valid / may not point to the same sub shape anymore. + /// If you want to know if this is the last contact between the two bodies, use PhysicsSystem::WereBodiesInContact. + virtual void OnContactRemoved([[maybe_unused]] const SubShapeIDPair &inSubShapePair) { /* Do nothing */ } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp new file mode 100644 index 000000000000..53cf12b5d41d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp @@ -0,0 +1,213 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution, uint inNumIterations) +{ + // Note this code is based on AxisConstraintPart, see that class for more comments on the math + + ContactPoints::size_type num_points = inManifold.mRelativeContactPointsOn1.size(); + JPH_ASSERT(num_points == inManifold.mRelativeContactPointsOn2.size()); + + // Start with zero impulses + outResult.mImpulses.resize(num_points); + memset(outResult.mImpulses.data(), 0, num_points * sizeof(CollisionEstimationResult::Impulse)); + + // Calculate friction directions + outResult.mTangent1 = inManifold.mWorldSpaceNormal.GetNormalizedPerpendicular(); + outResult.mTangent2 = inManifold.mWorldSpaceNormal.Cross(outResult.mTangent1); + + // Get body velocities + EMotionType motion_type1 = inBody1.GetMotionType(); + const MotionProperties *motion_properties1 = inBody1.GetMotionPropertiesUnchecked(); + if (motion_type1 != EMotionType::Static) + { + outResult.mLinearVelocity1 = motion_properties1->GetLinearVelocity(); + outResult.mAngularVelocity1 = motion_properties1->GetAngularVelocity(); + } + else + outResult.mLinearVelocity1 = outResult.mAngularVelocity1 = Vec3::sZero(); + + EMotionType motion_type2 = inBody2.GetMotionType(); + const MotionProperties *motion_properties2 = inBody2.GetMotionPropertiesUnchecked(); + if (motion_type2 != EMotionType::Static) + { + outResult.mLinearVelocity2 = motion_properties2->GetLinearVelocity(); + outResult.mAngularVelocity2 = motion_properties2->GetAngularVelocity(); + } + else + outResult.mLinearVelocity2 = outResult.mAngularVelocity2 = Vec3::sZero(); + + // Get inverse mass and inertia + float inv_m1, inv_m2; + Mat44 inv_i1, inv_i2; + if (motion_type1 == EMotionType::Dynamic) + { + inv_m1 = motion_properties1->GetInverseMass(); + inv_i1 = inBody1.GetInverseInertia(); + } + else + { + inv_m1 = 0.0f; + inv_i1 = Mat44::sZero(); + } + + if (motion_type2 == EMotionType::Dynamic) + { + inv_m2 = motion_properties2->GetInverseMass(); + inv_i2 = inBody2.GetInverseInertia(); + } + else + { + inv_m2 = 0.0f; + inv_i2 = Mat44::sZero(); + } + + // Get center of masses relative to the base offset + Vec3 com1 = Vec3(inBody1.GetCenterOfMassPosition() - inManifold.mBaseOffset); + Vec3 com2 = Vec3(inBody2.GetCenterOfMassPosition() - inManifold.mBaseOffset); + + struct AxisConstraint + { + inline void Initialize(Vec3Arg inR1, Vec3Arg inR2, Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2) + { + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + mR1PlusUxAxis = inR1.Cross(inWorldSpaceNormal); + mR2xAxis = inR2.Cross(inWorldSpaceNormal); + mInvI1_R1PlusUxAxis = inInvI1.Multiply3x3(mR1PlusUxAxis); + mInvI2_R2xAxis = inInvI2.Multiply3x3(mR2xAxis); + mEffectiveMass = 1.0f / (inInvM1 + mInvI1_R1PlusUxAxis.Dot(mR1PlusUxAxis) + inInvM2 + mInvI2_R2xAxis.Dot(mR2xAxis)); + mBias = 0.0f; + } + + inline float SolveGetLambda(Vec3Arg inWorldSpaceNormal, const CollisionEstimationResult &inResult) const + { + // Calculate jacobian multiplied by linear/angular velocity + float jv = inWorldSpaceNormal.Dot(inResult.mLinearVelocity1 - inResult.mLinearVelocity2) + mR1PlusUxAxis.Dot(inResult.mAngularVelocity1) - mR2xAxis.Dot(inResult.mAngularVelocity2); + + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + return mEffectiveMass * (jv - mBias); + } + + inline void SolveApplyLambda(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inLambda, CollisionEstimationResult &ioResult) const + { + // Apply impulse to body velocities + ioResult.mLinearVelocity1 -= (inLambda * inInvM1) * inWorldSpaceNormal; + ioResult.mAngularVelocity1 -= inLambda * mInvI1_R1PlusUxAxis; + ioResult.mLinearVelocity2 += (inLambda * inInvM2) * inWorldSpaceNormal; + ioResult.mAngularVelocity2 += inLambda * mInvI2_R2xAxis; + } + + inline void Solve(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inMinLambda, float inMaxLambda, float &ioTotalLambda, CollisionEstimationResult &ioResult) const + { + // Calculate new total lambda + float total_lambda = ioTotalLambda + SolveGetLambda(inWorldSpaceNormal, ioResult); + + // Clamp impulse + total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda); + + SolveApplyLambda(inWorldSpaceNormal, inInvM1, inInvM2, total_lambda - ioTotalLambda, ioResult); + + ioTotalLambda = total_lambda; + } + + Vec3 mR1PlusUxAxis; + Vec3 mR2xAxis; + Vec3 mInvI1_R1PlusUxAxis; + Vec3 mInvI2_R2xAxis; + float mEffectiveMass; + float mBias; + }; + + struct Constraint + { + AxisConstraint mContact; + AxisConstraint mFriction1; + AxisConstraint mFriction2; + }; + + // Initialize the constraint properties + Constraint constraints[ContactPoints::Capacity]; + for (uint c = 0; c < num_points; ++c) + { + Constraint &constraint = constraints[c]; + + // Calculate contact points relative to body 1 and 2 + Vec3 p = 0.5f * (inManifold.mRelativeContactPointsOn1[c] + inManifold.mRelativeContactPointsOn2[c]); + Vec3 r1 = p - com1; + Vec3 r2 = p - com2; + + // Initialize contact constraint + constraint.mContact.Initialize(r1, r2, inManifold.mWorldSpaceNormal, inv_m1, inv_m2, inv_i1, inv_i2); + + // Handle elastic collisions + if (inCombinedRestitution > 0.0f) + { + // Calculate velocity of contact point + Vec3 relative_velocity = outResult.mLinearVelocity2 + outResult.mAngularVelocity2.Cross(r2) - outResult.mLinearVelocity1 - outResult.mAngularVelocity1.Cross(r1); + float normal_velocity = relative_velocity.Dot(inManifold.mWorldSpaceNormal); + + // If it is big enough, apply restitution + if (normal_velocity < -inMinVelocityForRestitution) + constraint.mContact.mBias = inCombinedRestitution * normal_velocity; + } + + if (inCombinedFriction > 0.0f) + { + // Initialize friction constraints + constraint.mFriction1.Initialize(r1, r2, outResult.mTangent1, inv_m1, inv_m2, inv_i1, inv_i2); + constraint.mFriction2.Initialize(r1, r2, outResult.mTangent2, inv_m1, inv_m2, inv_i1, inv_i2); + } + } + + // If there's only 1 contact point, we only need 1 iteration + int num_iterations = inCombinedFriction <= 0.0f && num_points == 1? 1 : inNumIterations; + + // Solve iteratively + for (int iteration = 0; iteration < num_iterations; ++iteration) + { + // Solve friction constraints first + if (inCombinedFriction > 0.0f && iteration > 0) // For first iteration the contact impulse is zero so there's no point in applying friction + for (uint c = 0; c < num_points; ++c) + { + const Constraint &constraint = constraints[c]; + CollisionEstimationResult::Impulse &impulse = outResult.mImpulses[c]; + + float lambda1 = impulse.mFrictionImpulse1 + constraint.mFriction1.SolveGetLambda(outResult.mTangent1, outResult); + float lambda2 = impulse.mFrictionImpulse2 + constraint.mFriction2.SolveGetLambda(outResult.mTangent2, outResult); + + // Calculate max impulse based on contact impulse + float max_impulse = inCombinedFriction * impulse.mContactImpulse; + + // If the total lambda that we will apply is too large, scale it back + float total_lambda_sq = Square(lambda1) + Square(lambda2); + if (total_lambda_sq > Square(max_impulse)) + { + float scale = max_impulse / sqrt(total_lambda_sq); + lambda1 *= scale; + lambda2 *= scale; + } + + constraint.mFriction1.SolveApplyLambda(outResult.mTangent1, inv_m1, inv_m2, lambda1 - impulse.mFrictionImpulse1, outResult); + constraint.mFriction2.SolveApplyLambda(outResult.mTangent2, inv_m1, inv_m2, lambda2 - impulse.mFrictionImpulse2, outResult); + + impulse.mFrictionImpulse1 = lambda1; + impulse.mFrictionImpulse2 = lambda2; + } + + // Solve contact constraints last + for (uint c = 0; c < num_points; ++c) + constraints[c].mContact.Solve(inManifold.mWorldSpaceNormal, inv_m1, inv_m2, 0.0f, FLT_MAX, outResult.mImpulses[c].mContactImpulse, outResult); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h new file mode 100644 index 000000000000..45098d1477e6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A structure that contains the estimated contact and friction impulses and the resulting body velocities +struct CollisionEstimationResult +{ + Vec3 mLinearVelocity1; ///< The estimated linear velocity of body 1 after collision + Vec3 mAngularVelocity1; ///< The estimated angular velocity of body 1 after collision + Vec3 mLinearVelocity2; ///< The estimated linear velocity of body 2 after collision + Vec3 mAngularVelocity2; ///< The estimated angular velocity of body 2 after collision + + Vec3 mTangent1; ///< Normalized tangent of contact normal + Vec3 mTangent2; ///< Second normalized tangent of contact normal (forms a basis with mTangent1 and mWorldSpaceNormal) + + struct Impulse + { + float mContactImpulse; ///< Estimated contact impulses (kg m / s) + float mFrictionImpulse1; ///< Estimated friction impulses in the direction of tangent 1 (kg m / s) + float mFrictionImpulse2; ///< Estimated friction impulses in the direction of tangent 2 (kg m / s) + }; + + using Impulses = StaticArray; + + Impulses mImpulses; +}; + +/// This function estimates the contact impulses and body velocity changes as a result of a collision. +/// It can be used in the ContactListener::OnContactAdded to determine the strength of the collision to e.g. play a sound or trigger a particle system. +/// This function is accurate when two bodies collide but will not be accurate when more than 2 bodies collide at the same time as it does not know about these other collisions. +/// +/// @param inBody1 Colliding body 1 +/// @param inBody2 Colliding body 2 +/// @param inManifold The collision manifold +/// @param outResult A structure that contains the estimated contact and friction impulses and the resulting body velocities +/// @param inCombinedFriction The combined friction of body 1 and body 2 (see ContactSettings::mCombinedFriction) +/// @param inCombinedRestitution The combined restitution of body 1 and body 2 (see ContactSettings::mCombinedRestitution) +/// @param inMinVelocityForRestitution Minimal velocity required for restitution to be applied (see PhysicsSettings::mMinVelocityForRestitution) +/// @param inNumIterations Number of iterations to use for the impulse estimation (see PhysicsSettings::mNumVelocitySteps, note you can probably use a lower number for a decent estimate). If you set the number of iterations to 1 then no friction will be calculated. +JPH_EXPORT void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution = 1.0f, uint inNumIterations = 10); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp new file mode 100644 index 000000000000..80c7620fdad1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(GroupFilter) +{ + JPH_ADD_BASE_CLASS(GroupFilter, SerializableObject) +} + +void GroupFilter::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); +} + +void GroupFilter::RestoreBinaryState(StreamIn &inStream) +{ + // RTTI hash is read in sRestoreFromBinaryState +} + +GroupFilter::GroupFilterResult GroupFilter::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &GroupFilter::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h new file mode 100644 index 000000000000..5e01043a8f37 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h @@ -0,0 +1,41 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollisionGroup; +class StreamIn; +class StreamOut; + +/// Abstract class that checks if two CollisionGroups collide +class JPH_EXPORT GroupFilter : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, GroupFilter) + +public: + /// Virtual destructor + virtual ~GroupFilter() override = default; + + /// Check if two groups collide + virtual bool CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const = 0; + + /// Saves the contents of the group filter in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + using GroupFilterResult = Result>; + + /// Creates a GroupFilter of the correct type and restores its contents from the binary stream inStream. + static GroupFilterResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp new file mode 100644 index 000000000000..dd69d27c8522 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GroupFilterTable) +{ + JPH_ADD_BASE_CLASS(GroupFilterTable, GroupFilter) + + JPH_ADD_ATTRIBUTE(GroupFilterTable, mNumSubGroups) + JPH_ADD_ATTRIBUTE(GroupFilterTable, mTable) +} + +void GroupFilterTable::SaveBinaryState(StreamOut &inStream) const +{ + GroupFilter::SaveBinaryState(inStream); + + inStream.Write(mNumSubGroups); + inStream.Write(mTable); +} + +void GroupFilterTable::RestoreBinaryState(StreamIn &inStream) +{ + GroupFilter::RestoreBinaryState(inStream); + + inStream.Read(mNumSubGroups); + inStream.Read(mTable); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h new file mode 100644 index 000000000000..76cae277da6d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of GroupFilter that stores a bit table with one bit per sub shape ID pair to determine if they collide or not +/// +/// The collision rules: +/// - If one of the objects is in the cInvalidGroup the objects will collide. +/// - If the objects are in different groups they will collide. +/// - If they're in the same group but their collision filter is different they will not collide. +/// - If they're in the same group and their collision filters match, we'll use the SubGroupID and the table below. +/// +/// For N = 6 sub groups the table will look like: +/// +/// sub group 1 ---> +/// sub group 2 x..... +/// | ox.... +/// | oox... +/// V ooox.. +/// oooox. +/// ooooox +/// +/// * 'x' means sub group 1 == sub group 2 and we define this to never collide. +/// * 'o' is a bit that we have to store that defines if the sub groups collide or not. +/// * '.' is a bit we don't need to store because the table is symmetric, we take care that group 2 > group 1 by swapping sub group 1 and sub group 2 if needed. +/// +/// The total number of bits we need to store is (N * (N - 1)) / 2 +class JPH_EXPORT GroupFilterTable final : public GroupFilter +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GroupFilterTable) + +private: + using GroupID = CollisionGroup::GroupID; + using SubGroupID = CollisionGroup::SubGroupID; + + /// Get which bit corresponds to the pair (inSubGroup1, inSubGroup2) + int GetBit(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const + { + JPH_ASSERT(inSubGroup1 != inSubGroup2); + + // We store the lower left half only, so swap the inputs when trying to access the top right half + if (inSubGroup1 > inSubGroup2) + std::swap(inSubGroup1, inSubGroup2); + + JPH_ASSERT(inSubGroup2 < mNumSubGroups); + + // Calculate at which bit the entry for this pair resides + // We use the fact that a row always starts at inSubGroup2 * (inSubGroup2 - 1) / 2 + // (this is the amount of bits needed to store a table of inSubGroup2 entries) + return (inSubGroup2 * (inSubGroup2 - 1)) / 2 + inSubGroup1; + } + +public: + /// Constructs the table with inNumSubGroups subgroups, initially all collision pairs are enabled except when the sub group ID is the same + explicit GroupFilterTable(uint inNumSubGroups = 0) : + mNumSubGroups(inNumSubGroups) + { + // By default everything collides + int table_size = ((inNumSubGroups * (inNumSubGroups - 1)) / 2 + 7) / 8; + mTable.resize(table_size, 0xff); + } + + /// Copy constructor + GroupFilterTable(const GroupFilterTable &inRHS) : mNumSubGroups(inRHS.mNumSubGroups), mTable(inRHS.mTable) { } + + /// Disable collision between two sub groups + void DisableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2) + { + int bit = GetBit(inSubGroup1, inSubGroup2); + mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111))); + } + + /// Enable collision between two sub groups + void EnableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2) + { + int bit = GetBit(inSubGroup1, inSubGroup2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + + /// Check if the collision between two subgroups is enabled + inline bool IsCollisionEnabled(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const + { + // Test if the bit is set for this group pair + int bit = GetBit(inSubGroup1, inSubGroup2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + + /// Checks if two CollisionGroups collide + virtual bool CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const override + { + // If one of the groups is cInvalidGroup the objects will collide (note that the if following this if will ensure that group2 is not cInvalidGroup) + if (inGroup1.GetGroupID() == CollisionGroup::cInvalidGroup) + return true; + + // If the objects are in different groups, they collide + if (inGroup1.GetGroupID() != inGroup2.GetGroupID()) + return true; + + // If the collision filters do not match, but they're in the same group we ignore the collision + if (inGroup1.GetGroupFilter() != inGroup2.GetGroupFilter()) + return false; + + // If they are in the same sub group, they don't collide + if (inGroup1.GetSubGroupID() == inGroup2.GetSubGroupID()) + return false; + + // Check the bit table + return IsCollisionEnabled(inGroup1.GetSubGroupID(), inGroup2.GetSubGroupID()); + } + + // See: GroupFilter::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + +protected: + // See: GroupFilter::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + uint mNumSubGroups; ///< The number of subgroups that this group filter supports + Array mTable; ///< The table of bits that indicates which pairs collide +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h new file mode 100644 index 000000000000..0d67729125e4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h @@ -0,0 +1,250 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +//#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + +#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG +#include +#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + +JPH_NAMESPACE_BEGIN + +/// Removes internal edges from collision results. Can be used to filter out 'ghost collisions'. +/// Based on: Contact generation for meshes - Pierre Terdiman (https://www.codercorner.com/MeshContacts.pdf) +class InternalEdgeRemovingCollector : public CollideShapeCollector +{ + static constexpr uint cMaxDelayedResults = 16; + static constexpr uint cMaxVoidedFeatures = 128; + + /// Check if a vertex is voided + inline bool IsVoided(const SubShapeID &inSubShapeID, Vec3 inV) const + { + for (const Voided &vf : mVoidedFeatures) + if (vf.mSubShapeID == inSubShapeID + && inV.IsClose(Vec3::sLoadFloat3Unsafe(vf.mFeature), 1.0e-8f)) + return true; + return false; + } + + /// Add all vertices of a face to the voided features + inline void VoidFeatures(const CollideShapeResult &inResult) + { + if (mVoidedFeatures.size() < cMaxVoidedFeatures) + for (const Vec3 &v : inResult.mShape2Face) + if (!IsVoided(inResult.mSubShapeID1, v)) + { + Voided vf; + v.StoreFloat3(&vf.mFeature); + vf.mSubShapeID = inResult.mSubShapeID1; + mVoidedFeatures.push_back(vf); + if (mVoidedFeatures.size() == cMaxVoidedFeatures) + break; + } + } + + /// Call the chained collector + inline void Chain(const CollideShapeResult &inResult) + { + // Make sure the chained collector has the same context as we do + mChainedCollector.SetContext(GetContext()); + + // Forward the hit + mChainedCollector.AddHit(inResult); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mChainedCollector.GetEarlyOutFraction()); + } + + /// Call the chained collector and void all features of inResult + inline void ChainAndVoid(const CollideShapeResult &inResult) + { + Chain(inResult); + VoidFeatures(inResult); + + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), inResult.mShape2Face, Color::sGreen); + DebugRenderer::sInstance->DrawArrow(RVec3(inResult.mContactPointOn2), RVec3(inResult.mContactPointOn2) + inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f); + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + } + +public: + /// Constructor, configures a collector to be called with all the results that do not hit internal edges + explicit InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector) : + mChainedCollector(inChainedCollector) + { + } + + // See: CollideShapeCollector::Reset + virtual void Reset() override + { + CollideShapeCollector::Reset(); + + mChainedCollector.Reset(); + + mVoidedFeatures.clear(); + mDelayedResults.clear(); + } + + // See: CollideShapeCollector::OnBody + virtual void OnBody(const Body &inBody) override + { + // Just forward the call to our chained collector + mChainedCollector.OnBody(inBody); + } + + // See: CollideShapeCollector::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + // We only support welding when the shape is a triangle or has more vertices so that we can calculate a normal + if (inResult.mShape2Face.size() < 3) + return ChainAndVoid(inResult); + + // Get the triangle normal of shape 2 face + Vec3 triangle_normal = (inResult.mShape2Face[1] - inResult.mShape2Face[0]).Cross(inResult.mShape2Face[2] - inResult.mShape2Face[0]); + float triangle_normal_len = triangle_normal.Length(); + if (triangle_normal_len < 1e-6f) + return ChainAndVoid(inResult); + + // If the triangle normal matches the contact normal within 1 degree, we can process the contact immediately + // We make the assumption here that if the contact normal and the triangle normal align that the we're dealing with a 'face contact' + Vec3 contact_normal = -inResult.mPenetrationAxis; + float contact_normal_len = inResult.mPenetrationAxis.Length(); + if (triangle_normal.Dot(contact_normal) > 0.999848f * contact_normal_len * triangle_normal_len) // cos(1 degree) + return ChainAndVoid(inResult); + + // Delayed processing + if (mDelayedResults.size() == cMaxDelayedResults) + return ChainAndVoid(inResult); + mDelayedResults.push_back(inResult); + } + + /// After all hits have been added, call this function to process the delayed results + void Flush() + { + // Sort on biggest penetration depth first + uint sorted_indices[cMaxDelayedResults]; + for (uint i = 0; i < uint(mDelayedResults.size()); ++i) + sorted_indices[i] = i; + QuickSort(sorted_indices, sorted_indices + mDelayedResults.size(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; }); + + // Loop over all results + for (uint i = 0; i < uint(mDelayedResults.size()); ++i) + { + const CollideShapeResult &r = mDelayedResults[sorted_indices[i]]; + + // Determine which vertex or which edge is the closest to the contact point + float best_dist_sq = FLT_MAX; + uint best_v1_idx = 0; + uint best_v2_idx = 0; + uint num_v = uint(r.mShape2Face.size()); + uint v1_idx = num_v - 1; + Vec3 v1 = r.mShape2Face[v1_idx] - r.mContactPointOn2; + for (uint v2_idx = 0; v2_idx < num_v; ++v2_idx) + { + Vec3 v2 = r.mShape2Face[v2_idx] - r.mContactPointOn2; + Vec3 v1_v2 = v2 - v1; + float denominator = v1_v2.LengthSq(); + if (denominator < Square(FLT_EPSILON)) + { + // Degenerate, assume v1 is closest, v2 will be tested in a later iteration + float v1_len_sq = v1.LengthSq(); + if (v1_len_sq < best_dist_sq) + { + best_dist_sq = v1_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v1_idx; + } + } + else + { + // Taken from ClosestPoint::GetBaryCentricCoordinates + float fraction = -v1.Dot(v1_v2) / denominator; + if (fraction < 1.0e-6f) + { + // Closest lies on v1 + float v1_len_sq = v1.LengthSq(); + if (v1_len_sq < best_dist_sq) + { + best_dist_sq = v1_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v1_idx; + } + } + else if (fraction < 1.0f - 1.0e-6f) + { + // Closest lies on the line segment v1, v2 + Vec3 closest = v1 + fraction * v1_v2; + float closest_len_sq = closest.LengthSq(); + if (closest_len_sq < best_dist_sq) + { + best_dist_sq = closest_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v2_idx; + } + } + // else closest is v2, but v2 will be tested in a later iteration + } + + v1_idx = v2_idx; + v1 = v2; + } + + // Check if this vertex/edge is voided + bool voided = IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx]) + && (best_v1_idx == best_v2_idx || IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])); + + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + Color color = voided? Color::sRed : Color::sYellow; + DebugRenderer::sInstance->DrawText3D(RVec3(r.mContactPointOn2), StringFormat("%d: %g", i, r.mPenetrationDepth), color, 0.1f); + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), r.mShape2Face, color); + DebugRenderer::sInstance->DrawArrow(RVec3(r.mContactPointOn2), RVec3(r.mContactPointOn2) + r.mPenetrationAxis.NormalizedOr(Vec3::sZero()), color, 0.1f); + DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v1_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f); + DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v2_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f); + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + + // No voided features, accept the contact + if (!voided) + Chain(r); + + // Void the features of this face + VoidFeatures(r); + } + + // All delayed results have been processed + mVoidedFeatures.clear(); + mDelayedResults.clear(); + } + + /// Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges + static void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) + { + JPH_ASSERT(inCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideWithAll); // Won't work without colliding with all edges + JPH_ASSERT(inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces); // Won't work without collecting faces + + InternalEdgeRemovingCollector wrapper(ioCollector); + CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter); + wrapper.Flush(); + } + +private: + // This algorithm tests a convex shape (shape 1) against a set of polygons (shape 2). + // This assumption doesn't hold if the shape we're testing is a compound shape, so we must also + // store the sub shape ID and ignore voided features that belong to another sub shape ID. + struct Voided + { + Float3 mFeature; // Feature that is voided (of shape 2). Read with Vec3::sLoadFloat3Unsafe so must not be the last member. + SubShapeID mSubShapeID; // Sub shape ID of the shape that is colliding against the feature (of shape 1). + }; + + CollideShapeCollector & mChainedCollector; + StaticArray mVoidedFeatures; + StaticArray mDelayedResults; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp new file mode 100644 index 000000000000..3331f5d7a03d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp @@ -0,0 +1,237 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass)) +{ + // Makes no sense to call this with 4 or less points + JPH_ASSERT(ioContactPointsOn1.size() > 4); + + // Both arrays should have the same size + JPH_ASSERT(ioContactPointsOn1.size() == ioContactPointsOn2.size()); + + // Penetration axis must be normalized + JPH_ASSERT(inPenetrationAxis.IsNormalized()); + + // We use a heuristic of (distance to center of mass) * (penetration depth) to find the contact point that we should keep + // Neither of those two terms should ever become zero, so we clamp against this minimum value + constexpr float cMinDistanceSq = 1.0e-6f; // 1 mm + + ContactPoints projected; + StaticArray penetration_depth_sq; + for (ContactPoints::size_type i = 0; i < ioContactPointsOn1.size(); ++i) + { + // Project contact points on the plane through inCenterOfMass with normal inPenetrationAxis and center around the center of mass of body 1 + // (note that since all points are relative to inCenterOfMass we can project onto the plane through the origin) + Vec3 v1 = ioContactPointsOn1[i]; + projected.push_back(v1 - v1.Dot(inPenetrationAxis) * inPenetrationAxis); + + // Calculate penetration depth^2 of each point and clamp against the minimal distance + Vec3 v2 = ioContactPointsOn2[i]; + penetration_depth_sq.push_back(max(cMinDistanceSq, (v2 - v1).LengthSq())); + } + + // Find the point that is furthest away from the center of mass (its torque will have the biggest influence) + // and the point that has the deepest penetration depth. Use the heuristic (distance to center of mass) * (penetration depth) for this. + uint point1 = 0; + float val = max(cMinDistanceSq, projected[0].LengthSq()) * penetration_depth_sq[0]; + for (uint i = 0; i < projected.size(); ++i) + { + float v = max(cMinDistanceSq, projected[i].LengthSq()) * penetration_depth_sq[i]; + if (v > val) + { + val = v; + point1 = i; + } + } + Vec3 point1v = projected[point1]; + + // Find point furthest from the first point forming a line segment with point1. Again combine this with the heuristic + // for deepest point as per above. + uint point2 = uint(-1); + val = -FLT_MAX; + for (uint i = 0; i < projected.size(); ++i) + if (i != point1) + { + float v = max(cMinDistanceSq, (projected[i] - point1v).LengthSq()) * penetration_depth_sq[i]; + if (v > val) + { + val = v; + point2 = i; + } + } + JPH_ASSERT(point2 != uint(-1)); + Vec3 point2v = projected[point2]; + + // Find furthest points on both sides of the line segment in order to maximize the area + uint point3 = uint(-1); + uint point4 = uint(-1); + float min_val = 0.0f; + float max_val = 0.0f; + Vec3 perp = (point2v - point1v).Cross(inPenetrationAxis); + for (uint i = 0; i < projected.size(); ++i) + if (i != point1 && i != point2) + { + float v = perp.Dot(projected[i] - point1v); + if (v < min_val) + { + min_val = v; + point3 = i; + } + else if (v > max_val) + { + max_val = v; + point4 = i; + } + } + + // Add points to array (in order so they form a polygon) + StaticArray points_to_keep_on_1, points_to_keep_on_2; + points_to_keep_on_1.push_back(ioContactPointsOn1[point1]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point1]); + if (point3 != uint(-1)) + { + points_to_keep_on_1.push_back(ioContactPointsOn1[point3]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point3]); + } + points_to_keep_on_1.push_back(ioContactPointsOn1[point2]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point2]); + if (point4 != uint(-1)) + { + JPH_ASSERT(point3 != point4); + points_to_keep_on_1.push_back(ioContactPointsOn1[point4]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point4]); + } + +#ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawContactPointReduction) + { + // Draw input polygon + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sTranslation(inCenterOfMass), ioContactPointsOn1, Color::sOrange, 0.05f); + + // Draw primary axis + DebugRenderer::sInstance->DrawArrow(inCenterOfMass + ioContactPointsOn1[point1], inCenterOfMass + ioContactPointsOn1[point2], Color::sRed, 0.05f); + + // Draw contact points we kept + for (Vec3 p : points_to_keep_on_1) + DebugRenderer::sInstance->DrawMarker(inCenterOfMass + p, Color::sGreen, 0.1f); + } +#endif // JPH_DEBUG_RENDERER + + // Copy the points back to the input buffer + ioContactPointsOn1 = points_to_keep_on_1; + ioContactPointsOn2 = points_to_keep_on_2; +} + +void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq , const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass)) +{ +#ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawContactPoint) + { + RVec3 cp1 = inCenterOfMass + inContactPoint1; + RVec3 cp2 = inCenterOfMass + inContactPoint2; + + // Draw contact points + DebugRenderer::sInstance->DrawMarker(cp1, Color::sRed, 0.1f); + DebugRenderer::sInstance->DrawMarker(cp2, Color::sGreen, 0.1f); + + // Draw contact normal + DebugRenderer::sInstance->DrawArrow(cp1, cp1 + inPenetrationAxis.Normalized(), Color::sRed, 0.05f); + } +#endif // JPH_DEBUG_RENDERER + + // Remember size before adding new points, to check at the end if we added some + ContactPoints::size_type old_size = outContactPoints1.size(); + + // Check if both shapes have polygon faces + if (inShape1Face.size() >= 2 // The dynamic shape needs to have at least 2 points or else there can never be more than 1 contact point + && inShape2Face.size() >= 3) // The dynamic/static shape needs to have at least 3 points (in the case that it has 2 points only if the edges match exactly you can have 2 contact points, but this situation is unstable anyhow) + { + // Clip the polygon of face 2 against that of 1 + ConvexShape::SupportingFace clipped_face; + if (inShape1Face.size() >= 3) + ClipPolyVsPoly(inShape2Face, inShape1Face, inPenetrationAxis, clipped_face); + else if (inShape1Face.size() == 2) + ClipPolyVsEdge(inShape2Face, inShape1Face[0], inShape1Face[1], inPenetrationAxis, clipped_face); + + // Project the points back onto the plane of shape 1 face and only keep those that are behind the plane + Vec3 plane_origin = inShape1Face[0]; + Vec3 plane_normal; + Vec3 first_edge = inShape1Face[1] - plane_origin; + if (inShape1Face.size() >= 3) + { + // Three vertices, can just calculate the normal + plane_normal = first_edge.Cross(inShape1Face[2] - plane_origin); + } + else + { + // Two vertices, first find a perpendicular to the edge and penetration axis and then use the perpendicular together with the edge to form a normal + plane_normal = first_edge.Cross(inPenetrationAxis).Cross(first_edge); + } + + // Check if the plane normal has any length, if not the clipped shape is so small that we'll just use the contact points + float plane_normal_len_sq = plane_normal.LengthSq(); + if (plane_normal_len_sq > 0.0f) + { + // Discard points of faces that are too far away to collide + for (Vec3 p2 : clipped_face) + { + float distance = (p2 - plane_origin).Dot(plane_normal); // Note should divide by length of plane_normal (unnormalized here) + if (distance <= 0.0f || Square(distance) < inMaxContactDistanceSq * plane_normal_len_sq) // Must be close enough to plane, note we correct for not dividing by plane normal length here + { + // Project point back on shape 1 using the normal, note we correct for not dividing by plane normal length here: + // p1 = p2 - (distance / sqrt(plane_normal_len_sq)) * (plane_normal / sqrt(plane_normal_len_sq)); + Vec3 p1 = p2 - (distance / plane_normal_len_sq) * plane_normal; + + outContactPoints1.push_back(p1); + outContactPoints2.push_back(p2); + } + } + } + + #ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawSupportingFaces) + { + RMat44 com = RMat44::sTranslation(inCenterOfMass); + + // Draw clipped poly + DebugRenderer::sInstance->DrawWirePolygon(com, clipped_face, Color::sOrange); + + // Draw supporting faces + DebugRenderer::sInstance->DrawWirePolygon(com, inShape1Face, Color::sRed, 0.05f); + DebugRenderer::sInstance->DrawWirePolygon(com, inShape2Face, Color::sGreen, 0.05f); + + // Draw normal + if (plane_normal_len_sq > 0.0f) + { + RVec3 plane_origin_ws = inCenterOfMass + plane_origin; + DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / sqrt(plane_normal_len_sq), Color::sYellow, 0.05f); + } + + // Draw contact points that remain after distance check + for (ContactPoints::size_type p = old_size; p < outContactPoints1.size(); ++p) + DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints1[p], Color::sYellow, 0.1f); + } + #endif // JPH_DEBUG_RENDERER + } + + // If the clipping result is empty, use the contact point itself + if (outContactPoints1.size() == old_size) + { + outContactPoints1.push_back(inContactPoint1); + outContactPoints2.push_back(inContactPoint2); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h new file mode 100644 index 000000000000..72a5b8f0c14a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h @@ -0,0 +1,44 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Remove contact points if there are > 4 (no more than 4 are needed for a stable solution) +/// @param inPenetrationAxis is the world space penetration axis (must be normalized) +/// @param ioContactPointsOn1 The contact points on shape 1 relative to inCenterOfMass +/// @param ioContactPointsOn2 The contact points on shape 2 relative to inCenterOfMass +/// On output ioContactPointsOn1/2 are reduced to 4 or less points +#ifdef JPH_DEBUG_RENDERER +/// @param inCenterOfMass Center of mass position of body 1 +#endif +JPH_EXPORT void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 +#ifdef JPH_DEBUG_RENDERER + , RVec3Arg inCenterOfMass +#endif + ); + +/// Determine contact points between 2 faces of 2 shapes and return them in outContactPoints 1 & 2 +/// @param inContactPoint1 The contact point on shape 1 relative to inCenterOfMass +/// @param inContactPoint2 The contact point on shape 2 relative to inCenterOfMass +/// @param inPenetrationAxis The local space penetration axis in world space +/// @param inMaxContactDistanceSq After face 2 is clipped against face 1, each remaining point on face 2 is tested against the plane of face 1. If the distance^2 on the positive side of the plane is larger than this distance, the point will be discarded as a contact point. +/// @param inShape1Face The supporting faces on shape 1 relative to inCenterOfMass +/// @param inShape2Face The supporting faces on shape 2 relative to inCenterOfMass +/// @param outContactPoints1 Returns the contact points between the two shapes for shape 1 relative to inCenterOfMass (any existing points in the output array are left as is) +/// @param outContactPoints2 Returns the contact points between the two shapes for shape 2 relative to inCenterOfMass (any existing points in the output array are left as is) +#ifdef JPH_DEBUG_RENDERER +/// @param inCenterOfMass Center of mass position of body 1 +#endif +JPH_EXPORT void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 +#ifdef JPH_DEBUG_RENDERER + , RVec3Arg inCenterOfMass +#endif + ); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp new file mode 100644 index 000000000000..4431a1047e0c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp @@ -0,0 +1,493 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool NarrowPhaseQuery::CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public RayCastBodyCollector + { + public: + MyCollector(const RRayCast &inRay, RayCastResult &ioHit, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter) : + mRay(inRay), + mHit(ioHit), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter) + { + UpdateEarlyOutFraction(ioHit.mFraction); + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction < mHit.mFraction, "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + if (ts.CastRay(mRay, mHit)) + { + // Test that we didn't find a further hit by accident + JPH_ASSERT(mHit.mFraction >= 0.0f && mHit.mFraction < GetEarlyOutFraction()); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mHit.mFraction); + } + } + } + } + } + + RRayCast mRay; + RayCastResult & mHit; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + }; + + // Do broadphase test, note that the broadphase uses floats so we drop precision here + MyCollector collector(inRay, ioHit, *mBodyLockInterface, inBodyFilter); + mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); + return ioHit.mFraction <= 1.0f; +} + +void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public RayCastBodyCollector + { + public: + MyCollector(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + RayCastBodyCollector(ioCollector), + mRay(inRay), + mRayCastSettings(inRayCastSettings), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction < mCollector.GetEarlyOutFraction(), "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CastRay(mRay, mRayCastSettings, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RRayCast mRay; + RayCastSettings mRayCastSettings; + CastRayCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test, note that the broadphase uses floats so we drop precision here + MyCollector collector(inRay, inRayCastSettings, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mPoint(inPoint), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollidePoint(mPoint, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RVec3 mPoint; + CollidePointCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test (note: truncates double to single precision since the broadphase uses single precision) + MyCollector collector(inPoint, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollidePoint(Vec3(inPoint), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mShape(inShape), + mShapeScale(inShapeScale), + mCenterOfMassTransform(inCenterOfMassTransform), + mCollideShapeSettings(inCollideShapeSettings), + mBaseOffset(inBaseOffset), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const Shape * mShape; + Vec3 mShapeScale; + RMat44 mCenterOfMassTransform; + const CollideShapeSettings & mCollideShapeSettings; + RVec3 mBaseOffset; + CollideShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Calculate bounds for shape and expand by max separation distance + AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale); + bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + + // Do broadphase test + MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mShape(inShape), + mShapeScale(inShapeScale), + mCenterOfMassTransform(inCenterOfMassTransform), + mBaseOffset(inBaseOffset), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter), + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector) + { + // We require these settings for internal edge removal to work + mCollideShapeSettings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll; + mCollideShapeSettings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter); + + // After each body, we need to flush the InternalEdgeRemovingCollector because it uses 'ts' as context and it will go out of scope at the end of this block + mCollector.Flush(); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const Shape * mShape; + Vec3 mShapeScale; + RMat44 mCenterOfMassTransform; + RVec3 mBaseOffset; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + CollideShapeSettings mCollideShapeSettings; + InternalEdgeRemovingCollector mCollector; + }; + + // Calculate bounds for shape and expand by max separation distance + AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale); + bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + + // Do broadphase test + MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CastShapeBodyCollector + { + public: + MyCollector(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CastShapeBodyCollector(ioCollector), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mBaseOffset(inBaseOffset), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction <= max(0.0f, mCollector.GetEarlyOutFraction()), "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RShapeCast mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + RVec3 mBaseOffset; + CastShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test + MyCollector collector(inShapeCast, inShapeCastSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CastAABox({ inShapeCast.mShapeWorldBounds, inShapeCast.mDirection }, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const AABox &inBox, TransformedShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mBox(inBox), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollectTransformedShapes(mBox, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const AABox & mBox; + TransformedShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test + MyCollector collector(inBox, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h new file mode 100644 index 000000000000..087cc6ca3f95 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h @@ -0,0 +1,77 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Shape; +class CollideShapeSettings; +class RayCastResult; + +/// Class that provides an interface for doing precise collision detection against the broad and then the narrow phase. +/// Unlike a BroadPhaseQuery, the NarrowPhaseQuery will test against shapes and will return collision information against triangles, spheres etc. +class JPH_EXPORT NarrowPhaseQuery : public NonCopyable +{ +public: + /// Initialize the interface (should only be called by PhysicsSystem) + void Init(BodyLockInterface &inBodyLockInterface, BroadPhaseQuery &inBroadPhaseQuery) { mBodyLockInterface = &inBodyLockInterface; mBroadPhaseQuery = &inBroadPhaseQuery; } + + /// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned. + /// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on body with ID ioHit.mBodyID. + bool CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }) const; + + /// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on body with collected body ID. + void CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid. + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit + void CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Collide a shape with the system + /// @param inShape Shape to test + /// @param inShapeScale Scale in local space of shape + /// @param inCenterOfMassTransform Center of mass transform for the shape + /// @param inCollideShapeSettings Settings + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inCenterOfMassTransform.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inBroadPhaseLayerFilter Filter that filters at broadphase level + /// @param inObjectLayerFilter Filter that filters at layer level + /// @param inBodyFilter Filter that filters at body level + /// @param inShapeFilter Filter that filters at shape level + void CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Same as CollideShape, but uses InternalEdgeRemovingCollector to remove internal edges from the collision results (a.k.a. ghost collisions) + void CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Cast a shape and report any hits to ioCollector + /// @param inShapeCast The shape cast and its position and direction + /// @param inShapeCastSettings Settings for the shape cast + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inBroadPhaseLayerFilter Filter that filters at broadphase level + /// @param inObjectLayerFilter Filter that filters at layer level + /// @param inBodyFilter Filter that filters at body level + /// @param inShapeFilter Filter that filters at shape level + void CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Collect all leaf transformed shapes that fall inside world space box inBox + void CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + +private: + BodyLockInterface * mBodyLockInterface = nullptr; + BroadPhaseQuery * mBroadPhaseQuery = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp new file mode 100644 index 000000000000..69c611372537 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp @@ -0,0 +1,62 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_TRACK_NARROWPHASE_STATS + +JPH_NAMESPACE_BEGIN + +NarrowPhaseStat NarrowPhaseStat::sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; +NarrowPhaseStat NarrowPhaseStat::sCastShape[NumSubShapeTypes][NumSubShapeTypes]; + +thread_local TrackNarrowPhaseStat *TrackNarrowPhaseStat::sRoot = nullptr; + +void NarrowPhaseStat::ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const +{ + double total_pct = 100.0 * double(mTotalTicks) / double(inTicks100Pct); + double total_pct_excl_children = 100.0 * double(mTotalTicks - mChildTicks) / double(inTicks100Pct); + + std::stringstream str; + str << inName << ", " << sSubShapeTypeNames[(int)inType1] << ", " << sSubShapeTypeNames[(int)inType2] << ", " << mNumQueries << ", " << total_pct << ", " << total_pct_excl_children << ", " << total_pct_excl_children / mNumQueries << ", " << mHitsReported; + Trace(str.str().c_str()); +} + +void NarrowPhaseStat::sReportStats() +{ + Trace("Query Type, Shape Type 1, Shape Type 2, Num Queries, Total Time (%%), Total Time Excl Children (%%), Total Time Excl. Children / Query (%%), Hits Reported"); + + uint64 total_ticks = 0; + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &collide_stat = sCollideShape[(int)t1][(int)t2]; + total_ticks += collide_stat.mTotalTicks - collide_stat.mChildTicks; + + const NarrowPhaseStat &cast_stat = sCastShape[(int)t1][(int)t2]; + total_ticks += cast_stat.mTotalTicks - cast_stat.mChildTicks; + } + + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &stat = sCollideShape[(int)t1][(int)t2]; + if (stat.mNumQueries > 0) + stat.ReportStats("CollideShape", t1, t2, total_ticks); + } + + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &stat = sCastShape[(int)t1][(int)t2]; + if (stat.mNumQueries > 0) + stat.ReportStats("CastShape", t1, t2, total_ticks); + } +} + +JPH_NAMESPACE_END + +#endif // JPH_TRACK_NARROWPHASE_STATS diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h new file mode 100644 index 000000000000..813933bb754e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +// Shorthand function to ifdef out code if narrow phase stats tracking is off +#ifdef JPH_TRACK_NARROWPHASE_STATS + #define JPH_IF_TRACK_NARROWPHASE_STATS(...) __VA_ARGS__ +#else + #define JPH_IF_TRACK_NARROWPHASE_STATS(...) +#endif // JPH_TRACK_NARROWPHASE_STATS + +JPH_SUPPRESS_WARNING_POP + +#ifdef JPH_TRACK_NARROWPHASE_STATS + +JPH_NAMESPACE_BEGIN + +/// Structure that tracks narrow phase timing information for a particular combination of shapes +class NarrowPhaseStat +{ +public: + /// Trace an individual stat in CSV form. + void ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const; + + /// Trace the collected broadphase stats in CSV form. + /// This report can be used to judge and tweak the efficiency of the broadphase. + static void sReportStats(); + + atomic mNumQueries = 0; + atomic mHitsReported = 0; + atomic mTotalTicks = 0; + atomic mChildTicks = 0; + + static NarrowPhaseStat sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; + static NarrowPhaseStat sCastShape[NumSubShapeTypes][NumSubShapeTypes]; +}; + +/// Object that tracks the start and end of a narrow phase operation +class TrackNarrowPhaseStat +{ +public: + TrackNarrowPhaseStat(NarrowPhaseStat &inStat) : + mStat(inStat), + mParent(sRoot), + mStart(GetProcessorTickCount()) + { + // Make this the new root of the chain + sRoot = this; + } + + ~TrackNarrowPhaseStat() + { + uint64 delta_ticks = GetProcessorTickCount() - mStart; + + // Notify parent of time spent in child + if (mParent != nullptr) + mParent->mStat.mChildTicks += delta_ticks; + + // Increment stats at this level + mStat.mNumQueries++; + mStat.mTotalTicks += delta_ticks; + + // Restore root pointer + JPH_ASSERT(sRoot == this); + sRoot = mParent; + } + + NarrowPhaseStat & mStat; + TrackNarrowPhaseStat * mParent; + uint64 mStart; + + static thread_local TrackNarrowPhaseStat *sRoot; +}; + +/// Object that tracks the start and end of a hit being processed by a collision collector +class TrackNarrowPhaseCollector +{ +public: + TrackNarrowPhaseCollector() : + mStart(GetProcessorTickCount()) + { + } + + ~TrackNarrowPhaseCollector() + { + // Mark time spent in collector as 'child' time for the parent + uint64 delta_ticks = GetProcessorTickCount() - mStart; + if (TrackNarrowPhaseStat::sRoot != nullptr) + TrackNarrowPhaseStat::sRoot->mStat.mChildTicks += delta_ticks; + + // Notify all parents of a hit + for (TrackNarrowPhaseStat *track = TrackNarrowPhaseStat::sRoot; track != nullptr; track = track->mParent) + track->mStat.mHitsReported++; + } + +private: + uint64 mStart; +}; + +JPH_NAMESPACE_END + +#endif // JPH_TRACK_NARROWPHASE_STATS diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h new file mode 100644 index 000000000000..bfb9e6c9a88d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Layer that objects can be in, determines which other objects it can collide with +#ifndef JPH_OBJECT_LAYER_BITS + #define JPH_OBJECT_LAYER_BITS 16 +#endif // JPH_OBJECT_LAYER_BITS +#if JPH_OBJECT_LAYER_BITS == 16 + using ObjectLayer = uint16; +#elif JPH_OBJECT_LAYER_BITS == 32 + using ObjectLayer = uint32; +#else + #error "JPH_OBJECT_LAYER_BITS must be 16 or 32" +#endif + +/// Constant value used to indicate an invalid object layer +static constexpr ObjectLayer cObjectLayerInvalid = ObjectLayer(~ObjectLayer(0U)); + +/// Filter class for object layers +class JPH_EXPORT ObjectLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectLayerFilter() = default; + + /// Function to filter out object layers when doing collision query test (return true to allow testing against objects with this layer) + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer) const + { + return true; + } + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Get a string that describes this filter for stat tracking purposes + virtual String GetDescription() const + { + return "No Description"; + } +#endif // JPH_TRACK_BROADPHASE_STATS +}; + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +class JPH_EXPORT ObjectLayerPairFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectLayerPairFilter() = default; + + /// Returns true if two layers can collide + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] ObjectLayer inLayer2) const + { + return true; + } +}; + +/// Default filter class that uses the pair filter in combination with a specified layer to filter layers +class JPH_EXPORT DefaultObjectLayerFilter : public ObjectLayerFilter +{ +public: + /// Constructor + DefaultObjectLayerFilter(const ObjectLayerPairFilter &inObjectLayerPairFilter, ObjectLayer inLayer) : + mObjectLayerPairFilter(inObjectLayerPairFilter), + mLayer(inLayer) + { + } + + /// Copy constructor + DefaultObjectLayerFilter(const DefaultObjectLayerFilter &inRHS) : + mObjectLayerPairFilter(inRHS.mObjectLayerPairFilter), + mLayer(inRHS.mLayer) + { + } + + // See ObjectLayerFilter::ShouldCollide + virtual bool ShouldCollide(ObjectLayer inLayer) const override + { + return mObjectLayerPairFilter.ShouldCollide(mLayer, inLayer); + } + +private: + const ObjectLayerPairFilter & mObjectLayerPairFilter; + ObjectLayer mLayer; +}; + +/// Allows objects from a specific layer only +class JPH_EXPORT SpecifiedObjectLayerFilter : public ObjectLayerFilter +{ +public: + /// Constructor + explicit SpecifiedObjectLayerFilter(ObjectLayer inLayer) : + mLayer(inLayer) + { + } + + // See ObjectLayerFilter::ShouldCollide + virtual bool ShouldCollide(ObjectLayer inLayer) const override + { + return mLayer == inLayer; + } + +private: + ObjectLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h new file mode 100644 index 000000000000..dc3494c2ee3e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +/// Uses group bits and mask bits. Two layers can collide if Object1.Group & Object2.Mask is non-zero and Object2.Group & Object1.Mask is non-zero. +/// The behavior is similar to that in e.g. Bullet. +/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectVsBroadPhaseLayerFilterMask +class ObjectLayerPairFilterMask : public ObjectLayerPairFilter +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Number of bits for the group and mask bits + static constexpr uint32 cNumBits = JPH_OBJECT_LAYER_BITS / 2; + static constexpr uint32 cMask = (1 << cNumBits) - 1; + + /// Construct an ObjectLayer from a group and mask bits + static ObjectLayer sGetObjectLayer(uint32 inGroup, uint32 inMask = cMask) + { + JPH_ASSERT((inGroup & ~cMask) == 0); + JPH_ASSERT((inMask & ~cMask) == 0); + return ObjectLayer((inGroup & cMask) | (inMask << cNumBits)); + } + + /// Get the group bits from an ObjectLayer + static inline uint32 sGetGroup(ObjectLayer inObjectLayer) + { + return uint32(inObjectLayer) & cMask; + } + + /// Get the mask bits from an ObjectLayer + static inline uint32 sGetMask(ObjectLayer inObjectLayer) + { + return uint32(inObjectLayer) >> cNumBits; + } + + /// Returns true if two layers can collide + virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override + { + return (sGetGroup(inObject1) & sGetMask(inObject2)) != 0 + && (sGetGroup(inObject2) & sGetMask(inObject1)) != 0; + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h new file mode 100644 index 000000000000..cb17f3c5dae0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h @@ -0,0 +1,78 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +/// This implementation uses a table to determine if two layers can collide. +class ObjectLayerPairFilterTable : public ObjectLayerPairFilter +{ +private: + /// Get which bit corresponds to the pair (inLayer1, inLayer2) + uint GetBit(ObjectLayer inLayer1, ObjectLayer inLayer2) const + { + // We store the lower left half only, so swap the inputs when trying to access the top right half + if (inLayer1 > inLayer2) + std::swap(inLayer1, inLayer2); + + JPH_ASSERT(inLayer2 < mNumObjectLayers); + + // Calculate at which bit the entry for this pair resides + // We use the fact that a row always starts at inLayer2 * (inLayer2 + 1) / 2 + // (this is the amount of bits needed to store a table of inLayer2 entries) + return (inLayer2 * (inLayer2 + 1)) / 2 + inLayer1; + } + +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs the table with inNumObjectLayers Layers, initially all layer pairs are disabled + explicit ObjectLayerPairFilterTable(uint inNumObjectLayers) : + mNumObjectLayers(inNumObjectLayers) + { + // By default nothing collides + // For the first layer we only need to store 1 bit, for the second 2 bits, for the third 3 bits, etc. + // We use the formula Sum_i=1^N i = N * (N + 1) / 2 to calculate the size of the table + int table_size = (inNumObjectLayers * (inNumObjectLayers + 1) / 2 + 7) / 8; + mTable.resize(table_size, 0); + } + + /// Get the number of object layers + uint GetNumObjectLayers() const + { + return mNumObjectLayers; + } + + /// Disable collision between two object layers + void DisableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2) + { + uint bit = GetBit(inLayer1, inLayer2); + mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111))); + } + + /// Enable collision between two object layers + void EnableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2) + { + uint bit = GetBit(inLayer1, inLayer2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + + /// Returns true if two layers can collide + virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override + { + // Test if the bit is set for this group pair + uint bit = GetBit(inObject1, inObject2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + +private: + uint mNumObjectLayers; ///< The number of layers that this table supports + Array mTable; ///< The table of bits that indicates which layers collide +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp new file mode 100644 index 000000000000..17a982e74992 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +RefConst PhysicsMaterial::sDefault; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterial) +{ + JPH_ADD_BASE_CLASS(PhysicsMaterial, SerializableObject) +} + +void PhysicsMaterial::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); +} + +void PhysicsMaterial::RestoreBinaryState(StreamIn &inStream) +{ + // RTTI hash is read in sRestoreFromBinaryState +} + +PhysicsMaterial::PhysicsMaterialResult PhysicsMaterial::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &PhysicsMaterial::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h new file mode 100644 index 000000000000..d13c9644b6f8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// This structure describes the surface of (part of) a shape. You should inherit from it to define additional +/// information that is interesting for the simulation. The 2 materials involved in a contact could be used +/// to decide which sound or particle effects to play. +/// +/// If you inherit from this material, don't forget to create a suitable default material in sDefault +class JPH_EXPORT PhysicsMaterial : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterial) + +public: + /// Virtual destructor + virtual ~PhysicsMaterial() override = default; + + /// Default material that is used when a shape has no materials defined + static RefConst sDefault; + + // Properties + virtual const char * GetDebugName() const { return "Unknown"; } + virtual Color GetDebugColor() const { return Color::sGrey; } + + /// Saves the contents of the material in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + using PhysicsMaterialResult = Result>; + + /// Creates a PhysicsMaterial of the correct type and restores its contents from the binary stream inStream. + static PhysicsMaterialResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +using PhysicsMaterialList = Array>; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp new file mode 100644 index 000000000000..02a569822c63 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterialSimple) +{ + JPH_ADD_BASE_CLASS(PhysicsMaterialSimple, PhysicsMaterial) + + JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugName) + JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugColor) +} + +void PhysicsMaterialSimple::SaveBinaryState(StreamOut &inStream) const +{ + PhysicsMaterial::SaveBinaryState(inStream); + + inStream.Write(mDebugName); + inStream.Write(mDebugColor); +} + +void PhysicsMaterialSimple::RestoreBinaryState(StreamIn &inStream) +{ + PhysicsMaterial::RestoreBinaryState(inStream); + + inStream.Read(mDebugName); + inStream.Read(mDebugColor); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h new file mode 100644 index 000000000000..63ffff72d6ef --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Sample implementation of PhysicsMaterial that just holds the needed properties directly +class JPH_EXPORT PhysicsMaterialSimple : public PhysicsMaterial +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterialSimple) + +public: + /// Constructor + PhysicsMaterialSimple() = default; + PhysicsMaterialSimple(const string_view &inName, ColorArg inColor) : mDebugName(inName), mDebugColor(inColor) { } + + // Properties + virtual const char * GetDebugName() const override { return mDebugName.c_str(); } + virtual Color GetDebugColor() const override { return mDebugColor; } + + // See: PhysicsMaterial::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + +protected: + // See: PhysicsMaterial::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + String mDebugName; ///< Name of the material, used for debugging purposes + Color mDebugColor = Color::sGrey; ///< Color of the material, used to render the shapes +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h new file mode 100644 index 000000000000..c0a7ea66a618 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a single ray cast +template +struct RayCastT +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructors + RayCastT() = default; // Allow raycast to be created uninitialized + RayCastT(typename Vec::ArgType inOrigin, Vec3Arg inDirection) : mOrigin(inOrigin), mDirection(inDirection) { } + RayCastT(const RayCastT &) = default; + + /// Transform this ray using inTransform + RayCastType Transformed(typename Mat::ArgType inTransform) const + { + Vec ray_origin = inTransform * mOrigin; + Vec3 ray_direction(inTransform * (mOrigin + mDirection) - ray_origin); + return { ray_origin, ray_direction }; + } + + /// Translate ray using inTranslation + RayCastType Translated(typename Vec::ArgType inTranslation) const + { + return { inTranslation + mOrigin, mDirection }; + } + + /// Get point with fraction inFraction on ray (0 = start of ray, 1 = end of ray) + inline Vec GetPointOnRay(float inFraction) const + { + return mOrigin + inFraction * mDirection; + } + + Vec mOrigin; ///< Origin of the ray + Vec3 mDirection; ///< Direction and length of the ray (anything beyond this length will not be reported as a hit) +}; + +struct RayCast : public RayCastT +{ + using RayCastT::RayCastT; +}; + +struct RRayCast : public RayCastT +{ + using RayCastT::RayCastT; + + /// Convert from RayCast, converts single to double precision + explicit RRayCast(const RayCast &inRay) : + RRayCast(RVec3(inRay.mOrigin), inRay.mDirection) + { + } + + /// Convert to RayCast, which implies casting from double precision to single precision + explicit operator RayCast() const + { + return RayCast(Vec3(mOrigin), mDirection); + } +}; + +/// Settings to be passed with a ray cast +class RayCastSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Set the backfacing mode for all shapes + void SetBackFaceMode(EBackFaceMode inMode) { mBackFaceModeTriangles = mBackFaceModeConvex = inMode; } + + /// How backfacing triangles should be treated (should we report back facing hits for triangle based shapes, e.g. MeshShape/HeightFieldShape?) + EBackFaceMode mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces; + + /// How backfacing convex objects should be treated (should we report back facing hits for convex shapes?) + EBackFaceMode mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + + /// If convex shapes should be treated as solid. When true, a ray starting inside a convex shape will generate a hit at fraction 0. + bool mTreatConvexAsSolid = true; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp new file mode 100644 index 000000000000..dc7ea4828575 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp @@ -0,0 +1,312 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(BoxShapeSettings) +{ + JPH_ADD_BASE_CLASS(BoxShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(BoxShapeSettings, mHalfExtent) + JPH_ADD_ATTRIBUTE(BoxShapeSettings, mConvexRadius) +} + +static const Vec3 sUnitBoxTriangles[] = { + Vec3(-1, 1, -1), Vec3(-1, 1, 1), Vec3(1, 1, 1), + Vec3(-1, 1, -1), Vec3(1, 1, 1), Vec3(1, 1, -1), + Vec3(-1, -1, -1), Vec3(1, -1, -1), Vec3(1, -1, 1), + Vec3(-1, -1, -1), Vec3(1, -1, 1), Vec3(-1, -1, 1), + Vec3(-1, 1, -1), Vec3(-1, -1, -1), Vec3(-1, -1, 1), + Vec3(-1, 1, -1), Vec3(-1, -1, 1), Vec3(-1, 1, 1), + Vec3(1, 1, 1), Vec3(1, -1, 1), Vec3(1, -1, -1), + Vec3(1, 1, 1), Vec3(1, -1, -1), Vec3(1, 1, -1), + Vec3(-1, 1, 1), Vec3(-1, -1, 1), Vec3(1, -1, 1), + Vec3(-1, 1, 1), Vec3(1, -1, 1), Vec3(1, 1, 1), + Vec3(-1, 1, -1), Vec3(1, 1, -1), Vec3(1, -1, -1), + Vec3(-1, 1, -1), Vec3(1, -1, -1), Vec3(-1, -1, -1) +}; + +ShapeSettings::ShapeResult BoxShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new BoxShape(*this, mCachedResult); + return mCachedResult; +} + +BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Box, inSettings, outResult), + mHalfExtent(inSettings.mHalfExtent), + mConvexRadius(inSettings.mConvexRadius) +{ + // Check convex radius + if (inSettings.mConvexRadius < 0.0f + || inSettings.mHalfExtent.ReduceMin() <= inSettings.mConvexRadius) + { + outResult.SetError("Invalid convex radius"); + return; + } + + // Result is valid + outResult.Set(this); +} + +class BoxShape::Box final : public Support +{ +public: + Box(const AABox &inBox, float inConvexRadius) : + mBox(inBox), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(Box) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(Box))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return mBox.GetSupport(inDirection); + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + AABox mBox; + float mConvexRadius; +}; + +const ConvexShape::Support *BoxShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + // Scale our half extents + Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + { + // Make box out of our half extents + AABox box = AABox(-scaled_half_extent, scaled_half_extent); + JPH_ASSERT(box.IsValid()); + return new (&inBuffer) Box(box, 0.0f); + } + + case ESupportMode::ExcludeConvexRadius: + { + // Reduce the box by our convex radius + float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + Vec3 convex_radius3 = Vec3::sReplicate(convex_radius); + Vec3 reduced_half_extent = scaled_half_extent - convex_radius3; + AABox box = AABox(-reduced_half_extent, reduced_half_extent); + JPH_ASSERT(box.IsValid()); + return new (&inBuffer) Box(box, convex_radius); + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void BoxShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent; + AABox box(-scaled_half_extent, scaled_half_extent); + box.GetSupportingFace(inDirection, outVertices); + + // Transform to world space + for (Vec3 &v : outVertices) + v = inCenterOfMassTransform * v; +} + +MassProperties BoxShape::GetMassProperties() const +{ + MassProperties p; + p.SetMassAndInertiaOfSolidBox(2.0f * mHalfExtent, GetDensity()); + return p; +} + +Vec3 BoxShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Get component that is closest to the surface of the box + int index = (inLocalSurfacePosition.Abs() - mHalfExtent).Abs().GetLowestComponentIndex(); + + // Calculate normal + Vec3 normal = Vec3::sZero(); + normal.SetComponent(index, inLocalSurfacePosition[index] > 0.0f? 1.0f : -1.0f); + return normal; +} + +#ifdef JPH_DEBUG_RENDERER +void BoxShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawBox(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), GetLocalBounds(), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool BoxShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test hit against box + float fraction = max(RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent), 0.0f); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void BoxShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float min_fraction, max_fraction; + RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent, min_fraction, max_fraction); + if (min_fraction <= max_fraction // Ray should intersect + && max_fraction >= 0.0f // End of ray should be inside box + && min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = max(0.0f, min_fraction); + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + && max_fraction < ioCollector.GetEarlyOutFraction()) + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + if (Vec3::sLessOrEqual(inPoint.Abs(), mHalfExtent).TestAllXYZTrue()) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + Vec3 half_extent = inScale.Abs() * mHalfExtent; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + // Convert to local space + Vec3 local_pos = inverse_transform * v.GetPosition(); + + // Clamp point to inside box + Vec3 clamped_point = Vec3::sMax(Vec3::sMin(local_pos, half_extent), -half_extent); + + // Test if point was inside + if (clamped_point == local_pos) + { + // Calculate closest distance to surface + Vec3 delta = half_extent - local_pos.Abs(); + int index = delta.GetLowestComponentIndex(); + float penetration = delta[index]; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact point and normal + Vec3 possible_normals[] = { Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ() }; + Vec3 normal = local_pos.GetSign() * possible_normals[index]; + Vec3 point = normal * half_extent; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } + else + { + // Calculate normal + Vec3 normal = local_pos - clamped_point; + float normal_length = normal.Length(); + + // Penetration will be negative since we're not penetrating + float penetration = -normal_length; + if (v.UpdatePenetration(penetration)) + { + normal /= normal_length; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } + } +} + +void BoxShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, Mat44::sScale(mHalfExtent), sUnitBoxTriangles, sizeof(sUnitBoxTriangles) / sizeof(Vec3), GetMaterial()); +} + +int BoxShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void BoxShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mHalfExtent); + inStream.Write(mConvexRadius); +} + +void BoxShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mHalfExtent); + inStream.Read(mConvexRadius); +} + +void BoxShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Box); + f.mConstruct = []() -> Shape * { return new BoxShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h new file mode 100644 index 000000000000..030dd2db803e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h @@ -0,0 +1,115 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a BoxShape +class JPH_EXPORT BoxShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, BoxShapeSettings) + +public: + /// Default constructor for deserialization + BoxShapeSettings() = default; + + /// Create a box with half edge length inHalfExtent and convex radius inConvexRadius. + /// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius). + BoxShapeSettings(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mHalfExtent = Vec3::sZero(); ///< Half the size of the box (including convex radius) + float mConvexRadius = 0.0f; +}; + +/// A box, centered around the origin +class JPH_EXPORT BoxShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + BoxShape() : ConvexShape(EShapeSubType::Box) { } + BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a box with half edge length inHalfExtent and convex radius inConvexRadius. + /// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius). + BoxShape(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Box, inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); JPH_ASSERT(inHalfExtent.ReduceMin() >= inConvexRadius); } + + /// Get half extent of box + Vec3 GetHalfExtent() const { return mHalfExtent; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return AABox(-mHalfExtent, mHalfExtent); } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mHalfExtent.ReduceMin(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 12); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return GetLocalBounds().GetVolume(); } + + /// Get the convex radius of this box + float GetConvexRadius() const { return mConvexRadius; } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class Box; + + Vec3 mHalfExtent = Vec3::sZero(); ///< Half the size of the box (including convex radius) + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp new file mode 100644 index 000000000000..48a6cba3e623 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp @@ -0,0 +1,438 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CapsuleShapeSettings) +{ + JPH_ADD_BASE_CLASS(CapsuleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mRadius) + JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mHalfHeightOfCylinder) +} + +static const int cCapsuleDetailLevel = 2; + +static const StaticArray sCapsuleTopTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, cCapsuleDetailLevel); + return verts; +}(); + +static const StaticArray sCapsuleMiddleTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateUnitOpenCylinder(verts, cCapsuleDetailLevel); + return verts; +}(); + +static const StaticArray sCapsuleBottomTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, cCapsuleDetailLevel); + return verts; +}(); + +ShapeSettings::ShapeResult CapsuleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + { + Ref shape; + if (IsValid() && IsSphere()) + { + // If the capsule has no height, use a sphere instead + shape = new SphereShape(mRadius, mMaterial); + mCachedResult.Set(shape); + } + else + shape = new CapsuleShape(*this, mCachedResult); + } + return mCachedResult; +} + +CapsuleShape::CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Capsule, inSettings, outResult), + mRadius(inSettings.mRadius), + mHalfHeightOfCylinder(inSettings.mHalfHeightOfCylinder) +{ + if (inSettings.mHalfHeightOfCylinder <= 0.0f) + { + outResult.SetError("Invalid height"); + return; + } + + if (inSettings.mRadius <= 0.0f) + { + outResult.SetError("Invalid radius"); + return; + } + + outResult.Set(this); +} + +class CapsuleShape::CapsuleNoConvex final : public Support +{ +public: + CapsuleNoConvex(Vec3Arg inHalfHeightOfCylinder, float inConvexRadius) : + mHalfHeightOfCylinder(inHalfHeightOfCylinder), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(CapsuleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(CapsuleNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + if (inDirection.GetY() > 0) + return mHalfHeightOfCylinder; + else + return -mHalfHeightOfCylinder; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + Vec3 mHalfHeightOfCylinder; + float mConvexRadius; +}; + +class CapsuleShape::CapsuleWithConvex final : public Support +{ +public: + CapsuleWithConvex(Vec3Arg inHalfHeightOfCylinder, float inRadius) : + mHalfHeightOfCylinder(inHalfHeightOfCylinder), + mRadius(inRadius) + { + static_assert(sizeof(CapsuleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(CapsuleWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + float len = inDirection.Length(); + Vec3 radius = len > 0.0f? inDirection * (mRadius / len) : Vec3::sZero(); + + if (inDirection.GetY() > 0) + return radius + mHalfHeightOfCylinder; + else + return radius - mHalfHeightOfCylinder; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + Vec3 mHalfHeightOfCylinder; + float mRadius; +}; + +const ConvexShape::Support *CapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled capsule + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0); + float scaled_radius = scale * mRadius; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) CapsuleWithConvex(scaled_half_height_of_cylinder, scaled_radius); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) CapsuleNoConvex(scaled_half_height_of_cylinder, scaled_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +void CapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Get direction in horizontal plane + Vec3 direction = inDirection; + direction.SetComponent(1, 0.0f); + + // Check zero vector, in this case we're hitting from top/bottom so there's no supporting face + float len = direction.Length(); + if (len == 0.0f) + return; + + // Get scaled capsule + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0); + float scaled_radius = scale * mRadius; + + // Get support point for top and bottom sphere in the opposite of 'direction' (including convex radius) + Vec3 support = (scaled_radius / len) * direction; + Vec3 support_top = scaled_half_height_of_cylinder - support; + Vec3 support_bottom = -scaled_half_height_of_cylinder - support; + + // Get projection on inDirection + // Note that inDirection is not normalized, so we need to divide by inDirection.Length() to get the actual projection + // We've multiplied both sides of the if below with inDirection.Length() + float proj_top = support_top.Dot(inDirection); + float proj_bottom = support_bottom.Dot(inDirection); + + // If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point + if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * inDirection.Length()) + { + outVertices.push_back(inCenterOfMassTransform * support_top); + outVertices.push_back(inCenterOfMassTransform * support_bottom); + } +} + +MassProperties CapsuleShape::GetMassProperties() const +{ + MassProperties p; + + float density = GetDensity(); + + // Calculate inertia and mass according to: + // https://www.gamedev.net/resources/_/technical/math-and-physics/capsule-inertia-tensor-r3856 + // Note that there is an error in eq 14, H^2/2 should be H^2/4 in Ixx and Izz, eq 12 does contain the correct value + float radius_sq = Square(mRadius); + float height = 2.0f * mHalfHeightOfCylinder; + float cylinder_mass = JPH_PI * height * radius_sq * density; + float hemisphere_mass = (2.0f * JPH_PI / 3.0f) * radius_sq * mRadius * density; + + // From cylinder + float height_sq = Square(height); + float inertia_y = radius_sq * cylinder_mass * 0.5f; + float inertia_xz = inertia_y * 0.5f + cylinder_mass * height_sq / 12.0f; + + // From hemispheres + float temp = hemisphere_mass * 4.0f * radius_sq / 5.0f; + inertia_y += temp; + inertia_xz += temp + hemisphere_mass * (0.5f * height_sq + (3.0f / 4.0f) * height * mRadius); + + // Mass is cylinder + hemispheres + p.mMass = cylinder_mass + hemisphere_mass * 2.0f; + + // Set inertia + p.mInertia = Mat44::sScale(Vec3(inertia_xz, inertia_y, inertia_xz)); + + return p; +} + +Vec3 CapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + if (inLocalSurfacePosition.GetY() > mHalfHeightOfCylinder) + return (inLocalSurfacePosition - Vec3(0, mHalfHeightOfCylinder, 0)).Normalized(); + else if (inLocalSurfacePosition.GetY() < -mHalfHeightOfCylinder) + return (inLocalSurfacePosition - Vec3(0, -mHalfHeightOfCylinder, 0)).Normalized(); + else + return Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX()); +} + +AABox CapsuleShape::GetLocalBounds() const +{ + Vec3 extent = Vec3::sReplicate(mRadius) + Vec3(0, mHalfHeightOfCylinder, 0); + return AABox(-extent, extent); +} + +AABox CapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 extent = Vec3::sReplicate(scale * mRadius); + Vec3 height = Vec3(0, scale * mHalfHeightOfCylinder, 0); + Vec3 p1 = inCenterOfMassTransform * -height; + Vec3 p2 = inCenterOfMassTransform * height; + return AABox(Vec3::sMin(p1, p2) - extent, Vec3::sMax(p1, p2) + extent); +} + +#ifdef JPH_DEBUG_RENDERER +void CapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawCapsule(inCenterOfMassTransform * Mat44::sScale(inScale.Abs().GetX()), mHalfHeightOfCylinder, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool CapsuleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test ray against capsule + float fraction = RayCapsule(inRay.mOrigin, inRay.mDirection, mHalfHeightOfCylinder, mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float radius_sq = Square(mRadius); + + // Get vertical distance to the top/bottom sphere centers + float delta_y = abs(inPoint.GetY()) - mHalfHeightOfCylinder; + + // Get distance in horizontal plane + float xz_sq = Square(inPoint.GetX()) + Square(inPoint.GetZ()); + + // Check if the point is in one of the two spheres + bool in_sphere = xz_sq + Square(delta_y) <= radius_sq; + + // Check if the point is in the cylinder in the middle + bool in_cylinder = delta_y <= 0.0f && xz_sq <= radius_sq; + + if (in_sphere || in_cylinder) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled capsule + float scale = abs(inScale.GetX()); + float half_height_of_cylinder = scale * mHalfHeightOfCylinder; + float radius = scale * mRadius; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + // Calculate penetration + Vec3 local_pos = inverse_transform * v.GetPosition(); + if (abs(local_pos.GetY()) <= half_height_of_cylinder) + { + // Near cylinder + Vec3 normal = local_pos; + normal.SetY(0.0f); + float normal_length = normal.Length(); + float penetration = radius - normal_length; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact point and normal + normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX(); + Vec3 point = radius * normal; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } + else + { + // Near cap + Vec3 center = Vec3(0, Sign(local_pos.GetY()) * half_height_of_cylinder, 0); + Vec3 delta = local_pos - center; + float distance = delta.Length(); + float penetration = radius - distance; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact point and normal + Vec3 normal = delta / distance; + Vec3 point = center + radius * normal; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } + } +} + +void CapsuleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + + GetTrianglesContextMultiVertexList *context = new (&ioContext) GetTrianglesContextMultiVertexList(false, GetMaterial()); + + Mat44 world_matrix = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale); + + Mat44 top_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, mHalfHeightOfCylinder, 0, 1)); + context->AddPart(top_matrix, sCapsuleTopTriangles.data(), sCapsuleTopTriangles.size()); + + Mat44 middle_matrix = world_matrix * Mat44::sScale(Vec3(mRadius, mHalfHeightOfCylinder, mRadius)); + context->AddPart(middle_matrix, sCapsuleMiddleTriangles.data(), sCapsuleMiddleTriangles.size()); + + Mat44 bottom_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, -mHalfHeightOfCylinder, 0, 1)); + context->AddPart(bottom_matrix, sCapsuleBottomTriangles.data(), sCapsuleBottomTriangles.size()); +} + +int CapsuleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextMultiVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void CapsuleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mRadius); + inStream.Write(mHalfHeightOfCylinder); +} + +void CapsuleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mRadius); + inStream.Read(mHalfHeightOfCylinder); +} + +bool CapsuleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +Vec3 CapsuleShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()); +} + +void CapsuleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Capsule); + f.mConstruct = []() -> Shape * { return new CapsuleShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h new file mode 100644 index 000000000000..77af7eceb612 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h @@ -0,0 +1,129 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a CapsuleShape +class JPH_EXPORT CapsuleShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CapsuleShapeSettings) + +public: + /// Default constructor for deserialization + CapsuleShapeSettings() = default; + + /// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0) + CapsuleShapeSettings(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { } + + /// Check if this is a valid capsule shape + bool IsValid() const { return mRadius > 0.0f && mHalfHeightOfCylinder >= 0.0f; } + + /// Checks if the settings of this capsule make this shape a sphere + bool IsSphere() const { return mHalfHeightOfCylinder == 0.0f; } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mRadius = 0.0f; + float mHalfHeightOfCylinder = 0.0f; +}; + +/// A capsule, implemented as a line segment with convex radius +class JPH_EXPORT CapsuleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CapsuleShape() : ConvexShape(EShapeSubType::Capsule) { } + CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0) + CapsuleShape(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Capsule, inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { JPH_ASSERT(inHalfHeightOfCylinder > 0.0f); JPH_ASSERT(inRadius > 0.0f); } + + /// Radius of the cylinder + float GetRadius() const { return mRadius; } + + /// Get half of the height of the cylinder + float GetHalfHeightOfCylinder() const { return mHalfHeightOfCylinder; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + using ConvexShape::CastRay; + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 4.0f / 3.0f * JPH_PI * Cubed(mRadius) + 2.0f * JPH_PI * mHalfHeightOfCylinder * Square(mRadius); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Classes for GetSupportFunction + class CapsuleNoConvex; + class CapsuleWithConvex; + + float mRadius = 0.0f; + float mHalfHeightOfCylinder = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp new file mode 100644 index 000000000000..f5fe04ecf213 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp @@ -0,0 +1,428 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(CompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(CompoundShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(CompoundShapeSettings, mSubShapes) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CompoundShapeSettings::SubShapeSettings) +{ + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mShape) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mPosition) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mRotation) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mUserData) +} + +void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData) +{ + // Add shape + SubShapeSettings shape; + shape.mPosition = inPosition; + shape.mRotation = inRotation; + shape.mShape = inShape; + shape.mUserData = inUserData; + mSubShapes.push_back(shape); +} + +void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData) +{ + // Add shape + SubShapeSettings shape; + shape.mPosition = inPosition; + shape.mRotation = inRotation; + shape.mShapePtr = inShape; + shape.mUserData = inUserData; + mSubShapes.push_back(shape); +} + +bool CompoundShape::MustBeStatic() const +{ + for (const SubShape &shape : mSubShapes) + if (shape.mShape->MustBeStatic()) + return true; + + return false; +} + +MassProperties CompoundShape::GetMassProperties() const +{ + MassProperties p; + + // Calculate mass and inertia + p.mMass = 0.0f; + p.mInertia = Mat44::sZero(); + for (const SubShape &shape : mSubShapes) + { + // Rotate and translate inertia of child into place + MassProperties child = shape.mShape->GetMassProperties(); + child.Rotate(Mat44::sRotation(shape.GetRotation())); + child.Translate(shape.GetPositionCOM()); + + // Accumulate mass and inertia + p.mMass += child.mMass; + p.mInertia += child.mInertia; + } + + // Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change + p.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); + + return p; +} + +AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + if (mSubShapes.size() <= 10) + { + AABox bounds; + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale); + bounds.Encapsulate(shape.mShape->GetWorldSpaceBounds(transform, shape.TransformScale(inScale))); + } + return bounds; + } + else + { + // If there are too many shapes, use the base class function (this will result in a slightly wider bounding box) + return Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale); + } +} + +uint CompoundShape::GetSubShapeIDBitsRecursive() const +{ + // Add max of child bits to our bits + uint child_bits = 0; + for (const SubShape &shape : mSubShapes) + child_bits = max(child_bits, shape.mShape->GetSubShapeIDBitsRecursive()); + return child_bits + GetSubShapeIDBits(); +} + +const PhysicsMaterial *CompoundShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Pass call on + return mSubShapes[index].mShape->GetMaterial(remainder); +} + +const Shape *CompoundShape::GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + if (index >= mSubShapes.size()) + { + // No longer valid index + outRemainder = SubShapeID(); + return nullptr; + } + + // Pass call on + return mSubShapes[index].mShape->GetLeafShape(remainder, outRemainder); +} + +uint64 CompoundShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + if (index >= mSubShapes.size()) + return 0; // No longer valid index + + // Pass call on + return mSubShapes[index].mShape->GetSubShapeUserData(remainder); +} + +TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // Get the sub shape + const SubShape &sub_shape = mSubShapes[GetSubShapeIndexFromID(inSubShapeID, outRemainder)]; + + // Calculate transform for sub shape + Vec3 position = inPositionCOM + inRotation * (inScale * sub_shape.GetPositionCOM()); + Quat rotation = inRotation * sub_shape.GetRotation(); + Vec3 scale = sub_shape.TransformScale(inScale); + + // Return transformed shape + TransformedShape ts(RVec3(position), rotation, sub_shape.mShape, BodyID()); + ts.SetShapeScale(scale); + return ts; +} + +Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Transform surface position to local space and pass call on + const SubShape &shape = mSubShapes[index]; + Mat44 transform = Mat44::sInverseRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()); + Vec3 normal = shape.mShape->GetSurfaceNormal(remainder, transform * inLocalSurfacePosition); + + // Transform normal to this shape's space + return transform.Multiply3x3Transposed(normal); +} + +void CompoundShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Apply transform and pass on to sub shape + const SubShape &shape = mSubShapes[index]; + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->GetSupportingFace(remainder, transform.Multiply3x3Transposed(inDirection), shape.TransformScale(inScale), inCenterOfMassTransform * transform, outVertices); +} + +void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + outTotalVolume = 0.0f; + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + + for (const SubShape &shape : mSubShapes) + { + // Get center of mass transform of child + Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale); + + // Recurse to child + float total_volume, submerged_volume; + Vec3 center_of_buoyancy; + shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + // Accumulate volumes + outTotalVolume += total_volume; + outSubmergedVolume += submerged_volume; + + // The center of buoyancy is the weighted average of the center of buoyancy of our child shapes + outCenterOfBuoyancy += submerged_volume * center_of_buoyancy; + } + + if (outSubmergedVolume > 0.0f) + outCenterOfBuoyancy /= outSubmergedVolume; + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void CompoundShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->Draw(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe); + } +} + +void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inDrawSupportDirection); + } +} + +void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale)); + } +} +#endif // JPH_DEBUG_RENDERER + +void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), inVertices, inNumVertices, inCollidingShapeIndex); + } +} + +void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + for (const SubShape &shape : mSubShapes) + shape.mShape->TransformShape(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioCollector); +} + +void CompoundShape::sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + // Fetch compound shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Compound); + const CompoundShape *compound = static_cast(inShapeCast.mShape); + + // Number of sub shapes + int n = (int)compound->mSubShapes.size(); + + // Determine amount of bits for sub shape + uint sub_shape_bits = compound->GetSubShapeIDBits(); + + // Recurse to sub shapes + for (int i = 0; i < n; ++i) + { + const SubShape &shape = compound->mSubShapes[i]; + + // Create ID for sub shape + SubShapeIDCreator shape1_sub_shape_id = inSubShapeIDCreator1.PushID(i, sub_shape_bits); + + // Transform the shape cast and update the shape + Mat44 transform = inShapeCast.mCenterOfMassStart * shape.GetLocalTransformNoScale(inShapeCast.mScale); + Vec3 scale = shape.TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape.mShape, scale, transform, inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector); + + if (ioCollector.ShouldEarlyOut()) + break; + } +} + +void CompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mInnerRadius); + + // Write sub shapes + inStream.Write(mSubShapes, [](const SubShape &inElement, StreamOut &inS) { + inS.Write(inElement.mUserData); + inS.Write(inElement.mPositionCOM); + inS.Write(inElement.mRotation); + }); +} + +void CompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mInnerRadius); + + // Read sub shapes + inStream.Read(mSubShapes, [](StreamIn &inS, SubShape &outElement) { + inS.Read(outElement.mUserData); + inS.Read(outElement.mPositionCOM); + inS.Read(outElement.mRotation); + outElement.mIsRotationIdentity = outElement.mRotation == Float3(0, 0, 0); + }); +} + +void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const +{ + outSubShapes.clear(); + outSubShapes.reserve(mSubShapes.size()); + for (const SubShape &shape : mSubShapes) + outSubShapes.push_back(shape.mShape); +} + +void CompoundShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) +{ + JPH_ASSERT(mSubShapes.size() == inNumShapes); + for (uint i = 0; i < inNumShapes; ++i) + mSubShapes[i].mShape = inSubShapes[i]; +} + +Shape::Stats CompoundShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + // Get own stats + Stats stats = Shape::GetStatsRecursive(ioVisitedShapes); + + // Add child stats + for (const SubShape &shape : mSubShapes) + { + Stats child_stats = shape.mShape->GetStatsRecursive(ioVisitedShapes); + stats.mSizeBytes += child_stats.mSizeBytes; + stats.mNumTriangles += child_stats.mNumTriangles; + } + + return stats; +} + +float CompoundShape::GetVolume() const +{ + float volume = 0.0f; + for (const SubShape &shape : mSubShapes) + volume += shape.mShape->GetVolume(); + return volume; +} + +bool CompoundShape::IsValidScale(Vec3Arg inScale) const +{ + if (!Shape::IsValidScale(inScale)) + return false; + + for (const SubShape &shape : mSubShapes) + { + // Test if the scale is non-uniform and the shape is rotated + if (!shape.IsValidScale(inScale)) + return false; + + // Test the child shape + if (!shape.mShape->IsValidScale(shape.TransformScale(inScale))) + return false; + } + + return true; +} + +Vec3 CompoundShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + if (CompoundShape::IsValidScale(scale)) + return scale; + + Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs()); + Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale; + if (CompoundShape::IsValidScale(uniform_scale)) + return uniform_scale; + + return Sign(scale.GetX()) * abs_uniform_scale; +} + +void CompoundShape::sRegister() +{ + for (EShapeSubType s1 : sCompoundSubShapeTypes) + for (EShapeSubType s2 : sAllSubShapeTypes) + CollisionDispatch::sRegisterCastShape(s1, s2, sCastCompoundVsShape); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h new file mode 100644 index 000000000000..0bbd7c4d7a07 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h @@ -0,0 +1,350 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; +class OrientedBox; + +/// Base class settings to construct a compound shape +class JPH_EXPORT CompoundShapeSettings : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, CompoundShapeSettings) + +public: + /// Constructor. Use AddShape to add the parts. + CompoundShapeSettings() = default; + + /// Add a shape to the compound. + void AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData = 0); + + /// Add a shape to the compound. Variant that uses a concrete shape, which means this object cannot be serialized. + void AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0); + + struct SubShapeSettings + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SubShapeSettings) + + RefConst mShape; ///< Sub shape (either this or mShapePtr needs to be filled up) + RefConst mShapePtr; ///< Sub shape (either this or mShape needs to be filled up) + Vec3 mPosition; ///< Position of the sub shape + Quat mRotation; ///< Rotation of the sub shape + uint32 mUserData = 0; ///< User data value (can be used by the application for any purpose) + }; + + using SubShapes = Array; + + SubShapes mSubShapes; +}; + +/// Base class for a compound shape +class JPH_EXPORT CompoundShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit CompoundShape(EShapeSubType inSubType) : Shape(EShapeType::Compound, inSubType) { } + CompoundShape(EShapeSubType inSubType, const ShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Compound, inSubType, inSettings, outResult) { } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override; + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return mLocalBounds; } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetLeafShape + virtual const Shape * GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const override; + + // See Shape::GetSubShapeUserData + virtual uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + /// Get which sub shape's bounding boxes overlap with an axis aligned box + /// @param inBox The axis aligned box to test against (relative to the center of mass of this shape) + /// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect + /// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices) + /// @return How many indices were placed in outSubShapeIndices + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0; + + /// Get which sub shape's bounding boxes overlap with an axis aligned box + /// @param inBox The axis aligned box to test against (relative to the center of mass of this shape) + /// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect + /// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices) + /// @return How many indices were placed in outSubShapeIndices + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0; + + struct SubShape + { + /// Initialize sub shape from sub shape settings + /// @param inSettings Settings object + /// @param outResult Result object, only used in case of error + /// @return True on success, false on failure + bool FromSettings(const CompoundShapeSettings::SubShapeSettings &inSettings, ShapeResult &outResult) + { + if (inSettings.mShapePtr != nullptr) + { + // Use provided shape + mShape = inSettings.mShapePtr; + } + else + { + // Create child shape + ShapeResult child_result = inSettings.mShape->Create(); + if (!child_result.IsValid()) + { + outResult = child_result; + return false; + } + mShape = child_result.Get(); + } + + // Copy user data + mUserData = inSettings.mUserData; + + SetTransform(inSettings.mPosition, inSettings.mRotation, Vec3::sZero() /* Center of mass not yet calculated */); + return true; + } + + /// Update the transform of this sub shape + /// @param inPosition New position + /// @param inRotation New orientation + /// @param inCenterOfMass The center of mass of the compound shape + JPH_INLINE void SetTransform(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inCenterOfMass) + { + SetPositionCOM(inPosition - inCenterOfMass + inRotation * mShape->GetCenterOfMass()); + + mIsRotationIdentity = inRotation.IsClose(Quat::sIdentity()) || inRotation.IsClose(-Quat::sIdentity()); + SetRotation(mIsRotationIdentity? Quat::sIdentity() : inRotation); + } + + /// Get the local transform for this shape given the scale of the child shape + /// The total transform of the child shape will be GetLocalTransformNoScale(inScale) * Mat44::sScaling(TransformScale(inScale)) + /// @param inScale The scale of the child shape (in local space of this shape) + JPH_INLINE Mat44 GetLocalTransformNoScale(Vec3Arg inScale) const + { + JPH_ASSERT(IsValidScale(inScale)); + return Mat44::sRotationTranslation(GetRotation(), inScale * GetPositionCOM()); + } + + /// Test if inScale is valid for this sub shape + inline bool IsValidScale(Vec3Arg inScale) const + { + // We can always handle uniform scale or identity rotations + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return true; + + return ScaleHelpers::CanScaleBeRotated(GetRotation(), inScale); + } + + /// Transform the scale to the local space of the child shape + inline Vec3 TransformScale(Vec3Arg inScale) const + { + // We don't need to transform uniform scale or if the rotation is identity + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return inScale; + + return ScaleHelpers::RotateScale(GetRotation(), inScale); + } + + /// Compress the center of mass position + JPH_INLINE void SetPositionCOM(Vec3Arg inPositionCOM) + { + inPositionCOM.StoreFloat3(&mPositionCOM); + } + + /// Uncompress the center of mass position + JPH_INLINE Vec3 GetPositionCOM() const + { + return Vec3::sLoadFloat3Unsafe(mPositionCOM); + } + + /// Compress the rotation + JPH_INLINE void SetRotation(QuatArg inRotation) + { + inRotation.StoreFloat3(&mRotation); + } + + /// Uncompress the rotation + JPH_INLINE Quat GetRotation() const + { + return mIsRotationIdentity? Quat::sIdentity() : Quat::sLoadFloat3Unsafe(mRotation); + } + + RefConst mShape; + Float3 mPositionCOM; ///< Note: Position of center of mass of sub shape! + Float3 mRotation; ///< Note: X, Y, Z of rotation quaternion - note we read 4 bytes beyond this so make sure there's something there + uint32 mUserData; ///< User data value (put here because it falls in padding bytes) + bool mIsRotationIdentity; ///< If mRotation is close to identity (put here because it falls in padding bytes) + // 3 padding bytes left + }; + + static_assert(sizeof(SubShape) == (JPH_CPU_ADDRESS_BITS == 64? 40 : 36), "Compiler added unexpected padding"); + + using SubShapes = Array; + + /// Access to the sub shapes of this compound + const SubShapes & GetSubShapes() const { return mSubShapes; } + + /// Get the total number of sub shapes + uint GetNumSubShapes() const { return uint(mSubShapes.size()); } + + /// Access to a particular sub shape + const SubShape & GetSubShape(uint inIdx) const { return mSubShapes[inIdx]; } + + /// Get the user data associated with a shape in this compound + uint32 GetCompoundUserData(uint inIdx) const { return mSubShapes[inIdx].mUserData; } + + /// Set the user data associated with a shape in this compound + void SetCompoundUserData(uint inIdx, uint32 inUserData) { mSubShapes[inIdx].mUserData = inUserData; } + + /// Check if a sub shape ID is still valid for this shape + /// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape + /// @return True if the ID is valid, false if not + inline bool IsSubShapeIDValid(SubShapeID inSubShapeID) const + { + SubShapeID remainder; + return inSubShapeID.PopID(GetSubShapeIDBits(), remainder) < mSubShapes.size(); + } + + /// Convert SubShapeID to sub shape index + /// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape + /// @param outRemainder This is the sub shape ID for the sub shape of the compound after popping off the index + /// @return The index of the sub shape of this compound + inline uint32 GetSubShapeIndexFromID(SubShapeID inSubShapeID, SubShapeID &outRemainder) const + { + uint32 idx = inSubShapeID.PopID(GetSubShapeIDBits(), outRemainder); + JPH_ASSERT(idx < mSubShapes.size(), "Invalid SubShapeID"); + return idx; + } + + /// @brief Convert a sub shape index to a sub shape ID + /// @param inIdx Index of the sub shape of this compound + /// @param inParentSubShapeID Parent SubShapeID (describing the path to the compound shape) + /// @return A sub shape ID creator that contains the full path to the sub shape with index inIdx + inline SubShapeIDCreator GetSubShapeIDFromIndex(int inIdx, const SubShapeIDCreator &inParentSubShapeID) const + { + return inParentSubShapeID.PushID(inIdx, GetSubShapeIDBits()); + } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveSubShapeState(ShapeList &outSubShapes) const override; + virtual void RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override; + + // See Shape::GetStatsRecursive + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override; + + // See Shape::GetVolume + virtual float GetVolume() const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + + // Visitors for collision detection + struct CastRayVisitor; + struct CastRayVisitorCollector; + struct CollidePointVisitor; + struct CastShapeVisitor; + struct CollectTransformedShapesVisitor; + struct CollideCompoundVsShapeVisitor; + struct CollideShapeVsCompoundVisitor; + template struct GetIntersectingSubShapesVisitor; + + /// Determine amount of bits needed to encode sub shape id + inline uint GetSubShapeIDBits() const + { + // Ensure we have enough bits to encode our shape [0, n - 1] + uint32 n = uint32(mSubShapes.size()) - 1; + return 32 - CountLeadingZeros(n); + } + + /// Determine the inner radius of this shape + inline void CalculateInnerRadius() + { + mInnerRadius = FLT_MAX; + for (const SubShape &s : mSubShapes) + mInnerRadius = min(mInnerRadius, s.mShape->GetInnerRadius()); + } + + Vec3 mCenterOfMass { Vec3::sZero() }; ///< Center of mass of the compound + AABox mLocalBounds; + SubShapes mSubShapes; + float mInnerRadius = FLT_MAX; ///< Smallest radius of GetInnerRadius() of child shapes + +private: + // Helper functions called by CollisionDispatch + static void sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h new file mode 100644 index 000000000000..1b1e3867b07b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h @@ -0,0 +1,460 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct CompoundShape::CastRayVisitor +{ + JPH_INLINE CastRayVisitor(const RayCast &inRay, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) : + mRay(inRay), + mHit(ioHit), + mSubShapeIDCreator(inSubShapeIDCreator), + mSubShapeBits(inShape->GetSubShapeIDBits()) + { + // Determine ray properties of cast + mInvDirection.Set(inRay.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + /// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the ray against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the ray + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + RayCast ray = mRay.Transformed(transform); + if (inSubShape.mShape->CastRay(ray, shape2_sub_shape_id, mHit)) + mReturnValue = true; + } + + RayInvDirection mInvDirection; + const RayCast & mRay; + RayCastResult & mHit; + SubShapeIDCreator mSubShapeIDCreator; + uint mSubShapeBits; + bool mReturnValue = false; +}; + +struct CompoundShape::CastRayVisitorCollector +{ + JPH_INLINE CastRayVisitorCollector(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mRay(inRay), + mCollector(ioCollector), + mSubShapeIDCreator(inSubShapeIDCreator), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mRayCastSettings(inRayCastSettings), + mShapeFilter(inShapeFilter) + { + // Determine ray properties of cast + mInvDirection.Set(inRay.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the ray against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the ray + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + RayCast ray = mRay.Transformed(transform); + inSubShape.mShape->CastRay(ray, mRayCastSettings, shape2_sub_shape_id, mCollector, mShapeFilter); + } + + RayInvDirection mInvDirection; + const RayCast & mRay; + CastRayCollector & mCollector; + SubShapeIDCreator mSubShapeIDCreator; + uint mSubShapeBits; + RayCastSettings mRayCastSettings; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollidePointVisitor +{ + JPH_INLINE CollidePointVisitor(Vec3Arg inPoint, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mPoint(inPoint), + mSubShapeIDCreator(inSubShapeIDCreator), + mCollector(ioCollector), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Test if point overlaps with 4 boxes, returns true for the ones that do + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the point against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the point + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + inSubShape.mShape->CollidePoint(transform * mPoint, shape2_sub_shape_id, mCollector, mShapeFilter); + } + + Vec3 mPoint; + SubShapeIDCreator mSubShapeIDCreator; + CollidePointCollector & mCollector; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CastShapeVisitor +{ + JPH_INLINE CastShapeVisitor(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const CompoundShape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) : + mBoxCenter(inShapeCast.mShapeWorldBounds.GetCenter()), + mBoxExtent(inShapeCast.mShapeWorldBounds.GetExtent()), + mScale(inScale), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mShapeFilter(inShapeFilter), + mCollector(ioCollector), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape->GetSubShapeIDBits()) + { + // Determine ray properties of cast + mInvDirection.Set(inShapeCast.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the shape cast against 4 bounding boxes, returns the distance along the shape cast where the shape first enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test ray against the bounding boxes + return RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the cast shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(inSubShape.IsValidScale(mScale)); + + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits); + + // Calculate the local transform for this sub shape + Mat44 local_transform = Mat44::sRotationTranslation(inSubShape.GetRotation(), mScale * inSubShape.GetPositionCOM()); + + // Transform the center of mass of 2 + Mat44 center_of_mass_transform2 = mCenterOfMassTransform2 * local_transform; + + // Transform the shape cast + ShapeCast shape_cast = mShapeCast.PostTransformed(local_transform.InversedRotationTranslation()); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, mShapeCastSettings, inSubShape.mShape, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector); + } + + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + Vec3 mScale; + const ShapeCast & mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + const ShapeFilter & mShapeFilter; + CastShapeCollector & mCollector; + Mat44 mCenterOfMassTransform2; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; +}; + +struct CompoundShape::CollectTransformedShapesVisitor +{ + JPH_INLINE CollectTransformedShapesVisitor(const AABox &inBox, const CompoundShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mBox(inBox), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mPositionCOM(inPositionCOM), + mRotation(inRotation), + mScale(inScale), + mSubShapeIDCreator(inSubShapeIDCreator), + mCollector(ioCollector), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests 4 bounding boxes against the query box, returns true for the ones that collide + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + return AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Collect the transformed sub shapes for a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(inSubShape.IsValidScale(mScale)); + + // Create ID for sub shape + SubShapeIDCreator sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Calculate world transform for sub shape + Vec3 position = mPositionCOM + mRotation * (mScale * inSubShape.GetPositionCOM()); + Quat rotation = mRotation * inSubShape.GetRotation(); + + // Recurse to sub shape + inSubShape.mShape->CollectTransformedShapes(mBox, position, rotation, inSubShape.TransformScale(mScale), sub_shape_id, mCollector, mShapeFilter); + } + + AABox mBox; + OrientedBox mLocalBox; + Vec3 mPositionCOM; + Quat mRotation; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator; + TransformedShapeCollector & mCollector; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollideCompoundVsShapeVisitor +{ + JPH_INLINE CollideCompoundVsShapeVisitor(const CompoundShape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape2(inShape2), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape1->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + // Get transform from shape 2 to shape 1 + Mat44 transform2_to_1 = inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2; + + // Convert bounding box of 2 into space of 1 + mBoundsOf2InSpaceOf1 = inShape2->GetLocalBounds().Scaled(inScale2).Transformed(transform2_to_1); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the bounds of shape 2 vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale1, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which boxes collide + return AABox4VsBox(mBoundsOf2InSpaceOf1, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Get world transform of 1 + Mat44 transform1 = mTransform1 * inSubShape.GetLocalTransformNoScale(mScale1); + + // Create ID for sub shape + SubShapeIDCreator shape1_sub_shape_id = mSubShapeIDCreator1.PushID(inSubShapeIndex, mSubShapeBits); + + CollisionDispatch::sCollideShapeVsShape(inSubShape.mShape, mShape2, inSubShape.TransformScale(mScale1), mScale2, transform1, mTransform2, shape1_sub_shape_id, mSubShapeIDCreator2, mCollideShapeSettings, mCollector, mShapeFilter); + } + + const CollideShapeSettings & mCollideShapeSettings; + CollideShapeCollector & mCollector; + const Shape * mShape2; + Vec3 mScale1; + Vec3 mScale2; + Mat44 mTransform1; + Mat44 mTransform2; + AABox mBoundsOf2InSpaceOf1; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollideShapeVsCompoundVisitor +{ + JPH_INLINE CollideShapeVsCompoundVisitor(const Shape *inShape1, const CompoundShape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape2->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + // Get transform from shape 1 to shape 2 + Mat44 transform1_to_2 = inCenterOfMassTransform2.InversedRotationTranslation() * inCenterOfMassTransform1; + + // Convert bounding box of 1 into space of 2 + mBoundsOf1InSpaceOf2 = inShape1->GetLocalBounds().Scaled(inScale1).Transformed(transform1_to_2); + mBoundsOf1InSpaceOf2.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the bounds of shape 1 vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which bounding boxes collide + return AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits); + + // Get world transform of 2 + Mat44 transform2 = mTransform2 * inSubShape.GetLocalTransformNoScale(mScale2); + + CollisionDispatch::sCollideShapeVsShape(mShape1, inSubShape.mShape, mScale1, inSubShape.TransformScale(mScale2), mTransform1, transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollideShapeSettings, mCollector, mShapeFilter); + } + + const CollideShapeSettings & mCollideShapeSettings; + CollideShapeCollector & mCollector; + const Shape * mShape1; + Vec3 mScale1; + Vec3 mScale2; + Mat44 mTransform1; + Mat44 mTransform2; + AABox mBoundsOf1InSpaceOf2; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +template +struct CompoundShape::GetIntersectingSubShapesVisitor +{ + JPH_INLINE GetIntersectingSubShapesVisitor(const BoxType &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) : + mBox(inBox), + mSubShapeIndices(outSubShapeIndices), + mMaxSubShapeIndices(inMaxSubShapeIndices) + { + } + + /// Returns true when collision detection should abort because the buffer is full + JPH_INLINE bool ShouldAbort() const + { + return mNumResults >= mMaxSubShapeIndices; + } + + /// Tests the box vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Test which bounding boxes collide + return AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Records a hit + JPH_INLINE void VisitShape([[maybe_unused]] const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(mNumResults < mMaxSubShapeIndices); + *mSubShapeIndices++ = inSubShapeIndex; + mNumResults++; + } + + /// Get the number of indices that were found + JPH_INLINE int GetNumResults() const + { + return mNumResults; + } + +private: + BoxType mBox; + uint * mSubShapeIndices; + int mMaxSubShapeIndices; + int mNumResults = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp new file mode 100644 index 000000000000..1a1126e19056 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp @@ -0,0 +1,1308 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConvexHullShapeSettings) +{ + JPH_ADD_BASE_CLASS(ConvexHullShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mPoints) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mMaxConvexRadius) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mMaxErrorConvexRadius) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mHullTolerance) +} + +ShapeSettings::ShapeResult ConvexHullShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new ConvexHullShape(*this, mCachedResult); + return mCachedResult; +} + +ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::ConvexHull, inSettings, outResult), + mConvexRadius(inSettings.mMaxConvexRadius) +{ + using BuilderFace = ConvexHullBuilder::Face; + using Edge = ConvexHullBuilder::Edge; + using Faces = Array; + + // Check convex radius + if (mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + // Build convex hull + const char *error = nullptr; + ConvexHullBuilder builder(inSettings.mPoints); + ConvexHullBuilder::EResult result = builder.Initialize(cMaxPointsInHull, inSettings.mHullTolerance, error); + if (result != ConvexHullBuilder::EResult::Success && result != ConvexHullBuilder::EResult::MaxVerticesReached) + { + outResult.SetError(error); + return; + } + const Faces &builder_faces = builder.GetFaces(); + + // Check the consistency of the resulting hull if we fully built it + if (result == ConvexHullBuilder::EResult::Success) + { + ConvexHullBuilder::Face *max_error_face; + float max_error_distance, coplanar_distance; + int max_error_idx; + builder.DetermineMaxError(max_error_face, max_error_distance, max_error_idx, coplanar_distance); + if (max_error_distance > 4.0f * max(coplanar_distance, inSettings.mHullTolerance)) // Coplanar distance could be bigger than the allowed tolerance if the points are far apart + { + outResult.SetError(StringFormat("Hull building failed, point %d had an error of %g (relative to tolerance: %g)", max_error_idx, (double)max_error_distance, double(max_error_distance / inSettings.mHullTolerance))); + return; + } + } + + // Calculate center of mass and volume + builder.GetCenterOfMassAndVolume(mCenterOfMass, mVolume); + + // Calculate covariance matrix + // See: + // - Why the inertia tensor is the inertia tensor - Jonathan Blow (http://number-none.com/blow/inertia/deriving_i.html) + // - How to find the inertia tensor (or other mass properties) of a 3D solid body represented by a triangle mesh (Draft) - Jonathan Blow, Atman J Binstock (http://number-none.com/blow/inertia/bb_inertia.doc) + Mat44 covariance_canonical(Vec4(1.0f / 60.0f, 1.0f / 120.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 60.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 120.0f, 1.0f / 60.0f, 0), Vec4(0, 0, 0, 1)); + Mat44 covariance_matrix = Mat44::sZero(); + for (BuilderFace *f : builder_faces) + { + // Fourth point of the tetrahedron is at the center of mass, we subtract it from the other points so we get a tetrahedron with one vertex at zero + // The first point on the face will be used to form a triangle fan + Edge *e = f->mFirstEdge; + Vec3 v1 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Get the 2nd point + e = e->mNextEdge; + Vec3 v2 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Loop over the triangle fan + for (e = e->mNextEdge; e != f->mFirstEdge; e = e->mNextEdge) + { + Vec3 v3 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Affine transform that transforms a unit tetrahedon (with vertices (0, 0, 0), (1, 0, 0), (0, 1, 0) and (0, 0, 1) to this tetrahedron + Mat44 a(Vec4(v1, 0), Vec4(v2, 0), Vec4(v3, 0), Vec4(0, 0, 0, 1)); + + // Calculate covariance matrix for this tetrahedron + float det_a = a.GetDeterminant3x3(); + Mat44 c = det_a * (a * covariance_canonical * a.Transposed()); + + // Add it + covariance_matrix += c; + + // Prepare for next triangle + v2 = v3; + } + } + + // Calculate inertia matrix assuming density is 1, note that element (3, 3) is garbage + mInertia = Mat44::sIdentity() * (covariance_matrix(0, 0) + covariance_matrix(1, 1) + covariance_matrix(2, 2)) - covariance_matrix; + + // Convert polygons from the builder to our internal representation + using VtxMap = UnorderedMap; + VtxMap vertex_map; + vertex_map.reserve(VtxMap::size_type(inSettings.mPoints.size())); + for (BuilderFace *builder_face : builder_faces) + { + // Determine where the vertices go + JPH_ASSERT(mVertexIdx.size() <= 0xFFFF); + uint16 first_vertex = (uint16)mVertexIdx.size(); + uint16 num_vertices = 0; + + // Loop over vertices in face + Edge *edge = builder_face->mFirstEdge; + do + { + // Remap to new index, not all points in the original input set are required to form the hull + uint8 new_idx; + int original_idx = edge->mStartIdx; + VtxMap::iterator m = vertex_map.find(original_idx); + if (m != vertex_map.end()) + { + // Found, reuse + new_idx = m->second; + } + else + { + // This is a new point + // Make relative to center of mass + Vec3 p = inSettings.mPoints[original_idx] - mCenterOfMass; + + // Update local bounds + mLocalBounds.Encapsulate(p); + + // Add to point list + JPH_ASSERT(mPoints.size() <= 0xff); + new_idx = (uint8)mPoints.size(); + mPoints.push_back({ p }); + vertex_map[original_idx] = new_idx; + } + + // Append to vertex list + JPH_ASSERT(mVertexIdx.size() < 0xffff); + mVertexIdx.push_back(new_idx); + num_vertices++; + + edge = edge->mNextEdge; + } while (edge != builder_face->mFirstEdge); + + // Add face + mFaces.push_back({ first_vertex, num_vertices }); + + // Add plane + Plane plane = Plane::sFromPointAndNormal(builder_face->mCentroid - mCenterOfMass, builder_face->mNormal.Normalized()); + mPlanes.push_back(plane); + } + + // Test if GetSupportFunction can support this many points + if (mPoints.size() > cMaxPointsInHull) + { + outResult.SetError(StringFormat("Internal error: Too many points in hull (%u), max allowed %d", (uint)mPoints.size(), cMaxPointsInHull)); + return; + } + + for (int p = 0; p < (int)mPoints.size(); ++p) + { + // For each point, find faces that use the point + Array faces; + for (int f = 0; f < (int)mFaces.size(); ++f) + { + const Face &face = mFaces[f]; + for (int v = 0; v < face.mNumVertices; ++v) + if (mVertexIdx[face.mFirstVertex + v] == p) + { + faces.push_back(f); + break; + } + } + + if (faces.size() < 2) + { + outResult.SetError("A point must be connected to 2 or more faces!"); + return; + } + + // Find the 3 normals that form the largest tetrahedron + // The largest tetrahedron we can get is ((1, 0, 0) x (0, 1, 0)) . (0, 0, 1) = 1, if the volume is only 5% of that, + // the three vectors are too coplanar and we fall back to using only 2 plane normals + float biggest_volume = 0.05f; + int best3[3] = { -1, -1, -1 }; + + // When using 2 normals, we get the two with the biggest angle between them with a minimal difference of 1 degree + // otherwise we fall back to just using 1 plane normal + float smallest_dot = Cos(DegreesToRadians(1.0f)); + int best2[2] = { -1, -1 }; + + for (int face1 = 0; face1 < (int)faces.size(); ++face1) + { + Vec3 normal1 = mPlanes[faces[face1]].GetNormal(); + for (int face2 = face1 + 1; face2 < (int)faces.size(); ++face2) + { + Vec3 normal2 = mPlanes[faces[face2]].GetNormal(); + Vec3 cross = normal1.Cross(normal2); + + // Determine the 2 face normals that are most apart + float dot = normal1.Dot(normal2); + if (dot < smallest_dot) + { + smallest_dot = dot; + best2[0] = faces[face1]; + best2[1] = faces[face2]; + } + + // Determine the 3 face normals that form the largest tetrahedron + for (int face3 = face2 + 1; face3 < (int)faces.size(); ++face3) + { + Vec3 normal3 = mPlanes[faces[face3]].GetNormal(); + float volume = abs(cross.Dot(normal3)); + if (volume > biggest_volume) + { + biggest_volume = volume; + best3[0] = faces[face1]; + best3[1] = faces[face2]; + best3[2] = faces[face3]; + } + } + } + } + + // If we didn't find 3 planes, use 2, if we didn't find 2 use 1 + if (best3[0] != -1) + faces = { best3[0], best3[1], best3[2] }; + else if (best2[0] != -1) + faces = { best2[0], best2[1] }; + else + faces = { faces[0] }; + + // Copy the faces to the points buffer + Point &point = mPoints[p]; + point.mNumFaces = (int)faces.size(); + for (int i = 0; i < (int)faces.size(); ++i) + point.mFaces[i] = faces[i]; + } + + // If the convex radius is already zero, there's no point in further reducing it + if (mConvexRadius > 0.0f) + { + // Find out how thin the hull is by walking over all planes and checking the thickness of the hull in that direction + float min_size = FLT_MAX; + for (const Plane &plane : mPlanes) + { + // Take the point that is furthest away from the plane as thickness of this hull + float max_dist = 0.0f; + for (const Point &point : mPoints) + { + float dist = -plane.SignedDistance(point.mPosition); // Point is always behind plane, so we need to negate + if (dist > max_dist) + max_dist = dist; + } + min_size = min(min_size, max_dist); + } + + // We need to fit in 2x the convex radius in min_size, so reduce the convex radius if it's bigger than that + mConvexRadius = min(mConvexRadius, 0.5f * min_size); + } + + // Now walk over all points and see if we have to further reduce the convex radius because of sharp edges + if (mConvexRadius > 0.0f) + { + for (const Point &point : mPoints) + if (point.mNumFaces != 1) // If we have a single face, shifting back is easy and we don't need to reduce the convex radius + { + // Get first two planes + Plane p1 = mPlanes[point.mFaces[0]]; + Plane p2 = mPlanes[point.mFaces[1]]; + Plane p3; + Vec3 offset_mask; + + if (point.mNumFaces == 3) + { + // Get third plane + p3 = mPlanes[point.mFaces[2]]; + + // All 3 planes will be offset by the convex radius + offset_mask = Vec3::sReplicate(1); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal())); + + // Only the first and 2nd plane will be offset, the 3rd plane is only there to guide the intersection point + offset_mask = Vec3(1, 1, 0); + } + + // Plane equation: point . normal + constant = 0 + // Offsetting the plane backwards with convex radius r: point . normal + constant + r = 0 + // To find the intersection 'point' of 3 planes we solve: + // |n1x n1y n1z| |x| | r + c1 | + // |n2x n2y n2z| |y| = - | r + c2 | <=> n point = -r (1, 1, 1) - (c1, c2, c3) + // |n3x n3y n3z| |z| | r + c3 | + // Where point = (x, y, z), n1x is the x component of the first plane, c1 = plane constant of plane 1, etc. + // The relation between how much the intersection point shifts as a function of r is: -r * n^-1 (1, 1, 1) = r * offset + // Where offset = -n^-1 (1, 1, 1) or -n^-1 (1, 1, 0) in case only the first 2 planes are offset + // The error that is introduced by a convex radius r is: error = r * |offset| - r + // So the max convex radius given error is: r = error / (|offset| - 1) + Mat44 n = Mat44(Vec4(p1.GetNormal(), 0), Vec4(p2.GetNormal(), 0), Vec4(p3.GetNormal(), 0), Vec4(0, 0, 0, 1)).Transposed(); + float det_n = n.GetDeterminant3x3(); + if (det_n == 0.0f) + { + // If the determinant is zero, the matrix is not invertible so no solution exists to move the point backwards and we have to choose a convex radius of zero + mConvexRadius = 0.0f; + break; + } + Mat44 adj_n = n.Adjointed3x3(); + float offset = ((adj_n * offset_mask) / det_n).Length(); + JPH_ASSERT(offset > 1.0f); + float max_convex_radius = inSettings.mMaxErrorConvexRadius / (offset - 1.0f); + mConvexRadius = min(mConvexRadius, max_convex_radius); + } + } + + // Calculate the inner radius by getting the minimum distance from the origin to the planes of the hull + mInnerRadius = FLT_MAX; + for (const Plane &p : mPlanes) + mInnerRadius = min(mInnerRadius, -p.GetConstant()); + mInnerRadius = max(0.0f, mInnerRadius); // Clamp against zero, this should do nothing as the shape is centered around the center of mass but for flat convex hulls there may be numerical round off issues + + outResult.Set(this); +} + +MassProperties ConvexHullShape::GetMassProperties() const +{ + MassProperties p; + + float density = GetDensity(); + + // Calculate mass + p.mMass = density * mVolume; + + // Calculate inertia matrix + p.mInertia = density * mInertia; + p.mInertia(3, 3) = 1.0f; + + return p; +} + +Vec3 ConvexHullShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + const Plane &first_plane = mPlanes[0]; + Vec3 best_normal = first_plane.GetNormal(); + float best_dist = abs(first_plane.SignedDistance(inLocalSurfacePosition)); + + // Find the face that has the shortest distance to the surface point + for (Array::size_type i = 1; i < mFaces.size(); ++i) + { + const Plane &plane = mPlanes[i]; + Vec3 plane_normal = plane.GetNormal(); + float dist = abs(plane.SignedDistance(inLocalSurfacePosition)); + if (dist < best_dist) + { + best_dist = dist; + best_normal = plane_normal; + } + } + + return best_normal; +} + +class ConvexHullShape::HullNoConvex final : public Support +{ +public: + explicit HullNoConvex(float inConvexRadius) : + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(HullNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (Vec3 point : mPoints) + { + // Check if its support is bigger than the current max + float dot = point.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = point; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + + using PointsArray = StaticArray; + + inline PointsArray & GetPoints() + { + return mPoints; + } + + const PointsArray & GetPoints() const + { + return mPoints; + } + +private: + float mConvexRadius; + PointsArray mPoints; +}; + +class ConvexHullShape::HullWithConvex final : public Support +{ +public: + explicit HullWithConvex(const ConvexHullShape *inShape) : + mShape(inShape) + { + static_assert(sizeof(HullWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (const Point &point : mShape->mPoints) + { + // Check if its support is bigger than the current max + float dot = point.mPosition.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = point.mPosition; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + const ConvexHullShape * mShape; +}; + +class ConvexHullShape::HullWithConvexScaled final : public Support +{ +public: + HullWithConvexScaled(const ConvexHullShape *inShape, Vec3Arg inScale) : + mShape(inShape), + mScale(inScale) + { + static_assert(sizeof(HullWithConvexScaled) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullWithConvexScaled))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (const Point &point : mShape->mPoints) + { + // Calculate scaled position + Vec3 pos = mScale * point.mPosition; + + // Check if its support is bigger than the current max + float dot = pos.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = pos; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + const ConvexHullShape * mShape; + Vec3 mScale; +}; + +const ConvexShape::Support *ConvexHullShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + // If there's no convex radius, we don't need to shrink the hull + if (mConvexRadius == 0.0f) + { + if (ScaleHelpers::IsNotScaled(inScale)) + return new (&inBuffer) HullWithConvex(this); + else + return new (&inBuffer) HullWithConvexScaled(this, inScale); + } + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + if (ScaleHelpers::IsNotScaled(inScale)) + return new (&inBuffer) HullWithConvex(this); + else + return new (&inBuffer) HullWithConvexScaled(this, inScale); + + case ESupportMode::ExcludeConvexRadius: + if (ScaleHelpers::IsNotScaled(inScale)) + { + // Create support function + HullNoConvex *hull = new (&inBuffer) HullNoConvex(mConvexRadius); + HullNoConvex::PointsArray &transformed_points = hull->GetPoints(); + JPH_ASSERT(mPoints.size() <= cMaxPointsInHull, "Not enough space, this should have been caught during shape creation!"); + + for (const Point &point : mPoints) + { + Vec3 new_point; + + if (point.mNumFaces == 1) + { + // Simply shift back by the convex radius using our 1 plane + new_point = point.mPosition - mPlanes[point.mFaces[0]].GetNormal() * mConvexRadius; + } + else + { + // Get first two planes and offset inwards by convex radius + Plane p1 = mPlanes[point.mFaces[0]].Offset(-mConvexRadius); + Plane p2 = mPlanes[point.mFaces[1]].Offset(-mConvexRadius); + Plane p3; + + if (point.mNumFaces == 3) + { + // Get third plane and offset inwards by convex radius + p3 = mPlanes[point.mFaces[2]].Offset(-mConvexRadius); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal())); + } + + // Find intersection point between the three planes + if (!Plane::sIntersectPlanes(p1, p2, p3, new_point)) + { + // Fallback: Just push point back using the first plane + new_point = point.mPosition - p1.GetNormal() * mConvexRadius; + } + } + + // Add point + transformed_points.push_back(new_point); + } + + return hull; + } + else + { + // Calculate scaled convex radius + float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + + // Create new support function + HullNoConvex *hull = new (&inBuffer) HullNoConvex(convex_radius); + HullNoConvex::PointsArray &transformed_points = hull->GetPoints(); + JPH_ASSERT(mPoints.size() <= cMaxPointsInHull, "Not enough space, this should have been caught during shape creation!"); + + // Precalculate inverse scale + Vec3 inv_scale = inScale.Reciprocal(); + + for (const Point &point : mPoints) + { + // Calculate scaled position + Vec3 pos = inScale * point.mPosition; + + // Transform normals for plane 1 with scale + Vec3 n1 = (inv_scale * mPlanes[point.mFaces[0]].GetNormal()).Normalized(); + + Vec3 new_point; + + if (point.mNumFaces == 1) + { + // Simply shift back by the convex radius using our 1 plane + new_point = pos - n1 * convex_radius; + } + else + { + // Transform normals for plane 2 with scale + Vec3 n2 = (inv_scale * mPlanes[point.mFaces[1]].GetNormal()).Normalized(); + + // Get first two planes and offset inwards by convex radius + Plane p1 = Plane::sFromPointAndNormal(pos, n1).Offset(-convex_radius); + Plane p2 = Plane::sFromPointAndNormal(pos, n2).Offset(-convex_radius); + Plane p3; + + if (point.mNumFaces == 3) + { + // Transform last normal with scale + Vec3 n3 = (inv_scale * mPlanes[point.mFaces[2]].GetNormal()).Normalized(); + + // Get third plane and offset inwards by convex radius + p3 = Plane::sFromPointAndNormal(pos, n3).Offset(-convex_radius); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(pos, n1.Cross(n2)); + } + + // Find intersection point between the three planes + if (!Plane::sIntersectPlanes(p1, p2, p3, new_point)) + { + // Fallback: Just push point back using the first plane + new_point = pos - n1 * convex_radius; + } + } + + // Add point + transformed_points.push_back(new_point); + } + + return hull; + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void ConvexHullShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 inv_scale = inScale.Reciprocal(); + + // Need to transform the plane normals using inScale + // Transforming a direction with matrix M is done through multiplying by (M^-1)^T + // In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards + Vec3 plane0_normal = inv_scale * mPlanes[0].GetNormal(); + float best_dot = plane0_normal.Dot(inDirection) / plane0_normal.Length(); + int best_face_idx = 0; + + for (Array::size_type i = 1; i < mPlanes.size(); ++i) + { + Vec3 plane_normal = inv_scale * mPlanes[i].GetNormal(); + float dot = plane_normal.Dot(inDirection) / plane_normal.Length(); + if (dot < best_dot) + { + best_dot = dot; + best_face_idx = (int)i; + } + } + + // Get vertices + const Face &best_face = mFaces[best_face_idx]; + const uint8 *first_vtx = mVertexIdx.data() + best_face.mFirstVertex; + const uint8 *end_vtx = first_vtx + best_face.mNumVertices; + + // If we have more than 1/2 the capacity of outVertices worth of vertices, we start skipping vertices (note we can't fill the buffer completely since extra edges will be generated by clipping). + // TODO: This really needs a better algorithm to determine which vertices are important! + int max_vertices_to_return = outVertices.capacity() / 2; + int delta_vtx = (int(best_face.mNumVertices) + max_vertices_to_return) / max_vertices_to_return; + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + if (ScaleHelpers::IsInsideOut(inScale)) + { + // Flip winding of supporting face + for (const uint8 *v = end_vtx - 1; v >= first_vtx; v -= delta_vtx) + outVertices.push_back(transform * mPoints[*v].mPosition); + } + else + { + // Normal winding of supporting face + for (const uint8 *v = first_vtx; v < end_vtx; v += delta_vtx) + outVertices.push_back(transform * mPoints[*v].mPosition); + } +} + +void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Trivially calculate total volume + Vec3 abs_scale = inScale.Abs(); + outTotalVolume = mVolume * abs_scale.GetX() * abs_scale.GetY() * abs_scale.GetZ(); + + // Check if shape has been scaled inside out + bool is_inside_out = ScaleHelpers::IsInsideOut(inScale); + + // Convert the points to world space and determine the distance to the surface + int num_points = int(mPoints.size()); + PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(num_points * sizeof(PolyhedronSubmergedVolumeCalculator::Point)); + PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(inScale), &mPoints[0].mPosition, sizeof(Point), num_points, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + if (submerged_vol_calc.AreAllAbove()) + { + // We're above the water + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (submerged_vol_calc.AreAllBelow()) + { + // We're fully submerged + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Calculate submerged volume + int reference_point_idx = submerged_vol_calc.GetReferencePointIdx(); + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // If any of the vertices of this face are the reference point, the volume will be zero so we can skip this face + bool degenerate = false; + for (const uint8 *v = first_vtx; v < end_vtx; ++v) + if (*v == reference_point_idx) + { + degenerate = true; + break; + } + if (degenerate) + continue; + + // Triangulate the face + int i1 = *first_vtx; + if (is_inside_out) + { + // Reverse winding + for (const uint8 *v = first_vtx + 2; v < end_vtx; ++v) + { + int i2 = *(v - 1); + int i3 = *v; + submerged_vol_calc.AddFace(i1, i3, i2); + } + } + else + { + // Normal winding + for (const uint8 *v = first_vtx + 2; v < end_vtx; ++v) + { + int i2 = *(v - 1); + int i3 = *v; + submerged_vol_calc.AddFace(i1, i2, i3); + } + } + } + + // Get the results + submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy); + } + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void ConvexHullShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + if (mGeometry == nullptr) + { + Array triangles; + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Draw first triangle of polygon + Vec3 v0 = mPoints[first_vtx[0]].mPosition; + Vec3 v1 = mPoints[first_vtx[1]].mPosition; + Vec3 v2 = mPoints[first_vtx[2]].mPosition; + Vec3 uv_direction = (v1 - v0).Normalized(); + triangles.push_back({ v0, v1, v2, Color::sWhite, v0, uv_direction }); + + // Draw any other triangles in this polygon + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + triangles.push_back({ v0, mPoints[*(v - 1)].mPosition, mPoints[*v].mPosition, Color::sWhite, v0, uv_direction }); + } + mGeometry = new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), GetLocalBounds()); + } + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor; + RMat44 transform = inCenterOfMassTransform.PreScaled(inScale); + inRenderer->DrawGeometry(transform, color, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + // Draw the outline if requested + if (sDrawFaceOutlines) + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Draw edges of face + inRenderer->DrawLine(transform * mPoints[*(end_vtx - 1)].mPosition, transform * mPoints[*first_vtx].mPosition, Color::sGrey); + for (const uint8 *v = first_vtx + 1; v < end_vtx; ++v) + inRenderer->DrawLine(transform * mPoints[*(v - 1)].mPosition, transform * mPoints[*v].mPosition, Color::sGrey); + } +} + +void ConvexHullShape::DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + // Get the shrunk points + SupportBuffer buffer; + const HullNoConvex *support = mConvexRadius > 0.0f? static_cast(GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale)) : nullptr; + + RMat44 transform = inCenterOfMassTransform * Mat44::sScale(inScale); + + for (int p = 0; p < (int)mPoints.size(); ++p) + { + const Point &point = mPoints[p]; + RVec3 position = transform * point.mPosition; + RVec3 shrunk_point = support != nullptr? transform * support->GetPoints()[p] : position; + + // Draw difference between shrunk position and position + inRenderer->DrawLine(position, shrunk_point, Color::sGreen); + + // Draw face normals that are contributing + for (int i = 0; i < point.mNumFaces; ++i) + inRenderer->DrawLine(position, position + 0.1f * mPlanes[point.mFaces[i]].GetNormal(), Color::sYellow); + + // Draw point index + inRenderer->DrawText3D(position, ConvertToString(p), Color::sWhite, 0.1f); + } +} +#endif // JPH_DEBUG_RENDERER + +bool ConvexHullShape::CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const +{ + if (mFaces.size() == 2) + { + // If we have only 2 faces, we're a flat convex hull and we need to test edges instead of planes + + // Check if plane is parallel to ray + const Plane &p = mPlanes.front(); + Vec3 plane_normal = p.GetNormal(); + float direction_projection = inRay.mDirection.Dot(plane_normal); + if (abs(direction_projection) >= 1.0e-12f) + { + // Calculate intersection point + float distance_to_plane = inRay.mOrigin.Dot(plane_normal) + p.GetConstant(); + float fraction = -distance_to_plane / direction_projection; + if (fraction < 0.0f || fraction > 1.0f) + { + // Does not hit plane, no hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + Vec3 intersection_point = inRay.mOrigin + fraction * inRay.mDirection; + + // Test all edges to see if point is inside polygon + const Face &f = mFaces.front(); + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + Vec3 p1 = mPoints[*end_vtx].mPosition; + for (const uint8 *v = first_vtx; v < end_vtx; ++v) + { + Vec3 p2 = mPoints[*v].mPosition; + if ((p2 - p1).Cross(intersection_point - p1).Dot(plane_normal) < 0.0f) + { + // Outside polygon, no hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + p1 = p2; + } + + // Inside polygon, a hit + outMinFraction = fraction; + outMaxFraction = fraction; + return true; + } + else + { + // Parallel ray doesn't hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + } + else + { + // Clip ray against all planes + int fractions_set = 0; + bool all_inside = true; + float min_fraction = 0.0f, max_fraction = 1.0f + FLT_EPSILON; + for (const Plane &p : mPlanes) + { + // Check if the ray origin is behind this plane + Vec3 plane_normal = p.GetNormal(); + float distance_to_plane = inRay.mOrigin.Dot(plane_normal) + p.GetConstant(); + bool is_outside = distance_to_plane > 0.0f; + all_inside &= !is_outside; + + // Check if plane is parallel to ray + float direction_projection = inRay.mDirection.Dot(plane_normal); + if (abs(direction_projection) >= 1.0e-12f) + { + // Get intersection fraction between ray and plane + float fraction = -distance_to_plane / direction_projection; + + // Update interval of ray that is inside the hull + if (direction_projection < 0.0f) + { + min_fraction = max(fraction, min_fraction); + fractions_set |= 1; + } + else + { + max_fraction = min(fraction, max_fraction); + fractions_set |= 2; + } + } + else if (is_outside) + return false; // Outside the plane and parallel, no hit! + } + + // Test if both min and max have been set + if (fractions_set == 3) + { + // Output fractions + outMinFraction = min_fraction; + outMaxFraction = max_fraction; + + // Test if the infinite ray intersects with the hull (the length will be checked later) + return min_fraction <= max_fraction && max_fraction >= 0.0f; + } + else + { + // Degenerate case, either the ray is parallel to all planes or the ray has zero length + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + + // Return if the origin is inside the hull + return all_inside; + } + } +} + +bool ConvexHullShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Determine if ray hits the shape + float min_fraction, max_fraction; + if (CastRayHelper(inRay, min_fraction, max_fraction) + && min_fraction < ioHit.mFraction) // Check if this is a closer hit + { + // Better hit than the current hit + ioHit.mFraction = min_fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void ConvexHullShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Determine if ray hits the shape + float min_fraction, max_fraction; + if (CastRayHelper(inRay, min_fraction, max_fraction) + && min_fraction < ioCollector.GetEarlyOutFraction()) // Check if this is closer than the early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side hit + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = min_fraction; + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + && max_fraction < ioCollector.GetEarlyOutFraction()) + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void ConvexHullShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if point is behind all planes + for (const Plane &p : mPlanes) + if (p.SignedDistance(inPoint) > 0.0f) + return; + + // Point is inside + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + Vec3 inv_scale = inScale.Reciprocal(); + bool is_not_scaled = ScaleHelpers::IsNotScaled(inScale); + float scale_flip = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + Vec3 local_pos = inverse_transform * v.GetPosition(); + + // Find most facing plane + float max_distance = -FLT_MAX; + Vec3 max_plane_normal = Vec3::sZero(); + uint max_plane_idx = 0; + if (is_not_scaled) + { + // Without scale, it is trivial to calculate the distance to the hull + for (const Plane &p : mPlanes) + { + float distance = p.SignedDistance(local_pos); + if (distance > max_distance) + { + max_distance = distance; + max_plane_normal = p.GetNormal(); + max_plane_idx = uint(&p - mPlanes.data()); + } + } + } + else + { + // When there's scale we need to calculate the planes first + for (uint i = 0; i < (uint)mPlanes.size(); ++i) + { + // Calculate plane normal and point by scaling the original plane + Vec3 plane_normal = (inv_scale * mPlanes[i].GetNormal()).Normalized(); + Vec3 plane_point = inScale * mPoints[mVertexIdx[mFaces[i].mFirstVertex]].mPosition; + + float distance = plane_normal.Dot(local_pos - plane_point); + if (distance > max_distance) + { + max_distance = distance; + max_plane_normal = plane_normal; + max_plane_idx = i; + } + } + } + bool is_outside = max_distance > 0.0f; + + // Project point onto that plane + Vec3 closest_point = local_pos - max_distance * max_plane_normal; + + // Check edges if we're outside the hull (when inside we know the closest face is also the closest point to the surface) + if (is_outside) + { + // Loop over edges + float closest_point_dist_sq = FLT_MAX; + const Face &face = mFaces[max_plane_idx]; + for (const uint8 *v_start = &mVertexIdx[face.mFirstVertex], *v1 = v_start, *v_end = v_start + face.mNumVertices; v1 < v_end; ++v1) + { + // Find second point + const uint8 *v2 = v1 + 1; + if (v2 == v_end) + v2 = v_start; + + // Get edge points + Vec3 p1 = inScale * mPoints[*v1].mPosition; + Vec3 p2 = inScale * mPoints[*v2].mPosition; + + // Check if the position is outside the edge (if not, the face will be closer) + Vec3 edge_normal = (p2 - p1).Cross(max_plane_normal); + if (scale_flip * edge_normal.Dot(local_pos - p1) > 0.0f) + { + // Get closest point on edge + uint32 set; + Vec3 closest = ClosestPoint::GetClosestPointOnLine(p1 - local_pos, p2 - local_pos, set); + float distance_sq = closest.LengthSq(); + if (distance_sq < closest_point_dist_sq) + closest_point = local_pos + closest; + } + } + } + + // Check if this is the largest penetration + Vec3 normal = local_pos - closest_point; + float normal_length = normal.Length(); + float penetration = normal_length; + if (is_outside) + penetration = -penetration; + else + normal = -normal; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact plane + normal = normal_length > 0.0f? normal / normal_length : max_plane_normal; + Plane plane = Plane::sFromPointAndNormal(closest_point, normal); + + // Store collision + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } +} + +class ConvexHullShape::CHSGetTrianglesContext +{ +public: + CHSGetTrianglesContext(Mat44Arg inTransform, bool inIsInsideOut) : mTransform(inTransform), mIsInsideOut(inIsInsideOut) { } + + Mat44 mTransform; + bool mIsInsideOut; + size_t mCurrentFace = 0; +}; + +void ConvexHullShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(CHSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(CHSGetTrianglesContext))); + + new (&ioContext) CHSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale), ScaleHelpers::IsInsideOut(inScale)); +} + +int ConvexHullShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 12, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + CHSGetTrianglesContext &context = (CHSGetTrianglesContext &)ioContext; + + int total_num_triangles = 0; + for (; context.mCurrentFace < mFaces.size(); ++context.mCurrentFace) + { + const Face &f = mFaces[context.mCurrentFace]; + + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Check if there is still room in the output buffer for this face + int num_triangles = f.mNumVertices - 2; + inMaxTrianglesRequested -= num_triangles; + if (inMaxTrianglesRequested < 0) + break; + total_num_triangles += num_triangles; + + // Get first triangle of polygon + Vec3 v0 = context.mTransform * mPoints[first_vtx[0]].mPosition; + Vec3 v1 = context.mTransform * mPoints[first_vtx[1]].mPosition; + Vec3 v2 = context.mTransform * mPoints[first_vtx[2]].mPosition; + v0.StoreFloat3(outTriangleVertices++); + if (context.mIsInsideOut) + { + // Store first triangle in this polygon flipped + v2.StoreFloat3(outTriangleVertices++); + v1.StoreFloat3(outTriangleVertices++); + + // Store other triangles in this polygon flipped + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + { + v0.StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*v].mPosition).StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*(v - 1)].mPosition).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store first triangle in this polygon + v1.StoreFloat3(outTriangleVertices++); + v2.StoreFloat3(outTriangleVertices++); + + // Store other triangles in this polygon + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + { + v0.StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*(v - 1)].mPosition).StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*v].mPosition).StoreFloat3(outTriangleVertices++); + } + } + } + + // Store materials + if (outMaterials != nullptr) + { + const PhysicsMaterial *material = GetMaterial(); + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = material; + } + + return total_num_triangles; +} + +void ConvexHullShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mInertia); + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mPoints); + inStream.Write(mFaces); + inStream.Write(mPlanes); + inStream.Write(mVertexIdx); + inStream.Write(mConvexRadius); + inStream.Write(mVolume); + inStream.Write(mInnerRadius); +} + +void ConvexHullShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mInertia); + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mPoints); + inStream.Read(mFaces); + inStream.Read(mPlanes); + inStream.Read(mVertexIdx); + inStream.Read(mConvexRadius); + inStream.Read(mVolume); + inStream.Read(mInnerRadius); +} + +Shape::Stats ConvexHullShape::GetStats() const +{ + // Count number of triangles + uint triangle_count = 0; + for (const Face &f : mFaces) + triangle_count += f.mNumVertices - 2; + + return Stats( + sizeof(*this) + + mPoints.size() * sizeof(Point) + + mFaces.size() * sizeof(Face) + + mPlanes.size() * sizeof(Plane) + + mVertexIdx.size() * sizeof(uint8), + triangle_count); +} + +void ConvexHullShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::ConvexHull); + f.mConstruct = []() -> Shape * { return new ConvexHullShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h new file mode 100644 index 000000000000..4068791f431a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h @@ -0,0 +1,202 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a ConvexHullShape +class JPH_EXPORT ConvexHullShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConvexHullShapeSettings) + +public: + /// Default constructor for deserialization + ConvexHullShapeSettings() = default; + + /// Create a convex hull from inPoints and maximum convex radius inMaxConvexRadius, the radius is automatically lowered if the hull requires it. + /// (internally this will be subtracted so the total size will not grow with the convex radius). + ConvexHullShapeSettings(const Vec3 *inPoints, int inNumPoints, float inMaxConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints, inPoints + inNumPoints), mMaxConvexRadius(inMaxConvexRadius) { } + ConvexHullShapeSettings(const Array &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Array mPoints; ///< Points to create the hull from + float mMaxConvexRadius = 0.0f; ///< Convex radius as supplied by the constructor. Note that during hull creation the convex radius can be made smaller if the value is too big for the hull. + float mMaxErrorConvexRadius = 0.05f; ///< Maximum distance between the shrunk hull + convex radius and the actual hull. + float mHullTolerance = 1.0e-3f; ///< Points are allowed this far outside of the hull (increasing this yields a hull with less vertices). Note that the actual used value can be larger if the points of the hull are far apart. +}; + +/// A convex hull +class JPH_EXPORT ConvexHullShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Maximum amount of points supported in a convex hull. Note that while constructing a hull, interior points are discarded so you can provide more points. + /// The ConvexHullShapeSettings::Create function will return an error when too many points are provided. + static constexpr int cMaxPointsInHull = 256; + + /// Constructor + ConvexHullShape() : ConvexShape(EShapeSubType::ConvexHull) { } + ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult); + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return mLocalBounds; } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + /// Debugging helper draw function that draws how all points are moved when a shape is shrunk by the convex radius + void DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return mVolume; } + + /// Get the convex radius of this convex hull + float GetConvexRadius() const { return mConvexRadius; } + + /// Get the planes of this convex hull + const Array & GetPlanes() const { return mPlanes; } + + /// Get the number of vertices in this convex hull + inline uint GetNumPoints() const { return uint(mPoints.size()); } + + /// Get a vertex of this convex hull relative to the center of mass + inline Vec3 GetPoint(uint inIndex) const { return mPoints[inIndex].mPosition; } + + /// Get the number of faces in this convex hull + inline uint GetNumFaces() const { return uint(mFaces.size()); } + + /// Get the number of vertices in a face + inline uint GetNumVerticesInFace(uint inFaceIndex) const { return mFaces[inFaceIndex].mNumVertices; } + + /// Get the vertices indices of a face + /// @param inFaceIndex Index of the face. + /// @param inMaxVertices Maximum number of vertices to return. + /// @param outVertices Array of vertices indices, must be at least inMaxVertices in size, the vertices are returned in counter clockwise order and the positions can be obtained using GetPoint(index). + /// @return Number of vertices in face, if this is bigger than inMaxVertices, not all vertices were retrieved. + inline uint GetFaceVertices(uint inFaceIndex, uint inMaxVertices, uint *outVertices) const + { + const Face &face = mFaces[inFaceIndex]; + const uint8 *first_vertex = mVertexIdx.data() + face.mFirstVertex; + uint num_vertices = min(face.mNumVertices, inMaxVertices); + for (uint i = 0; i < num_vertices; ++i) + outVertices[i] = first_vertex[i]; + return face.mNumVertices; + } + + // Register shape functions with the registry + static void sRegister(); + +#ifdef JPH_DEBUG_RENDERER + /// Draw the outlines of the faces of the convex hull when drawing the shape + inline static bool sDrawFaceOutlines = false; +#endif // JPH_DEBUG_RENDERER + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + /// Helper function that returns the min and max fraction along the ray that hits the convex hull. Returns false if there is no hit. + bool CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const; + + /// Class for GetTrianglesStart/Next + class CHSGetTrianglesContext; + + /// Classes for GetSupportFunction + class HullNoConvex; + class HullWithConvex; + class HullWithConvexScaled; + + struct Face + { + uint16 mFirstVertex; ///< First index in mVertexIdx to use + uint16 mNumVertices = 0; ///< Number of vertices in the mVertexIdx to use + }; + + static_assert(sizeof(Face) == 4, "Unexpected size"); + static_assert(alignof(Face) == 2, "Unexpected alignment"); + + struct Point + { + Vec3 mPosition; ///< Position of vertex + int mNumFaces = 0; ///< Number of faces in the face array below + int mFaces[3] = { -1, -1, -1 }; ///< Indices of 3 neighboring faces with the biggest difference in normal (used to shift vertices for convex radius) + }; + + static_assert(sizeof(Point) == 32, "Unexpected size"); + static_assert(alignof(Point) == JPH_VECTOR_ALIGNMENT, "Unexpected alignment"); + + Vec3 mCenterOfMass; ///< Center of mass of this convex hull + Mat44 mInertia; ///< Inertia matrix assuming density is 1 (needs to be multiplied by density) + AABox mLocalBounds; ///< Local bounding box for the convex hull + Array mPoints; ///< Points on the convex hull surface + Array mFaces; ///< Faces of the convex hull surface + Array mPlanes; ///< Planes for the faces (1-on-1 with mFaces array, separate because they need to be 16 byte aligned) + Array mVertexIdx; ///< A list of vertex indices (indexing in mPoints) for each of the faces + float mConvexRadius = 0.0f; ///< Convex radius + float mVolume; ///< Total volume of the convex hull + float mInnerRadius = FLT_MAX; ///< Radius of the biggest sphere that fits entirely in the convex hull + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp new file mode 100644 index 000000000000..35e23ee01cb6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp @@ -0,0 +1,559 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(ConvexShapeSettings) +{ + JPH_ADD_BASE_CLASS(ConvexShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mDensity) + JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mMaterial) +} + +const StaticArray ConvexShape::sUnitSphereTriangles = []() { + const int level = 2; + + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, level); + GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, level); + return verts; +}(); + +void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + const ConvexShape *shape2 = static_cast(inShape2); + + // Get transforms + Mat44 inverse_transform1 = inCenterOfMassTransform1.InversedRotationTranslation(); + Mat44 transform_2_to_1 = inverse_transform1 * inCenterOfMassTransform2; + + // Get bounding boxes + AABox shape1_bbox = shape1->GetLocalBounds().Scaled(inScale1); + shape1_bbox.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + AABox shape2_bbox = shape2->GetLocalBounds().Scaled(inScale2); + + // Check if they overlap + if (!OrientedBox(transform_2_to_1, shape2_bbox).Overlaps(shape1_bbox)) + return; + + // Note: As we don't remember the penetration axis from the last iteration, and it is likely that shape2 is pushed out of + // collision relative to shape1 by comparing their COM's, we use that as an initial penetration axis: shape2.com - shape1.com + // This has been seen to improve performance by approx. 1% over using a fixed axis like (1, 0, 0). + Vec3 penetration_axis = transform_2_to_1.GetTranslation(); + + // Ensure that we do not pass in a near zero penetration axis + if (penetration_axis.IsNearZero()) + penetration_axis = Vec3::sAxisX(); + + Vec3 point1, point2; + EPAPenetrationDepth pen_depth; + EPAPenetrationDepth::EStatus status; + + // Scope to limit lifetime of SupportBuffer + { + // Create support function + SupportBuffer buffer1_excl_cvx_radius, buffer2_excl_cvx_radius; + const Support *shape1_excl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer1_excl_cvx_radius, inScale1); + const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2); + + // Transform shape 2 in the space of shape 1 + TransformedConvexObject transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius); + + // Perform GJK step + status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + inCollideShapeSettings.mMaxSeparationDistance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); + } + + // Check result of collision detection + switch (status) + { + case EPAPenetrationDepth::EStatus::Colliding: + break; + + case EPAPenetrationDepth::EStatus::NotColliding: + return; + + case EPAPenetrationDepth::EStatus::Indeterminate: + { + // Need to run expensive EPA algorithm + + // Create support function + SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius; + const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1); + const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2); + + // Add separation distance + AddConvexRadius shape1_add_max_separation_distance(*shape1_incl_cvx_radius, inCollideShapeSettings.mMaxSeparationDistance); + + // Transform shape 2 in the space of shape 1 + TransformedConvexObject transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius); + + // Perform EPA step + if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2)) + return; + break; + } + } + + // Check if the penetration is bigger than the early out fraction + float penetration_depth = (point2 - point1).Length() - inCollideShapeSettings.mMaxSeparationDistance; + if (-penetration_depth >= ioCollector.GetEarlyOutFraction()) + return; + + // Correct point1 for the added separation distance + float penetration_axis_len = penetration_axis.Length(); + if (penetration_axis_len > 0.0f) + point1 -= penetration_axis * (inCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len); + + // Convert to world space + point1 = inCenterOfMassTransform1 * point1; + point2 = inCenterOfMassTransform1 * point2; + Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Gather faces + if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + shape1->GetSupportingFace(SubShapeID(), -penetration_axis, inScale1, inCenterOfMassTransform1, result.mShape1Face); + + // Get supporting face of shape 2 + shape2->GetSupportingFace(SubShapeID(), transform_2_to_1.Multiply3x3Transposed(penetration_axis), inScale2, inCenterOfMassTransform2, result.mShape2Face); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); +} + +bool ConvexShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Note: This is a fallback routine, most convex shapes should implement a more performant version! + + JPH_PROFILE_FUNCTION(); + + // Create support function + SupportBuffer buffer; + const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + + // Cast ray + GJKClosestPoint gjk; + if (gjk.CastRay(inRay.mOrigin, inRay.mDirection, cDefaultCollisionTolerance, *support, ioHit.mFraction)) + { + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + + return false; +} + +void ConvexShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Note: This is a fallback routine, most convex shapes should implement a more performant version! + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // First do a normal raycast, limited to the early out fraction + RayCastResult hit; + hit.mFraction = ioCollector.GetEarlyOutFraction(); + if (CastRay(inRay, inSubShapeIDCreator, hit)) + { + // Check front side + if (inRayCastSettings.mTreatConvexAsSolid || hit.mFraction > 0.0f) + { + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + ioCollector.AddHit(hit); + } + + // Check if we want back facing hits and the collector still accepts additional hits + if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces && !ioCollector.ShouldEarlyOut()) + { + // Invert the ray, going from the early out fraction back to the fraction where we found our forward hit + float start_fraction = min(1.0f, ioCollector.GetEarlyOutFraction()); + float delta_fraction = hit.mFraction - start_fraction; + if (delta_fraction < 0.0f) + { + RayCast inverted_ray { inRay.mOrigin + start_fraction * inRay.mDirection, delta_fraction * inRay.mDirection }; + + // Cast another ray + RayCastResult inverted_hit; + inverted_hit.mFraction = 1.0f; + if (CastRay(inverted_ray, inSubShapeIDCreator, inverted_hit) + && inverted_hit.mFraction > 0.0f) // Ignore hits with fraction 0, this means the ray ends inside the object and we don't want to report it as a back facing hit + { + // Invert fraction and rescale it to the fraction of the original ray + inverted_hit.mFraction = hit.mFraction + (inverted_hit.mFraction - 1.0f) * delta_fraction; + inverted_hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + ioCollector.AddHit(inverted_hit); + } + } + } + } +} + +void ConvexShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // First test bounding box + if (GetLocalBounds().Contains(inPoint)) + { + // Create support function + SupportBuffer buffer; + const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + + // Create support function for point + PointConvexSupport point { inPoint }; + + // Test intersection + GJKClosestPoint gjk; + Vec3 v = inPoint; + if (gjk.Intersects(*support, point, cDefaultCollisionTolerance, v)) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); + } +} + +void ConvexShape::sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + // Only supported for convex shapes + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); + const ConvexShape *cast_shape = static_cast(inShapeCast.mShape); + + JPH_ASSERT(inShape->GetType() == EShapeType::Convex); + const ConvexShape *shape = static_cast(inShape); + + // Determine if we want to use the actual shape or a shrunken shape with convex radius + ConvexShape::ESupportMode support_mode = inShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default; + + // Create support function for shape to cast + SupportBuffer cast_buffer; + const Support *cast_support = cast_shape->GetSupportFunction(support_mode, cast_buffer, inShapeCast.mScale); + + // Create support function for target shape + SupportBuffer target_buffer; + const Support *target_support = shape->GetSupportFunction(support_mode, target_buffer, inScale); + + // Do a raycast against the result + EPAPenetrationDepth epa; + float fraction = ioCollector.GetEarlyOutFraction(); + Vec3 contact_point_a, contact_point_b, contact_normal; + if (epa.CastShape(inShapeCast.mCenterOfMassStart, inShapeCast.mDirection, inShapeCastSettings.mCollisionTolerance, inShapeCastSettings.mPenetrationTolerance, *cast_support, *target_support, cast_support->GetConvexRadius(), target_support->GetConvexRadius(), inShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal) + && (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + || contact_normal.Dot(inShapeCast.mDirection) > 0.0f)) // Test if backfacing + { + // Convert to world space + contact_point_a = inCenterOfMassTransform2 * contact_point_a; + contact_point_b = inCenterOfMassTransform2 * contact_point_b; + Vec3 contact_normal_world = inCenterOfMassTransform2.Multiply3x3(contact_normal); + + ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, false, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Early out if this hit is deeper than the collector's early out value + if (fraction == 0.0f && -result.mPenetrationDepth >= ioCollector.GetEarlyOutFraction()) + return; + + // Gather faces + if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + Mat44 transform_1_to_2 = inShapeCast.mCenterOfMassStart; + transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * inShapeCast.mDirection); + cast_shape->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), inShapeCast.mScale, inCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face); + + // Get supporting face of shape 2 + shape->GetSupportingFace(SubShapeID(), contact_normal, inScale, inCenterOfMassTransform2, result.mShape2Face); + } + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); + } +} + +class ConvexShape::CSGetTrianglesContext +{ +public: + CSGetTrianglesContext(const ConvexShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + mSupport = inShape->GetSupportFunction(ESupportMode::IncludeConvexRadius, mSupportBuffer, Vec3::sReplicate(1.0f)); + } + + SupportBuffer mSupportBuffer; + const Support * mSupport; + Mat44 mLocalToWorld; + bool mIsInsideOut; + size_t mCurrentVertex = 0; +}; + +void ConvexShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(CSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(CSGetTrianglesContext))); + + new (&ioContext) CSGetTrianglesContext(this, inPositionCOM, inRotation, inScale); +} + +int ConvexShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + CSGetTrianglesContext &context = (CSGetTrianglesContext &)ioContext; + + int total_num_vertices = min(inMaxTrianglesRequested * 3, int(sUnitSphereTriangles.size() - context.mCurrentVertex)); + + if (context.mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++); + } + } + + context.mCurrentVertex += total_num_vertices; + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + { + const PhysicsMaterial *material = GetMaterial(); + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = material; + } + + return total_num_triangles; +} + +void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Calculate total volume + Vec3 abs_scale = inScale.Abs(); + Vec3 extent = GetLocalBounds().GetExtent() * abs_scale; + outTotalVolume = 8.0f * extent.GetX() * extent.GetY() * extent.GetZ(); + + // Points of the bounding box + Vec3 points[] = + { + Vec3(-1, -1, -1), + Vec3( 1, -1, -1), + Vec3(-1, 1, -1), + Vec3( 1, 1, -1), + Vec3(-1, -1, 1), + Vec3( 1, -1, 1), + Vec3(-1, 1, 1), + Vec3( 1, 1, 1), + }; + + // Faces of the bounding box + using Face = int[5]; + #define MAKE_FACE(a, b, c, d) { a, b, c, d, ((1 << a) | (1 << b) | (1 << c) | (1 << d)) } // Last int is a bit mask that indicates which indices are used + Face faces[] = + { + MAKE_FACE(0, 2, 3, 1), + MAKE_FACE(4, 6, 2, 0), + MAKE_FACE(4, 5, 7, 6), + MAKE_FACE(1, 3, 7, 5), + MAKE_FACE(2, 6, 7, 3), + MAKE_FACE(0, 1, 5, 4), + }; + + PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(8 * sizeof(PolyhedronSubmergedVolumeCalculator::Point)); + PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(extent), points, sizeof(Vec3), 8, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + if (submerged_vol_calc.AreAllAbove()) + { + // We're above the water + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (submerged_vol_calc.AreAllBelow()) + { + // We're fully submerged + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Calculate submerged volume + int reference_point_bit = 1 << submerged_vol_calc.GetReferencePointIdx(); + for (const Face &f : faces) + { + // Test if this face includes the reference point + if ((f[4] & reference_point_bit) == 0) + { + // Triangulate the face (a quad) + submerged_vol_calc.AddFace(f[0], f[1], f[2]); + submerged_vol_calc.AddFace(f[0], f[2], f[3]); + } + } + + submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy); + } +} + +#ifdef JPH_DEBUG_RENDERER +void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + // Get the support function with convex radius + SupportBuffer buffer; + const Support *support = GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale); + AddConvexRadius add_convex(*support, support->GetConvexRadius()); + + // Draw the shape + DebugRenderer::GeometryRef geometry = inRenderer->CreateTriangleGeometryForConvex([&add_convex](Vec3Arg inDirection) { return add_convex.GetSupport(inDirection); }); + AABox bounds = geometry->mBounds.Transformed(inCenterOfMassTransform); + float lod_scale_sq = geometry->mBounds.GetExtent().LengthSq(); + inRenderer->DrawGeometry(inCenterOfMassTransform, bounds, lod_scale_sq, inColor, geometry); + + if (inDrawSupportDirection) + { + // Iterate on all directions and draw the support point and an arrow in the direction that was sampled to test if the support points make sense + for (Vec3 v : Vec3::sUnitSphere) + { + Vec3 direction = 0.05f * v; + Vec3 pos = add_convex.GetSupport(direction); + RVec3 from = inCenterOfMassTransform * pos; + RVec3 to = inCenterOfMassTransform * (pos + direction); + inRenderer->DrawMarker(from, Color::sWhite, 0.001f); + inRenderer->DrawArrow(from, to, Color::sWhite, 0.001f); + } + } +} + +void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + // Sample directions and map which faces belong to which directions + using FaceToDirection = UnorderedMap>; + FaceToDirection faces; + for (Vec3 v : Vec3::sUnitSphere) + { + Vec3 direction = 0.05f * v; + + SupportingFace face; + GetSupportingFace(SubShapeID(), direction, inScale, Mat44::sIdentity(), face); + + if (!face.empty()) + { + JPH_ASSERT(face.size() >= 2, "The GetSupportingFace function should either return nothing or at least an edge"); + faces[face].push_back(direction); + } + } + + // Draw each face in a unique color and draw corresponding directions + int color_it = 0; + for (FaceToDirection::value_type &ftd : faces) + { + Color color = Color::sGetDistinctColor(color_it++); + + // Create copy of face (key in map is read only) + SupportingFace face = ftd.first; + + // Displace the face a little bit forward so it is easier to see + Vec3 normal = face.size() >= 3? (face[2] - face[1]).Cross(face[0] - face[1]).Normalized() : Vec3::sZero(); + Vec3 displacement = 0.001f * normal; + + // Transform face to world space and calculate center of mass + Vec3 com_ls = Vec3::sZero(); + for (Vec3 &v : face) + { + v = inCenterOfMassTransform.Multiply3x3(v + displacement); + com_ls += v; + } + RVec3 com = inCenterOfMassTransform.GetTranslation() + com_ls / (float)face.size(); + + // Draw the polygon and directions + inRenderer->DrawWirePolygon(RMat44::sTranslation(inCenterOfMassTransform.GetTranslation()), face, color, face.size() >= 3? 0.001f : 0.0f); + if (face.size() >= 3) + inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(normal), color, 0.01f); + for (Vec3 &v : ftd.second) + inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(-v), color, 0.001f); + } +} +#endif // JPH_DEBUG_RENDERER + +void ConvexShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mDensity); +} + +void ConvexShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mDensity); +} + +void ConvexShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials.clear(); + outMaterials.push_back(mMaterial); +} + +void ConvexShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + JPH_ASSERT(inNumMaterials == 1); + mMaterial = inMaterials[0]; +} + +void ConvexShape::sRegister() +{ + for (EShapeSubType s1 : sConvexSubShapeTypes) + for (EShapeSubType s2 : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s1, s2, sCollideConvexVsConvex); + CollisionDispatch::sRegisterCastShape(s1, s2, sCastConvexVsConvex); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h new file mode 100644 index 000000000000..a4eab8755342 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h @@ -0,0 +1,150 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a ConvexShape (abstract) +class JPH_EXPORT ConvexShapeSettings : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ConvexShapeSettings) + +public: + /// Constructor + ConvexShapeSettings() = default; + explicit ConvexShapeSettings(const PhysicsMaterial *inMaterial) : mMaterial(inMaterial) { } + + /// Set the density of the object in kg / m^3 + void SetDensity(float inDensity) { mDensity = inDensity; } + + // Properties + RefConst mMaterial; ///< Material assigned to this shape + float mDensity = 1000.0f; ///< Uniform density of the interior of the convex object (kg / m^3) +}; + +/// Base class for all convex shapes. Defines a virtual interface. +class JPH_EXPORT ConvexShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ConvexShape(EShapeSubType inSubType) : Shape(EShapeType::Convex, inSubType) { } + ConvexShape(EShapeSubType inSubType, const ConvexShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Convex, inSubType, inSettings, outResult), mMaterial(inSettings.mMaterial), mDensity(inSettings.mDensity) { } + ConvexShape(EShapeSubType inSubType, const PhysicsMaterial *inMaterial) : Shape(EShapeType::Convex, inSubType), mMaterial(inMaterial) { } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return 0; } // Convex shapes don't have sub shapes + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return GetMaterial(); } + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + + /// Function that provides an interface for GJK + class Support + { + public: + /// Warning: Virtual destructor will not be called on this object! + virtual ~Support() = default; + + /// Calculate the support vector for this convex shape (includes / excludes the convex radius depending on how this was obtained). + /// Support vector is relative to the center of mass of the shape. + virtual Vec3 GetSupport(Vec3Arg inDirection) const = 0; + + /// Convex radius of shape. Collision detection on penetrating shapes is much more expensive, + /// so you can add a radius around objects to increase the shape. This makes it far less likely that they will actually penetrate. + virtual float GetConvexRadius() const = 0; + }; + + /// Buffer to hold a Support object, used to avoid dynamic memory allocations + class alignas(16) SupportBuffer + { + public: + uint8 mData[4160]; + }; + + /// How the GetSupport function should behave + enum class ESupportMode + { + ExcludeConvexRadius, ///< Return the shape excluding the convex radius, Support::GetConvexRadius will return the convex radius if there is one, but adding this radius may not result in the most accurate/efficient representation of shapes with sharp edges + IncludeConvexRadius, ///< Return the shape including the convex radius, Support::GetSupport includes the convex radius if there is one, Support::GetConvexRadius will return 0 + Default, ///< Use both Support::GetSupport add Support::GetConvexRadius to get a support point that matches the original shape as accurately/efficiently as possible + }; + + /// Returns an object that provides the GetSupport function for this shape. + /// inMode determines if this support function includes or excludes the convex radius. + /// of the values returned by the GetSupport function. This improves numerical accuracy of the results. + /// inScale scales this shape in local space. + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const = 0; + + /// Material of the shape + void SetMaterial(const PhysicsMaterial *inMaterial) { mMaterial = inMaterial; } + const PhysicsMaterial * GetMaterial() const { return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; } + + /// Set density of the shape (kg / m^3) + void SetDensity(float inDensity) { mDensity = inDensity; } + + /// Get density of the shape (kg / m^3) + float GetDensity() const { return mDensity; } + +#ifdef JPH_DEBUG_RENDERER + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + + /// Vertex list that forms a unit sphere + static const StaticArray sUnitSphereTriangles; + +private: + // Class for GetTrianglesStart/Next + class CSGetTrianglesContext; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsConvex(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Properties + RefConst mMaterial; ///< Material assigned to this shape + float mDensity = 1000.0f; ///< Uniform density of the interior of the convex object (kg / m^3) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp new file mode 100644 index 000000000000..06fc5ec7184a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp @@ -0,0 +1,404 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CylinderShapeSettings) +{ + JPH_ADD_BASE_CLASS(CylinderShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mHalfHeight) + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mRadius) + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mConvexRadius) +} + +// Approximation of top face with 8 vertices +static const Vec3 cCylinderTopFace[] = +{ + Vec3(0.0f, 1.0f, 1.0f), + Vec3(0.707106769f, 1.0f, 0.707106769f), + Vec3(1.0f, 1.0f, 0.0f), + Vec3(0.707106769f, 1.0f, -0.707106769f), + Vec3(-0.0f, 1.0f, -1.0f), + Vec3(-0.707106769f, 1.0f, -0.707106769f), + Vec3(-1.0f, 1.0f, 0.0f), + Vec3(-0.707106769f, 1.0f, 0.707106769f) +}; + +static const StaticArray sUnitCylinderTriangles = []() { + StaticArray verts; + + const Vec3 bottom_offset(0.0f, -2.0f, 0.0f); + + int num_verts = sizeof(cCylinderTopFace) / sizeof(Vec3); + for (int i = 0; i < num_verts; ++i) + { + Vec3 t1 = cCylinderTopFace[i]; + Vec3 t2 = cCylinderTopFace[(i + 1) % num_verts]; + Vec3 b1 = cCylinderTopFace[i] + bottom_offset; + Vec3 b2 = cCylinderTopFace[(i + 1) % num_verts] + bottom_offset; + + // Top + verts.emplace_back(0.0f, 1.0f, 0.0f); + verts.push_back(t1); + verts.push_back(t2); + + // Bottom + verts.emplace_back(0.0f, -1.0f, 0.0f); + verts.push_back(b2); + verts.push_back(b1); + + // Side + verts.push_back(t1); + verts.push_back(b1); + verts.push_back(t2); + + verts.push_back(t2); + verts.push_back(b1); + verts.push_back(b2); + } + + return verts; +}(); + +ShapeSettings::ShapeResult CylinderShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new CylinderShape(*this, mCachedResult); + return mCachedResult; +} + +CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Cylinder, inSettings, outResult), + mHalfHeight(inSettings.mHalfHeight), + mRadius(inSettings.mRadius), + mConvexRadius(inSettings.mConvexRadius) +{ + if (inSettings.mHalfHeight < inSettings.mConvexRadius) + { + outResult.SetError("Invalid height"); + return; + } + + if (inSettings.mRadius < inSettings.mConvexRadius) + { + outResult.SetError("Invalid radius"); + return; + } + + if (inSettings.mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + outResult.Set(this); +} + +CylinderShape::CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) : + ConvexShape(EShapeSubType::Cylinder, inMaterial), + mHalfHeight(inHalfHeight), + mRadius(inRadius), + mConvexRadius(inConvexRadius) +{ + JPH_ASSERT(inHalfHeight >= inConvexRadius); + JPH_ASSERT(inRadius >= inConvexRadius); + JPH_ASSERT(inConvexRadius >= 0.0f); +} + +class CylinderShape::Cylinder final : public Support +{ +public: + Cylinder(float inHalfHeight, float inRadius, float inConvexRadius) : + mHalfHeight(inHalfHeight), + mRadius(inRadius), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(Cylinder) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(Cylinder))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Support mapping, taken from: + // A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen + // page 8 + float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ(); + float o = sqrt(Square(x) + Square(z)); + if (o > 0.0f) + return Vec3((mRadius * x) / o, Sign(y) * mHalfHeight, (mRadius * z) / o); + else + return Vec3(0, Sign(y) * mHalfHeight, 0); + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + float mHalfHeight; + float mRadius; + float mConvexRadius; +}; + +const ConvexShape::Support *CylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = abs_scale.GetY(); + float scaled_half_height = scale_y * mHalfHeight; + float scaled_radius = scale_xz * mRadius; + float scaled_convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) Cylinder(scaled_half_height, scaled_radius, 0.0f); + + case ESupportMode::ExcludeConvexRadius: + return new (&inBuffer) Cylinder(scaled_half_height - scaled_convex_radius, scaled_radius - scaled_convex_radius, scaled_convex_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = abs_scale.GetY(); + float scaled_half_height = scale_y * mHalfHeight; + float scaled_radius = scale_xz * mRadius; + + float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ(); + float o = sqrt(Square(x) + Square(z)); + + // If o / |y| > scaled_radius / scaled_half_height, we're hitting the side + if (o * scaled_half_height > scaled_radius * abs(y)) + { + // Hitting side + float f = -scaled_radius / o; + float vx = x * f; + float vz = z * f; + outVertices.push_back(inCenterOfMassTransform * Vec3(vx, scaled_half_height, vz)); + outVertices.push_back(inCenterOfMassTransform * Vec3(vx, -scaled_half_height, vz)); + } + else + { + // Hitting top or bottom + Vec3 multiplier = y < 0.0f? Vec3(scaled_radius, scaled_half_height, scaled_radius) : Vec3(-scaled_radius, -scaled_half_height, scaled_radius); + Mat44 transform = inCenterOfMassTransform.PreScaled(multiplier); + for (const Vec3 &v : cCylinderTopFace) + outVertices.push_back(transform * v); + } +} + +MassProperties CylinderShape::GetMassProperties() const +{ + MassProperties p; + + // Mass is surface of circle * height + float radius_sq = Square(mRadius); + float height = 2.0f * mHalfHeight; + p.mMass = JPH_PI * radius_sq * height * GetDensity(); + + // Inertia according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia: + float inertia_y = radius_sq * p.mMass * 0.5f; + float inertia_x = inertia_y * 0.5f + p.mMass * height * height / 12.0f; + float inertia_z = inertia_x; + + // Set inertia + p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z)); + + return p; +} + +Vec3 CylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Calculate distance to infinite cylinder surface + Vec3 local_surface_position_xz(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()); + float local_surface_position_xz_len = local_surface_position_xz.Length(); + float distance_to_curved_surface = abs(local_surface_position_xz_len - mRadius); + + // Calculate distance to top or bottom plane + float distance_to_top_or_bottom = abs(abs(inLocalSurfacePosition.GetY()) - mHalfHeight); + + // Return normal according to closest surface + if (distance_to_curved_surface < distance_to_top_or_bottom) + return local_surface_position_xz / local_surface_position_xz_len; + else + return inLocalSurfacePosition.GetY() > 0.0f? Vec3::sAxisY() : -Vec3::sAxisY(); +} + +AABox CylinderShape::GetLocalBounds() const +{ + Vec3 extent = Vec3(mRadius, mHalfHeight, mRadius); + return AABox(-extent, extent); +} + +#ifdef JPH_DEBUG_RENDERER +void CylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawCylinder(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), mHalfHeight, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool CylinderShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test ray against capsule + float fraction = RayCylinder(inRay.mOrigin, inRay.mDirection, mHalfHeight, mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if the point is in the cylinder + if (abs(inPoint.GetY()) <= mHalfHeight // Within the height + && Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mRadius)) // Within the radius + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float half_height = abs_scale.GetY() * mHalfHeight; + float radius = abs_scale.GetX() * mRadius; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + Vec3 local_pos = inverse_transform * v.GetPosition(); + + // Calculate penetration into side surface + Vec3 side_normal = local_pos; + side_normal.SetY(0.0f); + float side_normal_length = side_normal.Length(); + float side_penetration = radius - side_normal_length; + + // Calculate penetration into top or bottom plane + float top_penetration = half_height - abs(local_pos.GetY()); + + Vec3 point, normal; + if (side_penetration < 0.0f && top_penetration < 0.0f) + { + // We're outside the cylinder height and radius + point = side_normal * (radius / side_normal_length) + Vec3(0, half_height * Sign(local_pos.GetY()), 0); + normal = (local_pos - point).NormalizedOr(Vec3::sAxisY()); + } + else if (side_penetration < top_penetration) + { + // Side surface is closest + normal = side_normal_length > 0.0f? side_normal / side_normal_length : Vec3::sAxisX(); + point = radius * normal; + } + else + { + // Top or bottom plane is closest + normal = Vec3(0, Sign(local_pos.GetY()), 0); + point = half_height * normal; + } + + // Calculate penetration + Plane plane = Plane::sFromPointAndNormal(point, normal); + float penetration = -plane.SignedDistance(local_pos); + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } +} + +void CylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + Mat44 unit_cylinder_transform(Vec4(mRadius, 0, 0, 0), Vec4(0, mHalfHeight, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, 0, 0, 1)); + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, unit_cylinder_transform, sUnitCylinderTriangles.data(), sUnitCylinderTriangles.size(), GetMaterial()); +} + +int CylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void CylinderShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mHalfHeight); + inStream.Write(mRadius); + inStream.Write(mConvexRadius); +} + +void CylinderShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mHalfHeight); + inStream.Read(mRadius); + inStream.Read(mConvexRadius); +} + +bool CylinderShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs()); +} + +Vec3 CylinderShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs()); +} + +void CylinderShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Cylinder); + f.mConstruct = []() -> Shape * { return new CylinderShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h new file mode 100644 index 000000000000..302c97565b0b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h @@ -0,0 +1,126 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a CylinderShape +class JPH_EXPORT CylinderShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CylinderShapeSettings) + +public: + /// Default constructor for deserialization + CylinderShapeSettings() = default; + + /// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius. + /// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit). + CylinderShapeSettings(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfHeight(inHalfHeight), mRadius(inRadius), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mHalfHeight = 0.0f; + float mRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +/// A cylinder +class JPH_EXPORT CylinderShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CylinderShape() : ConvexShape(EShapeSubType::Cylinder) { } + CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius. + /// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit). + CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr); + + /// Get half height of cylinder + float GetHalfHeight() const { return mHalfHeight; } + + /// Get radius of cylinder + float GetRadius() const { return mRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return min(mHalfHeight, mRadius); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + using ConvexShape::CastRay; + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 2.0f * JPH_PI * mHalfHeight * Square(mRadius); } + + /// Get the convex radius of this cylinder + float GetConvexRadius() const { return mConvexRadius; } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class Cylinder; + + float mHalfHeight = 0.0f; + float mRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp new file mode 100644 index 000000000000..339f78363761 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(DecoratedShapeSettings) +{ + JPH_ADD_BASE_CLASS(DecoratedShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(DecoratedShapeSettings, mInnerShape) +} + +DecoratedShape::DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::Decorated, inSubType, inSettings, outResult) +{ + // Check that there's a shape + if (inSettings.mInnerShape == nullptr && inSettings.mInnerShapePtr == nullptr) + { + outResult.SetError("Inner shape is null!"); + return; + } + + if (inSettings.mInnerShapePtr != nullptr) + { + // Use provided shape + mInnerShape = inSettings.mInnerShapePtr; + } + else + { + // Create child shape + ShapeResult child_result = inSettings.mInnerShape->Create(); + if (!child_result.IsValid()) + { + outResult = child_result; + return; + } + mInnerShape = child_result.Get(); + } +} + +const PhysicsMaterial *DecoratedShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + return mInnerShape->GetMaterial(inSubShapeID); +} + +void DecoratedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform, outVertices); +} + +uint64 DecoratedShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const +{ + return mInnerShape->GetSubShapeUserData(inSubShapeID); +} + +void DecoratedShape::SaveSubShapeState(ShapeList &outSubShapes) const +{ + outSubShapes.clear(); + outSubShapes.push_back(mInnerShape); +} + +void DecoratedShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) +{ + JPH_ASSERT(inNumShapes == 1); + mInnerShape = inSubShapes[0]; +} + +Shape::Stats DecoratedShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + // Get own stats + Stats stats = Shape::GetStatsRecursive(ioVisitedShapes); + + // Add child stats + Stats child_stats = mInnerShape->GetStatsRecursive(ioVisitedShapes); + stats.mSizeBytes += child_stats.mSizeBytes; + stats.mNumTriangles += child_stats.mNumTriangles; + + return stats; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h new file mode 100644 index 000000000000..5ab8748fc91c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a DecoratedShape +class JPH_EXPORT DecoratedShapeSettings : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DecoratedShapeSettings) + +public: + /// Default constructor for deserialization + DecoratedShapeSettings() = default; + + /// Constructor that decorates another shape + explicit DecoratedShapeSettings(const ShapeSettings *inShape) : mInnerShape(inShape) { } + explicit DecoratedShapeSettings(const Shape *inShape) : mInnerShapePtr(inShape) { } + + RefConst mInnerShape; ///< Sub shape (either this or mShapePtr needs to be filled up) + RefConst mInnerShapePtr; ///< Sub shape (either this or mShape needs to be filled up) +}; + +/// Base class for shapes that decorate another shape with extra functionality (e.g. scale, translation etc.) +class JPH_EXPORT DecoratedShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit DecoratedShape(EShapeSubType inSubType) : Shape(EShapeType::Decorated, inSubType) { } + DecoratedShape(EShapeSubType inSubType, const Shape *inInnerShape) : Shape(EShapeType::Decorated, inSubType), mInnerShape(inInnerShape) { } + DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult); + + /// Access to the decorated inner shape + const Shape * GetInnerShape() const { return mInnerShape; } + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return mInnerShape->MustBeStatic(); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mInnerShape->GetCenterOfMass(); } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return mInnerShape->GetSubShapeIDBitsRecursive(); } + + // See Shape::GetLeafShape + virtual const Shape * GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const override { return mInnerShape->GetLeafShape(inSubShapeID, outRemainder); } + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubShapeUserData + virtual uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const override; + + // See Shape + virtual void SaveSubShapeState(ShapeList &outSubShapes) const override; + virtual void RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override; + + // See Shape::GetStatsRecursive + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override { return mInnerShape->IsValidScale(inScale); } + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override { return mInnerShape->MakeScaleValid(inScale); } + +protected: + RefConst mInnerShape; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp new file mode 100644 index 000000000000..8cb81f68d038 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp @@ -0,0 +1,65 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(EmptyShapeSettings) +{ + JPH_ADD_BASE_CLASS(EmptyShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(EmptyShapeSettings, mCenterOfMass) +} + +ShapeSettings::ShapeResult EmptyShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + new EmptyShape(*this, mCachedResult); + + return mCachedResult; +} + +MassProperties EmptyShape::GetMassProperties() const +{ + MassProperties mass_properties; + mass_properties.mMass = 1.0f; + mass_properties.mInertia = Mat44::sIdentity(); + return mass_properties; +} + +#ifdef JPH_DEBUG_RENDERER +void EmptyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, [[maybe_unused]] bool inUseMaterialColors, [[maybe_unused]] bool inDrawWireframe) const +{ + inRenderer->DrawMarker(inCenterOfMassTransform.GetTranslation(), inColor, abs(inScale.GetX()) * 0.1f); +} +#endif // JPH_DEBUG_RENDERER + +void EmptyShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Empty); + f.mConstruct = []() -> Shape * { return new EmptyShape; }; + f.mColor = Color::sBlack; + + auto collide_empty = []([[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] Vec3Arg inScale1, [[maybe_unused]] Vec3Arg inScale2, [[maybe_unused]] Mat44Arg inCenterOfMassTransform1, [[maybe_unused]] Mat44Arg inCenterOfMassTransform2, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator1, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator2, [[maybe_unused]] const CollideShapeSettings &inCollideShapeSettings, [[maybe_unused]] CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) { /* Do Nothing */ }; + auto cast_empty = []([[maybe_unused]] const ShapeCast &inShapeCast, [[maybe_unused]] const ShapeCastSettings &inShapeCastSettings, [[maybe_unused]] const Shape *inShape, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, [[maybe_unused]] Mat44Arg inCenterOfMassTransform2, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator1, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator2, [[maybe_unused]] CastShapeCollector &ioCollector) { /* Do nothing */ }; + + for (const EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Empty, s, collide_empty); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Empty, collide_empty); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::Empty, s, cast_empty); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Empty, cast_empty); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h new file mode 100644 index 000000000000..f3e7bdcf6ae6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h @@ -0,0 +1,75 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs an EmptyShape +class JPH_EXPORT EmptyShapeSettings final : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, EmptyShapeSettings) + +public: + EmptyShapeSettings() = default; + explicit EmptyShapeSettings(Vec3Arg inCenterOfMass) : mCenterOfMass(inCenterOfMass) { } + + ShapeResult Create() const override; + + Vec3 mCenterOfMass = Vec3::sZero(); ///< Determines the center of mass for this shape +}; + +/// An empty shape that has no volume and collides with nothing. +/// +/// Possible use cases: +/// - As a placeholder for a shape that will be created later. E.g. if you first need to create a body and only then know what shape it will have. +/// - If you need a kinematic body to attach a constraint to, but you don't want the body to collide with anything. +/// +/// Note that, if possible, you should also put your body in an ObjectLayer that doesn't collide with anything. +/// This ensures that collisions will be filtered out at broad phase level instead of at narrow phase level, this is more efficient. +class JPH_EXPORT EmptyShape final : public Shape +{ +public: + // Constructor + EmptyShape() : Shape(EShapeType::Empty, EShapeSubType::Empty) { } + explicit EmptyShape(Vec3Arg inCenterOfMass) : Shape(EShapeType::Empty, EShapeSubType::Empty), mCenterOfMass(inCenterOfMass) { } + EmptyShape(const EmptyShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Empty, EShapeSubType::Empty, inSettings, outResult), mCenterOfMass(inSettings.mCenterOfMass) { outResult.Set(this); } + + // See: Shape + Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + AABox GetLocalBounds() const override { return { Vec3::sZero(), Vec3::sZero() }; } + uint GetSubShapeIDBitsRecursive() const override { return 0; } + float GetInnerRadius() const override { return 0.0f; } + MassProperties GetMassProperties() const override; + const PhysicsMaterial * GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override { return PhysicsMaterial::sDefault; } + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { return Vec3::sZero(); } + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3Arg inBaseOffset +#endif + ) const override { outTotalVolume = 0.0f; outSubmergedVolume = 0.0f; outCenterOfBuoyancy = Vec3::sZero(); } +#ifdef JPH_DEBUG_RENDERER + virtual void Draw([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inUseMaterialColors, [[maybe_unused]] bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + virtual bool CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] RayCastResult &ioHit) const override { return false; } + virtual void CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const RayCastSettings &inRayCastSettings, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CastRayCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ } + virtual void CollidePoint([[maybe_unused]] Vec3Arg inPoint, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CollidePointCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ } + virtual void CollideSoftBodyVertices([[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const CollideSoftBodyVertexIterator &inVertices, [[maybe_unused]] uint inNumVertices, [[maybe_unused]] int inCollidingShapeIndex) const override { /* Do nothing */ } + virtual void GetTrianglesStart([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, [[maybe_unused]] Vec3Arg inPositionCOM, [[maybe_unused]] QuatArg inRotation, [[maybe_unused]] Vec3Arg inScale) const override { /* Do nothing */ } + virtual int GetTrianglesNext([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] int inMaxTrianglesRequested, [[maybe_unused]] Float3 *outTriangleVertices, [[maybe_unused]] const PhysicsMaterial **outMaterials = nullptr) const override { return 0; } + Stats GetStats() const override { return { sizeof(*this), 0 }; } + float GetVolume() const override { return 0.0f; } + bool IsValidScale([[maybe_unused]] Vec3Arg inScale) const override { return true; } + + // Register shape functions with the registry + static void sRegister(); + +private: + Vec3 mCenterOfMass = Vec3::sZero(); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h new file mode 100644 index 000000000000..df68ae2f55be --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h @@ -0,0 +1,248 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsMaterial; + +/// Implementation of GetTrianglesStart/Next that uses a fixed list of vertices for the triangles. These are transformed into world space when getting the triangles. +class GetTrianglesContextVertexList +{ +public: + /// Constructor, to be called in GetTrianglesStart + GetTrianglesContextVertexList(Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, Mat44Arg inLocalTransform, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices, const PhysicsMaterial *inMaterial) : + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale) * inLocalTransform), + mTriangleVertices(inTriangleVertices), + mNumTriangleVertices(inNumTriangleVertices), + mMaterial(inMaterial), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + static_assert(sizeof(GetTrianglesContextVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextVertexList))); + JPH_ASSERT(inNumTriangleVertices % 3 == 0); + } + + /// @see Shape::GetTrianglesNext + int GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) + { + JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested); + + int total_num_vertices = min(inMaxTrianglesRequested * 3, int(mNumTriangleVertices - mCurrentVertex)); + + if (mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + } + } + + // Update the current vertex to point to the next vertex to get + mCurrentVertex += total_num_vertices; + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = mMaterial; + + return total_num_triangles; + } + + /// Helper function that creates a vertex list of a half unit sphere (top part) + template + static void sCreateHalfUnitSphereTop(A &ioVertices, int inDetailLevel) + { + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel); + } + + /// Helper function that creates a vertex list of a half unit sphere (bottom part) + template + static void sCreateHalfUnitSphereBottom(A &ioVertices, int inDetailLevel) + { + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel); + } + + /// Helper function that creates an open cylinder of half height 1 and radius 1 + template + static void sCreateUnitOpenCylinder(A &ioVertices, int inDetailLevel) + { + const Vec3 bottom_offset(0.0f, -2.0f, 0.0f); + int num_verts = 4 * (1 << inDetailLevel); + for (int i = 0; i < num_verts; ++i) + { + float angle1 = 2.0f * JPH_PI * (float(i) / num_verts); + float angle2 = 2.0f * JPH_PI * (float(i + 1) / num_verts); + + Vec3 t1(Sin(angle1), 1.0f, Cos(angle1)); + Vec3 t2(Sin(angle2), 1.0f, Cos(angle2)); + Vec3 b1 = t1 + bottom_offset; + Vec3 b2 = t2 + bottom_offset; + + ioVertices.push_back(t1); + ioVertices.push_back(b1); + ioVertices.push_back(t2); + + ioVertices.push_back(t2); + ioVertices.push_back(b1); + ioVertices.push_back(b2); + } + } + +private: + /// Recursive helper function for creating a sphere + template + static void sCreateUnitSphereHelper(A &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, int inLevel) + { + Vec3 center1 = (inV1 + inV2).Normalized(); + Vec3 center2 = (inV2 + inV3).Normalized(); + Vec3 center3 = (inV3 + inV1).Normalized(); + + if (inLevel > 0) + { + int new_level = inLevel - 1; + sCreateUnitSphereHelper(ioVertices, inV1, center1, center3, new_level); + sCreateUnitSphereHelper(ioVertices, center1, center2, center3, new_level); + sCreateUnitSphereHelper(ioVertices, center1, inV2, center2, new_level); + sCreateUnitSphereHelper(ioVertices, center3, center2, inV3, new_level); + } + else + { + ioVertices.push_back(inV1); + ioVertices.push_back(inV2); + ioVertices.push_back(inV3); + } + } + + Mat44 mLocalToWorld; + const Vec3 * mTriangleVertices; + size_t mNumTriangleVertices; + size_t mCurrentVertex = 0; + const PhysicsMaterial * mMaterial; + bool mIsInsideOut; +}; + +/// Implementation of GetTrianglesStart/Next that uses a multiple fixed lists of vertices for the triangles. These are transformed into world space when getting the triangles. +class GetTrianglesContextMultiVertexList +{ +public: + /// Constructor, to be called in GetTrianglesStart + GetTrianglesContextMultiVertexList(bool inIsInsideOut, const PhysicsMaterial *inMaterial) : + mMaterial(inMaterial), + mIsInsideOut(inIsInsideOut) + { + static_assert(sizeof(GetTrianglesContextMultiVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextMultiVertexList))); + } + + /// Add a mesh part and its transform + void AddPart(Mat44Arg inLocalToWorld, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices) + { + JPH_ASSERT(inNumTriangleVertices % 3 == 0); + + mParts.push_back({ inLocalToWorld, inTriangleVertices, inNumTriangleVertices }); + } + + /// @see Shape::GetTrianglesNext + int GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) + { + JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested); + + int total_num_vertices = 0; + int max_vertices_requested = inMaxTrianglesRequested * 3; + + // Loop over parts + for (; mCurrentPart < mParts.size(); ++mCurrentPart) + { + const Part &part = mParts[mCurrentPart]; + + // Calculate how many vertices to take from this part + int part_num_vertices = min(max_vertices_requested, int(part.mNumTriangleVertices - mCurrentVertex)); + if (part_num_vertices == 0) + break; + + max_vertices_requested -= part_num_vertices; + total_num_vertices += part_num_vertices; + + if (mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3) + { + (part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3) + { + (part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + } + } + + // Update the current vertex to point to the next vertex to get + mCurrentVertex += part_num_vertices; + + // Check if we completed this part + if (mCurrentVertex < part.mNumTriangleVertices) + break; + + // Reset current vertex for the next part + mCurrentVertex = 0; + } + + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = mMaterial; + + return total_num_triangles; + } + +private: + struct Part + { + Mat44 mLocalToWorld; + const Vec3 * mTriangleVertices; + size_t mNumTriangleVertices; + }; + + StaticArray mParts; + uint mCurrentPart = 0; + size_t mCurrentVertex = 0; + const PhysicsMaterial * mMaterial; + bool mIsInsideOut; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp new file mode 100644 index 000000000000..fcecdaf92dc0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp @@ -0,0 +1,2714 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define JPH_DEBUG_HEIGHT_FIELD + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool HeightFieldShape::sDrawTriangleOutlines = false; +#endif // JPH_DEBUG_RENDERER + +using namespace HeightFieldShapeConstants; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HeightFieldShapeSettings) +{ + JPH_ADD_BASE_CLASS(HeightFieldShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mHeightSamples) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mOffset) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mScale) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMinHeightValue) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaxHeightValue) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialsCapacity) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mSampleCount) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBlockSize) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBitsPerSample) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialIndices) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterials) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mActiveEdgeCosThresholdAngle) +} + +const uint HeightFieldShape::sGridOffsets[] = +{ + 0, // level: 0, max x/y: 0, offset: 0 + 1, // level: 1, max x/y: 1, offset: 1 + 5, // level: 2, max x/y: 3, offset: 1 + 4 + 21, // level: 3, max x/y: 7, offset: 1 + 4 + 16 + 85, // level: 4, max x/y: 15, offset: 1 + 4 + 16 + 64 + 341, // level: 5, max x/y: 31, offset: 1 + 4 + 16 + 64 + 256 + 1365, // level: 6, max x/y: 63, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 5461, // level: 7, max x/y: 127, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + 21845, // level: 8, max x/y: 255, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 87381, // level: 9, max x/y: 511, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 349525, // level: 10, max x/y: 1023, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 1398101, // level: 11, max x/y: 2047, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 5592405, // level: 12, max x/y: 4095, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 22369621, // level: 13, max x/y: 8191, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 89478485, // level: 14, max x/y: 16383, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... +}; + +HeightFieldShapeSettings::HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices, const PhysicsMaterialList &inMaterialList) : + mOffset(inOffset), + mScale(inScale), + mSampleCount(inSampleCount) +{ + mHeightSamples.assign(inSamples, inSamples + Square(inSampleCount)); + + if (!inMaterialList.empty() && inMaterialIndices != nullptr) + { + mMaterialIndices.assign(inMaterialIndices, inMaterialIndices + Square(inSampleCount - 1)); + mMaterials = inMaterialList; + } + else + { + JPH_ASSERT(inMaterialList.empty()); + JPH_ASSERT(inMaterialIndices == nullptr); + } +} + +ShapeSettings::ShapeResult HeightFieldShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new HeightFieldShape(*this, mCachedResult); + return mCachedResult; +} + +void HeightFieldShapeSettings::DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const +{ + // Determine min and max value + outMinValue = mMinHeightValue; + outMaxValue = mMaxHeightValue; + for (float h : mHeightSamples) + if (h != cNoCollisionValue) + { + outMinValue = min(outMinValue, h); + outMaxValue = max(outMaxValue, h); + } + + // Prevent dividing by zero by setting a minimal height difference + float height_diff = max(outMaxValue - outMinValue, 1.0e-6f); + + // Calculate the scale factor to quantize to 16 bits + outQuantizationScale = float(cMaxHeightValue16) / height_diff; +} + +uint32 HeightFieldShapeSettings::CalculateBitsPerSampleForError(float inMaxError) const +{ + // Start with 1 bit per sample + uint32 bits_per_sample = 1; + + // Determine total range + float min_value, max_value, scale; + DetermineMinAndMaxSample(min_value, max_value, scale); + if (min_value < max_value) + { + // Loop over all blocks + for (uint y = 0; y < mSampleCount; y += mBlockSize) + for (uint x = 0; x < mSampleCount; x += mBlockSize) + { + // Determine min and max block value + take 1 sample border just like we do while building the hierarchical grids + float block_min_value = FLT_MAX, block_max_value = -FLT_MAX; + for (uint bx = x; bx < min(x + mBlockSize + 1, mSampleCount); ++bx) + for (uint by = y; by < min(y + mBlockSize + 1, mSampleCount); ++by) + { + float h = mHeightSamples[by * mSampleCount + bx]; + if (h != cNoCollisionValue) + { + block_min_value = min(block_min_value, h); + block_max_value = max(block_max_value, h); + } + } + + if (block_min_value < block_max_value) + { + // Quantize then dequantize block min/max value + block_min_value = min_value + floor((block_min_value - min_value) * scale) / scale; + block_max_value = min_value + ceil((block_max_value - min_value) * scale) / scale; + float block_height = block_max_value - block_min_value; + + // Loop over the block again + for (uint bx = x; bx < x + mBlockSize; ++bx) + for (uint by = y; by < y + mBlockSize; ++by) + { + // Get the height + float height = mHeightSamples[by * mSampleCount + bx]; + if (height != cNoCollisionValue) + { + for (;;) + { + // Determine bitmask for sample + uint32 sample_mask = (1 << bits_per_sample) - 1; + + // Quantize + float quantized_height = floor((height - block_min_value) * float(sample_mask) / block_height); + quantized_height = Clamp(quantized_height, 0.0f, float(sample_mask - 1)); + + // Dequantize and check error + float dequantized_height = block_min_value + (quantized_height + 0.5f) * block_height / float(sample_mask); + if (abs(dequantized_height - height) <= inMaxError) + break; + + // Not accurate enough, increase bits per sample + bits_per_sample++; + + // Don't go above 8 bits per sample + if (bits_per_sample == 8) + return bits_per_sample; + } + } + } + } + } + + } + + return bits_per_sample; +} + +void HeightFieldShape::CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator) +{ + // Allocate temporary buffer for normals + uint normals_size = 2 * inSizeX * inSizeY * sizeof(Vec3); + Vec3 *normals = (Vec3 *)inAllocator.Allocate(normals_size); + JPH_SCOPE_EXIT([&inAllocator, normals, normals_size]{ inAllocator.Free(normals, normals_size); }); + + // Calculate triangle normals and make normals zero for triangles that are missing + Vec3 *out_normal = normals; + for (uint y = 0; y < inSizeY; ++y) + for (uint x = 0; x < inSizeX; ++x) + { + // Get height on diagonal + const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x); + float x1y1_h = height_samples[0]; + float x2y2_h = height_samples[inHeightsStride + 1]; + if (x1y1_h != cNoCollisionValue && x2y2_h != cNoCollisionValue) + { + // Calculate normal for lower left triangle (e.g. T1A) + float x1y2_h = height_samples[inHeightsStride]; + if (x1y2_h != cNoCollisionValue) + { + Vec3 x2y2_minus_x1y2(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0); + Vec3 x1y1_minus_x1y2(0, inHeightsScale * (x1y1_h - x1y2_h), -mScale.GetZ()); + out_normal[0] = x2y2_minus_x1y2.Cross(x1y1_minus_x1y2).Normalized(); + } + else + out_normal[0] = Vec3::sZero(); + + // Calculate normal for upper right triangle (e.g. T1B) + float x2y1_h = height_samples[1]; + if (x2y1_h != cNoCollisionValue) + { + Vec3 x1y1_minus_x2y1(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y1_h), 0); + Vec3 x2y2_minus_x2y1(0, inHeightsScale * (x2y2_h - x2y1_h), mScale.GetZ()); + out_normal[1] = x1y1_minus_x2y1.Cross(x2y2_minus_x2y1).Normalized(); + } + else + out_normal[1] = Vec3::sZero(); + } + else + { + out_normal[0] = Vec3::sZero(); + out_normal[1] = Vec3::sZero(); + } + + out_normal += 2; + } + + // Calculate active edges + const Vec3 *in_normal = normals; + uint global_bit_pos = 3 * (inY * (mSampleCount - 1) + inX); + for (uint y = 0; y < inSizeY; ++y) + { + for (uint x = 0; x < inSizeX; ++x) + { + // Get vertex heights + const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x); + float x1y1_h = height_samples[0]; + float x1y2_h = height_samples[inHeightsStride]; + float x2y2_h = height_samples[inHeightsStride + 1]; + bool x1y1_valid = x1y1_h != cNoCollisionValue; + bool x1y2_valid = x1y2_h != cNoCollisionValue; + bool x2y2_valid = x2y2_h != cNoCollisionValue; + + // Calculate the edge flags (3 bits) + // See diagram in the next function for the edge numbering + uint16 edge_mask = 0b111; + uint16 edge_flags = 0; + + // Edge 0 + if (x == 0) + edge_mask &= 0b110; // We need normal x - 1 which we didn't calculate, don't update this edge + else if (x1y1_valid && x1y2_valid) + { + Vec3 edge0_direction(0, inHeightsScale * (x1y2_h - x1y1_h), mScale.GetZ()); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[-1], edge0_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b001; + } + + // Edge 1 + if (y == inSizeY - 1) + edge_mask &= 0b101; // We need normal y + 1 which we didn't calculate, don't update this edge + else if (x1y2_valid && x2y2_valid) + { + Vec3 edge1_direction(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[2 * inSizeX + 1], edge1_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b010; + } + + // Edge 2 + if (x1y1_valid && x2y2_valid) + { + Vec3 edge2_direction(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y2_h), -mScale.GetZ()); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[1], edge2_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b100; + } + + // Store the edge flags in the array + uint byte_pos = global_bit_pos >> 3; + uint bit_pos = global_bit_pos & 0b111; + JPH_ASSERT(byte_pos < mActiveEdgesSize); + uint8 *edge_flags_ptr = &mActiveEdges[byte_pos]; + uint16 combined_edge_flags = uint16(edge_flags_ptr[0]) | uint16(uint16(edge_flags_ptr[1]) << 8); + combined_edge_flags &= ~(edge_mask << bit_pos); + combined_edge_flags |= edge_flags << bit_pos; + edge_flags_ptr[0] = uint8(combined_edge_flags); + edge_flags_ptr[1] = uint8(combined_edge_flags >> 8); + + in_normal += 2; + global_bit_pos += 3; + } + + global_bit_pos += 3 * (mSampleCount - 1 - inSizeX); + } +} + +void HeightFieldShape::CalculateActiveEdges(const HeightFieldShapeSettings &inSettings) +{ + /* + Store active edges. The triangles are organized like this: + x ---> + + y + + + | \ T1B | \ T2B + | e0 e2 | \ + | | T1A \ | T2A \ + V +--e1---+-------+ + | \ T3B | \ T4B + | \ | \ + | T3A \ | T4A \ + +-------+-------+ + We store active edges e0 .. e2 as bits 0 .. 2. + We store triangles horizontally then vertically (order T1A, T2A, T3A and T4A). + The top edge and right edge of the heightfield are always active so we do not need to store them, + therefore we only need to store (mSampleCount - 1)^2 * 3-bit + The triangles T1B, T2B, T3B and T4B do not need to be stored, their active edges can be constructed from adjacent triangles. + Add 1 byte padding so we can always read 1 uint16 to get the bits that cross an 8 bit boundary + */ + + // Make all edges active (if mSampleCount is bigger than inSettings.mSampleCount we need to fill up the padding, + // also edges at x = 0 and y = inSettings.mSampleCount - 1 are not updated) + memset(mActiveEdges, 0xff, mActiveEdgesSize); + + // Now clear the edges that are not active + TempAllocatorMalloc allocator; + CalculateActiveEdges(0, 0, inSettings.mSampleCount - 1, inSettings.mSampleCount - 1, inSettings.mHeightSamples.data(), 0, 0, inSettings.mSampleCount, inSettings.mScale.GetY(), inSettings.mActiveEdgeCosThresholdAngle, allocator); +} + +void HeightFieldShape::StoreMaterialIndices(const HeightFieldShapeSettings &inSettings) +{ + // We need to account for any rounding of the sample count to the nearest block size + uint in_count_min_1 = inSettings.mSampleCount - 1; + uint out_count_min_1 = mSampleCount - 1; + + mNumBitsPerMaterialIndex = 32 - CountLeadingZeros(max((uint32)mMaterials.size(), inSettings.mMaterialsCapacity) - 1); + mMaterialIndices.resize(((Square(out_count_min_1) * mNumBitsPerMaterialIndex + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16 + + if (mMaterials.size() > 1) + for (uint y = 0; y < out_count_min_1; ++y) + for (uint x = 0; x < out_count_min_1; ++x) + { + // Read material + uint16 material_index = x < in_count_min_1 && y < in_count_min_1? uint16(inSettings.mMaterialIndices[x + y * in_count_min_1]) : 0; + + // Calculate byte and bit position where the material index needs to go + uint sample_pos = x + y * out_count_min_1; + uint bit_pos = sample_pos * mNumBitsPerMaterialIndex; + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + + // Write the material index + material_index <<= bit_pos; + JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size()); + mMaterialIndices[byte_pos] |= uint8(material_index); + mMaterialIndices[byte_pos + 1] |= uint8(material_index >> 8); + } +} + +void HeightFieldShape::CacheValues() +{ + mSampleMask = uint8((uint32(1) << mBitsPerSample) - 1); +} + +void HeightFieldShape::AllocateBuffers() +{ + uint num_blocks = GetNumBlocks(); + uint max_stride = (num_blocks + 1) >> 1; + mRangeBlocksSize = sGridOffsets[sGetMaxLevel(num_blocks) - 1] + Square(max_stride); + mHeightSamplesSize = (mSampleCount * mSampleCount * mBitsPerSample + 7) / 8 + 1; + mActiveEdgesSize = (Square(mSampleCount - 1) * 3 + 7) / 8 + 1; // See explanation at HeightFieldShape::CalculateActiveEdges + + JPH_ASSERT(mRangeBlocks == nullptr && mHeightSamples == nullptr && mActiveEdges == nullptr); + void *data = AlignedAllocate(mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize, alignof(RangeBlock)); + mRangeBlocks = reinterpret_cast(data); + mHeightSamples = reinterpret_cast(mRangeBlocks + mRangeBlocksSize); + mActiveEdges = mHeightSamples + mHeightSamplesSize; +} + +HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::HeightField, EShapeSubType::HeightField, inSettings, outResult), + mOffset(inSettings.mOffset), + mScale(inSettings.mScale), + mSampleCount(((inSettings.mSampleCount + inSettings.mBlockSize - 1) / inSettings.mBlockSize) * inSettings.mBlockSize), // Round sample count to nearest block size + mBlockSize(inSettings.mBlockSize), + mBitsPerSample(uint8(inSettings.mBitsPerSample)) +{ + CacheValues(); + + // Reserve a bigger materials list if requested + if (inSettings.mMaterialsCapacity > 0) + mMaterials.reserve(inSettings.mMaterialsCapacity); + mMaterials = inSettings.mMaterials; + + // Check block size + if (mBlockSize < 2 || mBlockSize > 8) + { + outResult.SetError("HeightFieldShape: Block size must be in the range [2, 8]!"); + return; + } + + // Check bits per sample + if (inSettings.mBitsPerSample < 1 || inSettings.mBitsPerSample > 8) + { + outResult.SetError("HeightFieldShape: Bits per sample must be in the range [1, 8]!"); + return; + } + + // We stop at mBlockSize x mBlockSize height sample blocks + uint num_blocks = GetNumBlocks(); + + // We want at least 1 grid layer + if (num_blocks < 2) + { + outResult.SetError("HeightFieldShape: Sample count too low!"); + return; + } + + // Check that we don't overflow our 32 bit 'properties' + if (num_blocks > (1 << cNumBitsXY)) + { + outResult.SetError("HeightFieldShape: Sample count too high!"); + return; + } + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("HeightFieldShape: Size exceeds the amount of available sub shape ID bits!"); + return; + } + + if (!mMaterials.empty()) + { + // Validate materials + if (mMaterials.size() > 256) + { + outResult.SetError("Supporting max 256 materials per height field"); + return; + } + for (uint8 s : inSettings.mMaterialIndices) + if (s >= mMaterials.size()) + { + outResult.SetError(StringFormat("Material %u is beyond material list (size: %u)", s, (uint)mMaterials.size())); + return; + } + } + else + { + // No materials assigned, validate that no materials have been specified + if (!inSettings.mMaterialIndices.empty()) + { + outResult.SetError("No materials present, mMaterialIndices should be empty"); + return; + } + } + + // Determine range + float min_value, max_value, scale; + inSettings.DetermineMinAndMaxSample(min_value, max_value, scale); + if (min_value > max_value) + { + // If there is no collision with this heightmap, leave everything empty + mMaterials.clear(); + outResult.Set(this); + return; + } + + // Allocate space for this shape + AllocateBuffers(); + + // Quantize to uint16 + Array quantized_samples; + quantized_samples.reserve(mSampleCount * mSampleCount); + for (uint y = 0; y < inSettings.mSampleCount; ++y) + { + for (uint x = 0; x < inSettings.mSampleCount; ++x) + { + float h = inSettings.mHeightSamples[x + y * inSettings.mSampleCount]; + if (h == cNoCollisionValue) + { + quantized_samples.push_back(cNoCollisionValue16); + } + else + { + // Floor the quantized height to get a lower bound for the quantized value + int quantized_height = (int)floor(scale * (h - min_value)); + + // Ensure that the height says below the max height value so we can safely add 1 to get the upper bound for the quantized value + quantized_height = Clamp(quantized_height, 0, int(cMaxHeightValue16 - 1)); + + quantized_samples.push_back(uint16(quantized_height)); + } + } + // Pad remaining columns with no collision + for (uint x = inSettings.mSampleCount; x < mSampleCount; ++x) + quantized_samples.push_back(cNoCollisionValue16); + } + // Pad remaining rows with no collision + for (uint y = inSettings.mSampleCount; y < mSampleCount; ++y) + for (uint x = 0; x < mSampleCount; ++x) + quantized_samples.push_back(cNoCollisionValue16); + + // Update offset and scale to account for the compression to uint16 + if (min_value <= max_value) // Only when there was collision + { + // In GetPosition we always add 0.5 to the quantized sample in order to reduce the average error. + // We want to be able to exactly quantize min_value (this is important in case the heightfield is entirely flat) so we subtract that value from min_value. + min_value -= 0.5f / (scale * mSampleMask); + + mOffset.SetY(mOffset.GetY() + mScale.GetY() * min_value); + } + mScale.SetY(mScale.GetY() / scale); + + // Calculate amount of grids + uint max_level = sGetMaxLevel(num_blocks); + + // Temporary data structure used during creating of a hierarchy of grids + struct Range + { + uint16 mMin; + uint16 mMax; + }; + + // Reserve size for temporary range data + reserve 1 extra for a 1x1 grid that we won't store but use for calculating the bounding box + Array> ranges; + ranges.resize(max_level + 1); + + // Calculate highest detail grid by combining mBlockSize x mBlockSize height samples + Array *cur_range_vector = &ranges.back(); + uint num_blocks_pow2 = GetNextPowerOf2(num_blocks); // We calculate the range blocks as if the heightfield was a power of 2, when we save the range blocks we'll ignore the extra samples (this makes downsampling easier) + cur_range_vector->resize(num_blocks_pow2 * num_blocks_pow2); + Range *range_dst = &cur_range_vector->front(); + for (uint y = 0; y < num_blocks_pow2; ++y) + for (uint x = 0; x < num_blocks_pow2; ++x) + { + range_dst->mMin = 0xffff; + range_dst->mMax = 0; + uint max_bx = x == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; // for interior blocks take 1 more because the triangles connect to the next block so we must include their height too + uint max_by = y == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; + for (uint by = 0; by < max_by; ++by) + for (uint bx = 0; bx < max_bx; ++bx) + { + uint sx = x * mBlockSize + bx; + uint sy = y * mBlockSize + by; + if (sx < mSampleCount && sy < mSampleCount) + { + uint16 h = quantized_samples[sy * mSampleCount + sx]; + if (h != cNoCollisionValue16) + { + range_dst->mMin = min(range_dst->mMin, h); + range_dst->mMax = max(range_dst->mMax, uint16(h + 1)); // Add 1 to the max so we know the real value is between mMin and mMax + } + } + } + ++range_dst; + } + + // Calculate remaining grids + for (uint n = num_blocks_pow2 >> 1; n >= 1; n >>= 1) + { + // Get source buffer + const Range *range_src = &cur_range_vector->front(); + + // Previous array element + --cur_range_vector; + + // Make space for this grid + cur_range_vector->resize(n * n); + + // Get target buffer + range_dst = &cur_range_vector->front(); + + // Combine the results of 2x2 ranges + for (uint y = 0; y < n; ++y) + for (uint x = 0; x < n; ++x) + { + range_dst->mMin = 0xffff; + range_dst->mMax = 0; + for (uint by = 0; by < 2; ++by) + for (uint bx = 0; bx < 2; ++bx) + { + const Range &r = range_src[(y * 2 + by) * n * 2 + x * 2 + bx]; + range_dst->mMin = min(range_dst->mMin, r.mMin); + range_dst->mMax = max(range_dst->mMax, r.mMax); + } + ++range_dst; + } + } + JPH_ASSERT(cur_range_vector == &ranges.front()); + + // Store global range for bounding box calculation + mMinSample = ranges[0][0].mMin; + mMaxSample = ranges[0][0].mMax; + +#ifdef JPH_ENABLE_ASSERTS + // Validate that we did not lose range along the way + uint16 minv = 0xffff, maxv = 0; + for (uint16 v : quantized_samples) + if (v != cNoCollisionValue16) + { + minv = min(minv, v); + maxv = max(maxv, uint16(v + 1)); + } + JPH_ASSERT(mMinSample == minv && mMaxSample == maxv); +#endif + + // Now erase the first element, we need a 2x2 grid to start with + ranges.erase(ranges.begin()); + + // Create blocks + uint max_stride = (num_blocks + 1) >> 1; + RangeBlock *current_block = mRangeBlocks; + for (uint level = 0; level < ranges.size(); ++level) + { + JPH_ASSERT(uint(current_block - mRangeBlocks) == sGridOffsets[level]); + + uint in_n = 1 << level; + uint out_n = min(in_n, max_stride); // At the most detailed level we store a non-power of 2 number of blocks + + for (uint y = 0; y < out_n; ++y) + for (uint x = 0; x < out_n; ++x) + { + // Convert from 2x2 Range structure to 1 RangeBlock structure + RangeBlock &rb = *current_block++; + for (uint by = 0; by < 2; ++by) + for (uint bx = 0; bx < 2; ++bx) + { + uint src_pos = (y * 2 + by) * 2 * in_n + (x * 2 + bx); + uint dst_pos = by * 2 + bx; + rb.mMin[dst_pos] = ranges[level][src_pos].mMin; + rb.mMax[dst_pos] = ranges[level][src_pos].mMax; + } + } + } + JPH_ASSERT(uint32(current_block - mRangeBlocks) == mRangeBlocksSize); + + // Quantize height samples + memset(mHeightSamples, 0, mHeightSamplesSize); + int sample = 0; + for (uint y = 0; y < mSampleCount; ++y) + for (uint x = 0; x < mSampleCount; ++x) + { + uint32 output_value; + + float h = x < inSettings.mSampleCount && y < inSettings.mSampleCount? inSettings.mHeightSamples[x + y * inSettings.mSampleCount] : cNoCollisionValue; + if (h == cNoCollisionValue) + { + // No collision + output_value = mSampleMask; + } + else + { + // Get range of block so we know what range to compress to + uint bx = x / mBlockSize; + uint by = y / mBlockSize; + const Range &range = ranges.back()[by * num_blocks_pow2 + bx]; + JPH_ASSERT(range.mMin < range.mMax); + + // Quantize to mBitsPerSample bits, note that mSampleMask is reserved for indicating that there's no collision. + // We divide the range into mSampleMask segments and use the mid points of these segments as the quantized values. + // This results in a lower error than if we had quantized our data using the lowest point of all these segments. + float h_min = min_value + range.mMin / scale; + float h_delta = float(range.mMax - range.mMin) / scale; + float quantized_height = floor((h - h_min) * float(mSampleMask) / h_delta); + output_value = uint32(Clamp((int)quantized_height, 0, int(mSampleMask) - 1)); // mSampleMask is reserved as 'no collision value' + } + + // Store the sample + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + output_value <<= bit_pos; + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + mHeightSamples[byte_pos] |= uint8(output_value); + mHeightSamples[byte_pos + 1] |= uint8(output_value >> 8); + sample += inSettings.mBitsPerSample; + } + + // Calculate the active edges + CalculateActiveEdges(inSettings); + + // Compress material indices + if (mMaterials.size() > 1 || inSettings.mMaterialsCapacity > 1) + StoreMaterialIndices(inSettings); + + outResult.Set(this); +} + +HeightFieldShape::~HeightFieldShape() +{ + if (mRangeBlocks != nullptr) + AlignedFree(mRangeBlocks); +} + +Ref HeightFieldShape::Clone() const +{ + Ref clone = new HeightFieldShape; + clone->SetUserData(GetUserData()); + + clone->mOffset = mOffset; + clone->mScale = mScale; + clone->mSampleCount = mSampleCount; + clone->mBlockSize = mBlockSize; + clone->mBitsPerSample = mBitsPerSample; + clone->mSampleMask = mSampleMask; + clone->mMinSample = mMinSample; + clone->mMaxSample = mMaxSample; + + clone->AllocateBuffers(); + memcpy(clone->mRangeBlocks, mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); // Copy the entire buffer in 1 go + + clone->mMaterials.reserve(mMaterials.capacity()); // Ensure we keep the capacity of the original + clone->mMaterials = mMaterials; + clone->mMaterialIndices = mMaterialIndices; + clone->mNumBitsPerMaterialIndex = mNumBitsPerMaterialIndex; + +#ifdef JPH_DEBUG_RENDERER + clone->mGeometry = mGeometry; + clone->mCachedUseMaterialColors = mCachedUseMaterialColors; +#endif // JPH_DEBUG_RENDERER + + return clone; +} + +inline void HeightFieldShape::sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride) +{ + outRangeBlockOffset = sGridOffsets[inMaxLevel - 1]; + outRangeBlockStride = (inNumBlocks + 1) >> 1; +} + +inline void HeightFieldShape::GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock) +{ + JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks()); + + // Convert to location of range block + uint rbx = inBlockX >> 1; + uint rby = inBlockY >> 1; + outIndexInBlock = ((inBlockY & 1) << 1) + (inBlockX & 1); + + uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx; + JPH_ASSERT(offset < mRangeBlocksSize); + outBlock = mRangeBlocks + offset; +} + +inline void HeightFieldShape::GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const +{ + JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks()); + + // Convert to location of range block + uint rbx = inBlockX >> 1; + uint rby = inBlockY >> 1; + uint n = ((inBlockY & 1) << 1) + (inBlockX & 1); + + // Calculate offset and scale + uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx; + JPH_ASSERT(offset < mRangeBlocksSize); + const RangeBlock &block = mRangeBlocks[offset]; + outBlockOffset = float(block.mMin[n]); + outBlockScale = float(block.mMax[n] - block.mMin[n]) / float(mSampleMask); +} + +inline uint8 HeightFieldShape::GetHeightSample(uint inX, uint inY) const +{ + JPH_ASSERT(inX < mSampleCount); + JPH_ASSERT(inY < mSampleCount); + + // Determine bit position of sample + uint sample = (inY * mSampleCount + inX) * uint(mBitsPerSample); + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + + // Fetch the height sample value + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + const uint8 *height_samples = mHeightSamples + byte_pos; + uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8); + return uint8(height_sample >> bit_pos) & mSampleMask; +} + +inline Vec3 HeightFieldShape::GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const +{ + // Get quantized value + uint8 height_sample = GetHeightSample(inX, inY); + outNoCollision = height_sample == mSampleMask; + + // Add 0.5 to the quantized value to minimize the error (see constructor) + return mOffset + mScale * Vec3(float(inX), inBlockOffset + (0.5f + height_sample) * inBlockScale, float(inY)); +} + +Vec3 HeightFieldShape::GetPosition(uint inX, uint inY) const +{ + // Test if there are any samples + if (mHeightSamplesSize == 0) + return mOffset + mScale * Vec3(float(inX), 0.0f, float(inY)); + + // Get block location + uint bx = inX / mBlockSize; + uint by = inY / mBlockSize; + + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride); + + float offset, scale; + GetBlockOffsetAndScale(bx, by, range_block_offset, range_block_stride, offset, scale); + + bool no_collision; + return GetPosition(inX, inY, offset, scale, no_collision); +} + +bool HeightFieldShape::IsNoCollision(uint inX, uint inY) const +{ + return mHeightSamplesSize == 0 || GetHeightSample(inX, inY) == mSampleMask; +} + +bool HeightFieldShape::ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const +{ + // Check if we have collision + if (mHeightSamplesSize == 0) + return false; + + // Convert coordinate to integer space + Vec3 integer_space = (inLocalPosition - mOffset) / mScale; + + // Get x coordinate and fraction + float x_frac = integer_space.GetX(); + if (x_frac < 0.0f || x_frac >= mSampleCount - 1) + return false; + uint x = (uint)floor(x_frac); + x_frac -= x; + + // Get y coordinate and fraction + float y_frac = integer_space.GetZ(); + if (y_frac < 0.0f || y_frac >= mSampleCount - 1) + return false; + uint y = (uint)floor(y_frac); + y_frac -= y; + + // If one of the diagonal points doesn't have collision, we don't have a height at this location + if (IsNoCollision(x, y) || IsNoCollision(x + 1, y + 1)) + return false; + + if (y_frac >= x_frac) + { + // Left bottom triangle, test the 3rd point + if (IsNoCollision(x, y + 1)) + return false; + + // Interpolate height value + Vec3 v1 = GetPosition(x, y); + Vec3 v2 = GetPosition(x, y + 1); + Vec3 v3 = GetPosition(x + 1, y + 1); + outSurfacePosition = v1 + y_frac * (v2 - v1) + x_frac * (v3 - v2); + SubShapeIDCreator creator; + outSubShapeID = EncodeSubShapeID(creator, x, y, 0); + return true; + } + else + { + // Right top triangle, test the third point + if (IsNoCollision(x + 1, y)) + return false; + + // Interpolate height value + Vec3 v1 = GetPosition(x, y); + Vec3 v2 = GetPosition(x + 1, y + 1); + Vec3 v3 = GetPosition(x + 1, y); + outSurfacePosition = v1 + y_frac * (v2 - v3) + x_frac * (v3 - v1); + SubShapeIDCreator creator; + outSubShapeID = EncodeSubShapeID(creator, x, y, 1); + return true; + } +} + +void HeightFieldShape::GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0); + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount); + + // Test if there are any samples + if (mHeightSamplesSize == 0) + { + // No samples, return the offset + float offset = mOffset.GetY(); + for (uint y = 0; y < inSizeY; ++y, outHeights += inHeightsStride) + for (uint x = 0; x < inSizeX; ++x) + outHeights[x] = offset; + } + else + { + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride); + + // Loop over blocks + uint block_start_x = inX / mBlockSize; + uint block_start_y = inY / mBlockSize; + uint num_blocks_x = inSizeX / mBlockSize; + uint num_blocks_y = inSizeY / mBlockSize; + for (uint block_y = 0; block_y < num_blocks_y; ++block_y) + for (uint block_x = 0; block_x < num_blocks_x; ++block_x) + { + // Get offset and scale for block + float offset, scale; + GetBlockOffsetAndScale(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, offset, scale); + + // Adjust by global offset and scale + // Note: This is the math applied in GetPosition() written out to reduce calculations in the inner loop + scale *= mScale.GetY(); + offset = mOffset.GetY() + mScale.GetY() * offset + 0.5f * scale; + + // Loop over samples in block + for (uint sample_y = 0; sample_y < mBlockSize; ++sample_y) + for (uint sample_x = 0; sample_x < mBlockSize; ++sample_x) + { + // Calculate output coordinate + uint output_x = block_x * mBlockSize + sample_x; + uint output_y = block_y * mBlockSize + sample_y; + + // Get quantized value + uint8 height_sample = GetHeightSample(inX + output_x, inY + output_y); + + // Dequantize + float h = height_sample != mSampleMask? offset + height_sample * scale : cNoCollisionValue; + outHeights[output_y * inHeightsStride + output_x] = h; + } + } + } +} + +void HeightFieldShape::SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle) +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + JPH_ASSERT(mHeightSamplesSize > 0); + JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0); + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount); + + // If we have a block in negative x/y direction, we will affect its range so we need to take it into account + bool need_temp_heights = false; + uint affected_x = inX; + uint affected_y = inY; + uint affected_size_x = inSizeX; + uint affected_size_y = inSizeY; + if (inX > 0) { affected_x -= mBlockSize; affected_size_x += mBlockSize; need_temp_heights = true; } + if (inY > 0) { affected_y -= mBlockSize; affected_size_y += mBlockSize; need_temp_heights = true; } + + // If we have a block in positive x/y direction, our ranges are affected by it so we need to take it into account + uint heights_size_x = affected_size_x; + uint heights_size_y = affected_size_y; + if (inX + inSizeX < mSampleCount) { heights_size_x += mBlockSize; need_temp_heights = true; } + if (inY + inSizeY < mSampleCount) { heights_size_y += mBlockSize; need_temp_heights = true; } + + // Get heights for affected area + const float *heights; + intptr_t heights_stride; + float *temp_heights; + if (need_temp_heights) + { + // Fetch the surrounding height data (note we're forced to recompress this data with a potentially different range so there will be some precision loss here) + temp_heights = (float *)inAllocator.Allocate(heights_size_x * heights_size_y * sizeof(float)); + heights = temp_heights; + heights_stride = heights_size_x; + + // We need to fill in the following areas: + // + // +-----------------+ + // | 2 | + // |---+---------+---| + // | | | | + // | 3 | 1 | 4 | + // | | | | + // |---+---------+---| + // | 5 | + // +-----------------+ + // + // 1. The area that is affected by the new heights (we just copy these) + // 2-5. These areas are either needed to calculate the range of the affected blocks or they need to be recompressed with a different range + uint offset_x = inX - affected_x; + uint offset_y = inY - affected_y; + + // Area 2 + GetHeights(affected_x, affected_y, heights_size_x, offset_y, temp_heights, heights_size_x); + float *area3_start = temp_heights + offset_y * heights_size_x; + + // Area 3 + GetHeights(affected_x, inY, offset_x, inSizeY, area3_start, heights_size_x); + + // Area 1 + float *area1_start = area3_start + offset_x; + for (uint y = 0; y < inSizeY; ++y, area1_start += heights_size_x, inHeights += inHeightsStride) + memcpy(area1_start, inHeights, inSizeX * sizeof(float)); + + // Area 4 + uint area4_x = inX + inSizeX; + GetHeights(area4_x, inY, affected_x + heights_size_x - area4_x, inSizeY, area3_start + area4_x - affected_x, heights_size_x); + + // Area 5 + uint area5_y = inY + inSizeY; + float *area5_start = temp_heights + (area5_y - affected_y) * heights_size_x; + GetHeights(affected_x, area5_y, heights_size_x, affected_y + heights_size_y - area5_y, area5_start, heights_size_x); + } + else + { + // We can directly use the input buffer because there are no extra edges to take into account + heights = inHeights; + heights_stride = inHeightsStride; + temp_heights = nullptr; + } + + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + uint max_level = sGetMaxLevel(num_blocks); + sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride); + + // Loop over blocks + uint block_start_x = affected_x / mBlockSize; + uint block_start_y = affected_y / mBlockSize; + uint num_blocks_x = affected_size_x / mBlockSize; + uint num_blocks_y = affected_size_y / mBlockSize; + for (uint block_y = 0, sample_start_y = 0; block_y < num_blocks_y; ++block_y, sample_start_y += mBlockSize) + for (uint block_x = 0, sample_start_x = 0; block_x < num_blocks_x; ++block_x, sample_start_x += mBlockSize) + { + // Determine quantized min and max value for block + // Note that we need to include 1 extra row in the positive x/y direction to account for connecting triangles + int min_value = 0xffff; + int max_value = 0; + uint sample_x_end = min(sample_start_x + mBlockSize + 1, mSampleCount - affected_x); + uint sample_y_end = min(sample_start_y + mBlockSize + 1, mSampleCount - affected_y); + for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y) + for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x) + { + float h = heights[sample_y * heights_stride + sample_x]; + if (h != cNoCollisionValue) + { + int quantized_height = Clamp((int)floor((h - mOffset.GetY()) / mScale.GetY()), 0, int(cMaxHeightValue16 - 1)); + min_value = min(min_value, quantized_height); + max_value = max(max_value, quantized_height + 1); + } + } + if (min_value > max_value) + min_value = max_value = cNoCollisionValue16; + + // Update range for block + RangeBlock *range_block; + uint index_in_block; + GetRangeBlock(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, range_block, index_in_block); + range_block->mMin[index_in_block] = uint16(min_value); + range_block->mMax[index_in_block] = uint16(max_value); + + // Get offset and scale for block + float offset_block = float(min_value); + float scale_block = float(max_value - min_value) / float(mSampleMask); + + // Calculate scale and offset using the formula used in GetPosition() solved for the quantized height (excluding 0.5 because we round down while quantizing) + float scale = scale_block * mScale.GetY(); + float offset = mOffset.GetY() + offset_block * mScale.GetY(); + + // Loop over samples in block + sample_x_end = sample_start_x + mBlockSize; + sample_y_end = sample_start_y + mBlockSize; + for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y) + for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x) + { + // Quantize height + float h = heights[sample_y * heights_stride + sample_x]; + uint8 quantized_height = h != cNoCollisionValue? uint8(Clamp((int)floor((h - offset) / scale), 0, int(mSampleMask) - 1)) : mSampleMask; + + // Determine bit position of sample + uint sample = ((affected_y + sample_y) * mSampleCount + affected_x + sample_x) * uint(mBitsPerSample); + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + + // Update the height value sample + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + uint8 *height_samples = mHeightSamples + byte_pos; + uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8); + height_sample &= ~(uint16(mSampleMask) << bit_pos); + height_sample |= uint16(quantized_height) << bit_pos; + height_samples[0] = uint8(height_sample); + height_samples[1] = uint8(height_sample >> 8); + } + } + + // Update active edges + // Note that we must take an extra row on all sides to account for connecting triangles + uint ae_x = inX > 1? inX - 2 : 0; + uint ae_y = inY > 1? inY - 2 : 0; + uint ae_sx = min(inX + inSizeX + 1, mSampleCount - 1) - ae_x; + uint ae_sy = min(inY + inSizeY + 1, mSampleCount - 1) - ae_y; + CalculateActiveEdges(ae_x, ae_y, ae_sx, ae_sy, heights, affected_x, affected_y, heights_stride, 1.0f, inActiveEdgeCosThresholdAngle, inAllocator); + + // Free temporary buffer + if (temp_heights != nullptr) + inAllocator.Free(temp_heights, heights_size_x * heights_size_y * sizeof(float)); + + // Update hierarchy of range blocks + while (max_level > 1) + { + // Get offset and stride for destination blocks + uint dst_range_block_offset, dst_range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks >> 1, max_level - 1, dst_range_block_offset, dst_range_block_stride); + + // We'll be processing 2x2 blocks below so we need the start coordinates to be even and we extend the number of blocks to correct for that + if (block_start_x & 1) { --block_start_x; ++num_blocks_x; } + if (block_start_y & 1) { --block_start_y; ++num_blocks_y; } + + // Loop over all affected blocks + uint block_end_x = block_start_x + num_blocks_x; + uint block_end_y = block_start_y + num_blocks_y; + for (uint block_y = block_start_y; block_y < block_end_y; block_y += 2) + for (uint block_x = block_start_x; block_x < block_end_x; block_x += 2) + { + // Get source range block + RangeBlock *src_range_block; + uint index_in_src_block; + GetRangeBlock(block_x, block_y, range_block_offset, range_block_stride, src_range_block, index_in_src_block); + + // Determine quantized min and max value for the entire 2x2 block + uint16 min_value = 0xffff; + uint16 max_value = 0; + for (uint i = 0; i < 4; ++i) + if (src_range_block->mMin[i] != cNoCollisionValue16) + { + min_value = min(min_value, src_range_block->mMin[i]); + max_value = max(max_value, src_range_block->mMax[i]); + } + + // Write to destination block + RangeBlock *dst_range_block; + uint index_in_dst_block; + GetRangeBlock(block_x >> 1, block_y >> 1, dst_range_block_offset, dst_range_block_stride, dst_range_block, index_in_dst_block); + dst_range_block->mMin[index_in_dst_block] = uint16(min_value); + dst_range_block->mMax[index_in_dst_block] = uint16(max_value); + } + + // Go up one level + --max_level; + num_blocks >>= 1; + block_start_x >>= 1; + block_start_y >>= 1; + num_blocks_x = min((num_blocks_x + 1) >> 1, num_blocks); + num_blocks_y = min((num_blocks_y + 1) >> 1, num_blocks); + + // Update stride and offset for source to old destination + range_block_offset = dst_range_block_offset; + range_block_stride = dst_range_block_stride; + } + + // Calculate new min and max sample for the entire height field + mMinSample = 0xffff; + mMaxSample = 0; + for (uint i = 0; i < 4; ++i) + if (mRangeBlocks[0].mMin[i] != cNoCollisionValue16) + { + mMinSample = min(mMinSample, mRangeBlocks[0].mMin[i]); + mMaxSample = max(mMaxSample, mRangeBlocks[0].mMax[i]); + } + +#ifdef JPH_DEBUG_RENDERER + // Invalidate temporary rendering data + mGeometry.clear(); +#endif +} + +void HeightFieldShape::GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + if (mMaterialIndices.empty()) + { + // Return all 0's + for (uint y = 0; y < inSizeY; ++y) + { + uint8 *out_indices = outMaterials + y * inMaterialsStride; + for (uint x = 0; x < inSizeX; ++x) + *out_indices++ = 0; + } + return; + } + + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount); + + uint count_min_1 = mSampleCount - 1; + uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + + for (uint y = 0; y < inSizeY; ++y) + { + // Calculate input position + uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex; + const uint8 *in_indices = mMaterialIndices.data() + (bit_pos >> 3); + bit_pos &= 0b111; + + // Calculate output position + uint8 *out_indices = outMaterials + y * inMaterialsStride; + + for (uint x = 0; x < inSizeX; ++x) + { + // Get material index + uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8); + material_index >>= bit_pos; + material_index &= material_index_mask; + *out_indices = uint8(material_index); + + // Go to the next index + bit_pos += mNumBitsPerMaterialIndex; + in_indices += bit_pos >> 3; + bit_pos &= 0b111; + ++out_indices; + } + } +} + +bool HeightFieldShape::SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator) +{ + if (inSizeX == 0 || inSizeY == 0) + return true; + + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount); + + // Remap materials + uint material_remap_table_size = uint(inMaterialList != nullptr? inMaterialList->size() : mMaterials.size()); + uint8 *material_remap_table = (uint8 *)inAllocator.Allocate(material_remap_table_size); + JPH_SCOPE_EXIT([&inAllocator, material_remap_table, material_remap_table_size]{ inAllocator.Free(material_remap_table, material_remap_table_size); }); + if (inMaterialList != nullptr) + { + // Conservatively reserve more space if the incoming material list is bigger + if (inMaterialList->size() > mMaterials.size()) + mMaterials.reserve(inMaterialList->size()); + + // Create a remap table + uint8 *remap_entry = material_remap_table; + for (const PhysicsMaterial *material : *inMaterialList) + { + // Try to find it in the existing list + PhysicsMaterialList::const_iterator it = std::find(mMaterials.begin(), mMaterials.end(), material); + if (it != mMaterials.end()) + { + // Found it, calculate index + *remap_entry = uint8(it - mMaterials.begin()); + } + else + { + // Not found, add it + if (mMaterials.size() >= 256) + { + // We can't have more than 256 materials since we use uint8 as indices + return false; + } + *remap_entry = uint8(mMaterials.size()); + mMaterials.push_back(material); + } + ++remap_entry; + } + } + else + { + // No remapping + for (uint i = 0; i < material_remap_table_size; ++i) + material_remap_table[i] = uint8(i); + } + + if (mMaterials.size() == 1) + { + // Only 1 material, we don't need to store the material indices + return true; + } + + // Check if we need to resize the material indices array + uint count_min_1 = mSampleCount - 1; + uint32 new_bits_per_material_index = 32 - CountLeadingZeros((uint32)mMaterials.size() - 1); + JPH_ASSERT(mNumBitsPerMaterialIndex <= 8 && new_bits_per_material_index <= 8); + if (new_bits_per_material_index > mNumBitsPerMaterialIndex) + { + // Resize the material indices array + mMaterialIndices.resize(((Square(count_min_1) * new_bits_per_material_index + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16 + + // Calculate old and new mask + uint16 old_material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + uint16 new_material_index_mask = uint16((1 << new_bits_per_material_index) - 1); + + // Loop through the array backwards to avoid overwriting data + int in_bit_pos = (count_min_1 * count_min_1 - 1) * mNumBitsPerMaterialIndex; + const uint8 *in_indices = mMaterialIndices.data() + (in_bit_pos >> 3); + in_bit_pos &= 0b111; + int out_bit_pos = (count_min_1 * count_min_1 - 1) * new_bits_per_material_index; + uint8 *out_indices = mMaterialIndices.data() + (out_bit_pos >> 3); + out_bit_pos &= 0b111; + + while (out_indices >= mMaterialIndices.data()) + { + // Read the material index + uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8); + material_index >>= in_bit_pos; + material_index &= old_material_index_mask; + + // Write the material index + uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8); + output_data &= ~(new_material_index_mask << out_bit_pos); + output_data |= material_index << out_bit_pos; + out_indices[0] = uint8(output_data); + out_indices[1] = uint8(output_data >> 8); + + // Go to the previous index + in_bit_pos -= int(mNumBitsPerMaterialIndex); + in_indices += in_bit_pos >> 3; + in_bit_pos &= 0b111; + out_bit_pos -= int(new_bits_per_material_index); + out_indices += out_bit_pos >> 3; + out_bit_pos &= 0b111; + } + + // Accept the new bits per material index + mNumBitsPerMaterialIndex = new_bits_per_material_index; + } + + uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + for (uint y = 0; y < inSizeY; ++y) + { + // Calculate input position + const uint8 *in_indices = inMaterials + y * inMaterialsStride; + + // Calculate output position + uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex; + uint8 *out_indices = mMaterialIndices.data() + (bit_pos >> 3); + bit_pos &= 0b111; + + for (uint x = 0; x < inSizeX; ++x) + { + // Update material + uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8); + output_data &= ~(material_index_mask << bit_pos); + output_data |= material_remap_table[*in_indices] << bit_pos; + out_indices[0] = uint8(output_data); + out_indices[1] = uint8(output_data >> 8); + + // Go to the next index + in_indices++; + bit_pos += mNumBitsPerMaterialIndex; + out_indices += bit_pos >> 3; + bit_pos &= 0b111; + } + } + + return true; +} + +MassProperties HeightFieldShape::GetMassProperties() const +{ + // Object should always be static, return default mass properties + return MassProperties(); +} + +const PhysicsMaterial *HeightFieldShape::GetMaterial(uint inX, uint inY) const +{ + if (mMaterials.empty()) + return PhysicsMaterial::sDefault; + if (mMaterials.size() == 1) + return mMaterials[0]; + + uint count_min_1 = mSampleCount - 1; + JPH_ASSERT(inX < count_min_1); + JPH_ASSERT(inY < count_min_1); + + // Calculate at which bit the material index starts + uint bit_pos = (inX + inY * count_min_1) * mNumBitsPerMaterialIndex; + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + + // Read the material index + JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size()); + const uint8 *material_indices = mMaterialIndices.data() + byte_pos; + uint16 material_index = uint16(material_indices[0]) + uint16(uint16(material_indices[1]) << 8); + material_index >>= bit_pos; + material_index &= (1 << mNumBitsPerMaterialIndex) - 1; + + // Return the material + return mMaterials[material_index]; +} + +uint HeightFieldShape::GetSubShapeIDBits() const +{ + // Need to store X, Y and 1 extra bit to specify the triangle number in the quad + return 2 * (32 - CountLeadingZeros(mSampleCount - 1)) + 1; +} + +SubShapeID HeightFieldShape::EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const +{ + return inCreator.PushID((inX + inY * mSampleCount) * 2 + inTriangle, GetSubShapeIDBits()).GetID(); +} + +void HeightFieldShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const +{ + // Decode sub shape id + SubShapeID remainder; + uint32 id = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID"); + + // Get triangle index + outTriangle = id & 1; + id >>= 1; + + // Fetch the x and y coordinate + outX = id % mSampleCount; + outY = id / mSampleCount; +} + +void HeightFieldShape::GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const +{ + DecodeSubShapeID(inSubShapeID, outX, outY, outTriangleIndex); +} + +const PhysicsMaterial *HeightFieldShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch the material + return GetMaterial(x, y); +} + +Vec3 HeightFieldShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch vertices that both triangles share + Vec3 x1y1 = GetPosition(x, y); + Vec3 x2y2 = GetPosition(x + 1, y + 1); + + // Get normal depending on which triangle was selected + Vec3 normal; + if (triangle == 0) + { + Vec3 x1y2 = GetPosition(x, y + 1); + normal = (x2y2 - x1y2).Cross(x1y1 - x1y2); + } + else + { + Vec3 x2y1 = GetPosition(x + 1, y); + normal = (x1y1 - x2y1).Cross(x2y2 - x2y1); + } + + return normal.Normalized(); +} + +void HeightFieldShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch the triangle + outVertices.resize(3); + outVertices[0] = GetPosition(x, y); + Vec3 v2 = GetPosition(x + 1, y + 1); + if (triangle == 0) + { + outVertices[1] = GetPosition(x, y + 1); + outVertices[2] = v2; + } + else + { + outVertices[1] = v2; + outVertices[2] = GetPosition(x + 1, y); + } + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + std::swap(outVertices[1], outVertices[2]); + + // Transform to world space + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + for (Vec3 &v : outVertices) + v = transform * v; +} + +inline uint8 HeightFieldShape::GetEdgeFlags(uint inX, uint inY, uint inTriangle) const +{ + JPH_ASSERT(inX < mSampleCount - 1 && inY < mSampleCount - 1); + + if (inTriangle == 0) + { + // The edge flags for this triangle are directly stored, find the right 3 bits + uint bit_pos = 3 * (inX + inY * (mSampleCount - 1)); + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + JPH_ASSERT(byte_pos + 1 < mActiveEdgesSize); + const uint8 *active_edges = mActiveEdges + byte_pos; + uint16 edge_flags = uint16(active_edges[0]) + uint16(uint16(active_edges[1]) << 8); + return uint8(edge_flags >> bit_pos) & 0b111; + } + else + { + // We don't store this triangle directly, we need to look at our three neighbours to construct the edge flags + uint8 edge0 = (GetEdgeFlags(inX, inY, 0) & 0b100) != 0? 0b001 : 0; // Diagonal edge + uint8 edge1 = inX == mSampleCount - 2 || (GetEdgeFlags(inX + 1, inY, 0) & 0b001) != 0? 0b010 : 0; // Vertical edge + uint8 edge2 = inY == 0 || (GetEdgeFlags(inX, inY - 1, 0) & 0b010) != 0? 0b100 : 0; // Horizontal edge + return edge0 | edge1 | edge2; + } +} + +AABox HeightFieldShape::GetLocalBounds() const +{ + if (mMinSample == cNoCollisionValue16) + { + // This whole height field shape doesn't have any collision, return the center point + Vec3 center = mOffset + 0.5f * mScale * Vec3(float(mSampleCount - 1), 0.0f, float(mSampleCount - 1)); + return AABox(center, center); + } + else + { + // Bounding box based on min and max sample height + Vec3 bmin = mOffset + mScale * Vec3(0.0f, float(mMinSample), 0.0f); + Vec3 bmax = mOffset + mScale * Vec3(float(mSampleCount - 1), float(mMaxSample), float(mSampleCount - 1)); + return AABox(bmin, bmax); + } +} + +#ifdef JPH_DEBUG_RENDERER +void HeightFieldShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Don't draw anything if we don't have any collision + if (mHeightSamplesSize == 0) + return; + + // Reset the batch if we switch coloring mode + if (mCachedUseMaterialColors != inUseMaterialColors) + { + mGeometry.clear(); + mCachedUseMaterialColors = inUseMaterialColors; + } + + if (mGeometry.empty()) + { + // Divide terrain in triangle batches of max 64x64x2 triangles to allow better culling of the terrain + uint32 block_size = min(mSampleCount, 64); + for (uint32 by = 0; by < mSampleCount; by += block_size) + for (uint32 bx = 0; bx < mSampleCount; bx += block_size) + { + // Create vertices for a block + Array triangles; + triangles.resize(block_size * block_size * 2); + DebugRenderer::Triangle *out_tri = &triangles[0]; + for (uint32 y = by, max_y = min(by + block_size, mSampleCount - 1); y < max_y; ++y) + for (uint32 x = bx, max_x = min(bx + block_size, mSampleCount - 1); x < max_x; ++x) + if (!IsNoCollision(x, y) && !IsNoCollision(x + 1, y + 1)) + { + Vec3 x1y1 = GetPosition(x, y); + Vec3 x2y2 = GetPosition(x + 1, y + 1); + Color color = inUseMaterialColors? GetMaterial(x, y)->GetDebugColor() : Color::sWhite; + + if (!IsNoCollision(x, y + 1)) + { + Vec3 x1y2 = GetPosition(x, y + 1); + + x1y1.StoreFloat3(&out_tri->mV[0].mPosition); + x1y2.StoreFloat3(&out_tri->mV[1].mPosition); + x2y2.StoreFloat3(&out_tri->mV[2].mPosition); + + Vec3 normal = (x2y2 - x1y2).Cross(x1y1 - x1y2).Normalized(); + for (DebugRenderer::Vertex &v : out_tri->mV) + { + v.mColor = color; + v.mUV = Float2(0, 0); + normal.StoreFloat3(&v.mNormal); + } + + ++out_tri; + } + + if (!IsNoCollision(x + 1, y)) + { + Vec3 x2y1 = GetPosition(x + 1, y); + + x1y1.StoreFloat3(&out_tri->mV[0].mPosition); + x2y2.StoreFloat3(&out_tri->mV[1].mPosition); + x2y1.StoreFloat3(&out_tri->mV[2].mPosition); + + Vec3 normal = (x1y1 - x2y1).Cross(x2y2 - x2y1).Normalized(); + for (DebugRenderer::Vertex &v : out_tri->mV) + { + v.mColor = color; + v.mUV = Float2(0, 0); + normal.StoreFloat3(&v.mNormal); + } + + ++out_tri; + } + } + + // Resize triangles array to actual amount of triangles written + size_t num_triangles = out_tri - &triangles[0]; + triangles.resize(num_triangles); + + // Create batch + if (num_triangles > 0) + mGeometry.push_back(new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), DebugRenderer::sCalculateBounds(&triangles[0].mV[0], int(3 * num_triangles)))); + } + } + + // Get transform including scale + RMat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + for (const DebugRenderer::GeometryRef &b : mGeometry) + inRenderer->DrawGeometry(transform, inColor, b, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + if (sDrawTriangleOutlines) + { + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, DebugRenderer *inRenderer, RMat44Arg inTransform) : + mShape(inShape), + mRenderer(inRenderer), + mTransform(inTransform) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + UVec4 valid = Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const + { + // Determine active edges + uint8 active_edges = mShape->GetEdgeFlags(inX, inY, inTriangle); + + // Loop through edges + Vec3 v[] = { inV0, inV1, inV2 }; + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + RVec3 v1 = mTransform * v[edge_idx]; + RVec3 v2 = mTransform * v[(edge_idx + 1) % 3]; + + // Draw active edge as a green arrow, other edges as grey + if (active_edges & (1 << edge_idx)) + mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f); + else + mRenderer->DrawLine(v1, v2, Color::sGrey); + } + } + + const HeightFieldShape *mShape; + DebugRenderer * mRenderer; + RMat44 mTransform; + }; + + Visitor visitor(this, inRenderer, inCenterOfMassTransform.PreScaled(inScale)); + WalkHeightField(visitor); + } +} +#endif // JPH_DEBUG_RENDERER + +class HeightFieldShape::DecodingContext +{ +public: + JPH_INLINE explicit DecodingContext(const HeightFieldShape *inShape) : + mShape(inShape) + { + static_assert(sizeof(sGridOffsets) / sizeof(uint) == cNumBitsXY + 1, "Offsets array is not long enough"); + + // Construct root stack entry + mPropertiesStack[0] = 0; // level: 0, x: 0, y: 0 + } + + template + JPH_INLINE void WalkHeightField(Visitor &ioVisitor) + { + // Early out if there's no collision + if (mShape->mHeightSamplesSize == 0) + return; + + // Assert that an inside-out bounding box does not collide + JPH_IF_ENABLE_ASSERTS(UVec4 dummy = UVec4::sReplicate(0);) + JPH_ASSERT(ioVisitor.VisitRangeBlock(Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), dummy, 0) == 0); + + // Precalculate values relating to sample count + uint32 sample_count = mShape->mSampleCount; + UVec4 sample_count_min_1 = UVec4::sReplicate(sample_count - 1); + + // Precalculate values relating to block size + uint32 block_size = mShape->mBlockSize; + uint32 block_size_plus_1 = block_size + 1; + uint num_blocks = mShape->GetNumBlocks(); + uint num_blocks_min_1 = num_blocks - 1; + uint max_level = HeightFieldShape::sGetMaxLevel(num_blocks); + uint32 max_stride = (num_blocks + 1) >> 1; + + // Precalculate range block offset and stride for GetBlockOffsetAndScale + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride); + + // Allocate space for vertices and 'no collision' flags + int array_size = Square(block_size_plus_1); + Vec3 *vertices = reinterpret_cast(JPH_STACK_ALLOC(array_size * sizeof(Vec3))); + bool *no_collision = reinterpret_cast(JPH_STACK_ALLOC(array_size * sizeof(bool))); + + // Splat offsets + Vec4 ox = mShape->mOffset.SplatX(); + Vec4 oy = mShape->mOffset.SplatY(); + Vec4 oz = mShape->mOffset.SplatZ(); + + // Splat scales + Vec4 sx = mShape->mScale.SplatX(); + Vec4 sy = mShape->mScale.SplatY(); + Vec4 sz = mShape->mScale.SplatZ(); + + do + { + // Decode properties + uint32 properties_top = mPropertiesStack[mTop]; + uint32 x = properties_top & cMaskBitsXY; + uint32 y = (properties_top >> cNumBitsXY) & cMaskBitsXY; + uint32 level = properties_top >> cLevelShift; + + if (level >= max_level) + { + // Determine actual range of samples (minus one because we eventually want to iterate over the triangles, not the samples) + uint32 min_x = x * block_size; + uint32 max_x = min_x + block_size; + uint32 min_y = y * block_size; + uint32 max_y = min_y + block_size; + + // Decompress vertices of block at (x, y) + Vec3 *dst_vertex = vertices; + bool *dst_no_collision = no_collision; + float block_offset, block_scale; + mShape->GetBlockOffsetAndScale(x, y, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_y = min_y; v_y < max_y; ++v_y) + { + for (uint32 v_x = min_x; v_x < max_x; ++v_x) + { + *dst_vertex = mShape->GetPosition(v_x, v_y, block_offset, block_scale, *dst_no_collision); + ++dst_vertex; + ++dst_no_collision; + } + + // Skip last column, these values come from a different block + ++dst_vertex; + ++dst_no_collision; + } + + // Decompress block (x + 1, y) + uint32 max_x_decrement = 0; + if (x < num_blocks_min_1) + { + dst_vertex = vertices + block_size; + dst_no_collision = no_collision + block_size; + mShape->GetBlockOffsetAndScale(x + 1, y, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_y = min_y; v_y < max_y; ++v_y) + { + *dst_vertex = mShape->GetPosition(max_x, v_y, block_offset, block_scale, *dst_no_collision); + dst_vertex += block_size_plus_1; + dst_no_collision += block_size_plus_1; + } + } + else + max_x_decrement = 1; // We don't have a next block, one less triangle to test + + // Decompress block (x, y + 1) + if (y < num_blocks_min_1) + { + uint start = block_size * block_size_plus_1; + dst_vertex = vertices + start; + dst_no_collision = no_collision + start; + mShape->GetBlockOffsetAndScale(x, y + 1, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_x = min_x; v_x < max_x; ++v_x) + { + *dst_vertex = mShape->GetPosition(v_x, max_y, block_offset, block_scale, *dst_no_collision); + ++dst_vertex; + ++dst_no_collision; + } + + // Decompress single sample of block at (x + 1, y + 1) + if (x < num_blocks_min_1) + { + mShape->GetBlockOffsetAndScale(x + 1, y + 1, range_block_offset, range_block_stride, block_offset, block_scale); + *dst_vertex = mShape->GetPosition(max_x, max_y, block_offset, block_scale, *dst_no_collision); + } + } + else + --max_y; // We don't have a next block, one less triangle to test + + // Update max_x (we've been using it so we couldn't update it earlier) + max_x -= max_x_decrement; + + // We're going to divide the vertices in 4 blocks to do one more runtime sub-division, calculate the ranges of those blocks + struct Range + { + uint32 mMinX, mMinY, mNumTrianglesX, mNumTrianglesY; + }; + uint32 half_block_size = block_size >> 1; + uint32 block_size_x = max_x - min_x - half_block_size; + uint32 block_size_y = max_y - min_y - half_block_size; + Range ranges[] = + { + { 0, 0, half_block_size, half_block_size }, + { half_block_size, 0, block_size_x, half_block_size }, + { 0, half_block_size, half_block_size, block_size_y }, + { half_block_size, half_block_size, block_size_x, block_size_y }, + }; + + // Calculate the min and max of each of the blocks + Mat44 block_min, block_max; + for (int block = 0; block < 4; ++block) + { + // Get the range for this block + const Range &range = ranges[block]; + uint32 start = range.mMinX + range.mMinY * block_size_plus_1; + uint32 size_x_plus_1 = range.mNumTrianglesX + 1; + uint32 size_y_plus_1 = range.mNumTrianglesY + 1; + + // Calculate where to start reading + const Vec3 *src_vertex = vertices + start; + const bool *src_no_collision = no_collision + start; + uint32 stride = block_size_plus_1 - size_x_plus_1; + + // Start range with a very large inside-out box + Vec3 value_min = Vec3::sReplicate(1.0e30f); + Vec3 value_max = Vec3::sReplicate(-1.0e30f); + + // Loop over the samples to determine the min and max of this block + for (uint32 block_y = 0; block_y < size_y_plus_1; ++block_y) + { + for (uint32 block_x = 0; block_x < size_x_plus_1; ++block_x) + { + if (!*src_no_collision) + { + value_min = Vec3::sMin(value_min, *src_vertex); + value_max = Vec3::sMax(value_max, *src_vertex); + } + ++src_vertex; + ++src_no_collision; + } + src_vertex += stride; + src_no_collision += stride; + } + block_min.SetColumn4(block, Vec4(value_min)); + block_max.SetColumn4(block, Vec4(value_max)); + } + + #ifdef JPH_DEBUG_HEIGHT_FIELD + // Draw the bounding boxes of the sub-nodes + for (int block = 0; block < 4; ++block) + { + AABox bounds(block_min.GetColumn3(block), block_max.GetColumn3(block)); + if (bounds.IsValid()) + DebugRenderer::sInstance->DrawWireBox(bounds, Color::sYellow); + } + #endif // JPH_DEBUG_HEIGHT_FIELD + + // Transpose so we have the mins and maxes of each of the blocks in rows instead of columns + Mat44 transposed_min = block_min.Transposed(); + Mat44 transposed_max = block_max.Transposed(); + + // Check which blocks collide + // Note: At this point we don't use our own stack but we do allow the visitor to use its own stack + // to store collision distances so that we can still early out when no closer hits have been found. + UVec4 colliding_blocks(0, 1, 2, 3); + int num_results = ioVisitor.VisitRangeBlock(transposed_min.GetColumn4(0), transposed_min.GetColumn4(1), transposed_min.GetColumn4(2), transposed_max.GetColumn4(0), transposed_max.GetColumn4(1), transposed_max.GetColumn4(2), colliding_blocks, mTop); + + // Loop through the results backwards (closest first) + int result = num_results - 1; + while (result >= 0) + { + // Calculate the min and max of this block + uint32 block = colliding_blocks[result]; + const Range &range = ranges[block]; + uint32 block_min_x = min_x + range.mMinX; + uint32 block_max_x = block_min_x + range.mNumTrianglesX; + uint32 block_min_y = min_y + range.mMinY; + uint32 block_max_y = block_min_y + range.mNumTrianglesY; + + // Loop triangles + for (uint32 v_y = block_min_y; v_y < block_max_y; ++v_y) + for (uint32 v_x = block_min_x; v_x < block_max_x; ++v_x) + { + // Get first vertex + const int offset = (v_y - min_y) * block_size_plus_1 + (v_x - min_x); + const Vec3 *start_vertex = vertices + offset; + const bool *start_no_collision = no_collision + offset; + + // Check if vertices shared by both triangles have collision + if (!start_no_collision[0] && !start_no_collision[block_size_plus_1 + 1]) + { + // Loop 2 triangles + for (uint t = 0; t < 2; ++t) + { + // Determine triangle vertices + Vec3 v0, v1, v2; + if (t == 0) + { + // Check third vertex + if (start_no_collision[block_size_plus_1]) + continue; + + // Get vertices for triangle + v0 = start_vertex[0]; + v1 = start_vertex[block_size_plus_1]; + v2 = start_vertex[block_size_plus_1 + 1]; + } + else + { + // Check third vertex + if (start_no_collision[1]) + continue; + + // Get vertices for triangle + v0 = start_vertex[0]; + v1 = start_vertex[block_size_plus_1 + 1]; + v2 = start_vertex[1]; + } + + #ifdef JPH_DEBUG_HEIGHT_FIELD + DebugRenderer::sInstance->DrawWireTriangle(RVec3(v0), RVec3(v1), RVec3(v2), Color::sWhite); + #endif + + // Call visitor + ioVisitor.VisitTriangle(v_x, v_y, t, v0, v1, v2); + + // Check if we're done + if (ioVisitor.ShouldAbort()) + return; + } + } + } + + // Fetch next block until we find one that the visitor wants to see + do + --result; + while (result >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop + result)); + } + } + else + { + // Visit child grid + uint32 stride = min(1U << level, max_stride); // At the most detailed level we store a non-power of 2 number of blocks + uint32 offset = sGridOffsets[level] + stride * y + x; + + // Decode min/max height + JPH_ASSERT(offset < mShape->mRangeBlocksSize); + UVec4 block = UVec4::sLoadInt4Aligned(reinterpret_cast(&mShape->mRangeBlocks[offset])); + Vec4 bounds_miny = oy + sy * block.Expand4Uint16Lo().ToFloat(); + Vec4 bounds_maxy = oy + sy * block.Expand4Uint16Hi().ToFloat(); + + // Calculate size of one cell at this grid level + UVec4 internal_cell_size = UVec4::sReplicate(block_size << (max_level - level - 1)); // subtract 1 from level because we have an internal grid of 2x2 + + // Calculate min/max x and z + UVec4 two_x = UVec4::sReplicate(2 * x); // multiply by two because we have an internal grid of 2x2 + Vec4 bounds_minx = ox + sx * (internal_cell_size * (two_x + UVec4(0, 1, 0, 1))).ToFloat(); + Vec4 bounds_maxx = ox + sx * UVec4::sMin(internal_cell_size * (two_x + UVec4(1, 2, 1, 2)), sample_count_min_1).ToFloat(); + + UVec4 two_y = UVec4::sReplicate(2 * y); + Vec4 bounds_minz = oz + sz * (internal_cell_size * (two_y + UVec4(0, 0, 1, 1))).ToFloat(); + Vec4 bounds_maxz = oz + sz * UVec4::sMin(internal_cell_size * (two_y + UVec4(1, 1, 2, 2)), sample_count_min_1).ToFloat(); + + // Calculate properties of child blocks + UVec4 properties = UVec4::sReplicate(((level + 1) << cLevelShift) + (y << (cNumBitsXY + 1)) + (x << 1)) + UVec4(0, 1, 1 << cNumBitsXY, (1 << cNumBitsXY) + 1); + + #ifdef JPH_DEBUG_HEIGHT_FIELD + // Draw boxes + for (int i = 0; i < 4; ++i) + { + AABox b(Vec3(bounds_minx[i], bounds_miny[i], bounds_minz[i]), Vec3(bounds_maxx[i], bounds_maxy[i], bounds_maxz[i])); + if (b.IsValid()) + DebugRenderer::sInstance->DrawWireBox(b, Color::sGreen); + } + #endif + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitRangeBlock(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop); + + // Push them onto the stack + JPH_ASSERT(mTop + 4 < cStackSize); + properties.StoreInt4(&mPropertiesStack[mTop]); + mTop += num_results; + } + + // Check if we're done + if (ioVisitor.ShouldAbort()) + return; + + // Fetch next node until we find one that the visitor wants to see + do + --mTop; + while (mTop >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop)); + } + while (mTop >= 0); + } + + // This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkHeightField() again) + JPH_INLINE bool IsDoneWalking() const + { + return mTop < 0; + } + +private: + const HeightFieldShape * mShape; + int mTop = 0; + uint32 mPropertiesStack[cStackSize]; +}; + +template +void HeightFieldShape::WalkHeightField(Visitor &ioVisitor) const +{ + DecodingContext ctx(this); + ctx.WalkHeightField(ioVisitor); +} + +bool HeightFieldShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) : + mHit(ioHit), + mRayOrigin(inRay.mOrigin), + mRayDirection(inRay.mDirection), + mRayInvDirection(inRay.mDirection), + mShape(inShape), + mSubShapeIDCreator(inSubShapeIDCreator) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mHit.mFraction) + { + // It's a closer hit + mHit.mFraction = fraction; + mHit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle); + mReturnValue = true; + } + } + + RayCastResult & mHit; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + const HeightFieldShape *mShape; + SubShapeIDCreator mSubShapeIDCreator; + bool mReturnValue = false; + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(this, inRay, inSubShapeIDCreator, ioHit); + WalkHeightField(visitor); + + return visitor.mReturnValue; +} + +void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector) : + mCollector(ioCollector), + mRayOrigin(inRay.mOrigin), + mRayDirection(inRay.mDirection), + mRayInvDirection(inRay.mDirection), + mBackFaceMode(inRayCastSettings.mBackFaceModeTriangles), + mShape(inShape), + mSubShapeIDCreator(inSubShapeIDCreator) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const + { + // Back facing check + if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0) + return; + + // Check the triangle + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mCollector.GetEarlyOutFraction()) + { + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle); + mCollector.AddHit(hit); + } + } + + CastRayCollector & mCollector; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + EBackFaceMode mBackFaceMode; + const HeightFieldShape *mShape; + SubShapeIDCreator mSubShapeIDCreator; + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(this, inRay, inRayCastSettings, inSubShapeIDCreator, ioCollector); + WalkHeightField(visitor); +} + +void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // A height field doesn't have volume, so we can't test insideness +} + +void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideSoftBodyVerticesVsTriangles + { + using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return mDistanceStack[inStackTop] < mClosestDistanceSq; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Get distance to vertex + Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Clear distance for invalid bounds + dist_sq = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), dist_sq, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle([[maybe_unused]] uint inX, [[maybe_unused]] uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + ProcessTriangle(inV0, inV1, inV2); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inCenterOfMassTransform, inScale); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + visitor.StartVertex(v); + WalkHeightField(visitor); + visitor.FinishVertex(v, inCollidingShapeIndex); + } +} + +void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastConvexVsTriangles + { + using CastConvexVsTriangles::CastConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Clear distance for invalid bounds + distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create sub shape id for this part + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + SubShapeIDCreator mSubShapeIDCreator2; + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField); + const HeightFieldShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mShape2 = shape; + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter(); + visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent(); + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape->WalkHeightField(visitor); +} + +void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastSphereVsTriangles + { + using CastSphereVsTriangles::CastSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the radius of the sphere + AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Clear distance for invalid bounds + distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create sub shape id for this part + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + RayInvDirection mInvDirection; + SubShapeIDCreator mSubShapeIDCreator2; + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField); + const HeightFieldShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mShape2 = shape; + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape->WalkHeightField(visitor); +} + +struct HeightFieldShape::HSGetTrianglesContext +{ + HSGetTrianglesContext(const HeightFieldShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mDecodeCtx(inShape), + mShape(inShape), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mHeightFieldScale(inScale), + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + } + + bool ShouldAbort() const + { + return mShouldAbort; + } + + bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mHeightFieldScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + void VisitTriangle(uint inX, uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // When the buffer is full and we cannot process the triangles, abort the height field walk. The next time GetTrianglesNext is called we will continue here. + if (mNumTrianglesFound + 1 > mMaxTrianglesRequested) + { + mShouldAbort = true; + return; + } + + // Store vertices as Float3 + if (mIsInsideOut) + { + // Reverse vertices + (mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++); + } + else + { + // Normal scale + (mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++); + } + + // Decode material + if (mMaterials != nullptr) + *mMaterials++ = mShape->GetMaterial(inX, inY); + + // Accumulate triangles found + mNumTrianglesFound++; + } + + DecodingContext mDecodeCtx; + const HeightFieldShape * mShape; + OrientedBox mLocalBox; + Vec3 mHeightFieldScale; + Mat44 mLocalToWorld; + int mMaxTrianglesRequested; + Float3 * mTriangleVertices; + int mNumTrianglesFound; + const PhysicsMaterial ** mMaterials; + bool mShouldAbort; + bool mIsInsideOut; +}; + +void HeightFieldShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(HSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(HSGetTrianglesContext))); + + new (&ioContext) HSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale); +} + +int HeightFieldShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 1, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + // Check if we're done + HSGetTrianglesContext &context = (HSGetTrianglesContext &)ioContext; + if (context.mDecodeCtx.IsDoneWalking()) + return 0; + + // Store parameters on context + context.mMaxTrianglesRequested = inMaxTrianglesRequested; + context.mTriangleVertices = outTriangleVertices; + context.mMaterials = outMaterials; + context.mShouldAbort = false; // Reset the abort flag + context.mNumTrianglesFound = 0; + + // Continue (or start) walking the height field + context.mDecodeCtx.WalkHeightField(context); + return context.mNumTrianglesFound; +} + +void HeightFieldShape::sCollideConvexVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField); + const ConvexShape *shape1 = static_cast(inShape1); + const HeightFieldShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideConvexVsTriangles + { + using CollideConvexVsTriangles::CollideConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create ID for triangle + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + SubShapeIDCreator mSubShapeIDCreator2; + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + visitor.mShape2 = shape2; + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape2->WalkHeightField(visitor); +} + +void HeightFieldShape::sCollideSphereVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField); + const SphereShape *shape1 = static_cast(inShape1); + const HeightFieldShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideSphereVsTriangles + { + using CollideSphereVsTriangles::CollideSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create ID for triangle + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + SubShapeIDCreator mSubShapeIDCreator2; + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + visitor.mShape2 = shape2; + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape2->WalkHeightField(visitor); +} + +void HeightFieldShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mOffset); + inStream.Write(mScale); + inStream.Write(mSampleCount); + inStream.Write(mBlockSize); + inStream.Write(mBitsPerSample); + inStream.Write(mMinSample); + inStream.Write(mMaxSample); + inStream.Write(mMaterialIndices); + inStream.Write(mNumBitsPerMaterialIndex); + + if (mRangeBlocks != nullptr) + { + inStream.Write(true); + inStream.WriteBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); + } + else + { + inStream.Write(false); + } +} + +void HeightFieldShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mOffset); + inStream.Read(mScale); + inStream.Read(mSampleCount); + inStream.Read(mBlockSize); + inStream.Read(mBitsPerSample); + inStream.Read(mMinSample); + inStream.Read(mMaxSample); + inStream.Read(mMaterialIndices); + inStream.Read(mNumBitsPerMaterialIndex); + + // We don't have the exact number of reserved materials anymore, but ensure that our array is big enough + // TODO: Next time when we bump the binary serialization format of this class we should store the capacity and allocate the right amount, for now we accept a little bit of waste + mMaterials.reserve(PhysicsMaterialList::size_type(1) << mNumBitsPerMaterialIndex); + + CacheValues(); + + bool has_heights = false; + inStream.Read(has_heights); + if (has_heights) + { + AllocateBuffers(); + inStream.ReadBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); + } +} + +void HeightFieldShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials = mMaterials; +} + +void HeightFieldShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + mMaterials.assign(inMaterials, inMaterials + inNumMaterials); +} + +Shape::Stats HeightFieldShape::GetStats() const +{ + return Stats( + sizeof(*this) + + mMaterials.size() * sizeof(Ref) + + mRangeBlocksSize * sizeof(RangeBlock) + + mHeightSamplesSize * sizeof(uint8) + + mActiveEdgesSize * sizeof(uint8) + + mMaterialIndices.size() * sizeof(uint8), + mHeightSamplesSize == 0? 0 : Square(mSampleCount - 1) * 2); +} + +void HeightFieldShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::HeightField); + f.mConstruct = []() -> Shape * { return new HeightFieldShape; }; + f.mColor = Color::sPurple; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::HeightField, sCollideConvexVsHeightField); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::HeightField, sCastConvexVsHeightField); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCastShape); + CollisionDispatch::sRegisterCollideShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCollideShape); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCollideSphereVsHeightField); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCastSphereVsHeightField); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h new file mode 100644 index 000000000000..16cbb76367bd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h @@ -0,0 +1,380 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class ConvexShape; +class CollideShapeSettings; +class TempAllocator; + +/// Constants for HeightFieldShape, this was moved out of the HeightFieldShape because of a linker bug +namespace HeightFieldShapeConstants +{ + /// Value used to create gaps in the height field + constexpr float cNoCollisionValue = FLT_MAX; + + /// Stack size to use during WalkHeightField + constexpr int cStackSize = 128; + + /// A position in the hierarchical grid is defined by a level (which grid), x and y position. We encode this in a single uint32 as: level << 28 | y << 14 | x + constexpr uint cNumBitsXY = 14; + constexpr uint cMaskBitsXY = (1 << cNumBitsXY) - 1; + constexpr uint cLevelShift = 2 * cNumBitsXY; + + /// When height samples are converted to 16 bit: + constexpr uint16 cNoCollisionValue16 = 0xffff; ///< This is the magic value for 'no collision' + constexpr uint16 cMaxHeightValue16 = 0xfffe; ///< This is the maximum allowed height value +}; + +/// Class that constructs a HeightFieldShape +class JPH_EXPORT HeightFieldShapeSettings final : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HeightFieldShapeSettings) + +public: + /// Default constructor for deserialization + HeightFieldShapeSettings() = default; + + /// Create a height field shape of inSampleCount * inSampleCount vertices. + /// The height field is a surface defined by: inOffset + inScale * (x, inSamples[y * inSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, inSampleCount - 1]. + /// inSampleCount: inSampleCount / mBlockSize must be minimally 2 and a power of 2 is the most efficient in terms of performance and storage. + /// inSamples: inSampleCount^2 vertices. + /// inMaterialIndices: (inSampleCount - 1)^2 indices that index into inMaterialList. + HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices = nullptr, const PhysicsMaterialList &inMaterialList = PhysicsMaterialList()); + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Determine the minimal and maximal value of mHeightSamples (will ignore cNoCollisionValue) + /// @param outMinValue The minimal value of mHeightSamples or FLT_MAX if no samples have collision + /// @param outMaxValue The maximal value of mHeightSamples or -FLT_MAX if no samples have collision + /// @param outQuantizationScale (value - outMinValue) * outQuantizationScale quantizes a height sample to 16 bits + void DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const; + + /// Given mBlockSize, mSampleCount and mHeightSamples, calculate the amount of bits needed to stay below absolute error inMaxError + /// @param inMaxError Maximum allowed error in mHeightSamples after compression (note that this does not take mScale.Y into account) + /// @return Needed bits per sample in the range [1, 8]. + uint32 CalculateBitsPerSampleForError(float inMaxError) const; + + /// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, mSampleCount - 1]. + Vec3 mOffset = Vec3::sZero(); + Vec3 mScale = Vec3::sReplicate(1.0f); + uint32 mSampleCount = 0; + + /// Artificial minimal value of mHeightSamples, used for compression and can be used to update the terrain after creating with lower height values. If there are any lower values in mHeightSamples, this value will be ignored. + float mMinHeightValue = FLT_MAX; + + /// Artificial maximum value of mHeightSamples, used for compression and can be used to update the terrain after creating with higher height values. If there are any higher values in mHeightSamples, this value will be ignored. + float mMaxHeightValue = -FLT_MAX; + + /// When bigger than mMaterials.size() the internal material list will be preallocated to support this number of materials. + /// This avoids reallocations when calling HeightFieldShape::SetMaterials with new materials later. + uint32 mMaterialsCapacity = 0; + + /// The heightfield is divided in blocks of mBlockSize * mBlockSize * 2 triangles and the acceleration structure culls blocks only, + /// bigger block sizes reduce memory consumption but also reduce query performance. Sensible values are [2, 8], does not need to be + /// a power of 2. Note that at run-time we'll perform one more grid subdivision, so the effective block size is half of what is provided here. + uint32 mBlockSize = 2; + + /// How many bits per sample to use to compress the height field. Can be in the range [1, 8]. + /// Note that each sample is compressed relative to the min/max value of its block of mBlockSize * mBlockSize pixels so the effective precision is higher. + /// Also note that increasing mBlockSize saves more memory than reducing the amount of bits per sample. + uint32 mBitsPerSample = 8; + + /// An array of mSampleCount^2 height samples. Samples are stored in row major order, so the sample at (x, y) is at index y * mSampleCount + x. + Array mHeightSamples; + + /// An array of (mSampleCount - 1)^2 material indices. + Array mMaterialIndices; + + /// The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]] + PhysicsMaterialList mMaterials; + + /// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + /// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly). + /// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees). + float mActiveEdgeCosThresholdAngle = 0.996195f; // cos(5 degrees) +}; + +/// A height field shape. Cannot be used as a dynamic object. +/// +/// Note: If you're using HeightFieldShape and are querying data while modifying the shape you'll have a race condition. +/// In this case it is best to create a new HeightFieldShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape. +/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes. +class JPH_EXPORT HeightFieldShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + HeightFieldShape() : Shape(EShapeType::HeightField, EShapeSubType::HeightField) { } + HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult); + virtual ~HeightFieldShape() override; + + /// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information. + Ref Clone() const; + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return true; } + + /// Get the size of the height field. Note that this will always be rounded up to the nearest multiple of GetBlockSize(). + inline uint GetSampleCount() const { return mSampleCount; } + + /// Get the size of a block + inline uint GetBlockSize() const { return mBlockSize; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return GetSubShapeIDBits(); } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return 0.0f; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + /// Overload to get the material at a particular location + const PhysicsMaterial * GetMaterial(uint inX, uint inY) const; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); } + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + /// Get height field position at sampled location (inX, inY). + /// where inX and inY are integers in the range inX e [0, mSampleCount - 1] and inY e [0, mSampleCount - 1]. + Vec3 GetPosition(uint inX, uint inY) const; + + /// Check if height field at sampled location (inX, inY) has collision (has a hole or not) + bool IsNoCollision(uint inX, uint inY) const; + + /// Projects inLocalPosition (a point in the space of the shape) along the Y axis onto the surface and returns it in outSurfacePosition. + /// When there is no surface position (because of a hole or because the point is outside the heightfield) the function will return false. + bool ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const; + + /// Returns the coordinates of the triangle that a sub shape ID represents + /// @param inSubShapeID The sub shape ID to decode + /// @param outX X coordinate of the triangle (in the range [0, mSampleCount - 2]) + /// @param outY Y coordinate of the triangle (in the range [0, mSampleCount - 2]) + /// @param outTriangleIndex Triangle within the quad (0 = lower triangle or 1 = upper triangle) + void GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const; + + /// Get the range of height values that this height field can encode. Can be used to determine the allowed range when setting the height values with SetHeights. + float GetMinHeightValue() const { return mOffset.GetY(); } + float GetMaxHeightValue() const { return mOffset.GetY() + mScale.GetY() * HeightFieldShapeConstants::cMaxHeightValue16; } + + /// Get the height values of a block of data. + /// Note that the height values are decompressed so will be slightly different from what the shape was originally created with. + /// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX] + /// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inY] + /// @param outHeights Returned height values, must be at least inSizeX * inSizeY floats. Values are returned in x-major order and can be cNoCollisionValue. + /// @param inHeightsStride Stride in floats between two consecutive rows of outHeights (can be negative if the data is upside down). + void GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const; + + /// Set the height values of a block of data. + /// Note that this requires decompressing and recompressing a border of size mBlockSize in the negative x/y direction so will cause some precision loss. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX] + /// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inY] + /// @param inHeights The new height values to set, must be an array of inSizeX * inSizeY floats, can be cNoCollisionValue. Values outside of the range [GetMinHeightValue(), GetMaxHeightValue()] will be clamped. + /// @param inHeightsStride Stride in floats between two consecutive rows of inHeights (can be negative if the data is upside down). + /// @param inAllocator Allocator to use for temporary memory + /// @param inActiveEdgeCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + void SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle = 0.996195f); + + /// Get the current list of materials, the indices returned by GetMaterials() will index into this list. + const PhysicsMaterialList & GetMaterialList() const { return mMaterials; } + + /// Get the material indices of a block of data. + /// @param inX Start X position, must in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction + /// @param inSizeY Number of samples in Y direction + /// @param outMaterials Returned material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order. + /// @param inMaterialsStride Stride in uint8s between two consecutive rows of outMaterials (can be negative if the data is upside down). + void GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const; + + /// Set the material indices of a block of data. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inX Start X position, must in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction + /// @param inSizeY Number of samples in Y direction + /// @param inMaterials The new material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order. + /// @param inMaterialsStride Stride in uint8s between two consecutive rows of inMaterials (can be negative if the data is upside down). + /// @param inMaterialList The material list to use for the new material indices or nullptr if the material list should not be updated + /// @param inAllocator Allocator to use for temporary memory + /// @return True if the material indices were set, false if the total number of materials exceeded 256 + bool SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator); + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + +#ifdef JPH_DEBUG_RENDERER + // Settings + static bool sDrawTriangleOutlines; +#endif // JPH_DEBUG_RENDERER + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + class DecodingContext; ///< Context class for walking through all nodes of a heightfield + struct HSGetTrianglesContext; ///< Context class for GetTrianglesStart/Next + + /// Calculate commonly used values and store them in the shape + void CacheValues(); + + /// Allocate the mRangeBlocks, mHeightSamples and mActiveEdges buffers as a single data block + void AllocateBuffers(); + + /// Calculate bit mask for all active edges in the heightfield for a specific region + void CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator); + + /// Calculate bit mask for all active edges in the heightfield + void CalculateActiveEdges(const HeightFieldShapeSettings &inSettings); + + /// Store material indices in the least amount of bits per index possible + void StoreMaterialIndices(const HeightFieldShapeSettings &inSettings); + + /// Get the amount of horizontal/vertical blocks + inline uint GetNumBlocks() const { return mSampleCount / mBlockSize; } + + /// Get the maximum level (amount of grids) of the tree + static inline uint sGetMaxLevel(uint inNumBlocks) { return 32 - CountLeadingZeros(inNumBlocks - 1); } + + /// Get the range block offset and stride for GetBlockOffsetAndScale + static inline void sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride); + + /// For block (inBlockX, inBlockY) get the offset and scale needed to decode a uint8 height sample to a uint16 + inline void GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const; + + /// Get the height sample at position (inX, inY) + inline uint8 GetHeightSample(uint inX, uint inY) const; + + /// Faster version of GetPosition when block offset and scale are already known + inline Vec3 GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const; + + /// Determine amount of bits needed to encode sub shape id + uint GetSubShapeIDBits() const; + + /// En/decode a sub shape ID. inX and inY specify the coordinate of the triangle. inTriangle == 0 is the lower triangle, inTriangle == 1 is the upper triangle. + inline SubShapeID EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const; + inline void DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const; + + /// Get the edge flags for a triangle + inline uint8 GetEdgeFlags(uint inX, uint inY, uint inTriangle) const; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideSphereVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Visit the entire height field using a visitor pattern + /// Note: Used to be inlined but this triggers a bug in MSVC where it will not free the memory allocated by alloca which causes a stack overflow when WalkHeightField is called in a loop (clang does it correct) + template + void WalkHeightField(Visitor &ioVisitor) const; + + /// A block of 2x2 ranges used to form a hierarchical grid, ordered left top, right top, left bottom, right bottom + struct alignas(16) RangeBlock + { + uint16 mMin[4]; + uint16 mMax[4]; + }; + + /// For block (inBlockX, inBlockY) get the range block and the entry in the range block + inline void GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock); + + /// Offset of first RangedBlock in grid per level + static const uint sGridOffsets[]; + + /// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, mSampleCount - 1]. + Vec3 mOffset = Vec3::sZero(); + Vec3 mScale = Vec3::sReplicate(1.0f); + + /// Height data + uint32 mSampleCount = 0; ///< See HeightFieldShapeSettings::mSampleCount + uint32 mBlockSize = 2; ///< See HeightFieldShapeSettings::mBlockSize + uint32 mHeightSamplesSize = 0; ///< Size of mHeightSamples in bytes + uint32 mRangeBlocksSize = 0; ///< Size of mRangeBlocks in elements + uint32 mActiveEdgesSize = 0; ///< Size of mActiveEdges in bytes + uint8 mBitsPerSample = 8; ///< See HeightFieldShapeSettings::mBitsPerSample + uint8 mSampleMask = 0xff; ///< All bits set for a sample: (1 << mBitsPerSample) - 1, used to indicate that there's no collision + uint16 mMinSample = HeightFieldShapeConstants::cNoCollisionValue16; ///< Min and max value in mHeightSamples quantized to 16 bit, for calculating bounding box + uint16 mMaxSample = HeightFieldShapeConstants::cNoCollisionValue16; + RangeBlock * mRangeBlocks = nullptr; ///< Hierarchical grid of range data describing the height variations within 1 block. The grid for level starts at offset sGridOffsets[] + uint8 * mHeightSamples = nullptr; ///< mBitsPerSample-bit height samples. Value [0, mMaxHeightValue] maps to highest detail grid in mRangeBlocks [mMin, mMax]. mNoCollisionValue is reserved to indicate no collision. + uint8 * mActiveEdges = nullptr; ///< (mSampleCount - 1)^2 * 3-bit active edge flags. + + /// Materials + PhysicsMaterialList mMaterials; ///< The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]] + Array mMaterialIndices; ///< Compressed to the minimum amount of bits per material index (mSampleCount - 1) * (mSampleCount - 1) * mNumBitsPerMaterialIndex bits of data + uint32 mNumBitsPerMaterialIndex = 0; ///< Number of bits per material index + +#ifdef JPH_DEBUG_RENDERER + /// Temporary rendering data + mutable Array mGeometry; + mutable bool mCachedUseMaterialColors = false; ///< This is used to regenerate the triangle batch if the drawing settings change +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp new file mode 100644 index 000000000000..852021a0407d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp @@ -0,0 +1,1266 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool MeshShape::sDrawTriangleGroups = false; +bool MeshShape::sDrawTriangleOutlines = false; +#endif // JPH_DEBUG_RENDERER + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MeshShapeSettings) +{ + JPH_ADD_BASE_CLASS(MeshShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mTriangleVertices) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mIndexedTriangles) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaterials) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaxTrianglesPerLeaf) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mActiveEdgeCosThresholdAngle) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mPerTriangleUserData) +} + +// Codecs this mesh shape is using +using TriangleCodec = TriangleCodecIndexed8BitPackSOA4Flags; +using NodeCodec = NodeCodecQuadTreeHalfFloat; + +// Get header for tree +static JPH_INLINE const NodeCodec::Header *sGetNodeHeader(const ByteBuffer &inTree) +{ + return inTree.Get(0); +} + +// Get header for triangles +static JPH_INLINE const TriangleCodec::TriangleHeader *sGetTriangleHeader(const ByteBuffer &inTree) +{ + return inTree.Get(NodeCodec::HeaderSize); +} + +MeshShapeSettings::MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials) : + mMaterials(std::move(inMaterials)) +{ + Indexify(inTriangles, mTriangleVertices, mIndexedTriangles); + + Sanitize(); +} + +MeshShapeSettings::MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials) : + mTriangleVertices(std::move(inVertices)), + mIndexedTriangles(std::move(inTriangles)), + mMaterials(std::move(inMaterials)) +{ + Sanitize(); +} + +void MeshShapeSettings::Sanitize() +{ + // Remove degenerate and duplicate triangles + UnorderedSet triangles; + triangles.reserve(UnorderedSet::size_type(mIndexedTriangles.size())); + TriangleCodec::ValidationContext validation_ctx(mIndexedTriangles, mTriangleVertices); + for (int t = (int)mIndexedTriangles.size() - 1; t >= 0; --t) + { + const IndexedTriangle &tri = mIndexedTriangles[t]; + + if (tri.IsDegenerate(mTriangleVertices) // Degenerate triangle + || validation_ctx.IsDegenerate(tri) // Triangle is degenerate in the quantized space + || !triangles.insert(tri.GetLowestIndexFirst()).second) // Duplicate triangle + { + // The order of triangles doesn't matter (gets reordered while building the tree), so we can just swap the last triangle into this slot + mIndexedTriangles[t] = mIndexedTriangles.back(); + mIndexedTriangles.pop_back(); + } + } +} + +ShapeSettings::ShapeResult MeshShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new MeshShape(*this, mCachedResult); + return mCachedResult; +} + +MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::Mesh, EShapeSubType::Mesh, inSettings, outResult) +{ + // Check if there are any triangles + if (inSettings.mIndexedTriangles.empty()) + { + outResult.SetError("Need triangles to create a mesh shape!"); + return; + } + + // Check triangles + TriangleCodec::ValidationContext validation_ctx(inSettings.mIndexedTriangles, inSettings.mTriangleVertices); + for (int t = (int)inSettings.mIndexedTriangles.size() - 1; t >= 0; --t) + { + const IndexedTriangle &triangle = inSettings.mIndexedTriangles[t]; + if (triangle.IsDegenerate(inSettings.mTriangleVertices) + || validation_ctx.IsDegenerate(triangle)) + { + outResult.SetError(StringFormat("Triangle %d is degenerate!", t)); + return; + } + else + { + // Check vertex indices + for (uint32 idx : triangle.mIdx) + if (idx >= inSettings.mTriangleVertices.size()) + { + outResult.SetError(StringFormat("Vertex index %u is beyond vertex list (size: %u)", idx, (uint)inSettings.mTriangleVertices.size())); + return; + } + } + } + + // Copy materials + mMaterials = inSettings.mMaterials; + if (!mMaterials.empty()) + { + // Validate materials + if (mMaterials.size() > (1 << FLAGS_MATERIAL_BITS)) + { + outResult.SetError(StringFormat("Supporting max %d materials per mesh", 1 << FLAGS_MATERIAL_BITS)); + return; + } + for (const IndexedTriangle &t : inSettings.mIndexedTriangles) + if (t.mMaterialIndex >= mMaterials.size()) + { + outResult.SetError(StringFormat("Triangle material %u is beyond material list (size: %u)", t.mMaterialIndex, (uint)mMaterials.size())); + return; + } + } + else + { + // No materials assigned, validate that all triangles use material index 0 + for (const IndexedTriangle &t : inSettings.mIndexedTriangles) + if (t.mMaterialIndex != 0) + { + outResult.SetError("No materials present, all triangles should have material index 0"); + return; + } + } + + // Check max triangles + if (inSettings.mMaxTrianglesPerLeaf < 1 || inSettings.mMaxTrianglesPerLeaf > MaxTrianglesPerLeaf) + { + outResult.SetError("Invalid max triangles per leaf"); + return; + } + + // Fill in active edge bits + IndexedTriangleList indexed_triangles = inSettings.mIndexedTriangles; // Copy indices since we're adding the 'active edge' flag + sFindActiveEdges(inSettings, indexed_triangles); + + // Create triangle splitter + TriangleSplitterBinning splitter(inSettings.mTriangleVertices, indexed_triangles); + + // Build tree + AABBTreeBuilder builder(splitter, inSettings.mMaxTrianglesPerLeaf); + AABBTreeBuilderStats builder_stats; + const AABBTreeBuilder::Node *root = builder.Build(builder_stats); + + // Convert to buffer + AABBTreeToBuffer buffer; + const char *error = nullptr; + if (!buffer.Convert(builder.GetTriangles(), builder.GetNodes(), inSettings.mTriangleVertices, root, inSettings.mPerTriangleUserData, error)) + { + outResult.SetError(error); + return; + } + + // Move data to this class + mTree.swap(buffer.GetBuffer()); + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Mesh is too big and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +void MeshShape::sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices) +{ + // A struct to hold the two vertex indices of an edge + struct Edge + { + Edge(int inIdx1, int inIdx2) : mIdx1(min(inIdx1, inIdx2)), mIdx2(max(inIdx1, inIdx2)) { } + + uint GetIndexInTriangle(const IndexedTriangle &inTriangle) const + { + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + Edge edge(inTriangle.mIdx[edge_idx], inTriangle.mIdx[(edge_idx + 1) % 3]); + if (*this == edge) + return edge_idx; + } + + JPH_ASSERT(false); + return ~uint(0); + } + + bool operator == (const Edge &inRHS) const + { + return mIdx1 == inRHS.mIdx1 && mIdx2 == inRHS.mIdx2; + } + + uint64 GetHash() const + { + static_assert(sizeof(*this) == 2 * sizeof(int), "No padding expected"); + return HashBytes(this, sizeof(*this)); + } + + int mIdx1; + int mIdx2; + }; + + // A struct to hold the triangles that are connected to an edge + struct TriangleIndices + { + uint mNumTriangles = 0; + uint mTriangleIndices[2]; + }; + + // Build a list of edge to triangles + using EdgeToTriangle = UnorderedMap; + EdgeToTriangle edge_to_triangle; + edge_to_triangle.reserve(EdgeToTriangle::size_type(ioIndices.size() * 3)); + for (uint triangle_idx = 0; triangle_idx < ioIndices.size(); ++triangle_idx) + { + IndexedTriangle &triangle = ioIndices[triangle_idx]; + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + Edge edge(triangle.mIdx[edge_idx], triangle.mIdx[(edge_idx + 1) % 3]); + EdgeToTriangle::iterator edge_to_triangle_it = edge_to_triangle.try_emplace(edge, TriangleIndices()).first; + TriangleIndices &indices = edge_to_triangle_it->second; + if (indices.mNumTriangles < 2) + { + // Store index of triangle that connects to this edge + indices.mTriangleIndices[indices.mNumTriangles] = triangle_idx; + indices.mNumTriangles++; + } + else + { + // 3 or more triangles share an edge, mark this edge as active + uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT); + JPH_ASSERT((triangle.mMaterialIndex & mask) == 0); + triangle.mMaterialIndex |= mask; + } + } + } + + // Walk over all edges and determine which ones are active + for (const EdgeToTriangle::value_type &edge : edge_to_triangle) + { + uint num_active = 0; + if (edge.second.mNumTriangles == 1) + { + // Edge is not shared, it is an active edge + num_active = 1; + } + else if (edge.second.mNumTriangles == 2) + { + // Simple shared edge, determine if edge is active based on the two adjacent triangles + const IndexedTriangle &triangle1 = ioIndices[edge.second.mTriangleIndices[0]]; + const IndexedTriangle &triangle2 = ioIndices[edge.second.mTriangleIndices[1]]; + + // Find which edge this is for both triangles + uint edge_idx1 = edge.first.GetIndexInTriangle(triangle1); + uint edge_idx2 = edge.first.GetIndexInTriangle(triangle2); + + // Construct a plane for triangle 1 (e1 = edge vertex 1, e2 = edge vertex 2, op = opposing vertex) + Vec3 triangle1_e1 = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[edge_idx1]]); + Vec3 triangle1_e2 = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[(edge_idx1 + 1) % 3]]); + Vec3 triangle1_op = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[(edge_idx1 + 2) % 3]]); + Plane triangle1_plane = Plane::sFromPointsCCW(triangle1_e1, triangle1_e2, triangle1_op); + + // Construct a plane for triangle 2 + Vec3 triangle2_e1 = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[edge_idx2]]); + Vec3 triangle2_e2 = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[(edge_idx2 + 1) % 3]]); + Vec3 triangle2_op = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[(edge_idx2 + 2) % 3]]); + Plane triangle2_plane = Plane::sFromPointsCCW(triangle2_e1, triangle2_e2, triangle2_op); + + // Determine if the edge is active + num_active = ActiveEdges::IsEdgeActive(triangle1_plane.GetNormal(), triangle2_plane.GetNormal(), triangle1_e2 - triangle1_e1, inSettings.mActiveEdgeCosThresholdAngle)? 2 : 0; + } + else + { + // More edges incoming, we've already marked all edges beyond the 2nd as active + num_active = 2; + } + + // Mark edges of all original triangles active + for (uint i = 0; i < num_active; ++i) + { + uint triangle_idx = edge.second.mTriangleIndices[i]; + IndexedTriangle &triangle = ioIndices[triangle_idx]; + uint edge_idx = edge.first.GetIndexInTriangle(triangle); + uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT); + JPH_ASSERT((triangle.mMaterialIndex & mask) == 0); + triangle.mMaterialIndex |= mask; + } + } +} + +MassProperties MeshShape::GetMassProperties() const +{ + // We cannot calculate the volume for an arbitrary mesh, so we return invalid mass properties. + // If you want your mesh to be dynamic, then you should provide the mass properties yourself when + // creating a Body: + // + // BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + // BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sReplicate(1.0f), 1000.0f); + // + // Note that for a mesh shape to simulate properly, it is best if the mesh is manifold + // (i.e. closed, all edges shared by only two triangles, consistent winding order). + return MassProperties(); +} + +void MeshShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const +{ + // Get block + SubShapeID triangle_idx_subshape_id; + uint32 block_id = inSubShapeID.PopID(NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)), triangle_idx_subshape_id); + outTriangleBlock = NodeCodec::DecodingContext::sGetTriangleBlockStart(&mTree[0], block_id); + + // Fetch the triangle index + SubShapeID remainder; + outTriangleIndex = triangle_idx_subshape_id.PopID(NumTriangleBits, remainder); + JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID"); +} + +uint MeshShape::GetMaterialIndex(const SubShapeID &inSubShapeID) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Fetch the flags + uint8 flags = TriangleCodec::DecodingContext::sGetFlags(block_start, triangle_idx); + return flags & FLAGS_MATERIAL_MASK; +} + +const PhysicsMaterial *MeshShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Return the default material if there are no materials on this shape + if (mMaterials.empty()) + return PhysicsMaterial::sDefault; + + return mMaterials[GetMaterialIndex(inSubShapeID)]; +} + +Vec3 MeshShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Decode triangle + Vec3 v1, v2, v3; + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + triangle_ctx.GetTriangle(block_start, triangle_idx, v1, v2, v3); + + // Calculate normal + return (v3 - v2).Cross(v1 - v2).Normalized(); +} + +void MeshShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Decode triangle + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + outVertices.resize(3); + triangle_ctx.GetTriangle(block_start, triangle_idx, outVertices[0], outVertices[1], outVertices[2]); + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + std::swap(outVertices[1], outVertices[2]); + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Transform to world space + for (Vec3 &v : outVertices) + v = transform * v; +} + +AABox MeshShape::GetLocalBounds() const +{ + const NodeCodec::Header *header = sGetNodeHeader(mTree); + return AABox(Vec3::sLoadFloat3Unsafe(header->mRootBoundsMin), Vec3::sLoadFloat3Unsafe(header->mRootBoundsMax)); +} + +uint MeshShape::GetSubShapeIDBitsRecursive() const +{ + return NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)) + NumTriangleBits; +} + +template +JPH_INLINE void MeshShape::WalkTree(Visitor &ioVisitor) const +{ + const NodeCodec::Header *header = sGetNodeHeader(mTree); + NodeCodec::DecodingContext node_ctx(header); + + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + const uint8 *buffer_start = &mTree[0]; + node_ctx.WalkTree(buffer_start, triangle_ctx, ioVisitor); +} + +template +JPH_INLINE void MeshShape::WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const +{ + struct ChainedVisitor + { + JPH_INLINE ChainedVisitor(Visitor &ioVisitor, const SubShapeIDCreator &inSubShapeIDCreator2, uint inTriangleBlockIDBits) : + mVisitor(ioVisitor), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mTriangleBlockIDBits(inTriangleBlockIDBits) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mVisitor.ShouldAbort(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mVisitor.ShouldVisitNode(inStackTop); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + return mVisitor.VisitNodes(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, ioProperties, inStackTop); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Create ID for triangle block + SubShapeIDCreator block_sub_shape_id = mSubShapeIDCreator2.PushID(inTriangleBlockID, mTriangleBlockIDBits); + + // Decode vertices and flags + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + uint8 flags[MaxTrianglesPerLeaf]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices, flags); + + int triangle_idx = 0; + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, triangle_idx++) + { + // Determine active edges + uint8 active_edges = (flags[triangle_idx] >> FLAGS_ACTIVE_EGDE_SHIFT) & FLAGS_ACTIVE_EDGE_MASK; + + // Create ID for triangle + SubShapeIDCreator triangle_sub_shape_id = block_sub_shape_id.PushID(triangle_idx, NumTriangleBits); + + mVisitor.VisitTriangle(v[0], v[1], v[2], active_edges, triangle_sub_shape_id.GetID()); + + // Check if we should early out now + if (mVisitor.ShouldAbort()) + break; + } + } + + Visitor & mVisitor; + SubShapeIDCreator mSubShapeIDCreator2; + uint mTriangleBlockIDBits; + }; + + ChainedVisitor visitor(ioVisitor, inSubShapeIDCreator2, NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree))); + WalkTree(visitor); +} + +#ifdef JPH_DEBUG_RENDERER +void MeshShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Reset the batch if we switch coloring mode + if (mCachedTrianglesColoredPerGroup != sDrawTriangleGroups || mCachedUseMaterialColors != inUseMaterialColors) + { + mGeometry = nullptr; + mCachedTrianglesColoredPerGroup = sDrawTriangleGroups; + mCachedUseMaterialColors = inUseMaterialColors; + } + + if (mGeometry == nullptr) + { + struct Visitor + { + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices); + + if (mDrawTriangleGroups || !mUseMaterialColors || mMaterials.empty()) + { + // Single color for mesh + Color color = mDrawTriangleGroups? Color::sGetDistinctColor(mColorIdx++) : (mUseMaterialColors? PhysicsMaterial::sDefault->GetDebugColor() : Color::sWhite); + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3) + mTriangles.push_back({ v[0], v[1], v[2], color }); + } + else + { + // Per triangle color + uint8 flags[MaxTrianglesPerLeaf]; + TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags); + + const uint8 *f = flags; + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, f++) + mTriangles.push_back({ v[0], v[1], v[2], mMaterials[*f & FLAGS_MATERIAL_MASK]->GetDebugColor() }); + } + } + + Array & mTriangles; + const PhysicsMaterialList & mMaterials; + bool mUseMaterialColors; + bool mDrawTriangleGroups; + int mColorIdx = 0; + }; + + Array triangles; + Visitor visitor { triangles, mMaterials, mCachedUseMaterialColors, mCachedTrianglesColoredPerGroup }; + WalkTree(visitor); + mGeometry = new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), GetLocalBounds()); + } + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + inRenderer->DrawGeometry(inCenterOfMassTransform * Mat44::sScale(inScale), inColor, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + if (sDrawTriangleOutlines) + { + struct Visitor + { + JPH_INLINE Visitor(DebugRenderer *inRenderer, RMat44Arg inTransform) : + mRenderer(inRenderer), + mTransform(inTransform) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Decode vertices and flags + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + uint8 flags[MaxTrianglesPerLeaf]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices, flags); + + // Loop through triangles + const uint8 *f = flags; + for (Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, ++f) + { + // Loop through edges + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + RVec3 v1 = mTransform * v[edge_idx]; + RVec3 v2 = mTransform * v[(edge_idx + 1) % 3]; + + // Draw active edge as a green arrow, other edges as grey + if (*f & (1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT))) + mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f); + else + mRenderer->DrawLine(v1, v2, Color::sGrey); + } + } + } + + DebugRenderer * mRenderer; + RMat44 mTransform; + }; + + Visitor visitor { inRenderer, inCenterOfMassTransform.PreScaled(inScale) }; + WalkTree(visitor); + } +} +#endif // JPH_DEBUG_RENDERER + +bool MeshShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor + { + JPH_INLINE explicit Visitor(RayCastResult &ioHit) : + mHit(ioHit) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Test against triangles + uint32 triangle_idx; + float fraction = ioContext.TestRay(mRayOrigin, mRayDirection, inTriangles, inNumTriangles, mHit.mFraction, triangle_idx); + if (fraction < mHit.mFraction) + { + mHit.mFraction = fraction; + mHit.mSubShapeID2 = mSubShapeIDCreator.PushID(inTriangleBlockID, mTriangleBlockIDBits).PushID(triangle_idx, NumTriangleBits).GetID(); + mReturnValue = true; + } + } + + RayCastResult & mHit; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + uint mTriangleBlockIDBits; + SubShapeIDCreator mSubShapeIDCreator; + bool mReturnValue = false; + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(ioHit); + visitor.mRayOrigin = inRay.mOrigin; + visitor.mRayDirection = inRay.mDirection; + visitor.mRayInvDirection.Set(inRay.mDirection); + visitor.mTriangleBlockIDBits = NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)); + visitor.mSubShapeIDCreator = inSubShapeIDCreator; + WalkTree(visitor); + + return visitor.mReturnValue; +} + +void MeshShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor + { + JPH_INLINE explicit Visitor(CastRayCollector &ioCollector) : + mCollector(ioCollector) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + // Back facing check + if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0) + return; + + // Check the triangle + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mCollector.GetEarlyOutFraction()) + { + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeID2; + mCollector.AddHit(hit); + } + } + + CastRayCollector & mCollector; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + EBackFaceMode mBackFaceMode; + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(ioCollector); + visitor.mBackFaceMode = inRayCastSettings.mBackFaceModeTriangles; + visitor.mRayOrigin = inRay.mOrigin; + visitor.mRayDirection = inRay.mDirection; + visitor.mRayInvDirection.Set(inRay.mDirection); + WalkTreePerTriangle(inSubShapeIDCreator, visitor); +} + +void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideSoftBodyVerticesVsTriangles + { + using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return mDistanceStack[inStackTop] < mClosestDistanceSq; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Get distance to vertex + Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, [[maybe_unused]] SubShapeID inSubShapeID2) + { + ProcessTriangle(inV0, inV1, inV2); + } + + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(inCenterOfMassTransform, inScale); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + visitor.StartVertex(v); + WalkTreePerTriangle(SubShapeIDCreator(), visitor); + visitor.FinishVertex(v, inCollidingShapeIndex); + } +} + +void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastConvexVsTriangles + { + using CastConvexVsTriangles::CastConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + float mDistanceStack[NodeCodec::StackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh); + const MeshShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter(); + visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent(); + shape->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastSphereVsTriangles + { + using CastSphereVsTriangles::CastSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the radius of the sphere + AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + + RayInvDirection mInvDirection; + float mDistanceStack[NodeCodec::StackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh); + const MeshShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mInvDirection.Set(inShapeCast.mDirection); + shape->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +struct MeshShape::MSGetTrianglesContext +{ + JPH_INLINE MSGetTrianglesContext(const MeshShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mDecodeCtx(sGetNodeHeader(inShape->mTree)), + mShape(inShape), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mMeshScale(inScale), + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mShouldAbort; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mMeshScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + // When the buffer is full and we cannot process the triangles, abort the tree walk. The next time GetTrianglesNext is called we will continue here. + if (mNumTrianglesFound + inNumTriangles > mMaxTrianglesRequested) + { + mShouldAbort = true; + return; + } + + // Decode vertices + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices); + + // Store vertices as Float3 + if (mIsInsideOut) + { + // Scaled inside out, flip the triangles + for (const Vec3 *v = vertices, *v_end = v + 3 * inNumTriangles; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(mTriangleVertices++); + } + } + else + { + // Normal scale + for (const Vec3 *v = vertices, *v_end = v + 3 * inNumTriangles; v < v_end; ++v) + (mLocalToWorld * *v).StoreFloat3(mTriangleVertices++); + } + + if (mMaterials != nullptr) + { + if (mShape->mMaterials.empty()) + { + // No materials, output default + const PhysicsMaterial *default_material = PhysicsMaterial::sDefault; + for (int m = 0; m < inNumTriangles; ++m) + *mMaterials++ = default_material; + } + else + { + // Decode triangle flags + uint8 flags[MaxTrianglesPerLeaf]; + TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags); + + // Store materials + for (const uint8 *f = flags, *f_end = f + inNumTriangles; f < f_end; ++f) + *mMaterials++ = mShape->mMaterials[*f & FLAGS_MATERIAL_MASK].GetPtr(); + } + } + + // Accumulate triangles found + mNumTrianglesFound += inNumTriangles; + } + + NodeCodec::DecodingContext mDecodeCtx; + const MeshShape * mShape; + OrientedBox mLocalBox; + Vec3 mMeshScale; + Mat44 mLocalToWorld; + int mMaxTrianglesRequested; + Float3 * mTriangleVertices; + int mNumTrianglesFound; + const PhysicsMaterial ** mMaterials; + bool mShouldAbort; + bool mIsInsideOut; +}; + +void MeshShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(MSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(MSGetTrianglesContext))); + + new (&ioContext) MSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale); +} + +int MeshShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= MaxTrianglesPerLeaf, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + // Check if we're done + MSGetTrianglesContext &context = (MSGetTrianglesContext &)ioContext; + if (context.mDecodeCtx.IsDoneWalking()) + return 0; + + // Store parameters on context + context.mMaxTrianglesRequested = inMaxTrianglesRequested; + context.mTriangleVertices = outTriangleVertices; + context.mMaterials = outMaterials; + context.mShouldAbort = false; // Reset the abort flag + context.mNumTrianglesFound = 0; + + // Continue (or start) walking the tree + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + const uint8 *buffer_start = &mTree[0]; + context.mDecodeCtx.WalkTree(buffer_start, triangle_ctx, context); + return context.mNumTrianglesFound; +} + +void MeshShape::sCollideConvexVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::Mesh); + const ConvexShape *shape1 = static_cast(inShape1); + const MeshShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideConvexVsTriangles + { + using CollideConvexVsTriangles::CollideConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + shape2->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::sCollideSphereVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + JPH_ASSERT(inShape2->GetType() == EShapeType::Mesh); + const SphereShape *shape1 = static_cast(inShape1); + const MeshShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideSphereVsTriangles + { + using CollideSphereVsTriangles::CollideSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + shape2->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(static_cast(mTree)); // Make sure we use the Array<> overload +} + +void MeshShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(static_cast(mTree)); // Make sure we use the Array<> overload +} + +void MeshShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials = mMaterials; +} + +void MeshShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + mMaterials.assign(inMaterials, inMaterials + inNumMaterials); +} + +Shape::Stats MeshShape::GetStats() const +{ + // Walk the tree to count the triangles + struct Visitor + { + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Visit all valid children + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles([[maybe_unused]] const TriangleCodec::DecodingContext &ioContext, [[maybe_unused]] const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + mNumTriangles += inNumTriangles; + } + + uint mNumTriangles = 0; + }; + + Visitor visitor; + WalkTree(visitor); + + return Stats(sizeof(*this) + mMaterials.size() * sizeof(Ref) + mTree.size() * sizeof(uint8), visitor.mNumTriangles); +} + +uint32 MeshShape::GetTriangleUserData(const SubShapeID &inSubShapeID) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Decode triangle + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + return triangle_ctx.GetUserData(block_start, triangle_idx); +} + +void MeshShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Mesh); + f.mConstruct = []() -> Shape * { return new MeshShape; }; + f.mColor = Color::sRed; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Mesh, sCollideConvexVsMesh); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Mesh, sCastConvexVsMesh); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::Mesh, s, CollisionDispatch::sReversedCastShape); + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Mesh, s, CollisionDispatch::sReversedCollideShape); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Mesh, sCollideSphereVsMesh); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Mesh, sCastSphereVsMesh); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h new file mode 100644 index 000000000000..a211b7a0ad3c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h @@ -0,0 +1,217 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class ConvexShape; +class CollideShapeSettings; + +/// Class that constructs a MeshShape +class JPH_EXPORT MeshShapeSettings final : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MeshShapeSettings) + +public: + /// Default constructor for deserialization + MeshShapeSettings() = default; + + /// Create a mesh shape. + MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList()); + MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList()); + + /// Sanitize the mesh data. Remove duplicate and degenerate triangles. This is called automatically when constructing the MeshShapeSettings with a list of (indexed-) triangles. + void Sanitize(); + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Vertices belonging to mIndexedTriangles + VertexList mTriangleVertices; + + /// Original list of indexed triangles (triangles will be reordered internally in the mesh shape). + /// Triangles must be provided in counter clockwise order. + /// Degenerate triangles will automatically be removed during mesh creation but no other mesh simplifications are performed, use an external library if this is desired. + /// For simulation, the triangles are considered to be single sided. + /// For ray casts you can choose to make triangles double sided by setting RayCastSettings::mBackFaceMode to EBackFaceMode::CollideWithBackFaces. + /// For collide shape tests you can use CollideShapeSettings::mBackFaceMode and for shape casts you can use ShapeCastSettings::mBackFaceModeTriangles. + IndexedTriangleList mIndexedTriangles; + + /// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex + PhysicsMaterialList mMaterials; + + /// Maximum number of triangles in each leaf of the axis aligned box tree. This is a balance between memory and performance. Can be in the range [1, MeshShape::MaxTrianglesPerLeaf]. + /// Sensible values are between 4 (for better performance) and 8 (for less memory usage). + uint mMaxTrianglesPerLeaf = 8; + + /// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + /// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly). + /// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees). + float mActiveEdgeCosThresholdAngle = 0.996195f; // cos(5 degrees) + + /// When true, we store the user data coming from Triangle::mUserData or IndexedTriangle::mUserData in the mesh shape. + /// This can be used to store additional data like the original index of the triangle in the mesh. + /// Can be retrieved using MeshShape::GetTriangleUserData. + /// Turning this on increases the memory used by the MeshShape by roughly 25%. + bool mPerTriangleUserData = false; +}; + +/// A mesh shape, consisting of triangles. Mesh shapes are mostly used for static geometry. +/// They can be used by dynamic or kinematic objects but only if they don't collide with other mesh or heightfield shapes as those collisions are currently not supported. +/// Note that if you make a mesh shape a dynamic or kinematic object, you need to provide a mass yourself as mesh shapes don't need to form a closed hull so don't have a well defined volume from which the mass can be calculated. +class JPH_EXPORT MeshShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MeshShape() : Shape(EShapeType::Mesh, EShapeSubType::Mesh) { } + MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult); + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return true; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return 0.0f; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + /// Get the list of all materials + const PhysicsMaterialList & GetMaterialList() const { return mMaterials; } + + /// Determine which material index a particular sub shape uses (note that if there are no materials this function will return 0 so check the array size) + /// Note: This could for example be used to create a decorator shape around a mesh shape that overrides the GetMaterial call to replace a material with another material. + uint GetMaterialIndex(const SubShapeID &inSubShapeID) const; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + /// See: Shape::CollidePoint + /// Note that for CollidePoint to work for a mesh shape, the mesh needs to be closed (a manifold) or multiple non-intersecting manifolds. Triangles may be facing the interior of the manifold. + /// Insideness is tested by counting the amount of triangles encountered when casting an infinite ray from inPoint. If the number of hits is odd we're inside, if it's even we're outside. + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + + // When MeshShape::mPerTriangleUserData is true, this function can be used to retrieve the user data that was stored in the mesh shape. + uint32 GetTriangleUserData(const SubShapeID &inSubShapeID) const; + +#ifdef JPH_DEBUG_RENDERER + // Settings + static bool sDrawTriangleGroups; + static bool sDrawTriangleOutlines; +#endif // JPH_DEBUG_RENDERER + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + struct MSGetTrianglesContext; ///< Context class for GetTrianglesStart/Next + + static constexpr int NumTriangleBits = 3; ///< How many bits to reserve to encode the triangle index + static constexpr int MaxTrianglesPerLeaf = 1 << NumTriangleBits; ///< Number of triangles that are stored max per leaf aabb node + + /// Find and flag active edges + static void sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices); + + /// Visit the entire tree using a visitor pattern + template + void WalkTree(Visitor &ioVisitor) const; + + /// Same as above but with a callback per triangle instead of per block of triangles + template + void WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const; + + /// Decode a sub shape ID + inline void DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideSphereVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex + PhysicsMaterialList mMaterials; + + ByteBuffer mTree; ///< Resulting packed data structure + + /// 8 bit flags stored per triangle + enum ETriangleFlags + { + /// Material index + FLAGS_MATERIAL_BITS = 5, + FLAGS_MATERIAL_MASK = (1 << FLAGS_MATERIAL_BITS) - 1, + + /// Active edge bits + FLAGS_ACTIVE_EGDE_SHIFT = FLAGS_MATERIAL_BITS, + FLAGS_ACTIVE_EDGE_BITS = 3, + FLAGS_ACTIVE_EDGE_MASK = (1 << FLAGS_ACTIVE_EDGE_BITS) - 1 + }; + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; ///< Debug rendering data + mutable bool mCachedTrianglesColoredPerGroup = false; ///< This is used to regenerate the triangle batch if the drawing settings change + mutable bool mCachedUseMaterialColors = false; ///< This is used to regenerate the triangle batch if the drawing settings change +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp new file mode 100644 index 000000000000..fdedc9a31715 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp @@ -0,0 +1,589 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MutableCompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(MutableCompoundShapeSettings, CompoundShapeSettings) +} + +ShapeSettings::ShapeResult MutableCompoundShapeSettings::Create() const +{ + // Build a mutable compound shape + if (mCachedResult.IsEmpty()) + Ref shape = new MutableCompoundShape(*this, mCachedResult); + + return mCachedResult; +} + +MutableCompoundShape::MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult) : + CompoundShape(EShapeSubType::MutableCompound, inSettings, outResult) +{ + mSubShapes.reserve(inSettings.mSubShapes.size()); + for (const CompoundShapeSettings::SubShapeSettings &shape : inSettings.mSubShapes) + { + // Start constructing the runtime sub shape + SubShape out_shape; + if (!out_shape.FromSettings(shape, outResult)) + return; + + mSubShapes.push_back(out_shape); + } + + AdjustCenterOfMass(); + + CalculateSubShapeBounds(0, (uint)mSubShapes.size()); + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +Ref MutableCompoundShape::Clone() const +{ + Ref clone = new MutableCompoundShape(); + clone->SetUserData(GetUserData()); + + clone->mCenterOfMass = mCenterOfMass; + clone->mLocalBounds = mLocalBounds; + clone->mSubShapes = mSubShapes; + clone->mInnerRadius = mInnerRadius; + clone->mSubShapeBounds = mSubShapeBounds; + + return clone; +} + +void MutableCompoundShape::AdjustCenterOfMass() +{ + // First calculate the delta of the center of mass + float mass = 0.0f; + Vec3 center_of_mass = Vec3::sZero(); + for (const CompoundShape::SubShape &sub_shape : mSubShapes) + { + MassProperties child = sub_shape.mShape->GetMassProperties(); + mass += child.mMass; + center_of_mass += sub_shape.GetPositionCOM() * child.mMass; + } + if (mass > 0.0f) + center_of_mass /= mass; + + // Now adjust all shapes to recenter around center of mass + for (CompoundShape::SubShape &sub_shape : mSubShapes) + sub_shape.SetPositionCOM(sub_shape.GetPositionCOM() - center_of_mass); + + // Update bounding boxes + for (Bounds &bounds : mSubShapeBounds) + { + Vec4 xxxx = center_of_mass.SplatX(); + Vec4 yyyy = center_of_mass.SplatY(); + Vec4 zzzz = center_of_mass.SplatZ(); + bounds.mMinX -= xxxx; + bounds.mMinY -= yyyy; + bounds.mMinZ -= zzzz; + bounds.mMaxX -= xxxx; + bounds.mMaxY -= yyyy; + bounds.mMaxZ -= zzzz; + } + mLocalBounds.Translate(-center_of_mass); + + // And adjust the center of mass for this shape in the opposite direction + mCenterOfMass += center_of_mass; +} + +void MutableCompoundShape::CalculateLocalBounds() +{ + uint num_blocks = GetNumBlocks(); + if (num_blocks > 0) + { + // Initialize min/max for first block + const Bounds *bounds = mSubShapeBounds.data(); + Vec4 min_x = bounds->mMinX; + Vec4 min_y = bounds->mMinY; + Vec4 min_z = bounds->mMinZ; + Vec4 max_x = bounds->mMaxX; + Vec4 max_y = bounds->mMaxY; + Vec4 max_z = bounds->mMaxZ; + + // Accumulate other blocks + const Bounds *bounds_end = bounds + num_blocks; + for (++bounds; bounds < bounds_end; ++bounds) + { + min_x = Vec4::sMin(min_x, bounds->mMinX); + min_y = Vec4::sMin(min_y, bounds->mMinY); + min_z = Vec4::sMin(min_z, bounds->mMinZ); + max_x = Vec4::sMax(max_x, bounds->mMaxX); + max_y = Vec4::sMax(max_y, bounds->mMaxY); + max_z = Vec4::sMax(max_z, bounds->mMaxZ); + } + + // Calculate resulting bounding box + mLocalBounds.mMin.SetX(min_x.ReduceMin()); + mLocalBounds.mMin.SetY(min_y.ReduceMin()); + mLocalBounds.mMin.SetZ(min_z.ReduceMin()); + mLocalBounds.mMax.SetX(max_x.ReduceMax()); + mLocalBounds.mMax.SetY(max_y.ReduceMax()); + mLocalBounds.mMax.SetZ(max_z.ReduceMax()); + } + else + { + // There are no subshapes, set the bounding box to invalid + mLocalBounds.SetEmpty(); + } + + // Cache the inner radius as it can take a while to recursively iterate over all sub shapes + CalculateInnerRadius(); +} + +void MutableCompoundShape::EnsureSubShapeBoundsCapacity() +{ + // Check if we have enough space + uint new_capacity = ((uint)mSubShapes.size() + 3) >> 2; + if (mSubShapeBounds.size() < new_capacity) + mSubShapeBounds.resize(new_capacity); +} + +void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumber) +{ + // Ensure that we have allocated the required space for mSubShapeBounds + EnsureSubShapeBoundsCapacity(); + + // Loop over blocks of 4 sub shapes + for (uint sub_shape_idx_start = inStartIdx & ~uint(3), sub_shape_idx_end = inStartIdx + inNumber; sub_shape_idx_start < sub_shape_idx_end; sub_shape_idx_start += 4) + { + Mat44 bounds_min; + Mat44 bounds_max; + + AABox sub_shape_bounds; + for (uint col = 0; col < 4; ++col) + { + uint sub_shape_idx = sub_shape_idx_start + col; + if (sub_shape_idx < mSubShapes.size()) // else reuse sub_shape_bounds from previous iteration + { + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + + // Transform the shape's bounds into our local space + Mat44 transform = Mat44::sRotationTranslation(sub_shape.GetRotation(), sub_shape.GetPositionCOM()); + + // Get the bounding box + sub_shape_bounds = sub_shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f)); + } + + // Put the bounds as columns in a matrix + bounds_min.SetColumn3(col, sub_shape_bounds.mMin); + bounds_max.SetColumn3(col, sub_shape_bounds.mMax); + } + + // Transpose to go to structure of arrays format + Mat44 bounds_min_t = bounds_min.Transposed(); + Mat44 bounds_max_t = bounds_max.Transposed(); + + // Store in our bounds array + Bounds &bounds = mSubShapeBounds[sub_shape_idx_start >> 2]; + bounds.mMinX = bounds_min_t.GetColumn4(0); + bounds.mMinY = bounds_min_t.GetColumn4(1); + bounds.mMinZ = bounds_min_t.GetColumn4(2); + bounds.mMaxX = bounds_max_t.GetColumn4(0); + bounds.mMaxY = bounds_max_t.GetColumn4(1); + bounds.mMaxZ = bounds_max_t.GetColumn4(2); + } + + CalculateLocalBounds(); +} + +uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData) +{ + SubShape sub_shape; + sub_shape.mShape = inShape; + sub_shape.mUserData = inUserData; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + mSubShapes.push_back(sub_shape); + uint shape_idx = (uint)mSubShapes.size() - 1; + + CalculateSubShapeBounds(shape_idx, 1); + + return shape_idx; +} + +void MutableCompoundShape::RemoveShape(uint inIndex) +{ + mSubShapes.erase(mSubShapes.begin() + inIndex); + + uint num_bounds = (uint)mSubShapes.size() - inIndex; + if (num_bounds > 0) + CalculateSubShapeBounds(inIndex, num_bounds); + else + CalculateLocalBounds(); +} + +void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation) +{ + SubShape &sub_shape = mSubShapes[inIndex]; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + + CalculateSubShapeBounds(inIndex, 1); +} + +void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) +{ + SubShape &sub_shape = mSubShapes[inIndex]; + sub_shape.mShape = inShape; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + + CalculateSubShapeBounds(inIndex, 1); +} + +void MutableCompoundShape::ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride, uint inRotationStride) +{ + JPH_ASSERT(inStartIndex + inNumber <= mSubShapes.size()); + + const Vec3 *pos = inPositions; + const Quat *rot = inRotations; + for (SubShape *dest = &mSubShapes[inStartIndex], *dest_end = dest + inNumber; dest < dest_end; ++dest) + { + // Update transform + dest->SetTransform(*pos, *rot, mCenterOfMass); + + // Advance pointer in position / rotation buffer + pos = reinterpret_cast(reinterpret_cast(pos) + inPositionStride); + rot = reinterpret_cast(reinterpret_cast(rot) + inRotationStride); + } + + CalculateSubShapeBounds(inStartIndex, inNumber); +} + +template +inline void MutableCompoundShape::WalkSubShapes(Visitor &ioVisitor) const +{ + // Loop over all blocks of 4 bounding boxes + for (uint block = 0, num_blocks = GetNumBlocks(); block < num_blocks; ++block) + { + // Test the bounding boxes + const Bounds &bounds = mSubShapeBounds[block]; + typename Visitor::Result result = ioVisitor.TestBlock(bounds.mMinX, bounds.mMinY, bounds.mMinZ, bounds.mMaxX, bounds.mMaxY, bounds.mMaxZ); + + // Check if any of the bounding boxes collided + if (ioVisitor.ShouldVisitBlock(result)) + { + // Go through the individual boxes + uint sub_shape_start_idx = block << 2; + for (uint col = 0, max_col = min(4, (uint)mSubShapes.size() - sub_shape_start_idx); col < max_col; ++col) // Don't read beyond the end of the subshapes array + if (ioVisitor.ShouldVisitSubShape(result, col)) // Because the early out fraction can change, we need to retest every shape + { + // Test sub shape + uint sub_shape_idx = sub_shape_start_idx + col; + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + ioVisitor.VisitShape(sub_shape, sub_shape_idx); + + // If no better collision is available abort + if (ioVisitor.ShouldAbort()) + break; + } + } + } +} + +bool MutableCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitor + { + using CastRayVisitor::CastRayVisitor; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mHit.mFraction)); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mHit.mFraction; + } + }; + + Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit); + WalkSubShapes(visitor); + return visitor.mReturnValue; +} + +void MutableCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CastRayVisitorCollector + { + using CastRayVisitorCollector::CastRayVisitorCollector; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetEarlyOutFraction())); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mCollector.GetEarlyOutFraction(); + } + }; + + Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +void MutableCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollidePointVisitor + { + using CollidePointVisitor::CollidePointVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +void MutableCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastShapeVisitor + { + using CastShapeVisitor::CastShapeVisitor; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetPositiveEarlyOutFraction())); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mCollector.GetPositiveEarlyOutFraction(); + } + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + shape->WalkSubShapes(visitor); +} + +void MutableCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollectTransformedShapesVisitor + { + using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +int MutableCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorMC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkSubShapes(visitor); + return visitor.GetNumResults(); +} + +int MutableCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorMC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkSubShapes(visitor); + return visitor.GetNumResults(); +} + +void MutableCompoundShape::sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape1 = static_cast(inShape1); + + struct Visitor : public CollideCompoundVsShapeVisitor + { + using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape1->WalkSubShapes(visitor); +} + +void MutableCompoundShape::sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideShapeVsCompoundVisitor + { + using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape2->WalkSubShapes(visitor); +} + +void MutableCompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + CompoundShape::SaveBinaryState(inStream); + + // Write bounds + uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds); + inStream.WriteBytes(mSubShapeBounds.data(), bounds_size); +} + +void MutableCompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + CompoundShape::RestoreBinaryState(inStream); + + // Ensure that we have allocated the required space for mSubShapeBounds + EnsureSubShapeBoundsCapacity(); + + // Read bounds + uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds); + inStream.ReadBytes(mSubShapeBounds.data(), bounds_size); +} + +void MutableCompoundShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::MutableCompound); + f.mConstruct = []() -> Shape * { return new MutableCompoundShape; }; + f.mColor = Color::sDarkOrange; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::MutableCompound, s, sCollideCompoundVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::MutableCompound, sCollideShapeVsCompound); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::MutableCompound, sCastShapeVsCompound); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h new file mode 100644 index 000000000000..c3f6ff296496 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a MutableCompoundShape. +class JPH_EXPORT MutableCompoundShapeSettings final : public CompoundShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MutableCompoundShapeSettings) + +public: + // See: ShapeSettings + virtual ShapeResult Create() const override; +}; + +/// A compound shape, sub shapes can be rotated and translated. +/// This shape is optimized for adding / removing and changing the rotation / translation of sub shapes but is less efficient in querying. +/// Shifts all child objects so that they're centered around the center of mass (which needs to be kept up to date by calling AdjustCenterOfMass). +/// +/// Note: If you're using MutableCompoundShape and are querying data while modifying the shape you'll have a race condition. +/// In this case it is best to create a new MutableCompoundShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape. +/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes. +class JPH_EXPORT MutableCompoundShape final : public CompoundShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MutableCompoundShape() : CompoundShape(EShapeSubType::MutableCompound) { } + MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult); + + /// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information. + Ref Clone() const; + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mSubShapeBounds.size() * sizeof(Bounds), 0); } + + ///@{ + /// @name Mutating shapes. Note that this is not thread safe, so you need to ensure that any bodies that use this shape are locked at the time of modification using BodyLockWrite. After modification you need to call BodyInterface::NotifyShapeChanged to update the broadphase and collision caches. + + /// Adding a new shape. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @return The index of the newly added shape + uint AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0); + + /// Remove a shape by index. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void RemoveShape(uint inIndex); + + /// Modify the position / orientation of a shape. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation); + + /// Modify the position / orientation and shape at the same time. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape); + + /// @brief Batch set positions / orientations, this avoids duplicate work due to bounding box calculation. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inStartIndex Index of first shape to update + /// @param inNumber Number of shapes to update + /// @param inPositions A list of positions with arbitrary stride + /// @param inRotations A list of orientations with arbitrary stride + /// @param inPositionStride The position stride (the number of bytes between the first and second element) + /// @param inRotationStride The orientation stride (the number of bytes between the first and second element) + void ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride = sizeof(Vec3), uint inRotationStride = sizeof(Quat)); + + /// Recalculate the center of mass and shift all objects so they're centered around it + /// (this needs to be done of dynamic bodies and if the center of mass changes significantly due to adding / removing / repositioning sub shapes or else the simulation will look unnatural) + /// Note that after adjusting the center of mass of an object you need to call BodyInterface::NotifyShapeChanged and Constraint::NotifyShapeChanged on the relevant bodies / constraints. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void AdjustCenterOfMass(); + + ///@} + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Visitor for GetIntersectingSubShapes + template + struct GetIntersectingSubShapesVisitorMC : public GetIntersectingSubShapesVisitor + { + using GetIntersectingSubShapesVisitor::GetIntersectingSubShapesVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return GetIntersectingSubShapesVisitor::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + /// Get the number of blocks of 4 bounding boxes + inline uint GetNumBlocks() const { return ((uint)mSubShapes.size() + 3) >> 2; } + + /// Ensure that the mSubShapeBounds has enough space to store bounding boxes equivalent to the number of shapes in mSubShapes + void EnsureSubShapeBoundsCapacity(); + + /// Update mSubShapeBounds + /// @param inStartIdx First sub shape to update + /// @param inNumber Number of shapes to update + void CalculateSubShapeBounds(uint inStartIdx, uint inNumber); + + /// Calculate mLocalBounds from mSubShapeBounds + void CalculateLocalBounds(); + + template + JPH_INLINE void WalkSubShapes(Visitor &ioVisitor) const; ///< Walk the sub shapes and call Visitor::VisitShape for each sub shape encountered + + // Helper functions called by CollisionDispatch + static void sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + struct Bounds + { + Vec4 mMinX; + Vec4 mMinY; + Vec4 mMinZ; + Vec4 mMaxX; + Vec4 mMaxY; + Vec4 mMaxZ; + }; + + Array mSubShapeBounds; ///< Bounding boxes of all sub shapes in SOA format (in blocks of 4 boxes), MinX 0..3, MinY 0..3, MinZ 0..3, MaxX 0..3, MaxY 0..3, MaxZ 0..3, MinX 4..7, MinY 4..7, ... +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp new file mode 100644 index 000000000000..8996b46c440a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp @@ -0,0 +1,217 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(OffsetCenterOfMassShapeSettings) +{ + JPH_ADD_BASE_CLASS(OffsetCenterOfMassShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(OffsetCenterOfMassShapeSettings, mOffset) +} + +ShapeSettings::ShapeResult OffsetCenterOfMassShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new OffsetCenterOfMassShape(*this, mCachedResult); + return mCachedResult; +} + +OffsetCenterOfMassShape::OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::OffsetCenterOfMass, inSettings, outResult), + mOffset(inSettings.mOffset) +{ + if (outResult.HasError()) + return; + + outResult.Set(this); +} + +AABox OffsetCenterOfMassShape::GetLocalBounds() const +{ + AABox bounds = mInnerShape->GetLocalBounds(); + bounds.mMin -= mOffset; + bounds.mMax -= mOffset; + return bounds; +} + +AABox OffsetCenterOfMassShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale); +} + +TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM - inRotation * (inScale * mOffset)), inRotation, mInnerShape, BodyID()); + ts.SetShapeScale(inScale); + return ts; +} + +Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform surface position to local space and pass call on + return mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition + mOffset); +} + +void OffsetCenterOfMassShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), outVertices); +} + +void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe); +} + +void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inDrawSupportDirection); +} + +void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale); +} +#endif // JPH_DEBUG_RENDERER + +bool OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Transform the ray to local space + RayCast ray = inRay; + ray.mOrigin += mOffset; + + return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit); +} + +void OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the ray to local space + RayCast ray = inRay; + ray.mOrigin += mOffset; + + return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Pass the point on to the inner shape in local space + mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inVertices, inNumVertices, inCollidingShapeIndex); +} + +void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM - inRotation * (inScale * mOffset), inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform.PreTranslated(-mOffset), ioCollector); +} + +void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape1 = static_cast(inShape1); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, inScale1, inScale2, inCenterOfMassTransform1.PreTranslated(-inScale1 * shape1->mOffset), inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape2 = static_cast(inShape2); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2.PreTranslated(-inScale2 * shape2->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // Fetch offset center of mass shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape1 = static_cast(inShapeCast.mShape); + + // Transform the shape cast and update the shape + ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart.PreTranslated(-inShapeCast.mScale * shape1->mOffset), inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape = static_cast(inShape); + + // Transform the shape cast + ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(inScale * shape->mOffset)); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2.PreTranslated(-inScale * shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void OffsetCenterOfMassShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mOffset); +} + +void OffsetCenterOfMassShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mOffset); +} + +void OffsetCenterOfMassShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::OffsetCenterOfMass); + f.mConstruct = []() -> Shape * { return new OffsetCenterOfMassShape; }; + f.mColor = Color::sCyan; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::OffsetCenterOfMass, s, sCollideOffsetCenterOfMassVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::OffsetCenterOfMass, sCollideShapeVsOffsetCenterOfMass); + CollisionDispatch::sRegisterCastShape(EShapeSubType::OffsetCenterOfMass, s, sCastOffsetCenterOfMassVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::OffsetCenterOfMass, sCastShapeVsOffsetCenterOfMass); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h new file mode 100644 index 000000000000..7ba6a5a7a01d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h @@ -0,0 +1,140 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs an OffsetCenterOfMassShape +class JPH_EXPORT OffsetCenterOfMassShapeSettings final : public DecoratedShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, OffsetCenterOfMassShapeSettings) + +public: + /// Constructor + OffsetCenterOfMassShapeSettings() = default; + + /// Construct with shape settings, can be serialized. + OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mOffset(inOffset) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const Shape *inShape): DecoratedShapeSettings(inShape), mOffset(inOffset) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mOffset; ///< Offset to be applied to the center of mass of the child shape +}; + +/// This shape will shift the center of mass of a child shape, it can e.g. be used to lower the center of mass of an unstable object like a boat to make it stable +class JPH_EXPORT OffsetCenterOfMassShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + OffsetCenterOfMassShape() : DecoratedShape(EShapeSubType::OffsetCenterOfMass) { } + OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult); + OffsetCenterOfMassShape(const Shape *inShape, Vec3Arg inOffset) : DecoratedShape(EShapeSubType::OffsetCenterOfMass, inShape), mOffset(inOffset) { } + + /// Access the offset that is applied to the center of mass + Vec3 GetOffset() const { return mOffset; } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mInnerShape->GetCenterOfMass() + mOffset; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override + { + MassProperties mp = mInnerShape->GetMassProperties(); + mp.Translate(mOffset); + return mp; + } + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return mInnerShape->GetVolume(); } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + Vec3 mOffset; ///< Offset of the center of mass +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp new file mode 100644 index 000000000000..c1401fd7fcc8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp @@ -0,0 +1,541 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PlaneShapeSettings) +{ + JPH_ADD_BASE_CLASS(PlaneShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mPlane) + JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mMaterial) + JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mHalfExtent) +} + +ShapeSettings::ShapeResult PlaneShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new PlaneShape(*this, mCachedResult); + return mCachedResult; +} + +inline static void sPlaneGetOrthogonalBasis(Vec3Arg inNormal, Vec3 &outPerp1, Vec3 &outPerp2) +{ + outPerp1 = inNormal.Cross(Vec3::sAxisY()).NormalizedOr(Vec3::sAxisX()); + outPerp2 = outPerp1.Cross(inNormal).Normalized(); + outPerp1 = inNormal.Cross(outPerp2); +} + +void PlaneShape::GetVertices(Vec3 *outVertices) const +{ + // Create orthogonal basis + Vec3 normal = mPlane.GetNormal(); + Vec3 perp1, perp2; + sPlaneGetOrthogonalBasis(normal, perp1, perp2); + + // Scale basis + perp1 *= mHalfExtent; + perp2 *= mHalfExtent; + + // Calculate corners + Vec3 point = -normal * mPlane.GetConstant(); + outVertices[0] = point + perp1 + perp2; + outVertices[1] = point + perp1 - perp2; + outVertices[2] = point - perp1 - perp2; + outVertices[3] = point - perp1 + perp2; +} + +void PlaneShape::CalculateLocalBounds() +{ + // Get the vertices of the plane + Vec3 vertices[4]; + GetVertices(vertices); + + // Encapsulate the vertices and a point mHalfExtent behind the plane + mLocalBounds = AABox(); + Vec3 normal = mPlane.GetNormal(); + for (const Vec3 &v : vertices) + { + mLocalBounds.Encapsulate(v); + mLocalBounds.Encapsulate(v - mHalfExtent * normal); + } +} + +PlaneShape::PlaneShape(const PlaneShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::Plane, EShapeSubType::Plane, inSettings, outResult), + mPlane(inSettings.mPlane), + mMaterial(inSettings.mMaterial), + mHalfExtent(inSettings.mHalfExtent) +{ + if (!mPlane.GetNormal().IsNormalized()) + { + outResult.SetError("Plane normal needs to be normalized!"); + return; + } + + CalculateLocalBounds(); + + outResult.Set(this); +} + +MassProperties PlaneShape::GetMassProperties() const +{ + // Object should always be static, return default mass properties + return MassProperties(); +} + +void PlaneShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Get the vertices of the plane + Vec3 vertices[4]; + GetVertices(vertices); + + // Reverse if scale is inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + std::swap(vertices[0], vertices[3]); + std::swap(vertices[1], vertices[2]); + } + + // Transform them to world space + outVertices.clear(); + Mat44 com = inCenterOfMassTransform.PreScaled(inScale); + for (const Vec3 &v : vertices) + outVertices.push_back(com * v); +} + +#ifdef JPH_DEBUG_RENDERER +void PlaneShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Get the vertices of the plane + Vec3 local_vertices[4]; + GetVertices(local_vertices); + + // Reverse if scale is inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + std::swap(local_vertices[0], local_vertices[3]); + std::swap(local_vertices[1], local_vertices[2]); + } + + // Transform them to world space + RMat44 com = inCenterOfMassTransform.PreScaled(inScale); + RVec3 vertices[4]; + for (uint i = 0; i < 4; ++i) + vertices[i] = com * local_vertices[i]; + + // Determine the color + Color color = inUseMaterialColors? GetMaterial(SubShapeID())->GetDebugColor() : inColor; + + // Draw the plane + if (inDrawWireframe) + { + inRenderer->DrawWireTriangle(vertices[0], vertices[1], vertices[2], color); + inRenderer->DrawWireTriangle(vertices[0], vertices[2], vertices[3], color); + } + else + { + inRenderer->DrawTriangle(vertices[0], vertices[1], vertices[2], color, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(vertices[0], vertices[2], vertices[3], color, DebugRenderer::ECastShadow::On); + } +} +#endif // JPH_DEBUG_RENDERER + +bool PlaneShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + // Test starting inside of negative half space + float distance = mPlane.SignedDistance(inRay.mOrigin); + if (distance <= 0.0f) + { + ioHit.mFraction = 0.0f; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + + // Test ray parallel to plane + float dot = inRay.mDirection.Dot(mPlane.GetNormal()); + if (dot == 0.0f) + return false; + + // Calculate hit fraction + float fraction = -distance / dot; + if (fraction >= 0.0f && fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + + return false; +} + +void PlaneShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Inside solid half space? + float distance = mPlane.SignedDistance(inRay.mOrigin); + if (inRayCastSettings.mTreatConvexAsSolid + && distance <= 0.0f // Inside plane + && ioCollector.GetEarlyOutFraction() > 0.0f) // Willing to accept hits at fraction 0 + { + // Hit at fraction 0 + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = 0.0f; + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + ioCollector.AddHit(hit); + } + + float dot = inRay.mDirection.Dot(mPlane.GetNormal()); + if (dot != 0.0f // Parallel ray will not hit plane + && (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces || dot < 0.0f)) // Back face culling + { + // Calculate hit with plane + float fraction = -distance / dot; + if (fraction >= 0.0f && fraction < ioCollector.GetEarlyOutFraction()) + { + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + ioCollector.AddHit(hit); + } + } +} + +void PlaneShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if the point is inside the plane + if (mPlane.SignedDistance(inPoint) < 0.0f) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void PlaneShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_PROFILE_FUNCTION(); + + // Convert plane to world space + Plane plane = mPlane.Scaled(inScale).GetTransformed(inCenterOfMassTransform); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + // Calculate penetration + float penetration = -plane.SignedDistance(v.GetPosition()); + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane, inCollidingShapeIndex); + } +} + +// This is a version of GetSupportingFace that returns a face that is large enough to cover the shape we're colliding with but not as large as the regular GetSupportedFace to avoid numerical precision issues +inline static void sGetSupportingFace(const ConvexShape *inShape, Vec3Arg inShapeCOM, const Plane &inPlane, Mat44Arg inPlaneToWorld, ConvexShape::SupportingFace &outPlaneFace) +{ + // Project COM of shape onto plane + Plane world_plane = inPlane.GetTransformed(inPlaneToWorld); + Vec3 center = world_plane.ProjectPointOnPlane(inShapeCOM); + + // Create orthogonal basis for the plane + Vec3 normal = world_plane.GetNormal(); + Vec3 perp1, perp2; + sPlaneGetOrthogonalBasis(normal, perp1, perp2); + + // Base the size of the face on the bounding box of the shape, ensuring that it is large enough to cover the entire shape + float size = inShape->GetLocalBounds().GetSize().Length(); + perp1 *= size; + perp2 *= size; + + // Emit the vertices + outPlaneFace.resize(4); + outPlaneFace[0] = center + perp1 + perp2; + outPlaneFace[1] = center + perp1 - perp2; + outPlaneFace[2] = center - perp1 - perp2; + outPlaneFace[3] = center - perp1 + perp2; +} + +void PlaneShape::sCastConvexVsPlane(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape->GetType() == EShapeType::Plane); + const ConvexShape *convex_shape = static_cast(inShapeCast.mShape); + const PlaneShape *plane_shape = static_cast(inShape); + + // Shape cast is provided relative to COM of inShape, so all we need to do is transform our plane with inScale + Plane plane = plane_shape->mPlane.Scaled(inScale); + Vec3 normal = plane.GetNormal(); + + // Get support function + ConvexShape::SupportBuffer shape1_support_buffer; + const ConvexShape::Support *shape1_support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::Default, shape1_support_buffer, inShapeCast.mScale); + + // Get the support point of the convex shape in the opposite direction of the plane normal in our local space + Vec3 normal_in_convex_shape_space = inShapeCast.mCenterOfMassStart.Multiply3x3Transposed(normal); + Vec3 support_point = inShapeCast.mCenterOfMassStart * shape1_support->GetSupport(-normal_in_convex_shape_space); + float signed_distance = plane.SignedDistance(support_point); + float convex_radius = shape1_support->GetConvexRadius(); + float penetration_depth = -signed_distance + convex_radius; + float dot = inShapeCast.mDirection.Dot(normal); + + // Collision output + Mat44 com_hit; + Vec3 point1, point2; + float fraction; + + // Do we start in collision? + if (penetration_depth > 0.0f) + { + // Back face culling? + if (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::IgnoreBackFaces && dot > 0.0f) + return; + + // Shallower hit? + if (penetration_depth <= -ioCollector.GetEarlyOutFraction()) + return; + + // We're hitting at fraction 0 + fraction = 0.0f; + + // Get contact point + com_hit = inCenterOfMassTransform2; + point1 = inCenterOfMassTransform2 * (support_point - normal * convex_radius); + point2 = inCenterOfMassTransform2 * (support_point - normal * signed_distance); + } + else if (dot < 0.0f) // Moving towards the plane? + { + // Calculate hit fraction + fraction = penetration_depth / dot; + JPH_ASSERT(fraction >= 0.0f); + + // Further than early out fraction? + if (fraction >= ioCollector.GetEarlyOutFraction()) + return; + + // Get contact point + com_hit = inCenterOfMassTransform2.PostTranslated(fraction * inShapeCast.mDirection); + point1 = point2 = com_hit * (support_point - normal * convex_radius); + } + else + { + // Moving away from the plane + return; + } + + // Create cast result + Vec3 penetration_axis_world = com_hit.Multiply3x3(-normal); + bool back_facing = dot > 0.0f; + ShapeCastResult result(fraction, point1, point2, penetration_axis_world, back_facing, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Gather faces + if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of convex shape + Mat44 shape_to_world = com_hit * inShapeCast.mCenterOfMassStart; + convex_shape->GetSupportingFace(SubShapeID(), normal_in_convex_shape_space, inShapeCast.mScale, shape_to_world, result.mShape1Face); + + // Get supporting face of plane + if (!result.mShape1Face.empty()) + sGetSupportingFace(convex_shape, shape_to_world.GetTranslation(), plane, inCenterOfMassTransform2, result.mShape2Face); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); +} + +struct PlaneShape::PSGetTrianglesContext +{ + Float3 mVertices[4]; + bool mDone = false; +}; + +void PlaneShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(PSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(PSGetTrianglesContext))); + + PSGetTrianglesContext *context = new (&ioContext) PSGetTrianglesContext(); + + // Get the vertices of the plane + Vec3 vertices[4]; + GetVertices(vertices); + + // Reverse if scale is inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + std::swap(vertices[0], vertices[3]); + std::swap(vertices[1], vertices[2]); + } + + // Transform them to world space + Mat44 com = Mat44::sRotationTranslation(inRotation, inPositionCOM).PreScaled(inScale); + for (uint i = 0; i < 4; ++i) + (com * vertices[i]).StoreFloat3(&context->mVertices[i]); +} + +int PlaneShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 2, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + // Check if we're done + PSGetTrianglesContext &context = (PSGetTrianglesContext &)ioContext; + if (context.mDone) + return 0; + context.mDone = true; + + // 1st triangle + outTriangleVertices[0] = context.mVertices[0]; + outTriangleVertices[1] = context.mVertices[1]; + outTriangleVertices[2] = context.mVertices[2]; + + // 2nd triangle + outTriangleVertices[3] = context.mVertices[0]; + outTriangleVertices[4] = context.mVertices[2]; + outTriangleVertices[5] = context.mVertices[3]; + + if (outMaterials != nullptr) + { + // Get material + const PhysicsMaterial *material = GetMaterial(SubShapeID()); + outMaterials[0] = material; + outMaterials[1] = material; + } + + return 2; +} + +void PlaneShape::sCollideConvexVsPlane(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::Plane); + const ConvexShape *shape1 = static_cast(inShape1); + const PlaneShape *shape2 = static_cast(inShape2); + + // Transform the plane to the space of the convex shape + Plane scaled_plane = shape2->mPlane.Scaled(inScale2); + Plane plane = scaled_plane.GetTransformed(inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2); + Vec3 normal = plane.GetNormal(); + + // Get support function + ConvexShape::SupportBuffer shape1_support_buffer; + const ConvexShape::Support *shape1_support = shape1->GetSupportFunction(ConvexShape::ESupportMode::Default, shape1_support_buffer, inScale1); + + // Get the support point of the convex shape in the opposite direction of the plane normal + Vec3 support_point = shape1_support->GetSupport(-normal); + float signed_distance = plane.SignedDistance(support_point); + float convex_radius = shape1_support->GetConvexRadius(); + float penetration_depth = -signed_distance + convex_radius; + if (penetration_depth > -inCollideShapeSettings.mMaxSeparationDistance) + { + // Get contact point + Vec3 point1 = inCenterOfMassTransform1 * (support_point - normal * convex_radius); + Vec3 point2 = inCenterOfMassTransform1 * (support_point - normal * signed_distance); + Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(-normal); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Gather faces + if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + shape1->GetSupportingFace(SubShapeID(), normal, inScale1, inCenterOfMassTransform1, result.mShape1Face); + + // Get supporting face of shape 2 + if (!result.mShape1Face.empty()) + sGetSupportingFace(shape1, inCenterOfMassTransform1.GetTranslation(), scaled_plane, inCenterOfMassTransform2, result.mShape2Face); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); + } +} + +void PlaneShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mPlane); + inStream.Write(mHalfExtent); +} + +void PlaneShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mPlane); + inStream.Read(mHalfExtent); + + CalculateLocalBounds(); +} + +void PlaneShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials = { mMaterial }; +} + +void PlaneShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + JPH_ASSERT(inNumMaterials == 1); + mMaterial = inMaterials[0]; +} + +void PlaneShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Plane); + f.mConstruct = []() -> Shape * { return new PlaneShape; }; + f.mColor = Color::sDarkRed; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Plane, sCollideConvexVsPlane); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Plane, sCastConvexVsPlane); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::Plane, s, CollisionDispatch::sReversedCastShape); + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Plane, s, CollisionDispatch::sReversedCollideShape); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h new file mode 100644 index 000000000000..c9a7810b7445 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h @@ -0,0 +1,143 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a PlaneShape +class JPH_EXPORT PlaneShapeSettings final : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PlaneShapeSettings) + +public: + /// Default constructor for deserialization + PlaneShapeSettings() = default; + + /// Create a plane shape. + PlaneShapeSettings(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = cDefaultHalfExtent) : mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Plane mPlane; ///< Plane that describes the shape. The negative half space is considered solid. + + RefConst mMaterial; ///< Surface material of the plane + + static constexpr float cDefaultHalfExtent = 1000.0f; ///< Default half-extent of the plane (total size along 1 axis will be 2 * half-extent) + + float mHalfExtent = cDefaultHalfExtent; ///< The bounding box of this plane will run from [-half_extent, half_extent]. Keep this as low as possible for better broad phase performance. +}; + +/// A plane shape. The negative half space is considered solid. Planes cannot be dynamic objects, only static or kinematic. +/// The plane is considered an infinite shape, but testing collision outside of its bounding box (defined by the half-extent parameter) will not return a collision result. +/// At the edge of the bounding box collision with the plane will be inconsistent. If you need something of a well defined size, a box shape may be better. +class JPH_EXPORT PlaneShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + PlaneShape() : Shape(EShapeType::Plane, EShapeSubType::Plane) { } + PlaneShape(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = PlaneShapeSettings::cDefaultHalfExtent) : Shape(EShapeType::Plane, EShapeSubType::Plane), mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { CalculateLocalBounds(); } + PlaneShape(const PlaneShapeSettings &inSettings, ShapeResult &outResult); + + /// Get the plane + const Plane & GetPlane() const { return mPlane; } + + /// Get the half-extent of the bounding box of the plane + float GetHalfExtent() const { return mHalfExtent; } + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return true; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return mLocalBounds; } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return 0; } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return 0.0f; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; } + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mPlane.GetNormal(); } + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + struct PSGetTrianglesContext; ///< Context class for GetTrianglesStart/Next + + // Get 4 vertices that form the plane + void GetVertices(Vec3 *outVertices) const; + + // Cache the local bounds + void CalculateLocalBounds(); + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsPlane(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsPlane(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + Plane mPlane; + RefConst mMaterial; + float mHalfExtent; + AABox mLocalBounds; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h new file mode 100644 index 000000000000..96e69f0b19be --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h @@ -0,0 +1,319 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// This class calculates the intersection between a fluid surface and a polyhedron and returns the submerged volume and its center of buoyancy +/// Construct this class and then one by one add all faces of the polyhedron using the AddFace function. After all faces have been added the result +/// can be gotten through GetResult. +class PolyhedronSubmergedVolumeCalculator +{ +private: + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 4 vertices submerged + // inV1 .. inV4 are submerged + inline static void sTetrahedronVolume4(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // Calculate center of mass and mass of this tetrahedron, + // see: https://en.wikipedia.org/wiki/Tetrahedron#Volume + outVolumeTimes6 = max((inV1 - inV4).Dot((inV2 - inV4).Cross(inV3 - inV4)), 0.0f); // All contributions should be positive because we use a reference point that is on the surface of the hull + outCenterTimes4 = inV1 + inV2 + inV3 + inV4; + } + + // Get the intersection point with a plane. + // inV1 is inD1 distance away from the plane, inV2 is inD2 distance away from the plane + inline static Vec3 sGetPlaneIntersection(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2) + { + JPH_ASSERT(Sign(inD1) != Sign(inD2), "Assuming both points are on opposite ends of the plane"); + float delta = inD1 - inD2; + if (abs(delta) < 1.0e-6f) + return inV1; // Parallel to plane, just pick a point + else + return inV1 + inD1 * (inV2 - inV1) / delta; + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 1 vertex submerged + // inV1 is submerged, inV2 .. inV4 are not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 1 point submerged is cut along 3 edges forming a new tetrahedron + Vec3 v2 = sGetPlaneIntersection(inV1, inD1, inV2, inD2); + Vec3 v3 = sGetPlaneIntersection(inV1, inD1, inV3, inD3); + Vec3 v4 = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 v2w = mBaseOffset + v2; + RVec3 v3w = mBaseOffset + v3; + RVec3 v4w = mBaseOffset + v4; + + DebugRenderer::sInstance->DrawTriangle(v4w, v3w, v2w, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(v4w, v3w, v2w, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + sTetrahedronVolume4(inV1, v2, v3, v4, outVolumeTimes6, outCenterTimes4); + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 2 vertices submerged + // inV1, inV2 are submerged, inV3, inV4 are not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 2 points submerged is cut along 4 edges forming a quad + Vec3 c = sGetPlaneIntersection(inV1, inD1, inV3, inD3); + Vec3 d = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + Vec3 e = sGetPlaneIntersection(inV2, inD2, inV4, inD4); + Vec3 f = sGetPlaneIntersection(inV2, inD2, inV3, inD3); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 cw = mBaseOffset + c; + RVec3 dw = mBaseOffset + d; + RVec3 ew = mBaseOffset + e; + RVec3 fw = mBaseOffset + f; + + DebugRenderer::sInstance->DrawTriangle(cw, ew, dw, Color::sGreen); + DebugRenderer::sInstance->DrawTriangle(cw, fw, ew, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(cw, ew, dw, Color::sWhite); + DebugRenderer::sInstance->DrawWireTriangle(cw, fw, ew, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + // We pick point c as reference (which is on the cut off surface) + // This leaves us with three tetrahedrons to sum up (any faces that are in the same plane as c will have zero volume) + Vec3 center1, center2, center3; + float volume1, volume2, volume3; + sTetrahedronVolume4(e, f, inV2, c, volume1, center1); + sTetrahedronVolume4(e, inV1, d, c, volume2, center2); + sTetrahedronVolume4(e, inV2, inV1, c, volume3, center3); + + // Tally up the totals + outVolumeTimes6 = volume1 + volume2 + volume3; + outCenterTimes4 = outVolumeTimes6 > 0.0f? (volume1 * center1 + volume2 * center2 + volume3 * center3) / outVolumeTimes6 : Vec3::sZero(); + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 3 vertices submerged + // inV1, inV2, inV3 are submerged, inV4 is not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 1 point above the surface is cut along 3 edges forming a new tetrahedron + Vec3 v1 = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + Vec3 v2 = sGetPlaneIntersection(inV2, inD2, inV4, inD4); + Vec3 v3 = sGetPlaneIntersection(inV3, inD3, inV4, inD4); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 v1w = mBaseOffset + v1; + RVec3 v2w = mBaseOffset + v2; + RVec3 v3w = mBaseOffset + v3; + + DebugRenderer::sInstance->DrawTriangle(v3w, v2w, v1w, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(v3w, v2w, v1w, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + Vec3 dry_center, total_center; + float dry_volume, total_volume; + + // We first calculate the part that is above the surface + sTetrahedronVolume4(v1, v2, v3, inV4, dry_volume, dry_center); + + // Calculate the total volume + sTetrahedronVolume4(inV1, inV2, inV3, inV4, total_volume, total_center); + + // From this we can calculate the center and volume of the submerged part + outVolumeTimes6 = max(total_volume - dry_volume, 0.0f); + outCenterTimes4 = outVolumeTimes6 > 0.0f? (total_center * total_volume - dry_center * dry_volume) / outVolumeTimes6 : Vec3::sZero(); + } + +public: + /// A helper class that contains cached information about a polyhedron vertex + class Point + { + public: + Vec3 mPosition; ///< World space position of vertex + float mDistanceToSurface; ///< Signed distance to the surface (> 0 is above, < 0 is below) + bool mAboveSurface; ///< If the point is above the surface (mDistanceToSurface > 0) + }; + + /// Constructor + /// @param inTransform Transform to transform all incoming points with + /// @param inPoints Array of points that are part of the polyhedron + /// @param inPointStride Amount of bytes between each point (should usually be sizeof(Vec3)) + /// @param inNumPoints The amount of points + /// @param inSurface The plane that forms the fluid surface (normal should point up) + /// @param ioBuffer A temporary buffer of Point's that should have inNumPoints entries and should stay alive while this class is alive +#ifdef JPH_DEBUG_RENDERER + /// @param inBaseOffset The offset to transform inTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing. +#endif // JPH_DEBUG_RENDERER + PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3 inBaseOffset +#endif // JPH_DEBUG_RENDERER + ) : + mPoints(ioBuffer) +#ifdef JPH_DEBUG_RENDERER + , mBaseOffset(inBaseOffset) +#endif // JPH_DEBUG_RENDERER + { + // Convert the points to world space and determine the distance to the surface + float reference_dist = FLT_MAX; + for (int p = 0; p < inNumPoints; ++p) + { + // Calculate values + Vec3 transformed_point = inTransform * *reinterpret_cast(reinterpret_cast(inPoints) + p * inPointStride); + float dist = inSurface.SignedDistance(transformed_point); + bool above = dist >= 0.0f; + + // Keep track if all are above or below + mAllAbove &= above; + mAllBelow &= !above; + + // Calculate lowest point, we use this to create tetrahedrons out of all faces + if (reference_dist > dist) + { + mReferencePointIdx = p; + reference_dist = dist; + } + + // Store values + ioBuffer->mPosition = transformed_point; + ioBuffer->mDistanceToSurface = dist; + ioBuffer->mAboveSurface = above; + ++ioBuffer; + } + } + + /// Check if all points are above the surface. Should be used as early out. + inline bool AreAllAbove() const + { + return mAllAbove; + } + + /// Check if all points are below the surface. Should be used as early out. + inline bool AreAllBelow() const + { + return mAllBelow; + } + + /// Get the lowest point of the polyhedron. Used to form the 4th vertex to make a tetrahedron out of a polyhedron face. + inline int GetReferencePointIdx() const + { + return mReferencePointIdx; + } + + /// Add a polyhedron face. Supply the indices of the points that form the face (in counter clockwise order). + void AddFace(int inIdx1, int inIdx2, int inIdx3) + { + JPH_ASSERT(inIdx1 != mReferencePointIdx && inIdx2 != mReferencePointIdx && inIdx3 != mReferencePointIdx, "A face using the reference point will not contribute to the volume"); + + // Find the points + const Point &ref = mPoints[mReferencePointIdx]; + const Point &p1 = mPoints[inIdx1]; + const Point &p2 = mPoints[inIdx2]; + const Point &p3 = mPoints[inIdx3]; + + // Determine which vertices are submerged + uint code = (p1.mAboveSurface? 0 : 0b001) | (p2.mAboveSurface? 0 : 0b010) | (p3.mAboveSurface? 0 : 0b100); + + float volume; + Vec3 center; + switch (code) + { + case 0b000: + // One point submerged + sTetrahedronVolume1(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b001: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center); + break; + + case 0b010: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center); + break; + + case 0b100: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b011: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center); + break; + + case 0b101: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center); + break; + + case 0b110: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b111: + // Four points submerged + sTetrahedronVolume4(ref.mPosition, p3.mPosition, p2.mPosition, p1.mPosition, volume, center); + break; + + default: + // Should not be possible + JPH_ASSERT(false); + volume = 0.0f; + center = Vec3::sZero(); + break; + } + + mSubmergedVolume += volume; + mCenterOfBuoyancy += volume * center; + } + + /// Call after all faces have been added. Returns the submerged volume and the center of buoyancy for the submerged volume. + void GetResult(float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const + { + outCenterOfBuoyancy = mSubmergedVolume > 0.0f? mCenterOfBuoyancy / (4.0f * mSubmergedVolume) : Vec3::sZero(); // Do this before dividing submerged volume by 6 to get correct weight factor + outSubmergedVolume = mSubmergedVolume / 6.0f; + } + +private: + // The precalculated points for this polyhedron + const Point * mPoints; + + // If all points are above/below the surface + bool mAllBelow = true; + bool mAllAbove = true; + + // The lowest point + int mReferencePointIdx = 0; + + // Aggregator for submerged volume and center of buoyancy + float mSubmergedVolume = 0.0f; + Vec3 mCenterOfBuoyancy = Vec3::sZero(); + +#ifdef JPH_DEBUG_RENDERER + // Base offset used for drawing + RVec3 mBaseOffset; +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp new file mode 100644 index 000000000000..66b8bea1eb2a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp @@ -0,0 +1,333 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RotatedTranslatedShapeSettings) +{ + JPH_ADD_BASE_CLASS(RotatedTranslatedShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mPosition) + JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mRotation) +} + +ShapeSettings::ShapeResult RotatedTranslatedShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new RotatedTranslatedShape(*this, mCachedResult); + return mCachedResult; +} + +RotatedTranslatedShape::RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::RotatedTranslated, inSettings, outResult) +{ + if (outResult.HasError()) + return; + + // Calculate center of mass position + mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass(); + + // Store rotation (position is always zero because we center around the center of mass) + mRotation = inSettings.mRotation; + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); + + outResult.Set(this); +} + +RotatedTranslatedShape::RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) : + DecoratedShape(EShapeSubType::RotatedTranslated, inShape) +{ + // Calculate center of mass position + mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass(); + + // Store rotation (position is always zero because we center around the center of mass) + mRotation = inRotation; + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); +} + +MassProperties RotatedTranslatedShape::GetMassProperties() const +{ + // Rotate inertia of child into place + MassProperties p = mInnerShape->GetMassProperties(); + p.Rotate(Mat44::sRotation(mRotation)); + return p; +} + +AABox RotatedTranslatedShape::GetLocalBounds() const +{ + return mInnerShape->GetLocalBounds().Transformed(Mat44::sRotation(mRotation)); +} + +AABox RotatedTranslatedShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation); + return mInnerShape->GetWorldSpaceBounds(transform, TransformScale(inScale)); +} + +TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM), inRotation * mRotation, mInnerShape, BodyID()); + ts.SetShapeScale(TransformScale(inScale)); + return ts; +} + +Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform surface position to local space and pass call on + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, transform * inLocalSurfacePosition); + + // Transform normal to this shape's space + return transform.Multiply3x3Transposed(normal); +} + +void RotatedTranslatedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + Mat44 transform = Mat44::sRotation(mRotation); + mInnerShape->GetSupportingFace(inSubShapeID, transform.Multiply3x3Transposed(inDirection), TransformScale(inScale), inCenterOfMassTransform * transform, outVertices); +} + +void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Get center of mass transform of child + Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation); + + // Recurse to child + mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe); +} + +void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inDrawSupportDirection); +} + +void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale)); +} +#endif // JPH_DEBUG_RENDERER + +bool RotatedTranslatedShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Transform the ray + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + RayCast ray = inRay.Transformed(transform); + + return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit); +} + +void RotatedTranslatedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the ray + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + RayCast ray = inRay.Transformed(transform); + + return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the point + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, inVertices, inNumVertices, inCollidingShapeIndex); +} + +void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation * mRotation, TransformScale(inScale), inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioCollector); +} + +void RotatedTranslatedShape::sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShape1); + + // Get world transform of 1 + Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, shape1->TransformScale(inScale1), inScale2, transform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape2); + + // Get world transform of 2 + Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, shape2->TransformScale(inScale2), inCenterOfMassTransform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape2); + + // Get world transform of 1 and 2 + Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation); + Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, shape2->mInnerShape, shape1->TransformScale(inScale1), shape2->TransformScale(inScale2), transform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // Fetch rotated translated shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShapeCast.mShape); + + // Transform the shape cast and update the shape + Mat44 transform = inShapeCast.mCenterOfMassStart * Mat44::sRotation(shape1->mRotation); + Vec3 scale = shape1->TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape1->mInnerShape, scale, transform, inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape = static_cast(inShape); + + // Determine the local transform + Mat44 local_transform = Mat44::sRotation(shape->mRotation); + + // Transform the shape cast + ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3()); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShapeCast.mShape); + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape); + + // Determine the local transform of shape 2 + Mat44 local_transform2 = Mat44::sRotation(shape2->mRotation); + Mat44 local_transform2_transposed = local_transform2.Transposed3x3(); + + // Transform the shape cast and update the shape + Mat44 transform = (local_transform2_transposed * inShapeCast.mCenterOfMassStart) * Mat44::sRotation(shape1->mRotation); + Vec3 scale = shape1->TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape1->mInnerShape, scale, transform, local_transform2_transposed.Multiply3x3(inShapeCast.mDirection)); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape2->mInnerShape, shape2->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mRotation); +} + +void RotatedTranslatedShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mRotation); + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); +} + +bool RotatedTranslatedShape::IsValidScale(Vec3Arg inScale) const +{ + if (!Shape::IsValidScale(inScale)) + return false; + + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return mInnerShape->IsValidScale(inScale); + + if (!ScaleHelpers::CanScaleBeRotated(mRotation, inScale)) + return false; + + return mInnerShape->IsValidScale(ScaleHelpers::RotateScale(mRotation, inScale)); +} + +Vec3 RotatedTranslatedShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(scale)) + return mInnerShape->MakeScaleValid(scale); + + if (ScaleHelpers::CanScaleBeRotated(mRotation, scale)) + return ScaleHelpers::RotateScale(mRotation.Conjugated(), mInnerShape->MakeScaleValid(ScaleHelpers::RotateScale(mRotation, scale))); + + Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs()); + Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale; + if (ScaleHelpers::CanScaleBeRotated(mRotation, uniform_scale)) + return uniform_scale; + + return Sign(scale.GetX()) * abs_uniform_scale; +} + +void RotatedTranslatedShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::RotatedTranslated); + f.mConstruct = []() -> Shape * { return new RotatedTranslatedShape; }; + f.mColor = Color::sBlue; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, s, sCollideRotatedTranslatedVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::RotatedTranslated, sCollideShapeVsRotatedTranslated); + CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, s, sCastRotatedTranslatedVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::RotatedTranslated, sCastShapeVsRotatedTranslated); + } + + CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCollideRotatedTranslatedVsRotatedTranslated); + CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCastRotatedTranslatedVsRotatedTranslated); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h new file mode 100644 index 000000000000..2978a9559cbe --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h @@ -0,0 +1,161 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a RotatedTranslatedShape +class JPH_EXPORT RotatedTranslatedShapeSettings final : public DecoratedShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RotatedTranslatedShapeSettings) + +public: + /// Constructor + RotatedTranslatedShapeSettings() = default; + + /// Construct with shape settings, can be serialized. + RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape): DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mPosition; ///< Position of the sub shape + Quat mRotation; ///< Rotation of the sub shape +}; + +/// A rotated translated shape will rotate and translate a child shape. +/// Shifts the child object so that it is centered around the center of mass. +class JPH_EXPORT RotatedTranslatedShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + RotatedTranslatedShape() : DecoratedShape(EShapeSubType::RotatedTranslated) { } + RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult); + RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape); + + /// Access the rotation that is applied to the inner shape + Quat GetRotation() const { return mRotation; } + + /// Access the translation that has been applied to the inner shape + Vec3 GetPosition() const { return mCenterOfMass - mRotation * mInnerShape->GetCenterOfMass(); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return mInnerShape->GetVolume(); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + /// Transform the scale to the local space of the child shape + inline Vec3 TransformScale(Vec3Arg inScale) const + { + // We don't need to transform uniform scale or if the rotation is identity + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return inScale; + + return ScaleHelpers::RotateScale(mRotation, inScale); + } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + bool mIsRotationIdentity; ///< If mRotation is close to identity (put here because it falls in padding bytes) + Vec3 mCenterOfMass; ///< Position of the center of mass + Quat mRotation; ///< Rotation of the child shape +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h new file mode 100644 index 000000000000..4035dd77ae5a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h @@ -0,0 +1,83 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions to get properties of a scaling vector +namespace ScaleHelpers +{ + /// Minimum valid scale value. This is used to prevent division by zero when scaling a shape with a zero scale. + static constexpr float cMinScale = 1.0e-6f; + + /// The tolerance used to check if components of the scale vector are the same + static constexpr float cScaleToleranceSq = 1.0e-8f; + + /// Test if a scale is identity + inline bool IsNotScaled(Vec3Arg inScale) { return inScale.IsClose(Vec3::sReplicate(1.0f), cScaleToleranceSq); } + + /// Test if a scale is uniform + inline bool IsUniformScale(Vec3Arg inScale) { return inScale.Swizzle().IsClose(inScale, cScaleToleranceSq); } + + /// Test if a scale is uniform in XZ + inline bool IsUniformScaleXZ(Vec3Arg inScale) { return inScale.Swizzle().IsClose(inScale, ScaleHelpers::cScaleToleranceSq); } + + /// Scale the convex radius of an object + inline float ScaleConvexRadius(float inConvexRadius, Vec3Arg inScale) { return min(inConvexRadius * inScale.Abs().ReduceMin(), cDefaultConvexRadius); } + + /// Test if a scale flips an object inside out (which requires flipping all normals and polygon windings) + inline bool IsInsideOut(Vec3Arg inScale) { return (CountBits(Vec3::sLess(inScale, Vec3::sZero()).GetTrues() & 0x7) & 1) != 0; } + + /// Test if any of the components of the scale have a value below cMinScale + inline bool IsZeroScale(Vec3Arg inScale) { return Vec3::sLess(inScale.Abs(), Vec3::sReplicate(cMinScale)).TestAnyXYZTrue(); } + + /// Ensure that the scale for each component is at least cMinScale + inline Vec3 MakeNonZeroScale(Vec3Arg inScale) { return inScale.GetSign() * Vec3::sMax(inScale.Abs(), Vec3::sReplicate(cMinScale)); } + + /// Get the average scale if inScale, used to make the scale uniform when a shape doesn't support non-uniform scale + inline Vec3 MakeUniformScale(Vec3Arg inScale) { return Vec3::sReplicate((inScale.GetX() + inScale.GetY() + inScale.GetZ()) / 3.0f); } + + /// Average the scale in XZ, used to make the scale uniform when a shape doesn't support non-uniform scale in the XZ plane + inline Vec3 MakeUniformScaleXZ(Vec3Arg inScale) { return 0.5f * (inScale + inScale.Swizzle()); } + + /// Checks in scale can be rotated to child shape + /// @param inRotation Rotation of child shape + /// @param inScale Scale in local space of parent shape + /// @return True if the scale is valid (no shearing introduced) + inline bool CanScaleBeRotated(QuatArg inRotation, Vec3Arg inScale) + { + // inScale is a scale in local space of the shape, so the transform for the shape (ignoring translation) is: T = Mat44::sScale(inScale) * mRotation. + // when we pass the scale to the child it needs to be local to the child, so we want T = mRotation * Mat44::sScale(ChildScale). + // Solving for ChildScale: ChildScale = mRotation^-1 * Mat44::sScale(inScale) * mRotation = mRotation^T * Mat44::sScale(inScale) * mRotation + // If any of the off diagonal elements are non-zero, it means the scale / rotation is not compatible. + Mat44 r = Mat44::sRotation(inRotation); + Mat44 child_scale = r.Multiply3x3LeftTransposed(r.PostScaled(inScale)); + + // Get the columns, but zero the diagonal + Vec4 zero = Vec4::sZero(); + Vec4 c0 = Vec4::sSelect(child_scale.GetColumn4(0), zero, UVec4(0xffffffff, 0, 0, 0)).Abs(); + Vec4 c1 = Vec4::sSelect(child_scale.GetColumn4(1), zero, UVec4(0, 0xffffffff, 0, 0)).Abs(); + Vec4 c2 = Vec4::sSelect(child_scale.GetColumn4(2), zero, UVec4(0, 0, 0xffffffff, 0)).Abs(); + + // Check if all elements are less than epsilon + Vec4 epsilon = Vec4::sReplicate(1.0e-6f); + return UVec4::sAnd(UVec4::sAnd(Vec4::sLess(c0, epsilon), Vec4::sLess(c1, epsilon)), Vec4::sLess(c2, epsilon)).TestAllTrue(); + } + + /// Adjust scale for rotated child shape + /// @param inRotation Rotation of child shape + /// @param inScale Scale in local space of parent shape + /// @return Rotated scale + inline Vec3 RotateScale(QuatArg inRotation, Vec3Arg inScale) + { + // Get the diagonal of mRotation^T * Mat44::sScale(inScale) * mRotation (see comment at CanScaleBeRotated) + Mat44 r = Mat44::sRotation(inRotation); + return r.Multiply3x3LeftTransposed(r.PostScaled(inScale)).GetDiagonal3(); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp new file mode 100644 index 000000000000..1511813892bf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp @@ -0,0 +1,238 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ScaledShapeSettings) +{ + JPH_ADD_BASE_CLASS(ScaledShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(ScaledShapeSettings, mScale) +} + +ShapeSettings::ShapeResult ScaledShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new ScaledShape(*this, mCachedResult); + return mCachedResult; +} + +ScaledShape::ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::Scaled, inSettings, outResult), + mScale(inSettings.mScale) +{ + if (outResult.HasError()) + return; + + if (ScaleHelpers::IsZeroScale(inSettings.mScale)) + { + outResult.SetError("Can't use zero scale!"); + return; + } + + outResult.Set(this); +} + +MassProperties ScaledShape::GetMassProperties() const +{ + MassProperties p = mInnerShape->GetMassProperties(); + p.Scale(mScale); + return p; +} + +AABox ScaledShape::GetLocalBounds() const +{ + return mInnerShape->GetLocalBounds().Scaled(mScale); +} + +AABox ScaledShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform, inScale * mScale); +} + +TransformedShape ScaledShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM), inRotation, mInnerShape, BodyID()); + ts.SetShapeScale(inScale * mScale); + return ts; +} + +Vec3 ScaledShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform the surface point to local space and pass the query on + Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition / mScale); + + // Need to transform the plane normals using inScale + // Transforming a direction with matrix M is done through multiplying by (M^-1)^T + // In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards + return (normal / mScale).Normalized(); +} + +void ScaledShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale * mScale, inCenterOfMassTransform, outVertices); +} + +void ScaledShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + mInnerShape->GetSubmergedVolume(inCenterOfMassTransform, inScale * mScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void ScaledShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inUseMaterialColors, inDrawWireframe); +} + +void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inDrawSupportDirection); +} + +void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform, inScale * mScale); +} +#endif // JPH_DEBUG_RENDERER + +bool ScaledShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + Vec3 inv_scale = mScale.Reciprocal(); + RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection }; + return mInnerShape->CastRay(scaled_ray, inSubShapeIDCreator, ioHit); +} + +void ScaledShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + Vec3 inv_scale = mScale.Reciprocal(); + RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection }; + return mInnerShape->CastRay(scaled_ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + Vec3 inv_scale = mScale.Reciprocal(); + mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, inVertices, inNumVertices, inCollidingShapeIndex); +} + +void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation, inScale * mScale, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sScale(mScale), ioCollector); +} + +void ScaledShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mScale); +} + +void ScaledShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mScale); +} + +float ScaledShape::GetVolume() const +{ + return abs(mScale.GetX() * mScale.GetY() * mScale.GetZ()) * mInnerShape->GetVolume(); +} + +bool ScaledShape::IsValidScale(Vec3Arg inScale) const +{ + return mInnerShape->IsValidScale(inScale * mScale); +} + +Vec3 ScaledShape::MakeScaleValid(Vec3Arg inScale) const +{ + return mInnerShape->MakeScaleValid(mScale * inScale) / mScale; +} + +void ScaledShape::sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape1 = static_cast(inShape1); + + CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), inShape2, inScale1 * shape1->GetScale(), inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void ScaledShape::sCollideShapeVsScaled(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape2 = static_cast(inShape2); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->GetInnerShape(), inScale1, inScale2 * shape2->GetScale(), inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void ScaledShape::sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape = static_cast(inShapeCast.mShape); + + ShapeCast scaled_cast(shape->GetInnerShape(), inShapeCast.mScale * shape->GetScale(), inShapeCast.mCenterOfMassStart, inShapeCast.mDirection); + CollisionDispatch::sCastShapeVsShapeLocalSpace(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape = static_cast(inShape); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void ScaledShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Scaled); + f.mConstruct = []() -> Shape * { return new ScaledShape; }; + f.mColor = Color::sYellow; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Scaled, s, sCollideScaledVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Scaled, sCollideShapeVsScaled); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Scaled, s, sCastScaledVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Scaled, sCastShapeVsScaled); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h new file mode 100644 index 000000000000..6da92b6ed32a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h @@ -0,0 +1,145 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class SubShapeIDCreator; +class CollideShapeSettings; + +/// Class that constructs a ScaledShape +class JPH_EXPORT ScaledShapeSettings final : public DecoratedShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ScaledShapeSettings) + +public: + /// Default constructor for deserialization + ScaledShapeSettings() = default; + + /// Constructor that decorates another shape with a scale + ScaledShapeSettings(const ShapeSettings *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + ScaledShapeSettings(const Shape *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mScale = Vec3(1, 1, 1); +}; + +/// A shape that scales a child shape in local space of that shape. The scale can be non-uniform and can even turn it inside out when one or three components of the scale are negative. +class JPH_EXPORT ScaledShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + ScaledShape() : DecoratedShape(EShapeSubType::Scaled) { } + ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult); + + /// Constructor that decorates another shape with a scale + ScaledShape(const Shape *inShape, Vec3Arg inScale) : DecoratedShape(EShapeSubType::Scaled, inShape), mScale(inScale) { JPH_ASSERT(!ScaleHelpers::IsZeroScale(mScale)); } + + /// Get the scale + Vec3 GetScale() const { return mScale; } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mScale * mInnerShape->GetCenterOfMass(); } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mScale.ReduceMin() * mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsScaled(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + Vec3 mScale = Vec3(1, 1, 1); +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp new file mode 100644 index 000000000000..a3c37c0dae09 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp @@ -0,0 +1,325 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(ShapeSettings) +{ + JPH_ADD_BASE_CLASS(ShapeSettings, SerializableObject) + + JPH_ADD_ATTRIBUTE(ShapeSettings, mUserData) +} + +#ifdef JPH_DEBUG_RENDERER +bool Shape::sDrawSubmergedVolumes = false; +#endif // JPH_DEBUG_RENDERER + +ShapeFunctions ShapeFunctions::sRegistry[NumSubShapeTypes]; + +const Shape *Shape::GetLeafShape([[maybe_unused]] const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const +{ + outRemainder = inSubShapeID; + return this; +} + +TransformedShape Shape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We have reached the leaf shape so there is no remainder + outRemainder = SubShapeID(); + + // Just return the transformed shape for this shape + TransformedShape ts(RVec3(inPositionCOM), inRotation, this, BodyID()); + ts.SetShapeScale(inScale); + return ts; +} + +void Shape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + TransformedShape ts(RVec3(inPositionCOM), inRotation, this, TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator); + ts.SetShapeScale(inScale); + ioCollector.AddHit(ts); +} + +void Shape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + Vec3 scale; + Mat44 transform = inCenterOfMassTransform.Decompose(scale); + TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator()); + ts.SetShapeScale(MakeScaleValid(scale)); + ioCollector.AddHit(ts); +} + +void Shape::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mShapeSubType); + inStream.Write(mUserData); +} + +void Shape::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mUserData); +} + +Shape::ShapeResult Shape::sRestoreFromBinaryState(StreamIn &inStream) +{ + ShapeResult result; + + // Read the type of the shape + EShapeSubType shape_sub_type; + inStream.Read(shape_sub_type); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read type id"); + return result; + } + + // Construct and read the data of the shape + Ref shape = ShapeFunctions::sGet(shape_sub_type).mConstruct(); + shape->RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to restore shape"); + return result; + } + + result.Set(shape); + return result; +} + +void Shape::SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const +{ + ShapeToIDMap::const_iterator shape_id_iter = ioShapeMap.find(this); + if (shape_id_iter == ioShapeMap.end()) + { + // Write shape ID of this shape + uint32 shape_id = ioShapeMap.size(); + ioShapeMap[this] = shape_id; + inStream.Write(shape_id); + + // Write the shape itself + SaveBinaryState(inStream); + + // Write the ID's of all sub shapes + ShapeList sub_shapes; + SaveSubShapeState(sub_shapes); + inStream.Write(uint32(sub_shapes.size())); + for (const Shape *shape : sub_shapes) + { + if (shape == nullptr) + inStream.Write(~uint32(0)); + else + shape->SaveWithChildren(inStream, ioShapeMap, ioMaterialMap); + } + + // Write the materials + PhysicsMaterialList materials; + SaveMaterialState(materials); + StreamUtils::SaveObjectArray(inStream, materials, &ioMaterialMap); + } + else + { + // Known shape, just write the ID + inStream.Write(shape_id_iter->second); + } +} + +Shape::ShapeResult Shape::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap) +{ + ShapeResult result; + + // Read ID of this shape + uint32 shape_id; + inStream.Read(shape_id); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read shape id"); + return result; + } + + // Check nullptr shape + if (shape_id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if we already read this shape + if (shape_id < ioShapeMap.size()) + { + result.Set(ioShapeMap[shape_id]); + return result; + } + + // Read the shape + result = sRestoreFromBinaryState(inStream); + if (result.HasError()) + return result; + JPH_ASSERT(ioShapeMap.size() == shape_id); // Assert that this is the next ID in the map + ioShapeMap.push_back(result.Get()); + + // Read the sub shapes + uint32 len; + inStream.Read(len); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read stream"); + return result; + } + ShapeList sub_shapes; + sub_shapes.reserve(len); + for (size_t i = 0; i < len; ++i) + { + ShapeResult sub_shape_result = sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap); + if (sub_shape_result.HasError()) + return sub_shape_result; + sub_shapes.push_back(sub_shape_result.Get()); + } + result.Get()->RestoreSubShapeState(sub_shapes.data(), (uint)sub_shapes.size()); + + // Read the materials + Result mlresult = StreamUtils::RestoreObjectArray(inStream, ioMaterialMap); + if (mlresult.HasError()) + { + result.SetError(mlresult.GetError()); + return result; + } + const PhysicsMaterialList &materials = mlresult.Get(); + result.Get()->RestoreMaterialState(materials.data(), (uint)materials.size()); + + return result; +} + +Shape::Stats Shape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + Stats stats = GetStats(); + + // If shape is already visited, don't count its size again + if (!ioVisitedShapes.insert(this).second) + stats.mSizeBytes = 0; + + return stats; +} + +bool Shape::IsValidScale(Vec3Arg inScale) const +{ + return !ScaleHelpers::IsZeroScale(inScale); +} + +Vec3 Shape::MakeScaleValid(Vec3Arg inScale) const +{ + return ScaleHelpers::MakeNonZeroScale(inScale); +} + +Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const +{ + const Vec3 unit_scale = Vec3::sReplicate(1.0f); + + if (inScale.IsNearZero()) + { + ShapeResult result; + result.SetError("Can't use zero scale!"); + return result; + } + + // First test if we can just wrap this shape in a scaled shape + if (IsValidScale(inScale)) + { + // Test if the scale is near unit + ShapeResult result; + if (inScale.IsClose(unit_scale)) + result.Set(const_cast(this)); + else + result.Set(new ScaledShape(this, inScale)); + return result; + } + + // Collect the leaf shapes and their transforms + struct Collector : TransformedShapeCollector + { + virtual void AddHit(const ResultType &inResult) override + { + mShapes.push_back(inResult); + } + + Array mShapes; + }; + Collector collector; + TransformShape(Mat44::sScale(inScale) * Mat44::sTranslation(GetCenterOfMass()), collector); + + // Construct a compound shape + StaticCompoundShapeSettings compound; + compound.mSubShapes.reserve(collector.mShapes.size()); + for (const TransformedShape &ts : collector.mShapes) + { + const Shape *shape = ts.mShape; + + // Construct a scaled shape if scale is not unit + Vec3 scale = ts.GetShapeScale(); + if (!scale.IsClose(unit_scale)) + shape = new ScaledShape(shape, scale); + + // Add the shape + compound.AddShape(Vec3(ts.mShapePositionCOM) - ts.mShapeRotation * shape->GetCenterOfMass(), ts.mShapeRotation, shape); + } + + return compound.Create(); +} + +void Shape::sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + // First test if we're inside our bounding box + AABox bounds = inShape.GetLocalBounds(); + if (bounds.Contains(inPoint)) + { + // A collector that just counts the number of hits + class HitCountCollector : public CastRayCollector + { + public: + virtual void AddHit(const RayCastResult &inResult) override + { + // Store the last sub shape ID so that we can provide something to our outer hit collector + mSubShapeID = inResult.mSubShapeID2; + + ++mHitCount; + } + + int mHitCount = 0; + SubShapeID mSubShapeID; + }; + HitCountCollector collector; + + // Configure the raycast + RayCastSettings settings; + settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces); + + // Cast a ray that's 10% longer than the height of our bounding box + inShape.CastRay(RayCast { inPoint, 1.1f * bounds.GetSize().GetY() * Vec3::sAxisY() }, settings, inSubShapeIDCreator, collector, inShapeFilter); + + // Odd amount of hits means inside + if ((collector.mHitCount & 1) == 1) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), collector.mSubShapeID }); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h new file mode 100644 index 000000000000..f43b79c3f0cc --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h @@ -0,0 +1,466 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RayCast; +class RayCastSettings; +struct ShapeCast; +class ShapeCastSettings; +class RayCastResult; +class ShapeCastResult; +class CollidePointResult; +class CollideShapeResult; +class SubShapeIDCreator; +class SubShapeID; +class PhysicsMaterial; +class TransformedShape; +class Plane; +class CollideSoftBodyVertexIterator; +class Shape; +class StreamOut; +class StreamIn; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +using CastRayCollector = CollisionCollector; +using CastShapeCollector = CollisionCollector; +using CollidePointCollector = CollisionCollector; +using CollideShapeCollector = CollisionCollector; +using TransformedShapeCollector = CollisionCollector; + +using ShapeRefC = RefConst; +using ShapeList = Array; +using PhysicsMaterialRefC = RefConst; +using PhysicsMaterialList = Array; + +/// Shapes are categorized in groups, each shape can return which group it belongs to through its Shape::GetType function. +enum class EShapeType : uint8 +{ + Convex, ///< Used by ConvexShape, all shapes that use the generic convex vs convex collision detection system (box, sphere, capsule, tapered capsule, cylinder, triangle) + Compound, ///< Used by CompoundShape + Decorated, ///< Used by DecoratedShape + Mesh, ///< Used by MeshShape + HeightField, ///< Used by HeightFieldShape + SoftBody, ///< Used by SoftBodyShape + + // User defined shapes + User1, + User2, + User3, + User4, + + Plane, ///< Used by PlaneShape + Empty, ///< Used by EmptyShape +}; + +/// This enumerates all shape types, each shape can return its type through Shape::GetSubType +enum class EShapeSubType : uint8 +{ + // Convex shapes + Sphere, + Box, + Triangle, + Capsule, + TaperedCapsule, + Cylinder, + ConvexHull, + + // Compound shapes + StaticCompound, + MutableCompound, + + // Decorated shapes + RotatedTranslated, + Scaled, + OffsetCenterOfMass, + + // Other shapes + Mesh, + HeightField, + SoftBody, + + // User defined shapes + User1, + User2, + User3, + User4, + User5, + User6, + User7, + User8, + + // User defined convex shapes + UserConvex1, + UserConvex2, + UserConvex3, + UserConvex4, + UserConvex5, + UserConvex6, + UserConvex7, + UserConvex8, + + // Other shapes + Plane, + TaperedCylinder, + Empty, +}; + +// Sets of shape sub types +static constexpr EShapeSubType sAllSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::StaticCompound, EShapeSubType::MutableCompound, EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass, EShapeSubType::Mesh, EShapeSubType::HeightField, EShapeSubType::SoftBody, EShapeSubType::User1, EShapeSubType::User2, EShapeSubType::User3, EShapeSubType::User4, EShapeSubType::User5, EShapeSubType::User6, EShapeSubType::User7, EShapeSubType::User8, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8, EShapeSubType::Plane, EShapeSubType::TaperedCylinder, EShapeSubType::Empty }; +static constexpr EShapeSubType sConvexSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::TaperedCylinder, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 }; +static constexpr EShapeSubType sCompoundSubShapeTypes[] = { EShapeSubType::StaticCompound, EShapeSubType::MutableCompound }; +static constexpr EShapeSubType sDecoratorSubShapeTypes[] = { EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass }; + +/// How many shape types we support +static constexpr uint NumSubShapeTypes = uint(std::size(sAllSubShapeTypes)); + +/// Names of sub shape types +static constexpr const char *sSubShapeTypeNames[] = { "Sphere", "Box", "Triangle", "Capsule", "TaperedCapsule", "Cylinder", "ConvexHull", "StaticCompound", "MutableCompound", "RotatedTranslated", "Scaled", "OffsetCenterOfMass", "Mesh", "HeightField", "SoftBody", "User1", "User2", "User3", "User4", "User5", "User6", "User7", "User8", "UserConvex1", "UserConvex2", "UserConvex3", "UserConvex4", "UserConvex5", "UserConvex6", "UserConvex7", "UserConvex8", "Plane", "TaperedCylinder", "Empty" }; +static_assert(std::size(sSubShapeTypeNames) == NumSubShapeTypes); + +/// Class that can construct shapes and that is serializable using the ObjectStream system. +/// Can be used to store shape data in 'uncooked' form (i.e. in a form that is still human readable and authorable). +/// Once the shape has been created using the Create() function, the data will be moved into the Shape class +/// in a form that is optimized for collision detection. After this, the ShapeSettings object is no longer needed +/// and can be destroyed. Each shape class has a derived class of the ShapeSettings object to store shape specific +/// data. +class JPH_EXPORT ShapeSettings : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ShapeSettings) + +public: + using ShapeResult = Result>; + + /// Create a shape according to the settings specified by this object. + virtual ShapeResult Create() const = 0; + + /// When creating a shape, the result is cached so that calling Create() again will return the same shape. + /// If you make changes to the ShapeSettings you need to call this function to clear the cached result to allow Create() to build a new shape. + void ClearCachedResult() { mCachedResult.Clear(); } + + /// User data (to be used freely by the application) + uint64 mUserData = 0; + +protected: + mutable ShapeResult mCachedResult; +}; + +/// Function table for functions on shapes +class JPH_EXPORT ShapeFunctions +{ +public: + /// Construct a shape + Shape * (*mConstruct)() = nullptr; + + /// Color of the shape when drawing + Color mColor = Color::sBlack; + + /// Get an entry in the registry for a particular sub type + static inline ShapeFunctions & sGet(EShapeSubType inSubType) { return sRegistry[int(inSubType)]; } + +private: + static ShapeFunctions sRegistry[NumSubShapeTypes]; +}; + +/// Base class for all shapes (collision volume of a body). Defines a virtual interface for collision detection. +class JPH_EXPORT Shape : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + using ShapeResult = ShapeSettings::ShapeResult; + + /// Constructor + Shape(EShapeType inType, EShapeSubType inSubType) : mShapeType(inType), mShapeSubType(inSubType) { } + Shape(EShapeType inType, EShapeSubType inSubType, const ShapeSettings &inSettings, [[maybe_unused]] ShapeResult &outResult) : mUserData(inSettings.mUserData), mShapeType(inType), mShapeSubType(inSubType) { } + + /// Destructor + virtual ~Shape() = default; + + /// Get type + inline EShapeType GetType() const { return mShapeType; } + inline EShapeSubType GetSubType() const { return mShapeSubType; } + + /// User data (to be used freely by the application) + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Check if this shape can only be used to create a static body or if it can also be dynamic/kinematic + virtual bool MustBeStatic() const { return false; } + + /// All shapes are centered around their center of mass. This function returns the center of mass position that needs to be applied to transform the shape to where it was created. + virtual Vec3 GetCenterOfMass() const { return Vec3::sZero(); } + + /// Get local bounding box including convex radius, this box is centered around the center of mass rather than the world transform + virtual AABox GetLocalBounds() const = 0; + + /// Get the max number of sub shape ID bits that are needed to be able to address any leaf shape in this shape. Used mainly for checking that it is smaller or equal than SubShapeID::MaxBits. + virtual uint GetSubShapeIDBitsRecursive() const = 0; + + /// Get world space bounds including convex radius. + /// This shape is scaled by inScale in local space first. + /// This function can be overridden to return a closer fitting world space bounding box, by default it will just transform what GetLocalBounds() returns. + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { return GetLocalBounds().Scaled(inScale).Transformed(inCenterOfMassTransform); } + + /// Get world space bounds including convex radius. + AABox GetWorldSpaceBounds(DMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const + { + // Use single precision version using the rotation only + AABox bounds = GetWorldSpaceBounds(inCenterOfMassTransform.GetRotation(), inScale); + + // Apply translation + bounds.Translate(inCenterOfMassTransform.GetTranslation()); + + return bounds; + } + + /// Returns the radius of the biggest sphere that fits entirely in the shape. In case this shape consists of multiple sub shapes, it returns the smallest sphere of the parts. + /// This can be used as a measure of how far the shape can be moved without risking going through geometry. + virtual float GetInnerRadius() const = 0; + + /// Calculate the mass and inertia of this shape + virtual MassProperties GetMassProperties() const = 0; + + /// Get the leaf shape for a particular sub shape ID. + /// @param inSubShapeID The full sub shape ID that indicates the path to the leaf shape + /// @param outRemainder What remains of the sub shape ID after removing the path to the leaf shape (could e.g. refer to a triangle within a MeshShape) + /// @return The shape or null if the sub shape ID is invalid + virtual const Shape * GetLeafShape([[maybe_unused]] const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const; + + /// Get the material assigned to a particular sub shape ID + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const = 0; + + /// Get the surface normal of a particular sub shape ID and point on surface (all vectors are relative to center of mass for this shape). + /// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetSurfaceNormal will only return face normals (and not vertex or edge normals). + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const = 0; + + /// Type definition for a supporting face + using SupportingFace = StaticArray; + + /// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of + /// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided. + /// @param inSubShapeID Sub shape ID of target shape + /// @param inDirection Direction that the face should be facing (in local space to this shape) + /// @param inCenterOfMassTransform Transform to transform outVertices with + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param outVertices Resulting face. The returned face can be empty if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space. + virtual void GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const { /* Nothing */ } + + /// Get the user data of a particular sub shape ID. Corresponds with the value stored in Shape::GetUserData of the leaf shape pointed to by inSubShapeID. + virtual uint64 GetSubShapeUserData([[maybe_unused]] const SubShapeID &inSubShapeID) const { return mUserData; } + + /// Get the direct child sub shape and its transform for a sub shape ID. + /// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape + /// @param inPositionCOM The position of the center of mass of this shape + /// @param inRotation The orientation of this shape + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param outRemainder The remainder of the sub shape ID after removing the sub shape + /// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const; + + /// Gets the properties needed to do buoyancy calculations for a body using this shape + /// @param inCenterOfMassTransform Transform that takes this shape (centered around center of mass) to world space (or a desired other space) + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param inSurface The surface plane of the liquid relative to inCenterOfMassTransform + /// @param outTotalVolume On return this contains the total volume of the shape + /// @param outSubmergedVolume On return this contains the submerged volume of the shape + /// @param outCenterOfBuoyancy On return this contains the world space center of mass of the submerged volume +#ifdef JPH_DEBUG_RENDERER + /// @param inBaseOffset The offset to transform inCenterOfMassTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing. +#endif + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3Arg inBaseOffset +#endif + ) const = 0; + +#ifdef JPH_DEBUG_RENDERER + /// Draw the shape at a particular location with a particular color (debugging purposes) + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const = 0; + + /// Draw the results of the GetSupportFunction with the convex radius added back on to show any errors introduced by this process (only relevant for convex shapes) + virtual void DrawGetSupportFunction([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inDrawSupportDirection) const { /* Only implemented for convex shapes */ } + + /// Draw the results of the GetSupportingFace function to show any errors introduced by this process (only relevant for convex shapes) + virtual void DrawGetSupportingFace([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale) const { /* Only implemented for convex shapes */ } +#endif // JPH_DEBUG_RENDERER + + /// Cast a ray against this shape, returns true if it finds a hit closer than ioHit.mFraction and updates that fraction. Otherwise ioHit is left untouched and the function returns false. + /// Note that the ray should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from RayCast::mOrigin if you want to cast against the shape in the space it was created). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned. + /// If you want the surface normal of the hit use GetSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)). + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const = 0; + + /// Cast a ray against this shape. Allows returning multiple hits through ioCollector. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use GetSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected faction)). + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0; + + /// Check if inPoint is inside this shape. For this tests all shapes are treated as if they were solid. + /// Note that inPoint should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from inPoint if you want to test against the shape in the space it was created). + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit. + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0; + + /// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found. + /// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices. + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param inVertices The vertices of the soft body + /// @param inNumVertices The number of vertices in inVertices + /// @param inCollidingShapeIndex Value to store in CollideSoftBodyVertexIterator::mCollidingShapeIndex when a collision was found + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const = 0; + + /// Collect the leaf transformed shapes of all leaf shapes of this shape. + /// inBox is the world space axis aligned box which leaf shapes should collide with. + /// inPositionCOM/inRotation/inScale describes the transform of this shape. + /// inSubShapeIDCeator represents the current sub shape ID of this shape. + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const; + + /// Transforms this shape and all of its children with inTransform, resulting shape(s) are passed to ioCollector. + /// Note that not all shapes support all transforms (especially true for scaling), the resulting shape will try to match the transform as accurately as possible. + /// @param inCenterOfMassTransform The transform (rotation, translation, scale) that the center of mass of the shape should get + /// @param ioCollector The transformed shapes will be passed to this collector + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const; + + /// Scale this shape. Note that not all shapes support all scales, this will return a shape that matches the scale as accurately as possible. See Shape::IsValidScale for more information. + /// @param inScale The scale to use for this shape (note: this scale is applied to the entire shape in the space it was created, most other functions apply the scale in the space of the leaf shapes and from the center of mass!) + ShapeResult ScaleShape(Vec3Arg inScale) const; + + /// An opaque buffer that holds shape specific information during GetTrianglesStart/Next. + struct alignas(16) GetTrianglesContext { uint8 mData[4288]; }; + + /// This is the minimum amount of triangles that should be requested through GetTrianglesNext. + static constexpr int cGetTrianglesMinTrianglesRequested = 32; + + /// To start iterating over triangles, call this function first. + /// ioContext is a temporary buffer and should remain untouched until the last call to GetTrianglesNext. + /// inBox is the world space bounding in which you want to get the triangles. + /// inPositionCOM/inRotation/inScale describes the transform of this shape. + /// To get the actual triangles call GetTrianglesNext. + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const = 0; + + /// Call this repeatedly to get all triangles in the box. + /// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries. + /// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries. + /// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles. + /// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks). + /// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles. + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const = 0; + + ///@name Binary serialization of the shape. Note that this saves the 'cooked' shape in a format which will not be backwards compatible for newer library versions. + /// In this case you need to recreate the shape from the ShapeSettings object and save it again. The user is expected to call SaveBinaryState followed by SaveMaterialState and SaveSubShapeState. + /// The stream should be stored as is and the material and shape list should be saved using the applications own serialization system (e.g. by assigning an ID to each pointer). + /// When restoring data, call sRestoreFromBinaryState to get the shape and then call RestoreMaterialState and RestoreSubShapeState to restore the pointers to the external objects. + /// Alternatively you can use SaveWithChildren and sRestoreWithChildren to save and restore the shape and all its child shapes and materials in a single stream. + ///@{ + + /// Saves the contents of the shape in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a Shape of the correct type and restores its contents from the binary stream inStream. + static ShapeResult sRestoreFromBinaryState(StreamIn &inStream); + + /// Outputs the material references that this shape has to outMaterials. + virtual void SaveMaterialState([[maybe_unused]] PhysicsMaterialList &outMaterials) const { /* By default do nothing */ } + + /// Restore the material references after calling sRestoreFromBinaryState. Note that the exact same materials need to be provided in the same order as returned by SaveMaterialState. + virtual void RestoreMaterialState([[maybe_unused]] const PhysicsMaterialRefC *inMaterials, [[maybe_unused]] uint inNumMaterials) { JPH_ASSERT(inNumMaterials == 0); } + + /// Outputs the shape references that this shape has to outSubShapes. + virtual void SaveSubShapeState([[maybe_unused]] ShapeList &outSubShapes) const { /* By default do nothing */ } + + /// Restore the shape references after calling sRestoreFromBinaryState. Note that the exact same shapes need to be provided in the same order as returned by SaveSubShapeState. + virtual void RestoreSubShapeState([[maybe_unused]] const ShapeRefC *inSubShapes, [[maybe_unused]] uint inNumShapes) { JPH_ASSERT(inNumShapes == 0); } + + using ShapeToIDMap = StreamUtils::ObjectToIDMap; + using IDToShapeMap = StreamUtils::IDToObjectMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this shape, all its children and its materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + void SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const; + + /// Restore a shape, all its children and materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static ShapeResult sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap); + + ///@} + + /// Class that holds information about the shape that can be used for logging / data collection purposes + struct Stats + { + Stats(size_t inSizeBytes, uint inNumTriangles) : mSizeBytes(inSizeBytes), mNumTriangles(inNumTriangles) { } + + size_t mSizeBytes; ///< Amount of memory used by this shape (size in bytes) + uint mNumTriangles; ///< Number of triangles in this shape (when applicable) + }; + + /// Get stats of this shape. Use for logging / data collection purposes only. Does not add values from child shapes, use GetStatsRecursive for this. + virtual Stats GetStats() const = 0; + + using VisitedShapes = UnorderedSet; + + /// Get the combined stats of this shape and its children. + /// @param ioVisitedShapes is used to track which shapes have already been visited, to avoid calculating the wrong memory size. + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const; + + ///< Volume of this shape (m^3). Note that for compound shapes the volume may be incorrect since child shapes can overlap which is not accounted for. + virtual float GetVolume() const = 0; + + /// Test if inScale is a valid scale for this shape. Some shapes can only be scaled uniformly, compound shapes cannot handle shapes + /// being rotated and scaled (this would cause shearing), scale can never be zero. When the scale is invalid, the function will return false. + /// + /// Here's a list of supported scales: + /// * SphereShape: Scale must be uniform (signs of scale are ignored). + /// * BoxShape: Any scale supported (signs of scale are ignored). + /// * TriangleShape: Any scale supported when convex radius is zero, otherwise only uniform scale supported. + /// * CapsuleShape: Scale must be uniform (signs of scale are ignored). + /// * TaperedCapsuleShape: Scale must be uniform (sign of Y scale can be used to flip the capsule). + /// * CylinderShape: Scale must be uniform in XZ plane, Y can scale independently (signs of scale are ignored). + /// * RotatedTranslatedShape: Scale must not cause shear in the child shape. + /// * CompoundShape: Scale must not cause shear in any of the child shapes. + virtual bool IsValidScale(Vec3Arg inScale) const; + + /// This function will make sure that if you wrap this shape in a ScaledShape that the scale is valid. + /// Note that this involves discarding components of the scale that are invalid, so the resulting scaled shape may be different than the requested scale. + /// Compare the return value of this function with the scale you passed in to detect major inconsistencies and possibly warn the user. + /// @param inScale Local space scale for this shape. + /// @return Scale that can be used to wrap this shape in a ScaledShape. IsValidScale will return true for this scale. + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const; + +#ifdef JPH_DEBUG_RENDERER + /// Debug helper which draws the intersection between water and the shapes, the center of buoyancy and the submerged volume + static bool sDrawSubmergedVolumes; +#endif // JPH_DEBUG_RENDERER + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); + + /// A fallback version of CollidePoint that uses a ray cast and counts the number of hits to determine if the point is inside the shape. Odd number of hits means inside, even number of hits means outside. + static void sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter); + +private: + uint64 mUserData = 0; + EShapeType mShapeType; + EShapeSubType mShapeSubType; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp new file mode 100644 index 000000000000..b44dd77a1e63 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp @@ -0,0 +1,347 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SphereShapeSettings) +{ + JPH_ADD_BASE_CLASS(SphereShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(SphereShapeSettings, mRadius) +} + +ShapeSettings::ShapeResult SphereShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new SphereShape(*this, mCachedResult); + return mCachedResult; +} + +SphereShape::SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Sphere, inSettings, outResult), + mRadius(inSettings.mRadius) +{ + if (inSettings.mRadius <= 0.0f) + { + outResult.SetError("Invalid radius"); + return; + } + + outResult.Set(this); +} + +float SphereShape::GetScaledRadius(Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + return abs_scale.GetX() * mRadius; +} + +AABox SphereShape::GetLocalBounds() const +{ + Vec3 half_extent = Vec3::sReplicate(mRadius); + return AABox(-half_extent, half_extent); +} + +AABox SphereShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + Vec3 half_extent = Vec3::sReplicate(scaled_radius); + AABox bounds(-half_extent, half_extent); + bounds.Translate(inCenterOfMassTransform.GetTranslation()); + return bounds; +} + +class SphereShape::SphereNoConvex final : public Support +{ +public: + explicit SphereNoConvex(float inRadius) : + mRadius(inRadius) + { + static_assert(sizeof(SphereNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(SphereNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return Vec3::sZero(); + } + + virtual float GetConvexRadius() const override + { + return mRadius; + } + +private: + float mRadius; +}; + +class SphereShape::SphereWithConvex final : public Support +{ +public: + explicit SphereWithConvex(float inRadius) : + mRadius(inRadius) + { + static_assert(sizeof(SphereWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(SphereWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + float len = inDirection.Length(); + return len > 0.0f? (mRadius / len) * inDirection : Vec3::sZero(); + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + float mRadius; +}; + +const ConvexShape::Support *SphereShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) SphereWithConvex(scaled_radius); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) SphereNoConvex(scaled_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +MassProperties SphereShape::GetMassProperties() const +{ + MassProperties p; + + // Calculate mass + float r2 = mRadius * mRadius; + p.mMass = (4.0f / 3.0f * JPH_PI) * mRadius * r2 * GetDensity(); + + // Calculate inertia + float inertia = (2.0f / 5.0f) * p.mMass * r2; + p.mInertia = Mat44::sScale(inertia); + + return p; +} + +Vec3 SphereShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + float len = inLocalSurfacePosition.Length(); + return len != 0.0f? inLocalSurfacePosition / len : Vec3::sAxisY(); +} + +void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + float scaled_radius = GetScaledRadius(inScale); + outTotalVolume = (4.0f / 3.0f * JPH_PI) * Cubed(scaled_radius); + + float distance_to_surface = inSurface.SignedDistance(inCenterOfMassTransform.GetTranslation()); + if (distance_to_surface >= scaled_radius) + { + // Above surface + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (distance_to_surface <= -scaled_radius) + { + // Under surface + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Intersecting surface + + // Calculate submerged volume, see: https://en.wikipedia.org/wiki/Spherical_cap + float h = scaled_radius - distance_to_surface; + outSubmergedVolume = (JPH_PI / 3.0f) * Square(h) * (3.0f * scaled_radius - h); + + // Calculate center of buoyancy, see: http://mathworld.wolfram.com/SphericalCap.html (eq 10) + float z = (3.0f / 4.0f) * Square(2.0f * scaled_radius - h) / (3.0f * scaled_radius - h); + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation() - z * inSurface.GetNormal(); // Negative normal since we want the portion under the water + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between sphere and water plane + if (sDrawSubmergedVolumes) + { + Vec3 circle_center = inCenterOfMassTransform.GetTranslation() - distance_to_surface * inSurface.GetNormal(); + float circle_radius = sqrt(Square(scaled_radius) - Square(distance_to_surface)); + DebugRenderer::sInstance->DrawPie(inBaseOffset + circle_center, circle_radius, inSurface.GetNormal(), inSurface.GetNormal().GetNormalizedPerpendicular(), -JPH_PI, JPH_PI, Color::sGreen, DebugRenderer::ECastShadow::Off); + } + #endif // JPH_DEBUG_RENDERER + } + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void SphereShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawUnitSphere(inCenterOfMassTransform * Mat44::sScale(mRadius * inScale.Abs().GetX()), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool SphereShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + float fraction = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void SphereShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float min_fraction, max_fraction; + int num_results = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius, min_fraction, max_fraction); + if (num_results > 0 // Ray should intersect + && max_fraction >= 0.0f // End of ray should be inside sphere + && min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side hit + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = max(0.0f, min_fraction); + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + && num_results > 1 // Ray should have 2 intersections + && max_fraction < ioCollector.GetEarlyOutFraction()) // End of ray should be before early out fraction + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + if (inPoint.LengthSq() <= Square(mRadius)) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + Vec3 center = inCenterOfMassTransform.GetTranslation(); + float radius = GetScaledRadius(inScale); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + // Calculate penetration + Vec3 delta = v.GetPosition() - center; + float distance = delta.Length(); + float penetration = radius - distance; + if (v.UpdatePenetration(penetration)) + { + // Calculate contact point and normal + Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY(); + Vec3 point = center + radius * normal; + + // Store collision + v.SetCollision(Plane::sFromPointAndNormal(point, normal), inCollidingShapeIndex); + } + } +} + +void SphereShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, Vec3::sReplicate(1.0f), Mat44::sScale(scaled_radius), sUnitSphereTriangles.data(), sUnitSphereTriangles.size(), GetMaterial()); +} + +int SphereShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void SphereShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mRadius); +} + +void SphereShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mRadius); +} + +bool SphereShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +Vec3 SphereShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()); +} + +void SphereShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Sphere); + f.mConstruct = []() -> Shape * { return new SphereShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h new file mode 100644 index 000000000000..c305523396e9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h @@ -0,0 +1,125 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a SphereShape +class JPH_EXPORT SphereShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SphereShapeSettings) + +public: + /// Default constructor for deserialization + SphereShapeSettings() = default; + + /// Create a sphere with radius inRadius + SphereShapeSettings(float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mRadius = 0.0f; +}; + +/// A sphere, centered around the origin. +/// Note that it is implemented as a point with convex radius. +class JPH_EXPORT SphereShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SphereShape() : ConvexShape(EShapeSubType::Sphere) { } + SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a sphere with radius inRadius + SphereShape(float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Sphere, inMaterial), mRadius(inRadius) { JPH_ASSERT(inRadius > 0.0f); } + + /// Radius of the sphere + float GetRadius() const { return mRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const override { /* Hit is always a single point, no point in returning anything */ } + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 4.0f / 3.0f * JPH_PI * Cubed(mRadius); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Get the radius of this sphere scaled by inScale + inline float GetScaledRadius(Vec3Arg inScale) const; + + // Classes for GetSupportFunction + class SphereNoConvex; + class SphereWithConvex; + + float mRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp new file mode 100644 index 000000000000..eb9ec06b9e76 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp @@ -0,0 +1,674 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(StaticCompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(StaticCompoundShapeSettings, CompoundShapeSettings) +} + +ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create(TempAllocator &inTempAllocator) const +{ + if (mCachedResult.IsEmpty()) + { + if (mSubShapes.size() == 0) + { + // It's an error to create a compound with no subshapes (the compound cannot encode this) + mCachedResult.SetError("Compound needs a sub shape!"); + } + else if (mSubShapes.size() == 1) + { + // If there's only 1 part we don't need a StaticCompoundShape + const SubShapeSettings &s = mSubShapes[0]; + if (s.mPosition == Vec3::sZero() + && s.mRotation == Quat::sIdentity()) + { + // No rotation or translation, we can use the shape directly + if (s.mShapePtr != nullptr) + mCachedResult.Set(const_cast(s.mShapePtr.GetPtr())); + else if (s.mShape != nullptr) + mCachedResult = s.mShape->Create(); + else + mCachedResult.SetError("Sub shape is null!"); + } + else + { + // We can use a RotatedTranslatedShape instead + RotatedTranslatedShapeSettings settings; + settings.mPosition = s.mPosition; + settings.mRotation = s.mRotation; + settings.mInnerShape = s.mShape; + settings.mInnerShapePtr = s.mShapePtr; + Ref shape = new RotatedTranslatedShape(settings, mCachedResult); + } + } + else + { + // Build a regular compound shape + Ref shape = new StaticCompoundShape(*this, inTempAllocator, mCachedResult); + } + } + return mCachedResult; +} + +ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create() const +{ + TempAllocatorMalloc allocator; + return Create(allocator); +} + +void StaticCompoundShape::Node::SetChildInvalid(uint inIndex) +{ + // Make this an invalid node + mNodeProperties[inIndex] = INVALID_NODE; + + // Make bounding box invalid + mBoundsMinX[inIndex] = HALF_FLT_MAX; + mBoundsMinY[inIndex] = HALF_FLT_MAX; + mBoundsMinZ[inIndex] = HALF_FLT_MAX; + mBoundsMaxX[inIndex] = HALF_FLT_MAX; + mBoundsMaxY[inIndex] = HALF_FLT_MAX; + mBoundsMaxZ[inIndex] = HALF_FLT_MAX; +} + +void StaticCompoundShape::Node::SetChildBounds(uint inIndex, const AABox &inBounds) +{ + mBoundsMinX[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetX()); + mBoundsMinY[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetY()); + mBoundsMinZ[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetZ()); + mBoundsMaxX[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetX()); + mBoundsMaxY[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetY()); + mBoundsMaxZ[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetZ()); +} + +void StaticCompoundShape::sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint) +{ + // Handle trivial case + if (inNumber <= 4) + { + outMidPoint = inNumber / 2; + return; + } + + // Calculate bounding box of box centers + Vec3 center_min = Vec3::sReplicate(FLT_MAX); + Vec3 center_max = Vec3::sReplicate(-FLT_MAX); + for (const AABox *b = ioBounds, *b_end = ioBounds + inNumber; b < b_end; ++b) + { + Vec3 center = b->GetCenter(); + center_min = Vec3::sMin(center_min, center); + center_max = Vec3::sMax(center_max, center); + } + + // Calculate split plane + int dimension = (center_max - center_min).GetHighestComponentIndex(); + float split = 0.5f * (center_min + center_max)[dimension]; + + // Divide bodies + int start = 0, end = inNumber; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && ioBounds[start].GetCenter()[dimension] < split) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && ioBounds[end - 1].GetCenter()[dimension] >= split) + --end; + + if (start < end) + { + // Swap the two elements + std::swap(ioBodyIdx[start], ioBodyIdx[end - 1]); + std::swap(ioBounds[start], ioBounds[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + + if (start > 0 && start < inNumber) + { + // Success! + outMidPoint = start; + } + else + { + // Failed to divide bodies + outMidPoint = inNumber / 2; + } +} + +void StaticCompoundShape::sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit) +{ + uint *body_idx = ioBodyIdx + inBegin; + AABox *node_bounds = ioBounds + inBegin; + int number = inEnd - inBegin; + + // Partition entire range + sPartition(body_idx, node_bounds, number, outSplit[2]); + + // Partition lower half + sPartition(body_idx, node_bounds, outSplit[2], outSplit[1]); + + // Partition upper half + sPartition(body_idx + outSplit[2], node_bounds + outSplit[2], number - outSplit[2], outSplit[3]); + + // Convert to proper range + outSplit[0] = inBegin; + outSplit[1] += inBegin; + outSplit[2] += inBegin; + outSplit[3] += outSplit[2]; + outSplit[4] = inEnd; +} + +StaticCompoundShape::StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult) : + CompoundShape(EShapeSubType::StaticCompound, inSettings, outResult) +{ + // Check that there's at least 1 shape + uint num_subshapes = (uint)inSettings.mSubShapes.size(); + if (num_subshapes < 2) + { + outResult.SetError("Compound needs at least 2 sub shapes, otherwise you should use a RotatedTranslatedShape!"); + return; + } + + // Keep track of total mass to calculate center of mass + float mass = 0.0f; + + mSubShapes.resize(num_subshapes); + for (uint i = 0; i < num_subshapes; ++i) + { + const CompoundShapeSettings::SubShapeSettings &shape = inSettings.mSubShapes[i]; + + // Start constructing the runtime sub shape + SubShape &out_shape = mSubShapes[i]; + if (!out_shape.FromSettings(shape, outResult)) + return; + + // Calculate mass properties of child + MassProperties child = out_shape.mShape->GetMassProperties(); + + // Accumulate center of mass + mass += child.mMass; + mCenterOfMass += out_shape.GetPositionCOM() * child.mMass; + } + + if (mass > 0.0f) + mCenterOfMass /= mass; + + // Cache the inner radius as it can take a while to recursively iterate over all sub shapes + CalculateInnerRadius(); + + // Temporary storage for the bounding boxes of all shapes + uint bounds_size = num_subshapes * sizeof(AABox); + AABox *bounds = (AABox *)inTempAllocator.Allocate(bounds_size); + JPH_SCOPE_EXIT([&inTempAllocator, bounds, bounds_size]{ inTempAllocator.Free(bounds, bounds_size); }); + + // Temporary storage for body indexes (we're shuffling them) + uint body_idx_size = num_subshapes * sizeof(uint); + uint *body_idx = (uint *)inTempAllocator.Allocate(body_idx_size); + JPH_SCOPE_EXIT([&inTempAllocator, body_idx, body_idx_size]{ inTempAllocator.Free(body_idx, body_idx_size); }); + + // Shift all shapes so that the center of mass is now at the origin and calculate bounds + for (uint i = 0; i < num_subshapes; ++i) + { + SubShape &shape = mSubShapes[i]; + + // Shift the shape so it's centered around our center of mass + shape.SetPositionCOM(shape.GetPositionCOM() - mCenterOfMass); + + // Transform the shape's bounds into our local space + Mat44 transform = Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()); + AABox shape_bounds = shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f)); + + // Store bounds and body index for tree construction + bounds[i] = shape_bounds; + body_idx[i] = i; + + // Update our local bounds + mLocalBounds.Encapsulate(shape_bounds); + } + + // The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here + struct StackEntry + { + uint32 mNodeIdx; // Node index of node that is generated + int mChildIdx; // Index of child that we're currently processing + int mSplit[5]; // Indices where the node ID's have been split to form 4 partitions + AABox mBounds; // Bounding box of this node + }; + uint stack_size = num_subshapes * sizeof(StackEntry); + StackEntry *stack = (StackEntry *)inTempAllocator.Allocate(stack_size); + JPH_SCOPE_EXIT([&inTempAllocator, stack, stack_size]{ inTempAllocator.Free(stack, stack_size); }); + int top = 0; + + // Reserve enough space so that every sub shape gets its own leaf node + uint next_node_idx = 0; + mNodes.resize(num_subshapes + (num_subshapes + 2) / 3); // = Sum(num_subshapes * 4^-i) with i = [0, Inf]. + + // Create root node + stack[0].mNodeIdx = next_node_idx++; + stack[0].mChildIdx = -1; + stack[0].mBounds = AABox(); + sPartition4(body_idx, bounds, 0, num_subshapes, stack[0].mSplit); + + for (;;) + { + StackEntry &cur_stack = stack[top]; + + // Next child + cur_stack.mChildIdx++; + + // Check if all children processed + if (cur_stack.mChildIdx >= 4) + { + // Terminate if there's nothing left to pop + if (top <= 0) + break; + + // Add our bounds to our parents bounds + StackEntry &prev_stack = stack[top - 1]; + prev_stack.mBounds.Encapsulate(cur_stack.mBounds); + + // Store this node's properties in the parent node + Node &parent_node = mNodes[prev_stack.mNodeIdx]; + parent_node.mNodeProperties[prev_stack.mChildIdx] = cur_stack.mNodeIdx; + parent_node.SetChildBounds(prev_stack.mChildIdx, cur_stack.mBounds); + + // Pop entry from stack + --top; + } + else + { + // Get low and high index to bodies to process + int low = cur_stack.mSplit[cur_stack.mChildIdx]; + int high = cur_stack.mSplit[cur_stack.mChildIdx + 1]; + int num_bodies = high - low; + + if (num_bodies == 0) + { + // Mark invalid + Node &node = mNodes[cur_stack.mNodeIdx]; + node.SetChildInvalid(cur_stack.mChildIdx); + } + else if (num_bodies == 1) + { + // Get body info + uint child_node_idx = body_idx[low]; + const AABox &child_bounds = bounds[low]; + + // Update node + Node &node = mNodes[cur_stack.mNodeIdx]; + node.mNodeProperties[cur_stack.mChildIdx] = child_node_idx | IS_SUBSHAPE; + node.SetChildBounds(cur_stack.mChildIdx, child_bounds); + + // Encapsulate bounding box in parent + cur_stack.mBounds.Encapsulate(child_bounds); + } + else + { + // Allocate new node + StackEntry &new_stack = stack[++top]; + JPH_ASSERT(top < (int)num_subshapes); + new_stack.mNodeIdx = next_node_idx++; + new_stack.mChildIdx = -1; + new_stack.mBounds = AABox(); + sPartition4(body_idx, bounds, low, high, new_stack.mSplit); + } + } + } + + // Resize nodes to actual size + JPH_ASSERT(next_node_idx <= mNodes.size()); + mNodes.resize(next_node_idx); + mNodes.shrink_to_fit(); + + // Check if we ran out of bits for addressing a node + if (next_node_idx > IS_SUBSHAPE) + { + outResult.SetError("Compound hierarchy has too many nodes"); + return; + } + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +template +inline void StaticCompoundShape::WalkTree(Visitor &ioVisitor) const +{ + uint32 node_stack[cStackSize]; + node_stack[0] = 0; + int top = 0; + do + { + // Test if the node is valid, the node should rarely be invalid but it is possible when testing + // a really large box against the tree that the invalid nodes will intersect with the box + uint32 node_properties = node_stack[top]; + if (node_properties != INVALID_NODE) + { + // Test if node contains triangles + bool is_node = (node_properties & IS_SUBSHAPE) == 0; + if (is_node) + { + const Node &node = mNodes[node_properties]; + + // Unpack bounds + UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMinX[0])); + Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy); + Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle()); + + UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMinZ[0])); + Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx); + Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle()); + + UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMaxY[0])); + Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz); + Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle()); + + // Load properties for 4 children + UVec4 properties = UVec4::sLoadInt4(&node.mNodeProperties[0]); + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, top); + + // Push them onto the stack + JPH_ASSERT(top + 4 < cStackSize); + properties.StoreInt4(&node_stack[top]); + top += num_results; + } + else + { + // Points to a sub shape + uint32 sub_shape_idx = node_properties ^ IS_SUBSHAPE; + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + + ioVisitor.VisitShape(sub_shape, sub_shape_idx); + } + + // Check if we're done + if (ioVisitor.ShouldAbort()) + break; + } + + // Fetch next node until we find one that the visitor wants to see + do + --top; + while (top >= 0 && !ioVisitor.ShouldVisitNode(top)); + } + while (top >= 0); +} + +bool StaticCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitor + { + using CastRayVisitor::CastRayVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit); + WalkTree(visitor); + return visitor.mReturnValue; +} + +void StaticCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitorCollector + { + using CastRayVisitorCollector::CastRayVisitorCollector; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +void StaticCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollidePointVisitor + { + using CollidePointVisitor::CollidePointVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test if point overlaps with box + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +void StaticCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastShapeVisitor + { + using CastShapeVisitor::CastShapeVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + shape->WalkTree(visitor); +} + +void StaticCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollectTransformedShapesVisitor + { + using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +int StaticCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorSC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkTree(visitor); + return visitor.GetNumResults(); +} + +int StaticCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorSC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkTree(visitor); + return visitor.GetNumResults(); +} + +void StaticCompoundShape::sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape1 = static_cast(inShape1); + + struct Visitor : public CollideCompoundVsShapeVisitor + { + using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape1->WalkTree(visitor); +} + +void StaticCompoundShape::sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideShapeVsCompoundVisitor + { + using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape2 = static_cast(inShape2); + + Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape2->WalkTree(visitor); +} + +void StaticCompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + CompoundShape::SaveBinaryState(inStream); + + inStream.Write(mNodes); +} + +void StaticCompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + CompoundShape::RestoreBinaryState(inStream); + + inStream.Read(mNodes); +} + +void StaticCompoundShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::StaticCompound); + f.mConstruct = []() -> Shape * { return new StaticCompoundShape; }; + f.mColor = Color::sOrange; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::StaticCompound, s, sCollideCompoundVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::StaticCompound, sCollideShapeVsCompound); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::StaticCompound, sCastShapeVsCompound); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h new file mode 100644 index 000000000000..ea0a86fd3ba4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h @@ -0,0 +1,139 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; +class TempAllocator; + +/// Class that constructs a StaticCompoundShape. Note that if you only want a compound of 1 shape, use a RotatedTranslatedShape instead. +class JPH_EXPORT StaticCompoundShapeSettings final : public CompoundShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, StaticCompoundShapeSettings) + +public: + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Specialization of Create() function that allows specifying a temp allocator to avoid temporary memory allocations on the heap + ShapeResult Create(TempAllocator &inTempAllocator) const; +}; + +/// A compound shape, sub shapes can be rotated and translated. +/// Sub shapes cannot be modified once the shape is constructed. +/// Shifts all child objects so that they're centered around the center of mass. +class JPH_EXPORT StaticCompoundShape final : public CompoundShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + StaticCompoundShape() : CompoundShape(EShapeSubType::StaticCompound) { } + StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult); + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mNodes.size() * sizeof(Node), 0); } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Visitor for GetIntersectingSubShapes + template + struct GetIntersectingSubShapesVisitorSC : public GetIntersectingSubShapesVisitor + { + using GetIntersectingSubShapesVisitor::GetIntersectingSubShapesVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test if point overlaps with box + UVec4 collides = GetIntersectingSubShapesVisitor::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + /// Sorts ioBodyIdx spatially into 2 groups. Second groups starts at ioBodyIdx + outMidPoint. + /// After the function returns ioBodyIdx and ioBounds will be shuffled + static void sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint); + + /// Sorts ioBodyIdx from inBegin to (but excluding) inEnd spatially into 4 groups. + /// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1] + /// After the function returns ioBodyIdx and ioBounds will be shuffled + static void sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit); + + // Helper functions called by CollisionDispatch + static void sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Maximum size of the stack during tree walk + static constexpr int cStackSize = 128; + + template + JPH_INLINE void WalkTree(Visitor &ioVisitor) const; ///< Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitShape for each sub shape encountered + + /// Bits used in Node::mNodeProperties + enum : uint32 + { + IS_SUBSHAPE = 0x80000000, ///< If this bit is set, the other bits index in mSubShape, otherwise in mNodes + INVALID_NODE = 0x7fffffff, ///< Signifies an invalid node + }; + + /// Node structure + struct Node + { + void SetChildBounds(uint inIndex, const AABox &inBounds); ///< Set bounding box for child inIndex to inBounds + void SetChildInvalid(uint inIndex); ///< Mark the child inIndex as invalid and set its bounding box to invalid + + HalfFloat mBoundsMinX[4]; ///< 4 child bounding boxes + HalfFloat mBoundsMinY[4]; + HalfFloat mBoundsMinZ[4]; + HalfFloat mBoundsMaxX[4]; + HalfFloat mBoundsMaxY[4]; + HalfFloat mBoundsMaxZ[4]; + uint32 mNodeProperties[4]; ///< 4 child node properties + }; + + static_assert(sizeof(Node) == 64, "Node should be 64 bytes"); + + using Nodes = Array; + + Nodes mNodes; ///< Quad tree node structure +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h new file mode 100644 index 000000000000..f1e8d3713bc8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h @@ -0,0 +1,138 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// @brief A sub shape id contains a path to an element (usually a triangle or other primitive type) of a compound shape +/// +/// Each sub shape knows how many bits it needs to encode its ID, so knows how many bits to take from the sub shape ID. +/// +/// For example: +/// * We have a CompoundShape A with 5 child shapes (identify sub shape using 3 bits AAA) +/// * One of its child shapes is CompoundShape B which has 3 child shapes (identify sub shape using 2 bits BB) +/// * One of its child shapes is MeshShape C which contains enough triangles to need 7 bits to identify a triangle (identify sub shape using 7 bits CCCCCCC, note that MeshShape is block based and sorts triangles spatially, you can't assume that the first triangle will have bit pattern 0000000). +/// +/// The bit pattern of the sub shape ID to identify a triangle in MeshShape C will then be CCCCCCCBBAAA. +/// +/// A sub shape ID will become invalid when the structure of the shape changes. For example, if a child shape is removed from a compound shape, the sub shape ID will no longer be valid. +/// This can be a problem when caching sub shape IDs from one frame to the next. See comments at ContactListener::OnContactPersisted / OnContactRemoved. +class SubShapeID +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Underlying storage type + using Type = uint32; + + /// Type that is bigger than the underlying storage type for operations that would otherwise overflow + using BiggerType = uint64; + + static_assert(sizeof(BiggerType) > sizeof(Type), "The calculation below assumes BiggerType is a bigger type than Type"); + + /// How many bits we can store in this ID + static constexpr uint MaxBits = 8 * sizeof(Type); + + /// Constructor + SubShapeID() = default; + + /// Get the next id in the chain of ids (pops parents before children) + Type PopID(uint inBits, SubShapeID &outRemainder) const + { + Type mask_bits = Type((BiggerType(1) << inBits) - 1); + Type fill_bits = Type(BiggerType(cEmpty) << (MaxBits - inBits)); // Fill left side bits with 1 so that if there's no remainder all bits will be set, note that we do this using a BiggerType since on intel 0xffffffff << 32 == 0xffffffff + Type v = mValue & mask_bits; + outRemainder = SubShapeID(Type(BiggerType(mValue) >> inBits) | fill_bits); + return v; + } + + /// Get the value of the path to the sub shape ID + inline Type GetValue() const + { + return mValue; + } + + /// Set the value of the sub shape ID (use with care!) + inline void SetValue(Type inValue) + { + mValue = inValue; + } + + /// Check if there is any bits of subshape ID left. + /// Note that this is not a 100% guarantee as the subshape ID could consist of all 1 bits. Use for asserts only. + inline bool IsEmpty() const + { + return mValue == cEmpty; + } + + /// Check equal + inline bool operator == (const SubShapeID &inRHS) const + { + return mValue == inRHS.mValue; + } + + /// Check not-equal + inline bool operator != (const SubShapeID &inRHS) const + { + return mValue != inRHS.mValue; + } + +private: + friend class SubShapeIDCreator; + + /// An empty SubShapeID has all bits set + static constexpr Type cEmpty = ~Type(0); + + /// Constructor + explicit SubShapeID(const Type &inValue) : mValue(inValue) { } + + /// Adds an id at a particular position in the chain + /// (this should really only be called by the SubShapeIDCreator) + void PushID(Type inValue, uint inFirstBit, uint inBits) + { + // First clear the bits + mValue &= ~(Type((BiggerType(1) << inBits) - 1) << inFirstBit); + + // Then set them to the new value + mValue |= inValue << inFirstBit; + } + + Type mValue = cEmpty; +}; + +/// A sub shape id creator can be used to create a new sub shape id by recursing through the shape +/// hierarchy and pushing new ID's onto the chain +class SubShapeIDCreator +{ +public: + /// Add a new id to the chain of id's and return it + SubShapeIDCreator PushID(uint inValue, uint inBits) const + { + JPH_ASSERT(inValue < (SubShapeID::BiggerType(1) << inBits)); + SubShapeIDCreator copy = *this; + copy.mID.PushID(inValue, mCurrentBit, inBits); + copy.mCurrentBit += inBits; + JPH_ASSERT(copy.mCurrentBit <= SubShapeID::MaxBits); + return copy; + } + + // Get the resulting sub shape ID + const SubShapeID & GetID() const + { + return mID; + } + + /// Get the number of bits that have been written to the sub shape ID so far + inline uint GetNumBitsWritten() const + { + return mCurrentBit; + } + +private: + SubShapeID mID; + uint mCurrentBit = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h new file mode 100644 index 000000000000..f33ca310e70c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A pair of bodies and their sub shape ID's. Can be used as a key in a map to find a contact point. +class SubShapeIDPair +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SubShapeIDPair() = default; + SubShapeIDPair(const BodyID &inBody1ID, const SubShapeID &inSubShapeID1, const BodyID &inBody2ID, const SubShapeID &inSubShapeID2) : mBody1ID(inBody1ID), mSubShapeID1(inSubShapeID1), mBody2ID(inBody2ID), mSubShapeID2(inSubShapeID2) { } + SubShapeIDPair & operator = (const SubShapeIDPair &) = default; + SubShapeIDPair(const SubShapeIDPair &) = default; + + /// Equality operator + inline bool operator == (const SubShapeIDPair &inRHS) const + { + return UVec4::sLoadInt4(reinterpret_cast(this)) == UVec4::sLoadInt4(reinterpret_cast(&inRHS)); + } + + /// Less than operator, used to consistently order contact points for a deterministic simulation + inline bool operator < (const SubShapeIDPair &inRHS) const + { + if (mBody1ID != inRHS.mBody1ID) + return mBody1ID < inRHS.mBody1ID; + + if (mSubShapeID1.GetValue() != inRHS.mSubShapeID1.GetValue()) + return mSubShapeID1.GetValue() < inRHS.mSubShapeID1.GetValue(); + + if (mBody2ID != inRHS.mBody2ID) + return mBody2ID < inRHS.mBody2ID; + + return mSubShapeID2.GetValue() < inRHS.mSubShapeID2.GetValue(); + } + + const BodyID & GetBody1ID() const { return mBody1ID; } + const SubShapeID & GetSubShapeID1() const { return mSubShapeID1; } + const BodyID & GetBody2ID() const { return mBody2ID; } + const SubShapeID & GetSubShapeID2() const { return mSubShapeID2; } + + uint64 GetHash() const { return HashBytes(this, sizeof(SubShapeIDPair)); } + +private: + BodyID mBody1ID; + SubShapeID mSubShapeID1; + BodyID mBody2ID; + SubShapeID mSubShapeID2; +}; + +static_assert(sizeof(SubShapeIDPair) == 16, "Unexpected size"); +static_assert(alignof(SubShapeIDPair) == 4, "Assuming 4 byte aligned"); + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNINGS_STD_BEGIN + +namespace std +{ + /// Declare std::hash for SubShapeIDPair + template <> + struct hash + { + inline size_t operator () (const JPH::SubShapeIDPair &inRHS) const + { + return static_cast(inRHS.GetHash()); + } + }; +} + +JPH_SUPPRESS_WARNINGS_STD_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp new file mode 100644 index 000000000000..a3a9e020d5a5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp @@ -0,0 +1,453 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCapsuleShapeSettings) +{ + JPH_ADD_BASE_CLASS(TaperedCapsuleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mHalfHeightOfTaperedCylinder) + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mTopRadius) + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mBottomRadius) +} + +bool TaperedCapsuleShapeSettings::IsSphere() const +{ + return max(mTopRadius, mBottomRadius) >= 2.0f * mHalfHeightOfTaperedCylinder + min(mTopRadius, mBottomRadius); +} + +ShapeSettings::ShapeResult TaperedCapsuleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + { + Ref shape; + if (IsValid() && IsSphere()) + { + // Determine sphere center and radius + float radius, center; + if (mTopRadius > mBottomRadius) + { + radius = mTopRadius; + center = mHalfHeightOfTaperedCylinder; + } + else + { + radius = mBottomRadius; + center = -mHalfHeightOfTaperedCylinder; + } + + // Create sphere + shape = new SphereShape(radius, mMaterial); + + // Offset sphere if needed + if (abs(center) > 1.0e-6f) + { + RotatedTranslatedShapeSettings rot_trans(Vec3(0, center, 0), Quat::sIdentity(), shape); + mCachedResult = rot_trans.Create(); + } + else + mCachedResult.Set(shape); + } + else + { + // Normal tapered capsule shape + shape = new TaperedCapsuleShape(*this, mCachedResult); + } + } + return mCachedResult; +} + +TaperedCapsuleShapeSettings::TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial) : + ConvexShapeSettings(inMaterial), + mHalfHeightOfTaperedCylinder(inHalfHeightOfTaperedCylinder), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius) +{ +} + +TaperedCapsuleShape::TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::TaperedCapsule, inSettings, outResult), + mTopRadius(inSettings.mTopRadius), + mBottomRadius(inSettings.mBottomRadius) +{ + if (mTopRadius <= 0.0f) + { + outResult.SetError("Invalid top radius"); + return; + } + + if (mBottomRadius <= 0.0f) + { + outResult.SetError("Invalid bottom radius"); + return; + } + + if (inSettings.mHalfHeightOfTaperedCylinder <= 0.0f) + { + outResult.SetError("Invalid height"); + return; + } + + // If this goes off one of the sphere ends falls totally inside the other and you should use a sphere instead + if (inSettings.IsSphere()) + { + outResult.SetError("One sphere embedded in other sphere, please use sphere shape instead"); + return; + } + + // Approximation: The center of mass is exactly half way between the top and bottom cap of the tapered capsule + mTopCenter = inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius); + mBottomCenter = -inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius); + + // Calculate center of mass + mCenterOfMass = Vec3(0, inSettings.mHalfHeightOfTaperedCylinder - mTopCenter, 0); + + // Calculate convex radius + mConvexRadius = min(mTopRadius, mBottomRadius); + JPH_ASSERT(mConvexRadius > 0.0f); + + // Calculate the sin and tan of the angle that the cone surface makes with the Y axis + // See: TaperedCapsuleShape.gliffy + mSinAlpha = (mBottomRadius - mTopRadius) / (mTopCenter - mBottomCenter); + JPH_ASSERT(mSinAlpha >= -1.0f && mSinAlpha <= 1.0f); + mTanAlpha = Tan(ASin(mSinAlpha)); + + outResult.Set(this); +} + +class TaperedCapsuleShape::TaperedCapsule final : public Support +{ +public: + TaperedCapsule(Vec3Arg inTopCenter, Vec3Arg inBottomCenter, float inTopRadius, float inBottomRadius, float inConvexRadius) : + mTopCenter(inTopCenter), + mBottomCenter(inBottomCenter), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(TaperedCapsule) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TaperedCapsule))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Check zero vector + float len = inDirection.Length(); + if (len == 0.0f) + return mTopCenter + Vec3(0, mTopRadius, 0); // Return top + + // Check if the support of the top sphere or bottom sphere is bigger + Vec3 support_top = mTopCenter + (mTopRadius / len) * inDirection; + Vec3 support_bottom = mBottomCenter + (mBottomRadius / len) * inDirection; + if (support_top.Dot(inDirection) > support_bottom.Dot(inDirection)) + return support_top; + else + return support_bottom; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + Vec3 mTopCenter; + Vec3 mBottomCenter; + float mTopRadius; + float mBottomRadius; + float mConvexRadius; +}; + +const ConvexShape::Support *TaperedCapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + float scaled_convex_radius = scale_xz * mConvexRadius; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, scaled_top_radius, scaled_bottom_radius, 0.0f); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + { + // Get radii reduced by convex radius + float tr = scaled_top_radius - scaled_convex_radius; + float br = scaled_bottom_radius - scaled_convex_radius; + JPH_ASSERT(tr >= 0.0f && br >= 0.0f); + JPH_ASSERT(tr == 0.0f || br == 0.0f, "Convex radius should be that of the smallest sphere"); + return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, tr, br, scaled_convex_radius); + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void TaperedCapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Check zero vector + float len = inDirection.Length(); + if (len == 0.0f) + return; + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + + // Get support point for top and bottom sphere in the opposite of inDirection (including convex radius) + Vec3 support_top = scaled_top_center - (scaled_top_radius / len) * inDirection; + Vec3 support_bottom = scaled_bottom_center - (scaled_bottom_radius / len) * inDirection; + + // Get projection on inDirection + float proj_top = support_top.Dot(inDirection); + float proj_bottom = support_bottom.Dot(inDirection); + + // If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point + if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * len) + { + outVertices.push_back(inCenterOfMassTransform * support_top); + outVertices.push_back(inCenterOfMassTransform * support_bottom); + } +} + +MassProperties TaperedCapsuleShape::GetMassProperties() const +{ + AABox box = GetInertiaApproximation(); + + MassProperties p; + p.SetMassAndInertiaOfSolidBox(box.GetSize(), GetDensity()); + return p; +} + +Vec3 TaperedCapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // See: TaperedCapsuleShape.gliffy + // We need to calculate ty and by in order to see if the position is on the top or bottom sphere + // sin(alpha) = by / br = ty / tr + // => by = sin(alpha) * br, ty = sin(alpha) * tr + + if (inLocalSurfacePosition.GetY() > mTopCenter + mSinAlpha * mTopRadius) + return (inLocalSurfacePosition - Vec3(0, mTopCenter, 0)).Normalized(); + else if (inLocalSurfacePosition.GetY() < mBottomCenter + mSinAlpha * mBottomRadius) + return (inLocalSurfacePosition - Vec3(0, mBottomCenter, 0)).Normalized(); + else + { + // Get perpendicular vector to the surface in the xz plane + Vec3 perpendicular = Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX()); + + // We know that the perpendicular has length 1 and that it needs a y component where tan(alpha) = y / 1 in order to align it to the surface + perpendicular.SetY(mTanAlpha); + return perpendicular.Normalized(); + } +} + +AABox TaperedCapsuleShape::GetLocalBounds() const +{ + float max_radius = max(mTopRadius, mBottomRadius); + return AABox(Vec3(-max_radius, mBottomCenter - mBottomRadius, -max_radius), Vec3(max_radius, mTopCenter + mTopRadius, max_radius)); +} + +AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 bottom_extent = Vec3::sReplicate(scale_xz * mBottomRadius); + Vec3 bottom_center = inCenterOfMassTransform * Vec3(0, scale_y * mBottomCenter, 0); + Vec3 top_extent = Vec3::sReplicate(scale_xz * mTopRadius); + Vec3 top_center = inCenterOfMassTransform * Vec3(0, scale_y * mTopCenter, 0); + Vec3 p1 = Vec3::sMin(top_center - top_extent, bottom_center - bottom_extent); + Vec3 p2 = Vec3::sMax(top_center + top_extent, bottom_center + bottom_extent); + return AABox(p1, p2); +} + +void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_y = abs_scale.GetY(); + float scale_xz = abs_scale.GetX(); + Vec3 scale_y_flip(1, Sign(inScale.GetY()), 1); + Vec3 scaled_top_center(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + Vec3 local_pos = scale_y_flip * (inverse_transform * v.GetPosition()); + + Vec3 position, normal; + + // If the vertex is inside the cone starting at the top center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the top sphere + // This corresponds to: Dot(y-axis, (local_pos - top_center) / |local_pos - top_center|) >= cos(PI/2 - alpha) + // <=> (local_pos - top_center).y >= sin(alpha) * |local_pos - top_center| + Vec3 top_center_to_local_pos = local_pos - scaled_top_center; + float top_center_to_local_pos_len = top_center_to_local_pos.Length(); + if (top_center_to_local_pos.GetY() >= mSinAlpha * top_center_to_local_pos_len) + { + // Top sphere + normal = top_center_to_local_pos_len != 0.0f? top_center_to_local_pos / top_center_to_local_pos_len : Vec3::sAxisY(); + position = scaled_top_center + scaled_top_radius * normal; + } + else + { + // If the vertex is outside the cone starting at the bottom center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the bottom sphere + // This corresponds to: Dot(y-axis, (local_pos - bottom_center) / |local_pos - bottom_center|) <= cos(PI/2 - alpha) + // <=> (local_pos - bottom_center).y <= sin(alpha) * |local_pos - bottom_center| + Vec3 bottom_center_to_local_pos = local_pos - scaled_bottom_center; + float bottom_center_to_local_pos_len = bottom_center_to_local_pos.Length(); + if (bottom_center_to_local_pos.GetY() <= mSinAlpha * bottom_center_to_local_pos_len) + { + // Bottom sphere + normal = bottom_center_to_local_pos_len != 0.0f? bottom_center_to_local_pos / bottom_center_to_local_pos_len : -Vec3::sAxisY(); + } + else + { + // Tapered cylinder + normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX()); + normal.SetY(mTanAlpha); + normal = normal.NormalizedOr(Vec3::sAxisX()); + } + position = scaled_bottom_center + scaled_bottom_radius * normal; + } + + Plane plane = Plane::sFromPointAndNormal(position, normal); + float penetration = -plane.SignedDistance(local_pos); + if (v.UpdatePenetration(penetration)) + { + // Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y) + plane.SetNormal(scale_y_flip * plane.GetNormal()); + + // Store collision + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } + } +} + +#ifdef JPH_DEBUG_RENDERER +void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + if (mGeometry == nullptr) + { + SupportBuffer buffer; + const Support *support = GetSupportFunction(ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + mGeometry = inRenderer->CreateTriangleGeometryForConvex([support](Vec3Arg inDirection) { return support->GetSupport(inDirection); }); + } + + // Preserve flip along y axis but make sure we're not inside out + Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale; + RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale); + + AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale); + + float lod_scale_sq = Square(max(mTopRadius, mBottomRadius)); + + Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor; + + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + inRenderer->DrawGeometry(world_transform, bounds, lod_scale_sq, color, mGeometry, DebugRenderer::ECullMode::CullBackFace, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +AABox TaperedCapsuleShape::GetInertiaApproximation() const +{ + // TODO: For now the mass and inertia is that of a box + float avg_radius = 0.5f * (mTopRadius + mBottomRadius); + return AABox(Vec3(-avg_radius, mBottomCenter - mBottomRadius, -avg_radius), Vec3(avg_radius, mTopCenter + mTopRadius, avg_radius)); +} + +void TaperedCapsuleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mTopRadius); + inStream.Write(mBottomRadius); + inStream.Write(mTopCenter); + inStream.Write(mBottomCenter); + inStream.Write(mConvexRadius); + inStream.Write(mSinAlpha); + inStream.Write(mTanAlpha); +} + +void TaperedCapsuleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mTopRadius); + inStream.Read(mBottomRadius); + inStream.Read(mTopCenter); + inStream.Read(mBottomCenter); + inStream.Read(mConvexRadius); + inStream.Read(mSinAlpha); + inStream.Read(mTanAlpha); +} + +bool TaperedCapsuleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +Vec3 TaperedCapsuleShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()); +} + +void TaperedCapsuleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCapsule); + f.mConstruct = []() -> Shape * { return new TaperedCapsuleShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy new file mode 100644 index 000000000000..3e5221b86556 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy @@ -0,0 +1 @@ +{"contentType":"application/gliffy+json","version":"1.1","metadata":{"title":"untitled","revision":0,"exportBorder":false},"embeddedResources":{"index":0,"resources":[]},"stage":{"objects":[{"x":870,"y":406,"rotation":0,"id":62,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":62,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[1.5,5.5],[1.5,-46.196228102251325]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":4,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":410,"y":406,"rotation":0,"id":60,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":60,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,5.5],[0,-49.03668490108288]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":614,"y":385,"rotation":0,"id":58,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":58,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-204.06126531020038,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":626,"y":385,"rotation":0,"id":57,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":57,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[248.0020161208372,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":818,"y":520,"rotation":0,"id":55,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":55,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-48,76]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":830,"y":502,"rotation":0,"id":54,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":54,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[48,-82]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":373,"y":410,"rotation":0,"id":50,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":28,"height":23,"lockAspectRatio":false,"lockShape":false,"order":19,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

tx

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":387,"y":392,"rotation":0,"id":49,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":26,"height":23,"lockAspectRatio":false,"lockShape":false,"order":18,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

ty

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":722,"y":488,"rotation":0,"id":45,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":20,"lockAspectRatio":false,"lockShape":false,"order":16,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

bx

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":440,"y":411.5,"rotation":0,"id":40,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":13,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

α

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":411,"y":414,"rotation":0,"id":34,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":12,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-1,-2.5],[349,180]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":744,"y":613,"rotation":0,"id":32,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":11,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-1.1368683772161603e-13,-201.00995000248122]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":394,"y":436,"rotation":0,"id":30,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":10,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[0,-26.076809620810593]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":607,"y":368.5,"rotation":0,"id":26,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":9,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

h

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":401,"y":412.5,"rotation":0,"id":25,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":8,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

tr

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":808.5,"y":500,"rotation":0,"id":23,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":49,"height":27,"lockAspectRatio":false,"lockShape":false,"order":7,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

br - tr

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":402,"y":416,"rotation":0,"id":21,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":6,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[8,-4.5],[-7,21]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":347,"y":412,"rotation":0,"id":16,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":5,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[523.5,2.5]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":854,"y":433,"rotation":0,"id":14,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":4,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[17.5,-21.5],[-106,184]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":4,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":395,"y":437,"rotation":0,"id":9,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":3,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-52,-25],[695,360]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":395,"y":384,"rotation":0,"id":7,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":2,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-49,26],[703,-359]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":630,"y":169.99999999999997,"rotation":0,"id":4,"uid":"com.gliffy.shape.basic.basic_v1.default.circle","width":483.00000000000006,"height":483.00000000000006,"lockAspectRatio":true,"lockShape":false,"order":1,"graphic":{"type":"Shape","Shape":{"tid":"com.gliffy.stencil.ellipse.basic_v1","strokeWidth":2,"strokeColor":"#333333","fillColor":"#FFFFFF","gradient":false,"dropShadow":false,"state":0,"shadowX":0,"shadowY":0,"opacity":1}},"children":[],"linkMap":[]},{"x":380,"y":381.5,"rotation":0,"id":0,"uid":"com.gliffy.shape.basic.basic_v1.default.circle","width":60,"height":60,"lockAspectRatio":true,"lockShape":false,"order":0,"graphic":{"type":"Shape","Shape":{"tid":"com.gliffy.stencil.ellipse.basic_v1","strokeWidth":2,"strokeColor":"#333333","fillColor":"#FFFFFF","gradient":false,"dropShadow":false,"state":0,"shadowX":0,"shadowY":0,"opacity":1}},"children":[],"linkMap":[]},{"x":728,"y":612,"rotation":0,"id":41,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":14,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

α

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":375,"y":432.5,"rotation":0,"id":42,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":15,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

α

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":753,"y":595.5,"rotation":0,"id":53,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":20,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

tr

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":794,"y":400,"rotation":0,"id":65,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":65,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-48.01041553663117,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":790,"y":393.25,"rotation":0,"id":46,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":14,"lockAspectRatio":false,"lockShape":false,"order":17,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

by

","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":818,"y":400,"rotation":0,"id":66,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":66,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[54.230987451824944,0]],"lockSegments":{}}},"children":null,"linkMap":[]}],"background":"#FFFFFF","width":1113,"height":802,"maxWidth":5000,"maxHeight":5000,"nodeIndex":69,"autoFit":true,"exportBorder":false,"gridOn":true,"snapToGrid":false,"drawingGuidesOn":false,"pageBreaksOn":false,"printGridOn":false,"printPaper":"LETTER","printShrinkToFit":false,"printPortrait":true,"shapeStyles":{"com.gliffy.shape.basic.basic_v1.default":{"fill":"#FFFFFF","stroke":"#333333","strokeWidth":2}},"lineStyles":{"global":{"dashStyle":null,"endArrow":2,"startArrow":0}},"textStyles":{},"themeData":null}} \ No newline at end of file diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h new file mode 100644 index 000000000000..5e53ee136023 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h @@ -0,0 +1,135 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a TaperedCapsuleShape +class JPH_EXPORT TaperedCapsuleShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCapsuleShapeSettings) + +public: + /// Default constructor for deserialization + TaperedCapsuleShapeSettings() = default; + + /// Create a tapered capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and the other at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius + TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial = nullptr); + + /// Check if the settings are valid + bool IsValid() const { return mTopRadius > 0.0f && mBottomRadius > 0.0f && mHalfHeightOfTaperedCylinder >= 0.0f; } + + /// Checks if the settings of this tapered capsule make this shape a sphere + bool IsSphere() const; + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mHalfHeightOfTaperedCylinder = 0.0f; + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; +}; + +/// A capsule with different top and bottom radii +class JPH_EXPORT TaperedCapsuleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TaperedCapsuleShape() : ConvexShape(EShapeSubType::TaperedCapsule) { } + TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult); + + /// Get top radius of the tapered capsule + inline float GetTopRadius() const { return mTopRadius; } + + /// Get bottom radius of the tapered capsule + inline float GetBottomRadius() const { return mBottomRadius; } + + /// Get half height between the top and bottom sphere center + inline float GetHalfHeight() const { return 0.5f * (mTopCenter - mBottomCenter); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return min(mTopRadius, mBottomRadius); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return GetLocalBounds().GetVolume(); } // Volume is approximate! + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class TaperedCapsule; + + /// Returns box that approximates the inertia + AABox GetInertiaApproximation() const; + + Vec3 mCenterOfMass = Vec3::sZero(); + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; + float mTopCenter = 0.0f; + float mBottomCenter = 0.0f; + float mConvexRadius = 0.0f; + float mSinAlpha = 0.0f; + float mTanAlpha = 0.0f; + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp new file mode 100644 index 000000000000..d817644fb44d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp @@ -0,0 +1,687 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +// Approximation of a face of the tapered cylinder +static const Vec3 cTaperedCylinderFace[] = +{ + Vec3(0.0f, 0.0f, 1.0f), + Vec3(0.707106769f, 0.0f, 0.707106769f), + Vec3(1.0f, 0.0f, 0.0f), + Vec3(0.707106769f, 0.0f, -0.707106769f), + Vec3(-0.0f, 0.0f, -1.0f), + Vec3(-0.707106769f, 0.0f, -0.707106769f), + Vec3(-1.0f, 0.0f, 0.0f), + Vec3(-0.707106769f, 0.0f, 0.707106769f) +}; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCylinderShapeSettings) +{ + JPH_ADD_BASE_CLASS(TaperedCylinderShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mHalfHeight) + JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mTopRadius) + JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mBottomRadius) + JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mConvexRadius) +} + +ShapeSettings::ShapeResult TaperedCylinderShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + { + Ref shape; + if (mTopRadius == mBottomRadius) + { + // Convert to regular cylinder + CylinderShapeSettings settings; + settings.mHalfHeight = mHalfHeight; + settings.mRadius = mTopRadius; + settings.mMaterial = mMaterial; + settings.mConvexRadius = mConvexRadius; + new CylinderShape(settings, mCachedResult); + } + else + { + // Normal tapered cylinder shape + new TaperedCylinderShape(*this, mCachedResult); + } + } + return mCachedResult; +} + +TaperedCylinderShapeSettings::TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) : + ConvexShapeSettings(inMaterial), + mHalfHeight(inHalfHeightOfTaperedCylinder), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius), + mConvexRadius(inConvexRadius) +{ +} + +TaperedCylinderShape::TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::TaperedCylinder, inSettings, outResult), + mTopRadius(inSettings.mTopRadius), + mBottomRadius(inSettings.mBottomRadius), + mConvexRadius(inSettings.mConvexRadius) +{ + if (mTopRadius < 0.0f) + { + outResult.SetError("Invalid top radius"); + return; + } + + if (mBottomRadius < 0.0f) + { + outResult.SetError("Invalid bottom radius"); + return; + } + + if (inSettings.mHalfHeight <= 0.0f) + { + outResult.SetError("Invalid height"); + return; + } + + if (inSettings.mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + if (inSettings.mTopRadius < inSettings.mConvexRadius) + { + outResult.SetError("Convex radius must be smaller than convex radius"); + return; + } + + if (inSettings.mBottomRadius < inSettings.mConvexRadius) + { + outResult.SetError("Convex radius must be smaller than bottom radius"); + return; + } + + // Calculate the center of mass (using wxMaxima). + // Radius of cross section for tapered cylinder from 0 to h: + // r(x):=br+x*(tr-br)/h; + // Area: + // area(x):=%pi*r(x)^2; + // Total volume of cylinder: + // volume(h):=integrate(area(x),x,0,h); + // Center of mass: + // com(br,tr,h):=integrate(x*area(x),x,0,h)/volume(h); + // Results: + // ratsimp(com(br,tr,h),br,bt); + // Non-tapered cylinder should have com = 0.5: + // ratsimp(com(r,r,h)); + // Cone with tip at origin and height h should have com = 3/4 h + // ratsimp(com(0,r,h)); + float h = 2.0f * inSettings.mHalfHeight; + float tr = mTopRadius; + float tr2 = Square(tr); + float br = mBottomRadius; + float br2 = Square(br); + float com = h * (3 * tr2 + 2 * br * tr + br2) / (4.0f * (tr2 + br * tr + br2)); + mTop = h - com; + mBottom = -com; + + outResult.Set(this); +} + +class TaperedCylinderShape::TaperedCylinder final : public Support +{ +public: + TaperedCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, float inConvexRadius) : + mTop(inTop), + mBottom(inBottom), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(TaperedCylinder) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TaperedCylinder))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ(); + float o = sqrt(Square(x) + Square(z)); + if (o > 0.0f) + { + Vec3 top_support((mTopRadius * x) / o, mTop, (mTopRadius * z) / o); + Vec3 bottom_support((mBottomRadius * x) / o, mBottom, (mBottomRadius * z) / o); + return inDirection.Dot(top_support) > inDirection.Dot(bottom_support)? top_support : bottom_support; + } + else + { + if (y > 0.0f) + return Vec3(0, mTop, 0); + else + return Vec3(0, mBottom, 0); + } + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + float mTop; + float mBottom; + float mTopRadius; + float mBottomRadius; + float mConvexRadius; +}; + +JPH_INLINE void TaperedCylinderShape::GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const +{ + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); + + outTop = scale_y * mTop; + outBottom = scale_y * mBottom; + outTopRadius = scale_xz * mTopRadius; + outBottomRadius = scale_xz * mBottomRadius; + outConvexRadius = min(abs_scale.GetY(), scale_xz) * mConvexRadius; + + // Negative Y-scale flips the top and bottom + if (outBottom > outTop) + { + std::swap(outTop, outBottom); + std::swap(outTopRadius, outBottomRadius); + } +} + +const ConvexShape::Support *TaperedCylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled tapered cylinder + float top, bottom, top_radius, bottom_radius, convex_radius; + GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius); + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) TaperedCylinder(top, bottom, top_radius, bottom_radius, 0.0f); + + case ESupportMode::ExcludeConvexRadius: + return new (&inBuffer) TaperedCylinder(top - convex_radius, bottom + convex_radius, top_radius - convex_radius, bottom_radius - convex_radius, convex_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +JPH_INLINE static Vec3 sCalculateSideNormalXZ(Vec3Arg inSurfacePosition) +{ + return (Vec3(1, 0, 1) * inSurfacePosition).NormalizedOr(Vec3::sAxisX()); +} + +JPH_INLINE static Vec3 sCalculateSideNormal(Vec3Arg inNormalXZ, float inTop, float inBottom, float inTopRadius, float inBottomRadius) +{ + float tan_alpha = (inBottomRadius - inTopRadius) / (inTop - inBottom); + return Vec3(inNormalXZ.GetX(), tan_alpha, inNormalXZ.GetZ()).Normalized(); +} + +void TaperedCylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled tapered cylinder + float top, bottom, top_radius, bottom_radius, convex_radius; + GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius); + + // Get the normal of the side of the cylinder + Vec3 normal_xz = sCalculateSideNormalXZ(-inDirection); + Vec3 normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius); + + constexpr float cMinRadius = 1.0e-3f; + + // Check if the normal is closer to the side than to the top or bottom + if (abs(normal.Dot(inDirection)) > abs(inDirection.GetY())) + { + // Return the side of the cylinder + outVertices.push_back(inCenterOfMassTransform * (normal_xz * top_radius + Vec3(0, top, 0))); + outVertices.push_back(inCenterOfMassTransform * (normal_xz * bottom_radius + Vec3(0, bottom, 0))); + } + else if (inDirection.GetY() < 0.0f) + { + // Top of the cylinder + if (top_radius > cMinRadius) + { + Vec3 top_3d(0, top, 0); + for (Vec3 v : cTaperedCylinderFace) + outVertices.push_back(inCenterOfMassTransform * (top_radius * v + top_3d)); + } + } + else + { + // Bottom of the cylinder + if (bottom_radius > cMinRadius) + { + Vec3 bottom_3d(0, bottom, 0); + for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v) + outVertices.push_back(inCenterOfMassTransform * (bottom_radius * *v + bottom_3d)); + } + } +} + +MassProperties TaperedCylinderShape::GetMassProperties() const +{ + MassProperties p; + + // Calculate mass + float density = GetDensity(); + p.mMass = GetVolume() * density; + + // Calculate inertia of a tapered cylinder (using wxMaxima) + // Radius: + // r(x):=br+(x-b)*(tr-br)/(t-b); + // Where t=top, b=bottom, tr=top radius, br=bottom radius + // Area of the cross section of the cylinder at x: + // area(x):=%pi*r(x)^2; + // Inertia x slice at x (using inertia of a solid disc, see https://en.wikipedia.org/wiki/List_of_moments_of_inertia, note needs to be multiplied by density): + // dix(x):=area(x)*r(x)^2/4; + // Inertia y slice at y (note needs to be multiplied by density) + // diy(x):=area(x)*r(x)^2/2; + // Volume: + // volume(b,t):=integrate(area(x),x,b,t); + // The constant density (note that we have this through GetDensity() so we'll use that instead): + // density(b,t):=m/volume(b,t); + // Inertia tensor element xx, note that we use the parallel axis theorem to move the inertia: Ixx' = Ixx + m translation^2, also note we multiply by density here: + // Ixx(br,tr,b,t):=integrate(dix(x)+area(x)*x^2,x,b,t)*density(b,t); + // Inertia tensor element yy: + // Iyy(br,tr,b,t):=integrate(diy(x),x,b,t)*density(b,t); + // Note that we can simplfy Ixx by using: + // Ixx_delta(br,tr,b,t):=Ixx(br,tr,b,t)-Iyy(br,tr,b,t)/2; + // For a cylinder this formula matches what is listed on the wiki: + // factor(Ixx(r,r,-h/2,h/2)); + // factor(Iyy(r,r,-h/2,h/2)); + // For a cone with tip at origin too: + // factor(Ixx(0,r,0,h)); + // factor(Iyy(0,r,0,h)); + // Now for the tapered cylinder: + // rat(Ixx(br,tr,b,t),br,bt); + // rat(Iyy(br,tr,b,t),br,bt); + // rat(Ixx_delta(br,tr,b,t),br,bt); + float t = mTop; + float t2 = Square(t); + float t3 = t * t2; + + float b = mBottom; + float b2 = Square(b); + float b3 = b * b2; + + float br = mBottomRadius; + float br2 = Square(br); + float br3 = br * br2; + float br4 = Square(br2); + + float tr = mTopRadius; + float tr2 = Square(tr); + float tr3 = tr * tr2; + float tr4 = Square(tr2); + + float inertia_y = (JPH_PI / 10.0f) * density * (t - b) * (br4 + tr * br3 + tr2 * br2 + tr3 * br + tr4); + float inertia_x_delta = (JPH_PI / 30.0f) * density * ((t3 + 2 * b * t2 + 3 * b2 * t - 6 * b3) * br2 + (3 * t3 + b * t2 - b2 * t - 3 * b3) * tr * br + (6 * t3 - 3 * b * t2 - 2 * b2 * t - b3) * tr2); + float inertia_x = inertia_x_delta + inertia_y / 2; + float inertia_z = inertia_x; + p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z)); + return p; +} + +Vec3 TaperedCylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + constexpr float cEpsilon = 1.0e-5f; + + if (inLocalSurfacePosition.GetY() > mTop - cEpsilon) + return Vec3(0, 1, 0); + else if (inLocalSurfacePosition.GetY() < mBottom + cEpsilon) + return Vec3(0, -1, 0); + else + return sCalculateSideNormal(sCalculateSideNormalXZ(inLocalSurfacePosition), mTop, mBottom, mTopRadius, mBottomRadius); +} + +AABox TaperedCylinderShape::GetLocalBounds() const +{ + float max_radius = max(mTopRadius, mBottomRadius); + return AABox(Vec3(-max_radius, mBottom, -max_radius), Vec3(max_radius, mTop, max_radius)); +} + +void TaperedCylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if the point is in the tapered cylinder + if (inPoint.GetY() >= mBottom && inPoint.GetY() <= mTop // Within height + && Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mBottomRadius + (inPoint.GetY() - mBottom) * (mTopRadius - mBottomRadius) / (mTop - mBottom))) // Within the radius + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled tapered cylinder + float top, bottom, top_radius, bottom_radius, convex_radius; + GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius); + Vec3 top_3d(0, top, 0); + Vec3 bottom_3d(0, bottom, 0); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + Vec3 local_pos = inverse_transform * v.GetPosition(); + + // Calculate penetration into side surface + Vec3 normal_xz = sCalculateSideNormalXZ(local_pos); + Vec3 side_normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius); + Vec3 side_support_top = normal_xz * top_radius + top_3d; + float side_penetration = (side_support_top - local_pos).Dot(side_normal); + + // Calculate penetration into top and bottom plane + float top_penetration = top - local_pos.GetY(); + float bottom_penetration = local_pos.GetY() - bottom; + float min_top_bottom_penetration = min(top_penetration, bottom_penetration); + + Vec3 point, normal; + if (side_penetration < 0.0f || min_top_bottom_penetration < 0.0f) + { + // We're outside the cylinder + // Calculate the closest point on the line segment from bottom to top support point: + // closest_point = bottom + fraction * (top - bottom) / |top - bottom|^2 + Vec3 side_support_bottom = normal_xz * bottom_radius + bottom_3d; + Vec3 bottom_to_top = side_support_top - side_support_bottom; + float fraction = (local_pos - side_support_bottom).Dot(bottom_to_top); + + // Calculate the distance to the axis of the cylinder + float distance_to_axis = normal_xz.Dot(local_pos); + bool inside_top_radius = distance_to_axis <= top_radius; + bool inside_bottom_radius = distance_to_axis <= bottom_radius; + + /* + Regions of tapered cylinder (side view): + + _ B | | + --_ | A | + t-------+ + C / \ + / tapered \ + _ / cylinder \ + --_ / \ + b-----------------+ + D | E | + | | + + t = side_support_top, b = side_support_bottom + Lines between B and C and C and D are at a 90 degree angle to the line between t and b + */ + if (fraction >= bottom_to_top.LengthSq() // Region B: Above the line segment + && !inside_top_radius) // Outside the top radius + { + // Top support point is closest + point = side_support_top; + normal = (local_pos - point).NormalizedOr(Vec3::sAxisY()); + } + else if (fraction < 0.0f // Region D: Below the line segment + && !inside_bottom_radius) // Outside the bottom radius + { + // Bottom support point is closest + point = side_support_bottom; + normal = (local_pos - point).NormalizedOr(Vec3::sAxisY()); + } + else if (top_penetration < 0.0f // Region A: Above the top plane + && inside_top_radius) // Inside the top radius + { + // Top plane is closest + point = top_3d; + normal = Vec3(0, 1, 0); + } + else if (bottom_penetration < 0.0f // Region E: Below the bottom plane + && inside_bottom_radius) // Inside the bottom radius + { + // Bottom plane is closest + point = bottom_3d; + normal = Vec3(0, -1, 0); + } + else // Region C + { + // Side surface is closest + point = side_support_top; + normal = side_normal; + } + } + else if (side_penetration < min_top_bottom_penetration) + { + // Side surface is closest + point = side_support_top; + normal = side_normal; + } + else if (top_penetration < bottom_penetration) + { + // Top plane is closest + point = top_3d; + normal = Vec3(0, 1, 0); + } + else + { + // Bottom plane is closest + point = bottom_3d; + normal = Vec3(0, -1, 0); + } + + // Calculate penetration + Plane plane = Plane::sFromPointAndNormal(point, normal); + float penetration = -plane.SignedDistance(local_pos); + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); + } +} + +class TaperedCylinderShape::TCSGetTrianglesContext +{ +public: + explicit TCSGetTrianglesContext(Mat44Arg inTransform) : mTransform(inTransform) { } + + Mat44 mTransform; + uint mProcessed = 0; // Which elements we processed, bit 0 = top, bit 1 = bottom, bit 2 = side +}; + +void TaperedCylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(TCSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(TCSGetTrianglesContext))); + + // Make sure the scale is not inside out + Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale; + + // Mark top and bottom processed if their radius is too small + TCSGetTrianglesContext *context = new (&ioContext) TCSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale)); + constexpr float cMinRadius = 1.0e-3f; + if (mTopRadius < cMinRadius) + context->mProcessed |= 0b001; + if (mBottomRadius < cMinRadius) + context->mProcessed |= 0b010; +} + +int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + constexpr int cNumVertices = int(std::size(cTaperedCylinderFace)); + + static_assert(cGetTrianglesMinTrianglesRequested >= 2 * cNumVertices); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + TCSGetTrianglesContext &context = (TCSGetTrianglesContext &)ioContext; + + int total_num_triangles = 0; + + // Top cap + Vec3 top_3d(0, mTop, 0); + if ((context.mProcessed & 0b001) == 0) + { + Vec3 v0 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[0]); + Vec3 v1 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[1]); + + for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v) + { + Vec3 v2 = context.mTransform * (top_3d + mTopRadius * *v); + + v0.StoreFloat3(outTriangleVertices++); + v1.StoreFloat3(outTriangleVertices++); + v2.StoreFloat3(outTriangleVertices++); + + v1 = v2; + } + + total_num_triangles = cNumVertices - 2; + context.mProcessed |= 0b001; + } + + // Bottom cap + Vec3 bottom_3d(0, mBottom, 0); + if ((context.mProcessed & 0b010) == 0 + && total_num_triangles + cNumVertices - 2 < inMaxTrianglesRequested) + { + Vec3 v0 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[0]); + Vec3 v1 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[1]); + + for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v) + { + Vec3 v2 = context.mTransform * (bottom_3d + mBottomRadius * *v); + + v0.StoreFloat3(outTriangleVertices++); + v2.StoreFloat3(outTriangleVertices++); + v1.StoreFloat3(outTriangleVertices++); + + v1 = v2; + } + + total_num_triangles += cNumVertices - 2; + context.mProcessed |= 0b010; + } + + // Side + if ((context.mProcessed & 0b100) == 0 + && total_num_triangles + 2 * cNumVertices < inMaxTrianglesRequested) + { + Vec3 v0t = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[cNumVertices - 1]); + Vec3 v0b = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[cNumVertices - 1]); + + for (const Vec3 *v = cTaperedCylinderFace, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v) + { + Vec3 v1t = context.mTransform * (top_3d + mTopRadius * *v); + v0t.StoreFloat3(outTriangleVertices++); + v0b.StoreFloat3(outTriangleVertices++); + v1t.StoreFloat3(outTriangleVertices++); + + Vec3 v1b = context.mTransform * (bottom_3d + mBottomRadius * *v); + v1t.StoreFloat3(outTriangleVertices++); + v0b.StoreFloat3(outTriangleVertices++); + v1b.StoreFloat3(outTriangleVertices++); + + v0t = v1t; + v0b = v1b; + } + + total_num_triangles += 2 * cNumVertices; + context.mProcessed |= 0b100; + } + + // Store materials + if (outMaterials != nullptr) + { + const PhysicsMaterial *material = GetMaterial(); + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = material; + } + + return total_num_triangles; +} + +#ifdef JPH_DEBUG_RENDERER +void TaperedCylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Preserve flip along y axis but make sure we're not inside out + Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale; + RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale); + + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawTaperedCylinder(world_transform, mTop, mBottom, mTopRadius, mBottomRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +void TaperedCylinderShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mTop); + inStream.Write(mBottom); + inStream.Write(mTopRadius); + inStream.Write(mBottomRadius); + inStream.Write(mConvexRadius); +} + +void TaperedCylinderShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mTop); + inStream.Read(mBottom); + inStream.Read(mTopRadius); + inStream.Read(mBottomRadius); + inStream.Read(mConvexRadius); +} + +float TaperedCylinderShape::GetVolume() const +{ + // Volume of a tapered cylinder is: integrate(%pi*(b+x*(t-b)/h)^2,x,0,h) where t is the top radius, b is the bottom radius and h is the height + return (JPH_PI / 3.0f) * (mTop - mBottom) * (Square(mTopRadius) + mTopRadius * mBottomRadius + Square(mBottomRadius)); +} + +bool TaperedCylinderShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs()); +} + +Vec3 TaperedCylinderShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs()); +} + +void TaperedCylinderShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCylinder); + f.mConstruct = []() -> Shape * { return new TaperedCylinderShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h new file mode 100644 index 000000000000..e48ca93d0f8a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h @@ -0,0 +1,132 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a TaperedCylinderShape +class JPH_EXPORT TaperedCylinderShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCylinderShapeSettings) + +public: + /// Default constructor for deserialization + TaperedCylinderShapeSettings() = default; + + /// Create a tapered cylinder centered around the origin with bottom at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and top at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius + TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr); + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mHalfHeight = 0.0f; + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +/// A cylinder with different top and bottom radii +class JPH_EXPORT TaperedCylinderShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TaperedCylinderShape() : ConvexShape(EShapeSubType::TaperedCylinder) { } + TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult); + + /// Get top radius of the tapered cylinder + inline float GetTopRadius() const { return mTopRadius; } + + /// Get bottom radius of the tapered cylinder + inline float GetBottomRadius() const { return mBottomRadius; } + + /// Get convex radius of the tapered cylinder + inline float GetConvexRadius() const { return mConvexRadius; } + + /// Get half height of the tapered cylinder + inline float GetHalfHeight() const { return 0.5f * (mTop - mBottom); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return Vec3(0, -0.5f * (mTop + mBottom), 0); } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return min(mTopRadius, mBottomRadius); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class TaperedCylinder; + + // Class for GetTrianglesTart + class TCSGetTrianglesContext; + + // Scale the cylinder + JPH_INLINE void GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const; + + float mTop = 0.0f; + float mBottom = 0.0f; + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp new file mode 100644 index 000000000000..5f2454bbb5bf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp @@ -0,0 +1,423 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TriangleShapeSettings) +{ + JPH_ADD_BASE_CLASS(TriangleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV1) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV2) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV3) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mConvexRadius) +} + +ShapeSettings::ShapeResult TriangleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new TriangleShape(*this, mCachedResult); + return mCachedResult; +} + +TriangleShape::TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Triangle, inSettings, outResult), + mV1(inSettings.mV1), + mV2(inSettings.mV2), + mV3(inSettings.mV3), + mConvexRadius(inSettings.mConvexRadius) +{ + if (inSettings.mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + outResult.Set(this); +} + +AABox TriangleShape::GetLocalBounds() const +{ + AABox bounds(mV1, mV1); + bounds.Encapsulate(mV2); + bounds.Encapsulate(mV3); + bounds.ExpandBy(Vec3::sReplicate(mConvexRadius)); + return bounds; +} + +AABox TriangleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 v1 = inCenterOfMassTransform * (inScale * mV1); + Vec3 v2 = inCenterOfMassTransform * (inScale * mV2); + Vec3 v3 = inCenterOfMassTransform * (inScale * mV3); + + AABox bounds(v1, v1); + bounds.Encapsulate(v2); + bounds.Encapsulate(v3); + bounds.ExpandBy(inScale * mConvexRadius); + return bounds; +} + +class TriangleShape::TriangleNoConvex final : public Support +{ +public: + TriangleNoConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : + mTriangleSuport(inV1, inV2, inV3) + { + static_assert(sizeof(TriangleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TriangleNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return mTriangleSuport.GetSupport(inDirection); + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + TriangleConvexSupport mTriangleSuport; +}; + +class TriangleShape::TriangleWithConvex final : public Support +{ +public: + TriangleWithConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius) : + mConvexRadius(inConvexRadius), + mTriangleSuport(inV1, inV2, inV3) + { + static_assert(sizeof(TriangleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TriangleWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + Vec3 support = mTriangleSuport.GetSupport(inDirection); + float len = inDirection.Length(); + if (len > 0.0f) + support += (mConvexRadius / len) * inDirection; + return support; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + float mConvexRadius; + TriangleConvexSupport mTriangleSuport; +}; + +const ConvexShape::Support *TriangleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + if (mConvexRadius > 0.0f) + return new (&inBuffer) TriangleWithConvex(inScale * mV1, inScale * mV2, inScale * mV3, mConvexRadius); + [[fallthrough]]; + + case ESupportMode::ExcludeConvexRadius: + return new (&inBuffer) TriangleNoConvex(inScale * mV1, inScale * mV2, inScale * mV3); + } + + JPH_ASSERT(false); + return nullptr; +} + +void TriangleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + outVertices.push_back(transform * mV1); + outVertices.push_back(transform * mV3); + outVertices.push_back(transform * mV2); + } + else + { + outVertices.push_back(transform * mV1); + outVertices.push_back(transform * mV2); + outVertices.push_back(transform * mV3); + } +} + +MassProperties TriangleShape::GetMassProperties() const +{ + // We cannot calculate the volume for a triangle, so we return invalid mass properties. + // If you want your triangle to be dynamic, then you should provide the mass properties yourself when + // creating a Body: + // + // BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + // BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sReplicate(1.0f), 1000.0f); + // + // Note that this makes the triangle shape behave the same as a mesh shape with a single triangle. + // In practice there is very little use for a dynamic triangle shape as back side collisions will be ignored + // so if the triangle falls the wrong way it will sink through the floor. + return MassProperties(); +} + +Vec3 TriangleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 cross = (mV2 - mV1).Cross(mV3 - mV1); + float len = cross.Length(); + return len != 0.0f? cross / len : Vec3::sAxisY(); +} + +void TriangleShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // A triangle has no volume + outTotalVolume = outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); +} + +#ifdef JPH_DEBUG_RENDERER +void TriangleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + RVec3 v1 = inCenterOfMassTransform * (inScale * mV1); + RVec3 v2 = inCenterOfMassTransform * (inScale * mV2); + RVec3 v3 = inCenterOfMassTransform * (inScale * mV3); + + if (ScaleHelpers::IsInsideOut(inScale)) + std::swap(v1, v2); + + if (inDrawWireframe) + inRenderer->DrawWireTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor); + else + inRenderer->DrawTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor); +} +#endif // JPH_DEBUG_RENDERER + +bool TriangleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void TriangleShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Back facing check + if (inRayCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && (mV2 - mV1).Cross(mV3 - mV1).Dot(inRay.mDirection) > 0.0f) + return; + + // Test ray against triangle + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3); + if (fraction < ioCollector.GetEarlyOutFraction()) + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + ioCollector.AddHit(hit); + } +} + +void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Can't be inside a triangle +} + +void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + CollideSoftBodyVerticesVsTriangles collider(inCenterOfMassTransform, inScale); + + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) + { + collider.StartVertex(v); + collider.ProcessTriangle(mV1, mV2, mV3); + collider.FinishVertex(v, inCollidingShapeIndex); + } +} + +void TriangleShape::sCollideConvexVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape2 = static_cast(inShape2); + + CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCollideSphereVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + const SphereShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape2 = static_cast(inShape2); + + CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape = static_cast(inShape); + + CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape = static_cast(inShape); + + CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +class TriangleShape::TSGetTrianglesContext +{ +public: + TSGetTrianglesContext(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : mV1(inV1), mV2(inV2), mV3(inV3) { } + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + + bool mIsDone = false; +}; + +void TriangleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(TSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(TSGetTrianglesContext))); + + Mat44 m = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale); + + new (&ioContext) TSGetTrianglesContext(m * mV1, m * mV2, m * mV3); +} + +int TriangleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 3, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + TSGetTrianglesContext &context = (TSGetTrianglesContext &)ioContext; + + // Only return the triangle the 1st time + if (context.mIsDone) + return 0; + context.mIsDone = true; + + // Store triangle + context.mV1.StoreFloat3(outTriangleVertices); + context.mV2.StoreFloat3(outTriangleVertices + 1); + context.mV3.StoreFloat3(outTriangleVertices + 2); + + // Store material + if (outMaterials != nullptr) + *outMaterials = GetMaterial(); + + return 1; +} + +void TriangleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mV1); + inStream.Write(mV2); + inStream.Write(mV3); + inStream.Write(mConvexRadius); +} + +void TriangleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mV1); + inStream.Read(mV2); + inStream.Read(mV3); + inStream.Read(mConvexRadius); +} + +bool TriangleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && (mConvexRadius == 0.0f || ScaleHelpers::IsUniformScale(inScale.Abs())); +} + +Vec3 TriangleShape::MakeScaleValid(Vec3Arg inScale) const +{ + Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale); + + if (mConvexRadius == 0.0f) + return scale; + + return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()); +} + +void TriangleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Triangle); + f.mConstruct = []() -> Shape * { return new TriangleShape; }; + f.mColor = Color::sGreen; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCollideSphereVsTriangle); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCastSphereVsTriangle); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h new file mode 100644 index 000000000000..b56ccc1f691b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h @@ -0,0 +1,143 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a TriangleShape +class JPH_EXPORT TriangleShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TriangleShapeSettings) + +public: + /// Default constructor for deserialization + TriangleShapeSettings() = default; + + /// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius. + /// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin. + TriangleShapeSettings(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + float mConvexRadius = 0.0f; +}; + +/// A single triangle, not the most efficient way of creating a world filled with triangles but can be used as a query shape for example. +class JPH_EXPORT TriangleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TriangleShape() : ConvexShape(EShapeSubType::Triangle) { } + TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius. + /// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin. + TriangleShape(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Triangle, inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); } + + /// Get the vertices of the triangle + inline Vec3 GetVertex1() const { return mV1; } + inline Vec3 GetVertex2() const { return mV2; } + inline Vec3 GetVertex3() const { return mV3; } + + /// Convex radius + float GetConvexRadius() const { return mConvexRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mConvexRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 1); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // See Shape::MakeScaleValid + virtual Vec3 MakeScaleValid(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideConvexVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideSphereVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Context for GetTrianglesStart/Next + class TSGetTrianglesContext; + + // Classes for GetSupportFunction + class TriangleNoConvex; + class TriangleWithConvex; + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h new file mode 100644 index 000000000000..4271ac06f7e0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h @@ -0,0 +1,173 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a single shape cast (a shape moving along a linear path in 3d space with no rotation) +template +struct ShapeCastT +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection, const AABox &inWorldSpaceBounds) : + mShape(inShape), + mScale(inScale), + mCenterOfMassStart(inCenterOfMassStart), + mDirection(inDirection), + mShapeWorldBounds(inWorldSpaceBounds) + { + } + + /// Constructor + ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection) : + ShapeCastT(inShape, inScale, inCenterOfMassStart, inDirection, inShape->GetWorldSpaceBounds(inCenterOfMassStart, inScale)) + { + } + + /// Construct a shape cast using a world transform for a shape instead of a center of mass transform + static inline ShapeCastType sFromWorldTransform(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inWorldTransform, Vec3Arg inDirection) + { + return ShapeCastType(inShape, inScale, inWorldTransform.PreTranslated(inShape->GetCenterOfMass()), inDirection); + } + + /// Transform this shape cast using inTransform. Multiply transform on the left left hand side. + ShapeCastType PostTransformed(typename Mat::ArgType inTransform) const + { + Mat44 start = inTransform * mCenterOfMassStart; + Vec3 direction = inTransform.Multiply3x3(mDirection); + return { mShape, mScale, start, direction }; + } + + /// Translate this shape cast by inTranslation. + ShapeCastType PostTranslated(typename Vec::ArgType inTranslation) const + { + return { mShape, mScale, mCenterOfMassStart.PostTranslated(inTranslation), mDirection }; + } + + /// Get point with fraction inFraction on ray from mCenterOfMassStart to mCenterOfMassStart + mDirection (0 = start of ray, 1 = end of ray) + inline Vec GetPointOnRay(float inFraction) const + { + return mCenterOfMassStart.GetTranslation() + inFraction * mDirection; + } + + const Shape * mShape; ///< Shape that's being cast (cannot be mesh shape). Note that this structure does not assume ownership over the shape for performance reasons. + const Vec3 mScale; ///< Scale in local space of the shape being cast (scales relative to its center of mass) + const Mat mCenterOfMassStart; ///< Start position and orientation of the center of mass of the shape (construct using sFromWorldTransform if you have a world transform for your shape) + const Vec3 mDirection; ///< Direction and length of the cast (anything beyond this length will not be reported as a hit) + const AABox mShapeWorldBounds; ///< Cached shape's world bounds, calculated in constructor +}; + +struct ShapeCast : public ShapeCastT +{ + using ShapeCastT::ShapeCastT; +}; + +struct RShapeCast : public ShapeCastT +{ + using ShapeCastT::ShapeCastT; + + /// Convert from ShapeCast, converts single to double precision + explicit RShapeCast(const ShapeCast &inCast) : + RShapeCast(inCast.mShape, inCast.mScale, RMat44(inCast.mCenterOfMassStart), inCast.mDirection, inCast.mShapeWorldBounds) + { + } + + /// Convert to ShapeCast, which implies casting from double precision to single precision + explicit operator ShapeCast() const + { + return ShapeCast(mShape, mScale, mCenterOfMassStart.ToMat44(), mDirection, mShapeWorldBounds); + } +}; + +/// Settings to be passed with a shape cast +class ShapeCastSettings : public CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Set the backfacing mode for all shapes + void SetBackFaceMode(EBackFaceMode inMode) { mBackFaceModeTriangles = mBackFaceModeConvex = inMode; } + + /// How backfacing triangles should be treated (should we report moving from back to front for triangle based shapes, e.g. for MeshShape/HeightFieldShape?) + EBackFaceMode mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces; + + /// How backfacing convex objects should be treated (should we report starting inside an object and moving out?) + EBackFaceMode mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + + /// Indicates if we want to shrink the shape by the convex radius and then expand it again. This speeds up collision detection and gives a more accurate normal at the cost of a more 'rounded' shape. + bool mUseShrunkenShapeAndConvexRadius = false; + + /// When true, and the shape is intersecting at the beginning of the cast (fraction = 0) then this will calculate the deepest penetration point (costing additional CPU time) + bool mReturnDeepestPoint = false; +}; + +/// Result of a shape cast test +class ShapeCastResult : public CollideShapeResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor + ShapeCastResult() = default; + + /// Constructor + /// @param inFraction Fraction at which the cast hit + /// @param inContactPoint1 Contact point on shape 1 + /// @param inContactPoint2 Contact point on shape 2 + /// @param inContactNormalOrPenetrationDepth Contact normal pointing from shape 1 to 2 or penetration depth vector when the objects are penetrating (also from 1 to 2) + /// @param inBackFaceHit If this hit was a back face hit + /// @param inSubShapeID1 Sub shape id for shape 1 + /// @param inSubShapeID2 Sub shape id for shape 2 + /// @param inBodyID2 BodyID that was hit + ShapeCastResult(float inFraction, Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inContactNormalOrPenetrationDepth, bool inBackFaceHit, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) : + CollideShapeResult(inContactPoint1, inContactPoint2, inContactNormalOrPenetrationDepth, (inContactPoint2 - inContactPoint1).Length(), inSubShapeID1, inSubShapeID2, inBodyID2), + mFraction(inFraction), + mIsBackFaceHit(inBackFaceHit) + { + } + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction. The fraction and penetration depth are combined in such a way that deeper hits at fraction 0 go first. + inline float GetEarlyOutFraction() const { return mFraction > 0.0f? mFraction : -mPenetrationDepth; } + + /// Reverses the hit result, swapping contact point 1 with contact point 2 etc. + /// @param inWorldSpaceCastDirection Direction of the shape cast in world space + ShapeCastResult Reversed(Vec3Arg inWorldSpaceCastDirection) const + { + // Calculate by how much to shift the contact points + Vec3 delta = mFraction * inWorldSpaceCastDirection; + + ShapeCastResult result; + result.mContactPointOn2 = mContactPointOn1 - delta; + result.mContactPointOn1 = mContactPointOn2 - delta; + result.mPenetrationAxis = -mPenetrationAxis; + result.mPenetrationDepth = mPenetrationDepth; + result.mSubShapeID2 = mSubShapeID1; + result.mSubShapeID1 = mSubShapeID2; + result.mBodyID2 = mBodyID2; + result.mFraction = mFraction; + result.mIsBackFaceHit = mIsBackFaceHit; + + result.mShape2Face.resize(mShape1Face.size()); + for (Face::size_type i = 0; i < mShape1Face.size(); ++i) + result.mShape2Face[i] = mShape1Face[i] - delta; + + result.mShape1Face.resize(mShape2Face.size()); + for (Face::size_type i = 0; i < mShape2Face.size(); ++i) + result.mShape1Face[i] = mShape2Face[i] - delta; + + return result; + } + + float mFraction; ///< This is the fraction where the shape hit the other shape: CenterOfMassOnHit = Start + value * (End - Start) + bool mIsBackFaceHit; ///< True if the shape was hit from the back side +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h new file mode 100644 index 000000000000..3e9f4ff71a8d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Shape; +class SubShapeID; + +/// Filter class +class ShapeFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ShapeFilter() = default; + + /// Filter function to determine if we should collide with a shape. Returns true if the filter passes. + /// This overload is called when the query doesn't have a source shape (e.g. ray cast / collide point) + /// @param inShape2 Shape we're colliding against + /// @param inSubShapeIDOfShape2 The sub shape ID that will lead from the root shape to inShape2 (i.e. the shape of mBodyID2) + virtual bool ShouldCollide([[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const + { + return true; + } + + /// Filter function to determine if two shapes should collide. Returns true if the filter passes. + /// This overload is called when querying a shape vs a shape (e.g. collide object / cast object). + /// It is called at each level of the shape hierarchy, so if you have a compound shape with a box, this function will be called twice. + /// It will not be called on triangles that are part of another shape, i.e a mesh shape will not trigger a callback per triangle. You can filter out individual triangles in the CollisionCollector::AddHit function by their sub shape ID. + /// @param inShape1 1st shape that is colliding + /// @param inSubShapeIDOfShape1 The sub shape ID that will lead from the root shape to inShape1 (i.e. the shape that is used to collide or cast against shape 2) + /// @param inShape2 2nd shape that is colliding + /// @param inSubShapeIDOfShape2 The sub shape ID that will lead from the root shape to inShape2 (i.e. the shape of mBodyID2) + virtual bool ShouldCollide([[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape1, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const + { + return true; + } + + /// Used during NarrowPhase queries and TransformedShape queries. Set to the body ID of inShape2 before calling ShouldCollide. + /// Provides context to the filter to indicate which body is colliding. + mutable BodyID mBodyID2; +}; + +/// Helper class to reverse the order of the shapes in the ShouldCollide function +class ReversedShapeFilter : public ShapeFilter +{ +public: + /// Constructor + explicit ReversedShapeFilter(const ShapeFilter &inFilter) : mFilter(inFilter) + { + mBodyID2 = inFilter.mBodyID2; + } + + virtual bool ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter.ShouldCollide(inShape2, inSubShapeIDOfShape2); + } + + virtual bool ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter.ShouldCollide(inShape2, inSubShapeIDOfShape2, inShape1, inSubShapeIDOfShape1); + } + +private: + const ShapeFilter & mFilter; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h new file mode 100644 index 000000000000..8c0320a83d4d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h @@ -0,0 +1,40 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class Shape; +class SubShapeID; + +/// Filter class used during the simulation (PhysicsSystem::Update) to filter out collisions at shape level +class SimShapeFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~SimShapeFilter() = default; + + /// Filter function to determine if two shapes should collide. Returns true if the filter passes. + /// This overload is called during the simulation (PhysicsSystem::Update) and must be registered with PhysicsSystem::SetSimShapeFilter. + /// It is called at each level of the shape hierarchy, so if you have a compound shape with a box, this function will be called twice. + /// It will not be called on triangles that are part of another shape, i.e a mesh shape will not trigger a callback per triangle. + /// Note that this function is called from multiple threads and must be thread safe. All properties are read only. + /// @param inBody1 1st body that is colliding + /// @param inShape1 1st shape that is colliding + /// @param inSubShapeIDOfShape1 The sub shape ID that will lead from inBody1.GetShape() to inShape1 + /// @param inBody2 2nd body that is colliding + /// @param inShape2 2nd shape that is colliding + /// @param inSubShapeIDOfShape2 The sub shape ID that will lead from inBody2.GetShape() to inShape2 + virtual bool ShouldCollide([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape1, + [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const + { + return true; + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h new file mode 100644 index 000000000000..02e4f0dcfea3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h @@ -0,0 +1,81 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Helper class to forward ShapeFilter calls to a SimShapeFilter +/// INTERNAL CLASS DO NOT USE! +class SimShapeFilterWrapper : public ShapeFilter +{ +public: + /// Constructor + SimShapeFilterWrapper(const SimShapeFilter *inFilter, const Body *inBody1) : + mFilter(inFilter), + mBody1(inBody1) + { + } + + /// Forward to the simulation shape filter + virtual bool ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter->ShouldCollide(*mBody1, inShape1, inSubShapeIDOfShape1, *mBody2, inShape2, inSubShapeIDOfShape2); + } + + /// Forward to the simulation shape filter + virtual bool ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter->ShouldCollide(*mBody1, mBody1->GetShape(), SubShapeID(), *mBody2, inShape2, inSubShapeIDOfShape2); + } + + /// Set the body we're colliding against + void SetBody2(const Body *inBody2) + { + mBody2 = inBody2; + } + +private: + const SimShapeFilter * mFilter; + const Body * mBody1; + const Body * mBody2; +}; + +/// In case we don't have a simulation shape filter, we fall back to using a default shape filter that always returns true +/// INTERNAL CLASS DO NOT USE! +union SimShapeFilterWrapperUnion +{ +public: + /// Constructor + SimShapeFilterWrapperUnion(const SimShapeFilter *inFilter, const Body *inBody1) + { + // Dirty trick: if we don't have a filter, placement new a standard ShapeFilter so that we + // don't have to check for nullptr in the ShouldCollide function + if (inFilter != nullptr) + new (&mSimShapeFilterWrapper) SimShapeFilterWrapper(inFilter, inBody1); + else + new (&mSimShapeFilterWrapper) ShapeFilter(); + } + + /// Destructor + ~SimShapeFilterWrapperUnion() + { + // Doesn't need to be destructed + } + + /// Accessor + SimShapeFilterWrapper & GetSimShapeFilterWrapper() + { + return mSimShapeFilterWrapper; + } + +private: + SimShapeFilterWrapper mSimShapeFilterWrapper; + ShapeFilter mShapeFilter; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h new file mode 100644 index 000000000000..4a073096c264 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// This function will sort values from high to low and only keep the ones that are less than inMaxValue +/// @param inValues Values to be sorted +/// @param inMaxValue Values need to be less than this to keep them +/// @param ioIdentifiers 4 identifiers that will be sorted in the same way as the values +/// @param outValues The values are stored here from high to low +/// @return The number of values that were kept +JPH_INLINE int SortReverseAndStore(Vec4Arg inValues, float inMaxValue, UVec4 &ioIdentifiers, float *outValues) +{ + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + Vec4 values = inValues; + Vec4::sSort4Reverse(values, ioIdentifiers); + + // Count how many results are less than the max value + UVec4 closer = Vec4::sLess(values, Vec4::sReplicate(inMaxValue)); + int num_results = closer.CountTrues(); + + // Shift the values so that only the ones that are less than max are kept + values = values.ReinterpretAsInt().ShiftComponents4Minus(num_results).ReinterpretAsFloat(); + ioIdentifiers = ioIdentifiers.ShiftComponents4Minus(num_results); + + // Store the values + values.StoreFloat4(reinterpret_cast(outValues)); + + return num_results; +} + +/// Shift the elements so that the identifiers that correspond with the trues in inValue come first +/// @param inValue Values to test for true or false +/// @param ioIdentifiers the identifiers that are shifted, on return they are shifted +/// @return The number of trues +JPH_INLINE int CountAndSortTrues(UVec4Arg inValue, UVec4 &ioIdentifiers) +{ + // Sort the hits + ioIdentifiers = UVec4::sSort4True(inValue, ioIdentifiers); + + // Return the amount of hits + return inValue.CountTrues(); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp new file mode 100644 index 000000000000..470387b52a65 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp @@ -0,0 +1,180 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool TransformedShape::CastRay(const RRayCast &inRay, RayCastResult &ioHit) const +{ + if (mShape != nullptr) + { + // Transform the ray to local space, note that this drops precision which is possible because we're in local space now + RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform())); + + // Scale the ray + Vec3 inv_scale = GetShapeScale().Reciprocal(); + ray.mOrigin *= inv_scale; + ray.mDirection *= inv_scale; + + // Cast the ray on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + if (mShape->CastRay(ray, sub_shape_id, ioHit)) + { + // Set body ID on the hit result + ioHit.mBodyID = mBodyID; + + return true; + } + } + + return false; +} + +void TransformedShape::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Transform the ray to local space, note that this drops precision which is possible because we're in local space now + RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform())); + + // Scale the ray + Vec3 inv_scale = GetShapeScale().Reciprocal(); + ray.mOrigin *= inv_scale; + ray.mDirection *= inv_scale; + + // Cast the ray on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + mShape->CastRay(ray, inRayCastSettings, sub_shape_id, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Transform and scale the point to local space + Vec3 point = Vec3(GetInverseCenterOfMassTransform() * inPoint) / GetShapeScale(); + + // Do point collide on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + mShape->CollidePoint(point, sub_shape_id, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator); + Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44(); + Mat44 transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + CollisionDispatch::sCollideShapeVsShape(inShape, mShape, inShapeScale, GetShapeScale(), transform1, transform2, sub_shape_id1, sub_shape_id2, inCollideShapeSettings, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Get the shape cast relative to the base offset and convert it to floats + ShapeCast shape_cast(inShapeCast.PostTranslated(-inBaseOffset)); + + // Get center of mass of object we're casting against relative to the base offset and convert it to floats + Mat44 center_of_mass_transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + + SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator); + CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, mShape, GetShapeScale(), inShapeFilter, center_of_mass_transform2, sub_shape_id1, sub_shape_id2, ioCollector); + } +} + +void TransformedShape::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + struct MyCollector : public TransformedShapeCollector + { + MyCollector(TransformedShapeCollector &ioCollector, RVec3 inShapePositionCOM) : + TransformedShapeCollector(ioCollector), + mCollector(ioCollector), + mShapePositionCOM(inShapePositionCOM) + { + } + + virtual void AddHit(const TransformedShape &inResult) override + { + // Apply the center of mass offset + TransformedShape ts = inResult; + ts.mShapePositionCOM += mShapePositionCOM; + + // Pass hit on to child collector + mCollector.AddHit(ts); + + // Update early out fraction based on child collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + TransformedShapeCollector & mCollector; + RVec3 mShapePositionCOM; + }; + + // Set the context on the collector + ioCollector.SetContext(this); + + // Wrap the collector so we can add the center of mass precision, we do this to avoid losing precision because CollectTransformedShapes uses single precision floats + MyCollector collector(ioCollector, mShapePositionCOM); + + // Take box to local space for the shape + AABox box = inBox; + box.Translate(-mShapePositionCOM); + + mShape->CollectTransformedShapes(box, Vec3::sZero(), mShapeRotation, GetShapeScale(), mSubShapeIDCreator, collector, inShapeFilter); + } +} + +void TransformedShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const +{ + if (mShape != nullptr) + { + // Take box to local space for the shape + AABox box = inBox; + box.Translate(-inBaseOffset); + + mShape->GetTrianglesStart(ioContext, box, Vec3(mShapePositionCOM - inBaseOffset), mShapeRotation, GetShapeScale()); + } +} + +int TransformedShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + if (mShape != nullptr) + return mShape->GetTrianglesNext(ioContext, inMaxTrianglesRequested, outTriangleVertices, outMaterials); + else + return 0; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h new file mode 100644 index 000000000000..887a8ee44114 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h @@ -0,0 +1,194 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RRayCast; +struct RShapeCast; +class CollideShapeSettings; +class RayCastResult; + +/// Temporary data structure that contains a shape and a transform. +/// This structure can be obtained from a body (e.g. after a broad phase query) under lock protection. +/// The lock can then be released and collision detection operations can be safely performed since +/// the class takes a reference on the shape and does not use anything from the body anymore. +class JPH_EXPORT TransformedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TransformedShape() = default; + TransformedShape(RVec3Arg inPositionCOM, QuatArg inRotation, const Shape *inShape, const BodyID &inBodyID, const SubShapeIDCreator &inSubShapeIDCreator = SubShapeIDCreator()) : mShapePositionCOM(inPositionCOM), mShapeRotation(inRotation), mShape(inShape), mBodyID(inBodyID), mSubShapeIDCreator(inSubShapeIDCreator) { } + + /// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits are returned. + /// If you want the surface normal of the hit use GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on this object. + bool CastRay(const RRayCast &inRay, RayCastResult &ioHit) const; + + /// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on this object. + void CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid. + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit + void CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Collide a shape and report any hits to ioCollector + /// @param inShape Shape to test + /// @param inShapeScale Scale in local space of shape + /// @param inCenterOfMassTransform Center of mass transform for the shape + /// @param inCollideShapeSettings Settings + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inShapeFilter Filter that allows you to reject collisions + void CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Cast a shape and report any hits to ioCollector + /// @param inShapeCast The shape cast and its position and direction + /// @param inShapeCastSettings Settings for the shape cast + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM or inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inShapeFilter Filter that allows you to reject collisions + void CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Collect the leaf transformed shapes of all leaf shapes of this shape + /// inBox is the world space axis aligned box which leaf shapes should collide with + void CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Use the context from Shape + using GetTrianglesContext = Shape::GetTrianglesContext; + + /// To start iterating over triangles, call this function first. + /// To get the actual triangles call GetTrianglesNext. + /// @param ioContext A temporary buffer and should remain untouched until the last call to GetTrianglesNext. + /// @param inBox The world space bounding in which you want to get the triangles. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inBox.GetCenter() since floats are most accurate near the origin + void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const; + + /// Call this repeatedly to get all triangles in the box. + /// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries + /// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries + /// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles. + /// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks) + /// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles + int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const; + + /// Get/set the scale of the shape as a Vec3 + inline Vec3 GetShapeScale() const { return Vec3::sLoadFloat3Unsafe(mShapeScale); } + inline void SetShapeScale(Vec3Arg inScale) { inScale.StoreFloat3(&mShapeScale); } + + /// Calculates the transform for this shapes's center of mass (excluding scale) + inline RMat44 GetCenterOfMassTransform() const { return RMat44::sRotationTranslation(mShapeRotation, mShapePositionCOM); } + + /// Calculates the inverse of the transform for this shape's center of mass (excluding scale) + inline RMat44 GetInverseCenterOfMassTransform() const { return RMat44::sInverseRotationTranslation(mShapeRotation, mShapePositionCOM); } + + /// Sets the world transform (including scale) of this transformed shape (not from the center of mass but in the space the shape was created) + inline void SetWorldTransform(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inScale) + { + mShapePositionCOM = inPosition + inRotation * (inScale * mShape->GetCenterOfMass()); + mShapeRotation = inRotation; + SetShapeScale(inScale); + } + + /// Sets the world transform (including scale) of this transformed shape (not from the center of mass but in the space the shape was created) + inline void SetWorldTransform(RMat44Arg inTransform) + { + Vec3 scale; + RMat44 rot_trans = inTransform.Decompose(scale); + SetWorldTransform(rot_trans.GetTranslation(), rot_trans.GetQuaternion(), scale); + } + + /// Calculates the world transform including scale of this shape (not from the center of mass but in the space the shape was created) + inline RMat44 GetWorldTransform() const + { + RMat44 transform = RMat44::sRotation(mShapeRotation).PreScaled(GetShapeScale()); + transform.SetTranslation(mShapePositionCOM - transform.Multiply3x3(mShape->GetCenterOfMass())); + return transform; + } + + /// Get the world space bounding box for this transformed shape + AABox GetWorldSpaceBounds() const { return mShape != nullptr? mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), GetShapeScale()) : AABox(); } + + /// Make inSubShapeID relative to mShape. When mSubShapeIDCreator is not empty, this is needed in order to get the correct path to the sub shape. + inline SubShapeID MakeSubShapeIDRelativeToShape(const SubShapeID &inSubShapeID) const + { + // Take off the sub shape ID part that comes from mSubShapeIDCreator and validate that it is the same + SubShapeID sub_shape_id; + uint num_bits_written = mSubShapeIDCreator.GetNumBitsWritten(); + JPH_IF_ENABLE_ASSERTS(uint32 root_id =) inSubShapeID.PopID(num_bits_written, sub_shape_id); + JPH_ASSERT(root_id == (mSubShapeIDCreator.GetID().GetValue() & ((1 << num_bits_written) - 1))); + return sub_shape_id; + } + + /// Get surface normal of a particular sub shape and its world space surface position on this body. + /// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetWorldSpaceSurfaceNormal will only return face normals (and not vertex or edge normals). + inline Vec3 GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const + { + RMat44 inv_com = GetInverseCenterOfMassTransform(); + Vec3 scale = GetShapeScale(); // See comment at ScaledShape::GetSurfaceNormal for the math behind the scaling of the normal + return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(MakeSubShapeIDRelativeToShape(inSubShapeID), Vec3(inv_com * inPosition) / scale) / scale).Normalized(); + } + + /// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of + /// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided. + /// @param inSubShapeID Sub shape ID of target shape + /// @param inDirection Direction that the face should be facing (in world space) + /// @param inBaseOffset The vertices will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM since floats are most accurate near the origin + /// @param outVertices Resulting face. Note the returned face can have a single point if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space. + void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, RVec3Arg inBaseOffset, Shape::SupportingFace &outVertices) const + { + Mat44 com = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + mShape->GetSupportingFace(MakeSubShapeIDRelativeToShape(inSubShapeID), com.Multiply3x3Transposed(inDirection), GetShapeScale(), com, outVertices); + } + + /// Get material of a particular sub shape + inline const PhysicsMaterial *GetMaterial(const SubShapeID &inSubShapeID) const + { + return mShape->GetMaterial(MakeSubShapeIDRelativeToShape(inSubShapeID)); + } + + /// Get the user data of a particular sub shape + inline uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const + { + return mShape->GetSubShapeUserData(MakeSubShapeIDRelativeToShape(inSubShapeID)); + } + + /// Get the direct child sub shape and its transform for a sub shape ID. + /// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape + /// @param outRemainder The remainder of the sub shape ID after removing the sub shape + /// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid + TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const + { + TransformedShape ts = mShape->GetSubShapeTransformedShape(inSubShapeID, Vec3::sZero(), mShapeRotation, GetShapeScale(), outRemainder); + ts.mShapePositionCOM += mShapePositionCOM; + return ts; + } + + /// Helper function to return the body id from a transformed shape. If the transformed shape is null an invalid body ID will be returned. + inline static BodyID sGetBodyID(const TransformedShape *inTS) { return inTS != nullptr? inTS->mBodyID : BodyID(); } + + RVec3 mShapePositionCOM; ///< Center of mass world position of the shape + Quat mShapeRotation; ///< Rotation of the shape + RefConst mShape; ///< The shape itself + Float3 mShapeScale { 1, 1, 1 }; ///< Not stored as Vec3 to get a nicely packed structure + BodyID mBodyID; ///< Optional body ID from which this shape comes + SubShapeIDCreator mSubShapeIDCreator; ///< Optional sub shape ID creator for the shape (can be used when expanding compound shapes into multiple transformed shapes) +}; + +static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(TransformedShape) == JPH_IF_SINGLE_PRECISION_ELSE(64, 96), "Not properly packed"); +static_assert(alignof(TransformedShape) == JPH_RVECTOR_ALIGNMENT, "Not properly aligned"); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h new file mode 100644 index 000000000000..857eddfb8e17 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class used to calculate the total number of velocity and position steps +class CalculateSolverSteps +{ +public: + /// Constructor + JPH_INLINE explicit CalculateSolverSteps(const PhysicsSettings &inSettings) : mSettings(inSettings) { } + + /// Combine the number of velocity and position steps for this body/constraint with the current values + template + JPH_INLINE void operator () (const Type *inObject) + { + uint num_velocity_steps = inObject->GetNumVelocityStepsOverride(); + mNumVelocitySteps = max(mNumVelocitySteps, num_velocity_steps); + mApplyDefaultVelocity |= num_velocity_steps == 0; + + uint num_position_steps = inObject->GetNumPositionStepsOverride(); + mNumPositionSteps = max(mNumPositionSteps, num_position_steps); + mApplyDefaultPosition |= num_position_steps == 0; + } + + /// Must be called after all bodies/constraints have been processed + JPH_INLINE void Finalize() + { + // If we have a default velocity/position step count, take the max of the default and the overrides + if (mApplyDefaultVelocity) + mNumVelocitySteps = max(mNumVelocitySteps, mSettings.mNumVelocitySteps); + if (mApplyDefaultPosition) + mNumPositionSteps = max(mNumPositionSteps, mSettings.mNumPositionSteps); + } + + /// Get the results of the calculation + JPH_INLINE uint GetNumPositionSteps() const { return mNumPositionSteps; } + JPH_INLINE uint GetNumVelocitySteps() const { return mNumVelocitySteps; } + +private: + const PhysicsSettings & mSettings; + + uint mNumVelocitySteps = 0; + uint mNumPositionSteps = 0; + + bool mApplyDefaultVelocity = false; + bool mApplyDefaultPosition = false; +}; + +/// Dummy class to replace the steps calculator when we don't need the result +class DummyCalculateSolverSteps +{ +public: + template + JPH_INLINE void operator () (const Type *) const + { + /* Nothing to do */ + } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp new file mode 100644 index 000000000000..9889fa643de2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConeConstraintSettings) +{ + JPH_ADD_BASE_CLASS(ConeConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(ConeConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis1) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis2) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mHalfConeAngle) +} + +void ConeConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mTwistAxis1); + inStream.Write(mPoint2); + inStream.Write(mTwistAxis2); + inStream.Write(mHalfConeAngle); +} + +void ConeConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mTwistAxis1); + inStream.Read(mPoint2); + inStream.Read(mTwistAxis2); + inStream.Read(mHalfConeAngle); +} + +TwoBodyConstraint *ConeConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new ConeConstraint(inBody1, inBody2, *this); +} + +ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Store limits + SetHalfConeAngle(inSettings.mHalfConeAngle); + + // Initialize rotation axis to perpendicular of twist axis in case the angle between the twist axis is 0 in the first frame + mWorldSpaceRotationAxis = inSettings.mTwistAxis1.GetNormalizedPerpendicular(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(inSettings.mTwistAxis1); + + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(inSettings.mTwistAxis2); + } + else + { + // Properties already in local space + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mLocalSpaceTwistAxis1 = inSettings.mTwistAxis1; + mLocalSpaceTwistAxis2 = inSettings.mTwistAxis2; + + // If they were in local space, we need to take the initial rotation axis to world space + mWorldSpaceRotationAxis = inBody1.GetRotation() * mWorldSpaceRotationAxis; + } +} + +void ConeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void ConeConstraint::CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Rotation is along the cross product of both twist axis + Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1); + Vec3 twist2 = inRotation2.Multiply3x3(mLocalSpaceTwistAxis2); + + // Calculate dot product between twist axis, if it's smaller than the cone angle we need to correct + mCosTheta = twist1.Dot(twist2); + if (mCosTheta < mCosHalfConeAngle) + { + // Rotation axis is defined by the two twist axis + Vec3 rot_axis = twist2.Cross(twist1); + + // If we can't find a rotation axis because the twist is too small, we'll use last frame's rotation axis + float len = rot_axis.Length(); + if (len > 0.0f) + mWorldSpaceRotationAxis = rot_axis / len; + + mAngleConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceRotationAxis); + } + else + mAngleConstraintPart.Deactivate(); +} + +void ConeConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + CalculateRotationConstraintProperties(rotation1, rotation2); +} + +void ConeConstraint::ResetWarmStart() +{ + mPointConstraintPart.Deactivate(); + mAngleConstraintPart.Deactivate(); +} + +void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mAngleConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool ConeConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + bool rot = false; + if (mAngleConstraintPart.IsActive()) + rot = mAngleConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceRotationAxis, 0, FLT_MAX); + + return pos || rot; +} + +bool ConeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + bool rot = false; + CalculateRotationConstraintProperties(Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation())); + if (mAngleConstraintPart.IsActive()) + rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte); + + return pos || rot; +} + +#ifdef JPH_DEBUG_RENDERER +void ConeConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + RVec3 p1 = transform1 * mLocalSpacePosition1; + RVec3 p2 = transform2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawMarker(p1, Color::sRed, 0.1f); + inRenderer->DrawMarker(p2, Color::sGreen, 0.1f); + + // Draw twist axis + inRenderer->DrawLine(p1, p1 + mDrawConstraintSize * transform1.Multiply3x3(mLocalSpaceTwistAxis1), Color::sRed); + inRenderer->DrawLine(p2, p2 + mDrawConstraintSize * transform2.Multiply3x3(mLocalSpaceTwistAxis2), Color::sGreen); +} + +void ConeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Vec3 twist_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1); + Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1.GetNormalizedPerpendicular()); + + inRenderer->DrawOpenCone(position1, twist_axis1, normal_axis1, ACos(mCosHalfConeAngle), mDrawConstraintSize * mCosHalfConeAngle, Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void ConeConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); + mAngleConstraintPart.SaveState(inStream); + inStream.Write(mWorldSpaceRotationAxis); // When twist is too small, the rotation is used from last frame so we need to store it +} + +void ConeConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); + mAngleConstraintPart.RestoreState(inStream); + inStream.Read(mWorldSpaceRotationAxis); +} + +Ref ConeConstraint::GetConstraintSettings() const +{ + ConeConstraintSettings *settings = new ConeConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mTwistAxis1 = mLocalSpaceTwistAxis1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mTwistAxis2 = mLocalSpaceTwistAxis2; + settings->mHalfConeAngle = ACos(mCosHalfConeAngle); + return settings; +} + +Mat44 ConeConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceTwistAxis1.GetNormalizedPerpendicular(); + Vec3 perp2 = mLocalSpaceTwistAxis1.Cross(perp); + return Mat44(Vec4(mLocalSpaceTwistAxis1, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 ConeConstraint::GetConstraintToBody2Matrix() const +{ + // Note: Incorrect in rotation around the twist axis (the perpendicular does not match that of body 1), + // this should not matter as we're not limiting rotation around the twist axis. + Vec3 perp = mLocalSpaceTwistAxis2.GetNormalizedPerpendicular(); + Vec3 perp2 = mLocalSpaceTwistAxis2.Cross(perp); + return Mat44(Vec4(mLocalSpaceTwistAxis2, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition2, 1)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h new file mode 100644 index 000000000000..1e25ab208678 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h @@ -0,0 +1,133 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Cone constraint settings, used to create a cone constraint +class JPH_EXPORT ConeConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConeConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mTwistAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mTwistAxis2 = Vec3::sAxisX(); + + /// Half of maximum angle between twist axis of body 1 and 2 + float mHalfConeAngle = 0.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A cone constraint constraints 2 bodies to a single point and limits the swing between the twist axis within a cone: +/// +/// t1 . t2 <= cos(theta) +/// +/// Where: +/// +/// t1 = twist axis of body 1. +/// t2 = twist axis of body 2. +/// theta = half cone angle (angle from the principal axis of the cone to the edge). +/// +/// Calculating the Jacobian: +/// +/// Constraint equation: +/// +/// C = t1 . t2 - cos(theta) +/// +/// Derivative: +/// +/// d/dt C = d/dt (t1 . t2) = (d/dt t1) . t2 + t1 . (d/dt t2) = (w1 x t1) . t2 + t1 . (w2 x t2) = (t1 x t2) . w1 + (t2 x t1) . w2 +/// +/// d/dt C = J v = [0, -t2 x t1, 0, t2 x t1] [v1, w1, v2, w2] +/// +/// Where J is the Jacobian. +/// +/// Note that this is the exact same equation as used in AngleConstraintPart if we use t2 x t1 as the world space axis +class JPH_EXPORT ConeConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct cone constraint + ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Cone; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Update maximum angle between body 1 and 2 (see ConeConstraintSettings) + void SetHalfConeAngle(float inHalfConeAngle) { JPH_ASSERT(inHalfConeAngle >= 0.0f && inHalfConeAngle <= JPH_PI); mCosHalfConeAngle = Cos(inHalfConeAngle); } + float GetCosHalfConeAngle() const { return mCosHalfConeAngle; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaRotation() const { return mAngleConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space constraint axis + Vec3 mLocalSpaceTwistAxis1; + Vec3 mLocalSpaceTwistAxis2; + + // Angular limits + float mCosHalfConeAngle; + + // RUN TIME PROPERTIES FOLLOW + + // Axis and angle of rotation between the two bodies + Vec3 mWorldSpaceRotationAxis; + float mCosTheta; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + AngleConstraintPart mAngleConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp new file mode 100644 index 000000000000..b81a8d81a71d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConstraintSettings) +{ + JPH_ADD_BASE_CLASS(ConstraintSettings, SerializableObject) + + JPH_ADD_ATTRIBUTE(ConstraintSettings, mEnabled) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mDrawConstraintSize) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mConstraintPriority) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumVelocityStepsOverride) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumPositionStepsOverride) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mUserData) +} + +void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); + inStream.Write(mEnabled); + inStream.Write(mDrawConstraintSize); + inStream.Write(mConstraintPriority); + inStream.Write(mNumVelocityStepsOverride); + inStream.Write(mNumPositionStepsOverride); +} + +void ConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mEnabled); + inStream.Read(mDrawConstraintSize); + inStream.Read(mConstraintPriority); + inStream.Read(mNumVelocityStepsOverride); + inStream.Read(mNumPositionStepsOverride); +} + +ConstraintSettings::ConstraintResult ConstraintSettings::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &ConstraintSettings::RestoreBinaryState); +} + +void Constraint::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mEnabled); +} + +void Constraint::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mEnabled); +} + +void Constraint::ToConstraintSettings(ConstraintSettings &outSettings) const +{ + outSettings.mEnabled = mEnabled; + outSettings.mConstraintPriority = mConstraintPriority; + outSettings.mNumVelocityStepsOverride = mNumVelocityStepsOverride; + outSettings.mNumPositionStepsOverride = mNumPositionStepsOverride; + outSettings.mUserData = mUserData; +#ifdef JPH_DEBUG_RENDERER + outSettings.mDrawConstraintSize = mDrawConstraintSize; +#endif // JPH_DEBUG_RENDERER +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h new file mode 100644 index 000000000000..5127f3989577 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h @@ -0,0 +1,238 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class BodyID; +class IslandBuilder; +class LargeIslandSplitter; +class BodyManager; +class StateRecorder; +class StreamIn; +class StreamOut; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Enum to identify constraint type +enum class EConstraintType +{ + Constraint, + TwoBodyConstraint, +}; + +/// Enum to identify constraint sub type +enum class EConstraintSubType +{ + Fixed, + Point, + Hinge, + Slider, + Distance, + Cone, + SwingTwist, + SixDOF, + Path, + Vehicle, + RackAndPinion, + Gear, + Pulley, + + /// User defined constraint types start here + User1, + User2, + User3, + User4 +}; + +/// Certain constraints support setting them up in local or world space. This governs what is used. +enum class EConstraintSpace +{ + LocalToBodyCOM, ///< All constraint properties are specified in local space to center of mass of the bodies that are being constrained (so e.g. 'constraint position 1' will be local to body 1 COM, 'constraint position 2' will be local to body 2 COM). Note that this means you need to subtract Shape::GetCenterOfMass() from positions! + WorldSpace, ///< All constraint properties are specified in world space +}; + +/// Class used to store the configuration of a constraint. Allows run-time creation of constraints. +class JPH_EXPORT ConstraintSettings : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConstraintSettings) + +public: + using ConstraintResult = Result>; + + /// Saves the contents of the constraint settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a constraint of the correct type and restores its contents from the binary stream inStream. + static ConstraintResult sRestoreFromBinaryState(StreamIn &inStream); + + /// If this constraint is enabled initially. Use Constraint::SetEnabled to toggle after creation. + bool mEnabled = true; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + /// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering. + uint32 mConstraintPriority = 0; + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumVelocityStepsOverride = 0; + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumPositionStepsOverride = 0; + + /// Size of constraint when drawing it through the debug renderer + float mDrawConstraintSize = 1.0f; + + /// User data value (can be used by application) + uint64 mUserData = 0; + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +/// Base class for all physics constraints. A constraint removes one or more degrees of freedom for a rigid body. +class JPH_EXPORT Constraint : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Constraint(const ConstraintSettings &inSettings) : +#ifdef JPH_DEBUG_RENDERER + mDrawConstraintSize(inSettings.mDrawConstraintSize), +#endif // JPH_DEBUG_RENDERER + mConstraintPriority(inSettings.mConstraintPriority), + mNumVelocityStepsOverride(uint8(inSettings.mNumVelocityStepsOverride)), + mNumPositionStepsOverride(uint8(inSettings.mNumPositionStepsOverride)), + mEnabled(inSettings.mEnabled), + mUserData(inSettings.mUserData) + { + JPH_ASSERT(inSettings.mNumVelocityStepsOverride < 256); + JPH_ASSERT(inSettings.mNumPositionStepsOverride < 256); + } + + /// Virtual destructor + virtual ~Constraint() = default; + + /// Get the type of a constraint + virtual EConstraintType GetType() const { return EConstraintType::Constraint; } + + /// Get the sub type of a constraint + virtual EConstraintSubType GetSubType() const = 0; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + /// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering. + uint32 GetConstraintPriority() const { return mConstraintPriority; } + void SetConstraintPriority(uint32 inPriority) { mConstraintPriority = inPriority; } + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumVelocityStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); } + uint GetNumVelocityStepsOverride() const { return mNumVelocityStepsOverride; } + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumPositionStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); } + uint GetNumPositionStepsOverride() const { return mNumPositionStepsOverride; } + + /// Enable / disable this constraint. This can e.g. be used to implement a breakable constraint by detecting that the constraint impulse + /// (see e.g. PointConstraint::GetTotalLambdaPosition) went over a certain limit and then disabling the constraint. + /// Note that although a disabled constraint will not affect the simulation in any way anymore, it does incur some processing overhead. + /// Alternatively you can remove a constraint from the constraint manager (which may be more costly if you want to disable the constraint for a short while). + void SetEnabled(bool inEnabled) { mEnabled = inEnabled; } + + /// Test if a constraint is enabled. + bool GetEnabled() const { return mEnabled; } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Notify the constraint that the shape of a body has changed and that its center of mass has moved by inDeltaCOM. + /// Bodies don't know which constraints are connected to them so the user is responsible for notifying the relevant constraints when a body changes. + /// @param inBodyID ID of the body that has changed + /// @param inDeltaCOM The delta of the center of mass of the body (shape->GetCenterOfMass() - shape_before_change->GetCenterOfMass()) + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) = 0; + + /// Notify the system that the configuration of the bodies and/or constraint has changed enough so that the warm start impulses should not be applied the next frame. + /// You can use this function for example when repositioning a ragdoll through Ragdoll::SetPose in such a way that the orientation of the bodies completely changes so that + /// the previous frame impulses are no longer a good approximation of what the impulses will be in the next frame. Calling this function when there are no big changes + /// will result in the constraints being much 'softer' than usual so they are more easily violated (e.g. a long chain of bodies might sag a bit if you call this every frame). + virtual void ResetWarmStart() = 0; + + ///@name Solver interface + ///@{ + virtual bool IsActive() const { return mEnabled; } + virtual void SetupVelocityConstraint(float inDeltaTime) = 0; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) = 0; + virtual bool SolveVelocityConstraint(float inDeltaTime) = 0; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) = 0; + ///@} + + /// Link bodies that are connected by this constraint in the island builder + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) = 0; + + /// Link bodies that are connected by this constraint in the same split. Returns the split index. + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const = 0; + +#ifdef JPH_DEBUG_RENDERER + // Drawing interface + virtual void DrawConstraint(DebugRenderer *inRenderer) const = 0; + virtual void DrawConstraintLimits([[maybe_unused]] DebugRenderer *inRenderer) const { } + virtual void DrawConstraintReferenceFrame([[maybe_unused]] DebugRenderer *inRenderer) const { } + + /// Size of constraint when drawing it through the debug renderer + float GetDrawConstraintSize() const { return mDrawConstraintSize; } + void SetDrawConstraintSize(float inSize) { mDrawConstraintSize = inSize; } +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + virtual void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + virtual void RestoreState(StateRecorder &inStream); + + /// Debug function to convert a constraint to its settings, note that this will not save to which bodies the constraint is connected to + virtual Ref GetConstraintSettings() const = 0; + +protected: + /// Helper function to copy settings back to constraint settings for this base class + void ToConstraintSettings(ConstraintSettings &outSettings) const; + +#ifdef JPH_DEBUG_RENDERER + /// Size of constraint when drawing it through the debug renderer + float mDrawConstraintSize; +#endif // JPH_DEBUG_RENDERER + +private: + friend class ConstraintManager; + + /// Index that indicates this constraint is not in the constraint manager + static constexpr uint32 cInvalidConstraintIndex = 0xffffffff; + + /// Index in the mConstraints list of the ConstraintManager for easy finding + uint32 mConstraintIndex = cInvalidConstraintIndex; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + uint32 mConstraintPriority = 0; + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumVelocityStepsOverride = 0; + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumPositionStepsOverride = 0; + + /// If this constraint is currently enabled + bool mEnabled = true; + + /// User data value (can be used by application) + uint64 mUserData; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp new file mode 100644 index 000000000000..509bddbd9720 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp @@ -0,0 +1,289 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void ConstraintManager::Add(Constraint **inConstraints, int inNumber) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + mConstraints.reserve(mConstraints.size() + inNumber); + + for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c) + { + Constraint *constraint = *c; + + // Assume this constraint has not been added yet + JPH_ASSERT(constraint->mConstraintIndex == Constraint::cInvalidConstraintIndex); + + // Add to the list + constraint->mConstraintIndex = uint32(mConstraints.size()); + mConstraints.push_back(constraint); + } +} + +void ConstraintManager::Remove(Constraint **inConstraints, int inNumber) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c) + { + Constraint *constraint = *c; + + // Reset constraint index for this constraint + uint32 this_constraint_idx = constraint->mConstraintIndex; + constraint->mConstraintIndex = Constraint::cInvalidConstraintIndex; + JPH_ASSERT(this_constraint_idx != Constraint::cInvalidConstraintIndex); + + // Check if this constraint is somewhere in the middle of the constraints, in this case we need to move the last constraint to this position + uint32 last_constraint_idx = uint32(mConstraints.size() - 1); + if (this_constraint_idx < last_constraint_idx) + { + Constraint *last_constraint = mConstraints[last_constraint_idx]; + last_constraint->mConstraintIndex = this_constraint_idx; + mConstraints[this_constraint_idx] = last_constraint; + } + + // Pop last constraint + mConstraints.pop_back(); + } +} + +Constraints ConstraintManager::GetConstraints() const +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + Constraints copy = mConstraints; + return copy; +} + +void ConstraintManager::GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inEndConstraintIdx <= mConstraints.size()); + + uint32 num_active_constraints = 0; + for (uint32 constraint_idx = inStartConstraintIdx; constraint_idx < inEndConstraintIdx; ++constraint_idx) + { + Constraint *c = mConstraints[constraint_idx]; + JPH_ASSERT(c->mConstraintIndex == constraint_idx); + if (c->IsActive()) + { + *(outActiveConstraints++) = c; + num_active_constraints++; + } + } + + outNumActiveConstraints = num_active_constraints; +} + +void ConstraintManager::sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + JPH_PROFILE_FUNCTION(); + + for (uint32 constraint_idx = 0; constraint_idx < inNumActiveConstraints; ++constraint_idx) + { + Constraint *c = inActiveConstraints[constraint_idx]; + c->BuildIslands(constraint_idx, ioBuilder, inBodyManager); + } +} + +void ConstraintManager::sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [inActiveConstraints](uint32 inLHS, uint32 inRHS) { + const Constraint *lhs = inActiveConstraints[inLHS]; + const Constraint *rhs = inActiveConstraints[inRHS]; + + if (lhs->GetConstraintPriority() != rhs->GetConstraintPriority()) + return lhs->GetConstraintPriority() < rhs->GetConstraintPriority(); + + return lhs->mConstraintIndex < rhs->mConstraintIndex; + }); +} + +void ConstraintManager::sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + for (Constraint **c = inActiveConstraints, **c_end = inActiveConstraints + inNumActiveConstraints; c < c_end; ++c) + (*c)->SetupVelocityConstraint(inDeltaTime); +} + +template +void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback) +{ + JPH_PROFILE_FUNCTION(); + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + ioCallback(c); + c->WarmStartVelocityConstraint(inWarmStartImpulseRatio); + } +} + +// Specialize for the two constraint callback types +template void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback); +template void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback); + +bool ConstraintManager::sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + any_impulse_applied |= c->SolveVelocityConstraint(inDeltaTime); + } + + return any_impulse_applied; +} + +bool ConstraintManager::sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + any_impulse_applied |= c->SolvePositionConstraint(inDeltaTime, inBaumgarte); + } + + return any_impulse_applied; +} + +#ifdef JPH_DEBUG_RENDERER +void ConstraintManager::DrawConstraints(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraint(inRenderer); +} + +void ConstraintManager::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraintLimits(inRenderer); +} + +void ConstraintManager::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraintReferenceFrame(inRenderer); +} +#endif // JPH_DEBUG_RENDERER + +void ConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + // Write state of constraints + if (inFilter != nullptr) + { + // Determine which constraints to save + Array constraints; + constraints.reserve(mConstraints.size()); + for (const Ref &c : mConstraints) + if (inFilter->ShouldSaveConstraint(*c)) + constraints.push_back(c); + + // Save them + uint32 num_constraints = (uint32)constraints.size(); + inStream.Write(num_constraints); + for (const Constraint *c : constraints) + { + inStream.Write(c->mConstraintIndex); + c->SaveState(inStream); + } + } + else + { + // Save all constraints + uint32 num_constraints = (uint32)mConstraints.size(); + inStream.Write(num_constraints); + for (const Ref &c : mConstraints) + { + inStream.Write(c->mConstraintIndex); + c->SaveState(inStream); + } + } +} + +bool ConstraintManager::RestoreState(StateRecorder &inStream) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + if (inStream.IsValidating()) + { + // Read state of constraints + uint32 num_constraints = (uint32)mConstraints.size(); // Initialize to current value for validation + inStream.Read(num_constraints); + if (num_constraints != mConstraints.size()) + { + JPH_ASSERT(false, "Cannot handle adding/removing constraints"); + return false; + } + for (const Ref &c : mConstraints) + { + uint32 constraint_index = c->mConstraintIndex; + inStream.Read(constraint_index); + if (constraint_index != c->mConstraintIndex) + { + JPH_ASSERT(false, "Unexpected constraint index"); + return false; + } + c->RestoreState(inStream); + } + } + else + { + // Not validating, use more flexible reading, read number of constraints + uint32 num_constraints = 0; + inStream.Read(num_constraints); + + for (uint32 idx = 0; idx < num_constraints; ++idx) + { + uint32 constraint_index; + inStream.Read(constraint_index); + if (mConstraints.size() <= constraint_index) + { + JPH_ASSERT(false, "Restoring state for non-existing constraint"); + return false; + } + mConstraints[constraint_index]->RestoreState(inStream); + } + } + + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h new file mode 100644 index 000000000000..e1bfb0f21419 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class IslandBuilder; +class BodyManager; +class StateRecorderFilter; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// A list of constraints +using Constraints = Array>; + +/// A constraint manager manages all constraints of the same type +class JPH_EXPORT ConstraintManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + +#ifdef JPH_ENABLE_ASSERTS + /// Constructor + ConstraintManager(PhysicsLockContext inContext) : mLockContext(inContext) { } +#endif // JPH_ENABLE_ASSERTS + + /// Add a new constraint. This is thread safe. + void Add(Constraint **inConstraints, int inNumber); + + /// Remove a constraint. This is thread safe. + void Remove(Constraint **inConstraint, int inNumber); + + /// Get a list of all constraints + Constraints GetConstraints() const; + + /// Get total number of constraints + inline uint32 GetNumConstraints() const { return uint32(mConstraints.size()); } + + /// Determine the active constraints of a subset of the constraints + void GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const; + + /// Link bodies to form islands + static void sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager); + + /// In order to have a deterministic simulation, we need to sort the constraints of an island before solving them + static void sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd); + + /// Prior to solving the velocity constraints, you must call SetupVelocityConstraints once to precalculate values that are independent of velocity + static void sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime); + + /// Apply last frame's impulses, must be called prior to SolveVelocityConstraints + template + static void sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback); + + /// This function is called multiple times to iteratively come to a solution that meets all velocity constraints + static bool sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime); + + /// This function is called multiple times to iteratively come to a solution that meets all position constraints + static bool sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte); + +#ifdef JPH_DEBUG_RENDERER + /// Draw all constraints + void DrawConstraints(DebugRenderer *inRenderer) const; + + /// Draw all constraint limits + void DrawConstraintLimits(DebugRenderer *inRenderer) const; + + /// Draw all constraint reference frames + void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const; +#endif // JPH_DEBUG_RENDERER + + /// Save state of constraints + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restore the state of constraints. Returns false if failed. + bool RestoreState(StateRecorder &inStream); + + /// Lock all constraints. This should only be done during PhysicsSystem::Update(). + void LockAllConstraints() { PhysicsLock::sLock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); } + void UnlockAllConstraints() { PhysicsLock::sUnlock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); } + +private: +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mLockContext; +#endif // JPH_ENABLE_ASSERTS + Constraints mConstraints; + mutable Mutex mConstraintsMutex; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h new file mode 100644 index 000000000000..f7493102e2a0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h @@ -0,0 +1,257 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains rotation along 1 axis +/// +/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, see section 2.4.5 +/// +/// Constraint equation (eq 108): +/// +/// \f[C = \theta(t) - \theta_{min}\f] +/// +/// Jacobian (eq 109): +/// +/// \f[J = \begin{bmatrix}0 & -a^T & 0 & a^T\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which rotation is constrained (normalized).\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class AngleConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(inLambda * mInvI1_Axis); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_Axis); + return true; + } + + return false; + } + + /// Internal helper function to calculate the inverse effective mass + JPH_INLINE float CalculateInverseEffectiveMass(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis) + { + JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f)); + + // Calculate properties used below + mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero(); + mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero(); + + // Calculate inverse effective mass: K = J M^-1 J^T + return inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis); + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C) + /// @param inFrequency Oscillation frequency (Hz) + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping) + inline void CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass); + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C) + /// @param inStiffness Spring stiffness k. + /// @param inDamping Spring damping coefficient c. + inline void CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass); + } + + /// Selects one of the above functions based on the spring settings + inline void CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping) + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// @param inMinLambda Minimum angular impulse to apply (N m s) + /// @param inMaxLambda Maximum angular impulse to apply (N m s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (inWorldSpaceAxis.Dot(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()) - mSpringPart.GetBias(mTotalLambda)); + float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse + lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply + mTotalLambda = new_lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(lambda * mInvI1_Axis); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(lambda * mInvI2_Axis); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_Axis; + Vec3 mInvI2_Axis; + float mEffectiveMass = 0.0f; + SpringPart mSpringPart; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h new file mode 100644 index 000000000000..a41ee3af3c85 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h @@ -0,0 +1,682 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains motion along 1 axis +/// +/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.1.1 +/// (we're not using the approximation of eq 27 but instead add the U term as in eq 55) +/// +/// Constraint equation (eq 25): +/// +/// \f[C = (p_2 - p_1) \cdot n\f] +/// +/// Jacobian (eq 28): +/// +/// \f[J = \begin{bmatrix} -n^T & (-(r_1 + u) \times n)^T & n^T & (r_2 \times n)^T \end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// n = constraint axis (normalized).\n +/// p1, p2 = constraint points.\n +/// r1 = p1 - x1.\n +/// r2 = p2 - x2.\n +/// u = x2 + r2 - x1 - r1 = p2 - p1.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class AxisConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + template + JPH_INLINE bool ApplyVelocityStep(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if constexpr (Type1 == EMotionType::Dynamic) + { + ioMotionProperties1->SubLinearVelocityStep((inLambda * inInvMass1) * inWorldSpaceAxis); + ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if constexpr (Type2 == EMotionType::Dynamic) + { + ioMotionProperties2->AddLinearVelocityStep((inLambda * inInvMass2) * inWorldSpaceAxis); + ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Internal helper function to calculate the inverse effective mass + template + JPH_INLINE float TemplatedCalculateInverseEffectiveMass(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f)); + + // Calculate properties used below + Vec3 r1_plus_u_x_axis; + if constexpr (Type1 != EMotionType::Static) + { + r1_plus_u_x_axis = inR1PlusU.Cross(inWorldSpaceAxis); + r1_plus_u_x_axis.StoreFloat3(&mR1PlusUxAxis); + } + else + { + #ifdef JPH_DEBUG + Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis); + #endif + } + + Vec3 r2_x_axis; + if constexpr (Type2 != EMotionType::Static) + { + r2_x_axis = inR2.Cross(inWorldSpaceAxis); + r2_x_axis.StoreFloat3(&mR2xAxis); + } + else + { + #ifdef JPH_DEBUG + Vec3::sNaN().StoreFloat3(&mR2xAxis); + #endif + } + + // Calculate inverse effective mass: K = J M^-1 J^T + float inv_effective_mass; + + if constexpr (Type1 == EMotionType::Dynamic) + { + Vec3 invi1_r1_plus_u_x_axis = inInvI1.Multiply3x3(r1_plus_u_x_axis); + invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis); + inv_effective_mass = inInvMass1 + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis); + } + else + { + (void)r1_plus_u_x_axis; // Fix compiler warning: Not using this (it's not calculated either) + JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI1_R1PlusUxAxis);) + inv_effective_mass = 0.0f; + } + + if constexpr (Type2 == EMotionType::Dynamic) + { + Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis); + invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis); + inv_effective_mass += inInvMass2 + invi2_r2_x_axis.Dot(r2_x_axis); + } + else + { + (void)r2_x_axis; // Fix compiler warning: Not using this (it's not calculated either) + JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);) + } + + return inv_effective_mass; + } + + /// Internal helper function to calculate the inverse effective mass + JPH_INLINE float CalculateInverseEffectiveMass(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + float inv_m1 = mp1->GetInverseMass(); + Mat44 inv_i1 = inBody1.GetInverseInertia(); + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Kinematic: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + case EMotionType::Static: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + default: + break; + } + break; + } + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + default: + break; + } + + JPH_ASSERT(false); + return 0.0f; + } + + /// Internal helper function to calculate the inverse effective mass, version that supports mass scaling + JPH_INLINE float CalculateInverseEffectiveMassWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + { + Mat44 inv_i1 = inInvInertiaScale1 * inBody1.GetInverseInertia(); + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Kinematic: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + case EMotionType::Static: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + default: + break; + } + break; + } + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + default: + break; + } + + JPH_ASSERT(false); + return 0.0f; + } + +public: + /// Templated form of CalculateConstraintProperties with the motion types baked in + template + JPH_INLINE void TemplatedCalculateConstraintProperties(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = TemplatedCalculateInverseEffectiveMass(inInvMass1, inInvI1, inR1PlusU, inInvMass2, inInvI2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + + JPH_DET_LOG("TemplatedCalculateConstraintProperties: invM1: " << inInvMass1 << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invM2: " << inInvMass2 << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda); + } + + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below, version that supports mass scaling + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inInvInertiaScale1 Scale factor for the inverse inertia of body 1 + /// @param inInvInertiaScale2 Scale factor for the inverse inertia of body 2 + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintPropertiesWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMassWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMass2, inInvInertiaScale2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). + /// @param inFrequency Oscillation frequency (Hz). + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping). + inline void CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass); + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). + /// @param inStiffness Spring stiffness k. + /// @param inDamping Spring damping coefficient c. + inline void CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass); + } + + /// Selects one of the above functions based on the spring settings + inline void CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping) + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Templated form of WarmStart with the motion types baked in + template + inline void TemplatedWarmStart(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + + ApplyVelocityStep(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, mTotalLambda); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + // Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies + if (motion_type1 == EMotionType::Dynamic) + { + if (motion_type2 == EMotionType::Dynamic) + TemplatedWarmStart(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio); + else + TemplatedWarmStart(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inWarmStartImpulseRatio); + } + else + { + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + TemplatedWarmStart(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio); + } + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in, part 1: get the total lambda + template + JPH_INLINE float TemplatedSolveVelocityConstraintGetTotalLambda(const MotionProperties *ioMotionProperties1, const MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis) const + { + // Calculate jacobian multiplied by linear velocity + float jv; + if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity() - ioMotionProperties2->GetLinearVelocity()); + else if constexpr (Type1 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity()); + else if constexpr (Type2 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(-ioMotionProperties2->GetLinearVelocity()); + else + JPH_ASSERT(false); // Static vs static is nonsensical! + + // Calculate jacobian multiplied by angular velocity + if constexpr (Type1 != EMotionType::Static) + jv += Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(ioMotionProperties1->GetAngularVelocity()); + if constexpr (Type2 != EMotionType::Static) + jv -= Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(ioMotionProperties2->GetAngularVelocity()); + + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (jv - mSpringPart.GetBias(mTotalLambda)); + + // Return the total accumulated lambda + return mTotalLambda + lambda; + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in, part 2: apply new lambda + template + JPH_INLINE bool TemplatedSolveVelocityConstraintApplyLambda(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inTotalLambda) + { + float delta_lambda = inTotalLambda - mTotalLambda; // Calculate change in lambda + mTotalLambda = inTotalLambda; // Store accumulated impulse + + return ApplyVelocityStep(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, delta_lambda); + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in + template + inline bool TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + float total_lambda = TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis); + + // Clamp impulse to specified range + total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda); + + return TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, total_lambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inMinLambda Minimum value of constraint impulse to apply (N s) + /// @param inMaxLambda Maximum value of constraint impulse to apply (N s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Kinematic: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + + return false; + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inMinLambda Minimum value of constraint impulse to apply (N s) + /// @param inMaxLambda Maximum value of constraint impulse to apply (N s) + inline bool SolveVelocityConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Kinematic: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + + return false; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep((lambda * ioBody1.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis); + ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * ioBody2.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis); + ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep((lambda * inInvMass1) * inWorldSpaceAxis); + ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * inInvMass2) * inWorldSpaceAxis); + ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Override total lagrange multiplier, can be used to set the initial value for warm starting + inline void SetTotalLambda(float inLambda) + { + mTotalLambda = inLambda; + } + + /// Return lagrange multiplier + inline float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Float3 mR1PlusUxAxis; + Float3 mR2xAxis; + Float3 mInvI1_R1PlusUxAxis; + Float3 mInvI2_R2xAxis; + float mEffectiveMass = 0.0f; + SpringPart mSpringPart; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h new file mode 100644 index 000000000000..c0ebe5ac42e9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h @@ -0,0 +1,276 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/** + Constrains movement on 2 axis + + @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.3.1 + + Constraint equation (eq 51): + + \f[C = \begin{bmatrix} (p_2 - p_1) \cdot n_1 \\ (p_2 - p_1) \cdot n_2\end{bmatrix}\f] + + Jacobian (transposed) (eq 55): + + \f[J^T = \begin{bmatrix} + -n_1 & -n_2 \\ + -(r_1 + u) \times n_1 & -(r_1 + u) \times n_2 \\ + n_1 & n_2 \\ + r_2 \times n_1 & r_2 \times n_2 + \end{bmatrix}\f] + + Used terms (here and below, everything in world space):\n + n1, n2 = constraint axis (normalized).\n + p1, p2 = constraint points.\n + r1 = p1 - x1.\n + r2 = p2 - x2.\n + u = x2 + r2 - x1 - r1 = p2 - p1.\n + x1, x2 = center of mass for the bodies.\n + v = [v1, w1, v2, w2].\n + v1, v2 = linear velocity of body 1 and 2.\n + w1, w2 = angular velocity of body 1 and 2.\n + M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n + \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n + b = velocity bias.\n + \f$\beta\f$ = baumgarte constant. +**/ +class DualAxisConstraintPart +{ +public: + using Vec2 = Vector<2>; + using Mat22 = Matrix<2, 2>; + +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, const Vec2 &inLambda) const + { + // Apply impulse if delta is not zero + if (!inLambda.IsZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + Vec3 impulse = inN1 * inLambda[0] + inN2 * inLambda[1]; + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->SubLinearVelocityStep(mp1->GetInverseMass() * impulse); + mp1->SubAngularVelocityStep(mInvI1_R1PlusUxN1 * inLambda[0] + mInvI1_R1PlusUxN2 * inLambda[1]); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep(mp2->GetInverseMass() * impulse); + mp2->AddAngularVelocityStep(mInvI2_R2xN1 * inLambda[0] + mInvI2_R2xN2 * inLambda[1]); + } + return true; + } + + return false; + } + + /// Internal helper function to calculate the lagrange multiplier + inline void CalculateLagrangeMultiplier(const Body &inBody1, const Body &inBody2, Vec3Arg inN1, Vec3Arg inN2, Vec2 &outLambda) const + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 delta_lin = inBody1.GetLinearVelocity() - inBody2.GetLinearVelocity(); + Vec2 jv; + jv[0] = inN1.Dot(delta_lin) + mR1PlusUxN1.Dot(inBody1.GetAngularVelocity()) - mR2xN1.Dot(inBody2.GetAngularVelocity()); + jv[1] = inN2.Dot(delta_lin) + mR1PlusUxN2.Dot(inBody1.GetAngularVelocity()) - mR2xN2.Dot(inBody2.GetAngularVelocity()); + outLambda = mEffectiveMass * jv; + } + +public: + /// Calculate properties used during the functions below + /// All input vectors are in world space + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1PlusU, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2, Vec3Arg inN1, Vec3Arg inN2) + { + JPH_ASSERT(inN1.IsNormalized(1.0e-5f)); + JPH_ASSERT(inN2.IsNormalized(1.0e-5f)); + + // Calculate properties used during constraint solving + mR1PlusUxN1 = inR1PlusU.Cross(inN1); + mR1PlusUxN2 = inR1PlusU.Cross(inN2); + mR2xN1 = inR2.Cross(inN1); + mR2xN2 = inR2.Cross(inN2); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1, eq 59 + Mat22 inv_effective_mass; + if (inBody1.IsDynamic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1); + mInvI1_R1PlusUxN1 = inv_i1.Multiply3x3(mR1PlusUxN1); + mInvI1_R1PlusUxN2 = inv_i1.Multiply3x3(mR1PlusUxN2); + + inv_effective_mass(0, 0) = mp1->GetInverseMass() + mR1PlusUxN1.Dot(mInvI1_R1PlusUxN1); + inv_effective_mass(0, 1) = mR1PlusUxN1.Dot(mInvI1_R1PlusUxN2); + inv_effective_mass(1, 0) = mR1PlusUxN2.Dot(mInvI1_R1PlusUxN1); + inv_effective_mass(1, 1) = mp1->GetInverseMass() + mR1PlusUxN2.Dot(mInvI1_R1PlusUxN2); + } + else + { + JPH_IF_DEBUG(mInvI1_R1PlusUxN1 = Vec3::sNaN();) + JPH_IF_DEBUG(mInvI1_R1PlusUxN2 = Vec3::sNaN();) + + inv_effective_mass = Mat22::sZero(); + } + + if (inBody2.IsDynamic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2); + mInvI2_R2xN1 = inv_i2.Multiply3x3(mR2xN1); + mInvI2_R2xN2 = inv_i2.Multiply3x3(mR2xN2); + + inv_effective_mass(0, 0) += mp2->GetInverseMass() + mR2xN1.Dot(mInvI2_R2xN1); + inv_effective_mass(0, 1) += mR2xN1.Dot(mInvI2_R2xN2); + inv_effective_mass(1, 0) += mR2xN2.Dot(mInvI2_R2xN1); + inv_effective_mass(1, 1) += mp2->GetInverseMass() + mR2xN2.Dot(mInvI2_R2xN2); + } + else + { + JPH_IF_DEBUG(mInvI2_R2xN1 = Vec3::sNaN();) + JPH_IF_DEBUG(mInvI2_R2xN2 = Vec3::sNaN();) + } + + if (!mEffectiveMass.SetInversed(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass.SetZero(); + mTotalLambda.SetZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return !mEffectiveMass.IsZero(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// All input vectors are in world space + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// All input vectors are in world space + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2) + { + Vec2 lambda; + CalculateLagrangeMultiplier(ioBody1, ioBody2, inN1, inN2, lambda); + + // Store accumulated lambda + mTotalLambda += lambda; + + return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// All input vectors are in world space + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inU, Vec3Arg inN1, Vec3Arg inN2, float inBaumgarte) const + { + Vec2 c; + c[0] = inU.Dot(inN1); + c[1] = inU.Dot(inN2); + if (!c.IsZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec2 lambda = -inBaumgarte * (mEffectiveMass * c); + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + Vec3 impulse = inN1 * lambda[0] + inN2 * lambda[1]; + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * impulse); + ioBody1.SubRotationStep(mInvI1_R1PlusUxN1 * lambda[0] + mInvI1_R1PlusUxN2 * lambda[1]); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * impulse); + ioBody2.AddRotationStep(mInvI2_R2xN1 * lambda[0] + mInvI2_R2xN2 * lambda[1]); + } + return true; + } + + return false; + } + + /// Override total lagrange multiplier, can be used to set the initial value for warm starting + inline void SetTotalLambda(const Vec2 &inLambda) + { + mTotalLambda = inLambda; + } + + /// Return lagrange multiplier + inline const Vec2 & GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1PlusUxN1; + Vec3 mR1PlusUxN2; + Vec3 mR2xN1; + Vec3 mR2xN2; + Vec3 mInvI1_R1PlusUxN1; + Vec3 mInvI1_R1PlusUxN2; + Vec3 mInvI2_R2xN1; + Vec3 mInvI2_R2xN2; + Mat22 mEffectiveMass; + Vec2 mTotalLambda { Vec2::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h new file mode 100644 index 000000000000..6e277a64b563 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h @@ -0,0 +1,195 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains two rotations using a gear (rotating in opposite direction) +/// +/// Constraint equation: +/// +/// C = Rotation1(t) + r Rotation2(t) +/// +/// Derivative: +/// +/// d/dt C = 0 +/// <=> w1 . a + r w2 . b = 0 +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}0 & a^T & 0 & r b^T\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which body 1 rotates (normalized).\n +/// b = axis along which body 2 slides (normalized).\n +/// Rotation1(t) = rotation around a of body 1.\n +/// Rotation2(t) = rotation around b of body 2.\n +/// r = ratio between rotation for body 1 and 2.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// \f$\beta\f$ = baumgarte constant. +class GearConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A); + ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_B); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates + /// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates + /// @param inRatio The ratio between rotation and translation + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio) + { + JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-4f)); + JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-4f)); + + // Calculate: I1^-1 a + mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis1); + + // Calculate: I2^-1 b + mInvI2_B = inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceHingeAxis2); + + // K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + r^2 * b^T I2^-1 b) + float inv_effective_mass = (inWorldSpaceHingeAxis1.Dot(mInvI1_A) + inWorldSpaceHingeAxis2.Dot(mInvI2_B) * Square(inRatio)); + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates + /// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates + /// @param inRatio The ratio between rotation and translation + inline bool SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis1, Body &ioBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = -mEffectiveMass * (inWorldSpaceHingeAxis1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inWorldSpaceHingeAxis2.Dot(ioBody2.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.AddRotationStep(lambda * mInvI1_A); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(lambda * mInvI2_B); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_A; + Vec3 mInvI2_B; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h new file mode 100644 index 000000000000..5f566f701793 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h @@ -0,0 +1,222 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/** + Constrains rotation around 2 axis so that it only allows rotation around 1 axis + + Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.4.1 + + Constraint equation (eq 87): + + \f[C = \begin{bmatrix}a_1 \cdot b_2 \\ a_1 \cdot c_2\end{bmatrix}\f] + + Jacobian (eq 90): + + \f[J = \begin{bmatrix} + 0 & -b_2 \times a_1 & 0 & b_2 \times a_1 \\ + 0 & -c_2 \times a_1 & 0 & c2 \times a_1 + \end{bmatrix}\f] + + Used terms (here and below, everything in world space):\n + a1 = hinge axis on body 1.\n + b2, c2 = axis perpendicular to hinge axis on body 2.\n + x1, x2 = center of mass for the bodies.\n + v = [v1, w1, v2, w2].\n + v1, v2 = linear velocity of body 1 and 2.\n + w1, w2 = angular velocity of body 1 and 2.\n + M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n + \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n + b = velocity bias.\n + \f$\beta\f$ = baumgarte constant.\n + E = identity matrix. +**/ +class HingeRotationConstraintPart +{ +public: + using Vec2 = Vector<2>; + using Mat22 = Matrix<2, 2>; + +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, const Vec2 &inLambda) const + { + // Apply impulse if delta is not zero + if (!inLambda.IsZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + Vec3 impulse = mB2xA1 * inLambda[0] + mC2xA1 * inLambda[1]; + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(impulse)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(impulse)); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inWorldSpaceHingeAxis2) + { + JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-5f)); + JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-5f)); + + // Calculate hinge axis in world space + mA1 = inWorldSpaceHingeAxis1; + Vec3 a2 = inWorldSpaceHingeAxis2; + float dot = mA1.Dot(a2); + if (dot <= 1.0e-3f) + { + // World space axes are more than 90 degrees apart, get a perpendicular vector in the plane formed by mA1 and a2 as hinge axis until the rotation is less than 90 degrees + Vec3 perp = a2 - dot * mA1; + if (perp.LengthSq() < 1.0e-6f) + { + // mA1 ~ -a2, take random perpendicular + perp = mA1.GetNormalizedPerpendicular(); + } + + // Blend in a little bit from mA1 so we're less than 90 degrees apart + a2 = (0.99f * perp.Normalized() + 0.01f * mA1).Normalized(); + } + mB2 = a2.GetNormalizedPerpendicular(); + mC2 = a2.Cross(mB2); + + // Calculate properties used during constraint solving + mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + mB2xA1 = mB2.Cross(mA1); + mC2xA1 = mC2.Cross(mA1); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + Mat44 summed_inv_inertia = mInvI1 + mInvI2; + Mat22 inv_effective_mass; + inv_effective_mass(0, 0) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1)); + inv_effective_mass(0, 1) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1)); + inv_effective_mass(1, 0) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1)); + inv_effective_mass(1, 1) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1)); + if (!mEffectiveMass.SetInversed(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass.SetZero(); + mTotalLambda.SetZero(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 delta_ang = ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity(); + Vec2 jv; + jv[0] = mB2xA1.Dot(delta_ang); + jv[1] = mC2xA1.Dot(delta_ang); + Vec2 lambda = mEffectiveMass * jv; + + // Store accumulated lambda + mTotalLambda += lambda; + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const + { + // Constraint needs Axis of body 1 perpendicular to both B and C from body 2 (which are both perpendicular to the Axis of body 2) + Vec2 c; + c[0] = mA1.Dot(mB2); + c[1] = mA1.Dot(mC2); + if (!c.IsZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec2 lambda = -inBaumgarte * (mEffectiveMass * c); + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + Vec3 impulse = mB2xA1 * lambda[0] + mC2xA1 * lambda[1]; + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1.Multiply3x3(impulse)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2.Multiply3x3(impulse)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + const Vec2 & GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mA1; ///< World space hinge axis for body 1 + Vec3 mB2; ///< World space perpendiculars of hinge axis for body 2 + Vec3 mC2; + Mat44 mInvI1; + Mat44 mInvI2; + Vec3 mB2xA1; + Vec3 mC2xA1; + Mat22 mEffectiveMass; + Vec2 mTotalLambda { Vec2::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h new file mode 100644 index 000000000000..5b752c6c6936 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint part to an AxisConstraintPart but both bodies have an independent axis on which the force is applied. +/// +/// Constraint equation: +/// +/// \f[C = (x_1 + r_1 - f_1) . n_1 + r (x_2 + r_2 - f_2) \cdot n_2\f] +/// +/// Calculating the Jacobian: +/// +/// \f[dC/dt = (v_1 + w_1 \times r_1) \cdot n_1 + (x_1 + r_1 - f_1) \cdot d n_1/dt + r (v_2 + w_2 \times r_2) \cdot n_2 + r (x_2 + r_2 - f_2) \cdot d n_2/dt\f] +/// +/// Assuming that d n1/dt and d n2/dt are small this becomes: +/// +/// \f[(v_1 + w_1 \times r_1) \cdot n_1 + r (v_2 + w_2 \times r_2) \cdot n_2\f] +/// \f[= v_1 \cdot n_1 + r_1 \times n_1 \cdot w_1 + r v_2 \cdot n_2 + r r_2 \times n_2 \cdot w_2\f] +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}n_1 & r_1 \times n_1 & r n_2 & r r_2 \times n_2\end{bmatrix}\f] +/// +/// Effective mass: +/// +/// \f[K = m_1^{-1} + r_1 \times n_1 I_1^{-1} r_1 \times n_1 + r^2 m_2^{-1} + r^2 r_2 \times n_2 I_2^{-1} r_2 \times n_2\f] +/// +/// Used terms (here and below, everything in world space):\n +/// n1 = (x1 + r1 - f1) / |x1 + r1 - f1|, axis along which the force is applied for body 1\n +/// n2 = (x2 + r2 - f2) / |x2 + r2 - f2|, axis along which the force is applied for body 2\n +/// r = ratio how forces are applied between bodies.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class IndependentAxisConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->AddLinearVelocityStep((mp1->GetInverseMass() * inLambda) * inN1); + mp1->AddAngularVelocityStep(mInvI1_R1xN1 * inLambda); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep((inRatio * mp2->GetInverseMass() * inLambda) * inN2); + mp2->AddAngularVelocityStep(mInvI2_RatioR2xN2 * inLambda); + } + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1 The position on which the constraint operates on body 1 relative to COM + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inR2 The position on which the constraint operates on body 1 relative to COM + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inR1, Vec3Arg inN1, Vec3Arg inR2, Vec3Arg inN2, float inRatio) + { + JPH_ASSERT(inN1.IsNormalized(1.0e-4f) && inN2.IsNormalized(1.0e-4f)); + + float inv_effective_mass = 0.0f; + + if (!inBody1.IsStatic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + + mR1xN1 = inR1.Cross(inN1); + mInvI1_R1xN1 = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), mR1xN1); + + inv_effective_mass += mp1->GetInverseMass() + mInvI1_R1xN1.Dot(mR1xN1); + } + + if (!inBody2.IsStatic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + + mRatioR2xN2 = inRatio * inR2.Cross(inN2); + mInvI2_RatioR2xN2 = mp2->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), mRatioR2xN2); + + inv_effective_mass += Square(inRatio) * mp2->GetInverseMass() + mInvI2_RatioR2xN2.Dot(mRatioR2xN2); + } + + // Calculate inverse effective mass: K = J M^-1 J^T + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inMinLambda Minimum angular impulse to apply (N m s) + /// @param inMaxLambda Maximum angular impulse to apply (N m s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inMinLambda, float inMaxLambda) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = -mEffectiveMass * (inN1.Dot(ioBody1.GetLinearVelocity()) + mR1xN1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inN2.Dot(ioBody2.GetLinearVelocity()) + mRatioR2xN2.Dot(ioBody2.GetAngularVelocity())); + float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse + lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply + mTotalLambda = new_lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inC, float inBaumgarte) const + { + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.AddPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN1); + ioBody1.AddRotationStep(lambda * mInvI1_R1xN1); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * inRatio * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN2); + ioBody2.AddRotationStep(lambda * mInvI2_RatioR2xN2); + } + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1xN1; + Vec3 mInvI1_R1xN1; + Vec3 mRatioR2xN2; + Vec3 mInvI2_RatioR2xN2; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h new file mode 100644 index 000000000000..f9e86b86b2f0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h @@ -0,0 +1,239 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constrains movement along 3 axis +/// +/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.2.1 +/// +/// Constraint equation (eq 45): +/// +/// \f[C = p_2 - p_1\f] +/// +/// Jacobian (transposed) (eq 47): +/// +/// \f[J^T = \begin{bmatrix}-E & r1x & E & -r2x^T\end{bmatrix} +/// = \begin{bmatrix}-E^T \\ r1x^T \\ E^T \\ -r2x^T\end{bmatrix} +/// = \begin{bmatrix}-E \\ -r1x \\ E \\ r2x\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// p1, p2 = constraint points.\n +/// r1 = p1 - x1.\n +/// r2 = p2 - x2.\n +/// r1x = 3x3 matrix for which r1x v = r1 x v (cross product).\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant.\n +/// E = identity matrix. +class PointConstraintPart +{ + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->SubLinearVelocityStep(mp1->GetInverseMass() * inLambda); + mp1->SubAngularVelocityStep(mInvI1_R1X * inLambda); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep(mp2->GetInverseMass() * inLambda); + mp2->AddAngularVelocityStep(mInvI2_R2X * inLambda); + } + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inRotation1 The 3x3 rotation matrix for body 1 (translation part is ignored) + /// @param inRotation2 The 3x3 rotation matrix for body 2 (translation part is ignored) + /// @param inR1 Local space vector from center of mass to constraint point for body 1 + /// @param inR2 Local space vector from center of mass to constraint point for body 2 + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2) + { + // Positions where the point constraint acts on (middle point between center of masses) in world space + mR1 = inRotation1.Multiply3x3(inR1); + mR2 = inRotation2.Multiply3x3(inR2); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + // Using: I^-1 = R * Ibody^-1 * R^T + float summed_inv_mass; + Mat44 inv_effective_mass; + if (inBody1.IsDynamic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1); + summed_inv_mass = mp1->GetInverseMass(); + + Mat44 r1x = Mat44::sCrossProduct(mR1); + mInvI1_R1X = inv_i1.Multiply3x3(r1x); + inv_effective_mass = r1x.Multiply3x3(inv_i1).Multiply3x3RightTransposed(r1x); + } + else + { + JPH_IF_DEBUG(mInvI1_R1X = Mat44::sNaN();) + + summed_inv_mass = 0.0f; + inv_effective_mass = Mat44::sZero(); + } + + if (inBody2.IsDynamic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2); + summed_inv_mass += mp2->GetInverseMass(); + + Mat44 r2x = Mat44::sCrossProduct(mR2); + mInvI2_R2X = inv_i2.Multiply3x3(r2x); + inv_effective_mass += r2x.Multiply3x3(inv_i2).Multiply3x3RightTransposed(r2x); + } + else + { + JPH_IF_DEBUG(mInvI2_R2X = Mat44::sNaN();) + } + + inv_effective_mass += Mat44::sScale(summed_inv_mass); + if (!mEffectiveMass.SetInversed3x3(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass * (ioBody1.GetLinearVelocity() - mR1.Cross(ioBody1.GetAngularVelocity()) - ioBody2.GetLinearVelocity() + mR2.Cross(ioBody2.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated lambda + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const + { + Vec3 separation = (Vec3(ioBody2.GetCenterOfMassPosition() - ioBody1.GetCenterOfMassPosition()) + mR2 - mR1); + if (separation != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = mEffectiveMass * -inBaumgarte * separation; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * lambda); + ioBody1.SubRotationStep(mInvI1_R1X * lambda); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * lambda); + ioBody2.AddRotationStep(mInvI2_R2X * lambda); + } + + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1; + Vec3 mR2; + Mat44 mInvI1_R1X; + Mat44 mInvI2_R2X; + Mat44 mEffectiveMass; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h new file mode 100644 index 000000000000..641f4867a115 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h @@ -0,0 +1,196 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains a rotation to a translation +/// +/// Constraint equation: +/// +/// C = Theta(t) - r d(t) +/// +/// Derivative: +/// +/// d/dt C = 0 +/// <=> w1 . a - r v2 . b = 0 +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}0 & a^T & -r b^T & 0\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which body 1 rotates (normalized).\n +/// b = axis along which body 2 slides (normalized).\n +/// Theta(t) = rotation around a of body 1.\n +/// d(t) = distance body 2 slides.\n +/// r = ratio between rotation and translation.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// \f$\beta\f$ = baumgarte constant. +class RackAndPinionConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A); + ioBody2.GetMotionProperties()->SubLinearVelocityStep(inLambda * mRatio_InvM2_B); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis The axis around which body 1 rotates + /// @param inWorldSpaceSliderAxis The axis along which body 2 slides + /// @param inRatio The ratio between rotation and translation + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis, const Body &inBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio) + { + JPH_ASSERT(inWorldSpaceHingeAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inWorldSpaceSliderAxis.IsNormalized(1.0e-4f)); + + // Calculate: I1^-1 a + mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis); + + // Calculate: r/m2 b + float inv_m2 = inBody2.GetMotionProperties()->GetInverseMass(); + mRatio_InvM2_B = inRatio * inv_m2 * inWorldSpaceSliderAxis; + + // K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + 1/m2 * r^2 * b . b) + float inv_effective_mass = (inWorldSpaceHingeAxis.Dot(mInvI1_A) + inv_m2 * Square(inRatio)); + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis The axis around which body 1 rotates + /// @param inWorldSpaceSliderAxis The axis along which body 2 slides + /// @param inRatio The ratio between rotation and translation + inline bool SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis, Body &ioBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (inRatio * inWorldSpaceSliderAxis.Dot(ioBody2.GetLinearVelocity()) - inWorldSpaceHingeAxis.Dot(ioBody1.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.AddRotationStep(lambda * mInvI1_A); + if (ioBody2.IsDynamic()) + ioBody2.SubPositionStep(lambda * mRatio_InvM2_B); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_A; + Vec3 mRatio_InvM2_B; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h new file mode 100644 index 000000000000..ec84776a46f0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h @@ -0,0 +1,270 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constrains rotation around all axis so that only translation is allowed +/// +/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.5.1 +/// +/// Constraint equation (eq 129): +/// +/// \f[C = \begin{bmatrix}\Delta\theta_x, \Delta\theta_y, \Delta\theta_z\end{bmatrix}\f] +/// +/// Jacobian (eq 131): +/// +/// \f[J = \begin{bmatrix}0 & -E & 0 & E\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// delta_theta_* = difference in rotation between initial rotation of bodies 1 and 2.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant.\n +/// E = identity matrix.\n +class RotationEulerConstraintPart +{ +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(inLambda)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(inLambda)); + return true; + } + + return false; + } + +public: + /// Return inverse of initial rotation from body 1 to body 2 in body 1 space + static Quat sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2) + { + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation from body 1 to body 2 + return inBody2.GetRotation().Conjugated() * inBody1.GetRotation(); + } + + /// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space + /// @param inAxisX1 Reference axis X for body 1 + /// @param inAxisY1 Reference axis Y for body 1 + /// @param inAxisX2 Reference axis X for body 2 + /// @param inAxisY2 Reference axis Y for body 2 + static Quat sGetInvInitialOrientationXY(Vec3Arg inAxisX1, Vec3Arg inAxisY1, Vec3Arg inAxisX2, Vec3Arg inAxisY2) + { + // Store inverse of initial rotation from body 1 to body 2 in body 1 space: + // + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q10, q20 = world space initial orientation of body 1 and 2 + // r0 = initial rotation from body 1 to body 2 in local space of body 1 + // + // We can also write this in terms of the constraint matrices: + // + // q20 c2 = q10 c1 + // <=> q20 = q10 c1 c2^-1 + // => r0 = c1 c2^-1 + // <=> r0^-1 = c2 c1^-1 + // + // where: + // + // c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2 + if (inAxisX1 == inAxisX2 && inAxisY1 == inAxisY2) + { + // Axis are the same -> identity transform + return Quat::sIdentity(); + } + else + { + Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisY1, 0), Vec4(inAxisX1.Cross(inAxisY1), 0), Vec4(0, 0, 0, 1)); + Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisY2, 0), Vec4(inAxisX2.Cross(inAxisY2), 0), Vec4(0, 0, 0, 1)); + return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated(); + } + } + + /// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space + /// @param inAxisX1 Reference axis X for body 1 + /// @param inAxisZ1 Reference axis Z for body 1 + /// @param inAxisX2 Reference axis X for body 2 + /// @param inAxisZ2 Reference axis Z for body 2 + static Quat sGetInvInitialOrientationXZ(Vec3Arg inAxisX1, Vec3Arg inAxisZ1, Vec3Arg inAxisX2, Vec3Arg inAxisZ2) + { + // See comment at sGetInvInitialOrientationXY + if (inAxisX1 == inAxisX2 && inAxisZ1 == inAxisZ2) + { + return Quat::sIdentity(); + } + else + { + Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisZ1.Cross(inAxisX1), 0), Vec4(inAxisZ1, 0), Vec4(0, 0, 0, 1)); + Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisZ2.Cross(inAxisX2), 0), Vec4(inAxisZ2, 0), Vec4(0, 0, 0, 1)); + return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated(); + } + } + + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2) + { + // Calculate properties used during constraint solving + mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + if (!mEffectiveMass.SetInversed3x3(mInvI1 + mInvI2)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()); + mTotalLambda += lambda; + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const + { + // Calculate difference in rotation + // + // The rotation should be: + // + // q2 = q1 r0 + // + // But because of drift the actual rotation is + // + // q2 = diff q1 r0 + // <=> diff = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // diff = error that needs to be reduced to zero + Quat diff = ioBody2.GetRotation() * inInvInitialOrientation * ioBody1.GetRotation().Conjugated(); + + // A quaternion can be seen as: + // + // q = [sin(theta / 2) * v, cos(theta/2)] + // + // Where: + // v = rotation vector + // theta = rotation angle + // + // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: + Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ(); + if (error != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = -inBaumgarte * mEffectiveMass * error; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1.Multiply3x3(lambda)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2.Multiply3x3(lambda)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Mat44 mInvI1; + Mat44 mInvI2; + Mat44 mEffectiveMass; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h new file mode 100644 index 000000000000..a4675a51b9f1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Quaternion based constraint that constrains rotation around all axis so that only translation is allowed. +/// +/// NOTE: This constraint part is more expensive than the RotationEulerConstraintPart and slightly more correct since +/// RotationEulerConstraintPart::SolvePositionConstraint contains an approximation. In practice the difference +/// is small, so the RotationEulerConstraintPart is probably the better choice. +/// +/// Rotation is fixed between bodies like this: +/// +/// q2 = q1 r0 +/// +/// Where: +/// q1, q2 = world space quaternions representing rotation of body 1 and 2. +/// r0 = initial rotation between bodies in local space of body 1, this can be calculated by: +/// +/// q20 = q10 r0 +/// <=> r0 = q10^* q20 +/// +/// Where: +/// q10, q20 = initial world space rotations of body 1 and 2. +/// q10^* = conjugate of quaternion q10 (which is the same as the inverse for a unit quaternion) +/// +/// We exclusively use the conjugate below: +/// +/// r0^* = q20^* q10 +/// +/// The error in the rotation is (in local space of body 1): +/// +/// q2 = q1 error r0 +/// <=> error = q1^* q2 r0^* +/// +/// The imaginary part of the quaternion represents the rotation axis * sin(angle / 2). The real part of the quaternion +/// does not add any additional information (we know the quaternion in normalized) and we're removing 3 degrees of freedom +/// so we want 3 parameters. Therefore we define the constraint equation like: +/// +/// C = A q1^* q2 r0^* = 0 +/// +/// Where (if you write a quaternion as [real-part, i-part, j-part, k-part]): +/// +/// [0, 1, 0, 0] +/// A = [0, 0, 1, 0] +/// [0, 0, 0, 1] +/// +/// or in our case since we store a quaternion like [i-part, j-part, k-part, real-part]: +/// +/// [1, 0, 0, 0] +/// A = [0, 1, 0, 0] +/// [0, 0, 1, 0] +/// +/// Time derivative: +/// +/// d/dt C = A (q1^* d/dt(q2) + d/dt(q1^*) q2) r0^* +/// = A (q1^* (1/2 W2 q2) + (1/2 W1 q1)^* q2) r0^* +/// = 1/2 A (q1^* W2 q2 + q1^* W1^* q2) r0^* +/// = 1/2 A (q1^* W2 q2 - q1^* W1 * q2) r0^* +/// = 1/2 A ML(q1^*) MR(q2 r0^*) (W2 - W1) +/// = 1/2 A ML(q1^*) MR(q2 r0^*) A^T (w2 - w1) +/// +/// Where: +/// W1 = [0, w1], W2 = [0, w2] (converting angular velocity to imaginary part of quaternion). +/// w1, w2 = angular velocity of body 1 and 2. +/// d/dt(q) = 1/2 W q (time derivative of a quaternion). +/// W^* = -W (conjugate negates angular velocity as quaternion). +/// ML(q): 4x4 matrix so that q * p = ML(q) * p, where q and p are quaternions. +/// MR(p): 4x4 matrix so that q * p = MR(p) * q, where q and p are quaternions. +/// A^T: Transpose of A. +/// +/// Jacobian: +/// +/// J = [0, -1/2 A ML(q1^*) MR(q2 r0^*) A^T, 0, 1/2 A ML(q1^*) MR(q2 r0^*) A^T] +/// = [0, -JP, 0, JP] +/// +/// Suggested reading: +/// - 3D Constraint Derivations for Impulse Solvers - Marijn Tamis +/// - Game Physics Pearls - Section 9 - Quaternion Based Constraints - Claude Lacoursiere +class RotationQuatConstraintPart +{ +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1_JPT.Multiply3x3(inLambda)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2_JPT.Multiply3x3(inLambda)); + return true; + } + + return false; + } + +public: + /// Return inverse of initial rotation from body 1 to body 2 in body 1 space + static Quat sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2) + { + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation from body 1 to body 2 + return inBody2.GetRotation().Conjugated() * inBody1.GetRotation(); + } + + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2, QuatArg inInvInitialOrientation) + { + // Calculate: JP = 1/2 A ML(q1^*) MR(q2 r0^*) A^T + Mat44 jp = (Mat44::sQuatLeftMultiply(0.5f * inBody1.GetRotation().Conjugated()) * Mat44::sQuatRightMultiply(inBody2.GetRotation() * inInvInitialOrientation)).GetRotationSafe(); + + // Calculate properties used during constraint solving + Mat44 inv_i1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + Mat44 inv_i2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + mInvI1_JPT = inv_i1.Multiply3x3RightTransposed(jp); + mInvI2_JPT = inv_i2.Multiply3x3RightTransposed(jp); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + // = (JP * I1^-1 * JP^T + JP * I2^-1 * JP^T)^-1 + // = (JP * (I1^-1 + I2^-1) * JP^T)^-1 + if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(inv_i1 + inv_i2).Multiply3x3RightTransposed(jp))) + Deactivate(); + else + mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mEffectiveMass_JP = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass_JP.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()); + mTotalLambda += lambda; + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const + { + // Calculate constraint equation + Vec3 c = (ioBody1.GetRotation().Conjugated() * ioBody2.GetRotation() * inInvInitialOrientation).GetXYZ(); + if (c != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = -inBaumgarte * mEffectiveMass * c; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1_JPT.Multiply3x3(lambda)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2_JPT.Multiply3x3(lambda)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Mat44 mInvI1_JPT; + Mat44 mInvI2_JPT; + Mat44 mEffectiveMass; + Mat44 mEffectiveMass_JP; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h new file mode 100644 index 000000000000..0a8a4a9730c8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN +#ifndef JPH_PLATFORM_DOXYGEN // Somehow Doxygen gets confused and thinks the parameters to CalculateSpringProperties belong to this macro +JPH_MSVC_SUPPRESS_WARNING(4723) // potential divide by 0 - caused by line: outEffectiveMass = 1.0f / inInvEffectiveMass, note that JPH_NAMESPACE_BEGIN already pushes the warning state +#endif // !JPH_PLATFORM_DOXYGEN + +/// Class used in other constraint parts to calculate the required bias factor in the lagrange multiplier for creating springs +class SpringPart +{ +private: + JPH_INLINE void CalculateSpringPropertiesHelper(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass) + { + // Soft constraints as per: Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 + + // Note that the calculation of beta and gamma below are based on the solution of an implicit Euler integration scheme + // This scheme is unconditionally stable but has built in damping, so even when you set the damping ratio to 0 there will still + // be damping. See page 16 and 32. + + // Calculate softness (gamma in the slides) + // See page 34 and note that the gamma needs to be divided by delta time since we're working with impulses rather than forces: + // softness = 1 / (dt * (c + dt * k)) + // Note that the spring stiffness is k and the spring damping is c + mSoftness = 1.0f / (inDeltaTime * (inDamping + inDeltaTime * inStiffness)); + + // Calculate bias factor (baumgarte stabilization): + // beta = dt * k / (c + dt * k) = dt * k^2 * softness + // b = beta / dt * C = dt * k * softness * C + mBias = inBias + inDeltaTime * inStiffness * mSoftness * inC; + + // Update the effective mass, see post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354 + // + // Newton's Law: + // M * (v2 - v1) = J^T * lambda + // + // Velocity constraint with softness and Baumgarte: + // J * v2 + softness * lambda + b = 0 + // + // where b = beta * C / dt + // + // We know everything except v2 and lambda. + // + // First solve Newton's law for v2 in terms of lambda: + // + // v2 = v1 + M^-1 * J^T * lambda + // + // Substitute this expression into the velocity constraint: + // + // J * (v1 + M^-1 * J^T * lambda) + softness * lambda + b = 0 + // + // Now collect coefficients of lambda: + // + // (J * M^-1 * J^T + softness) * lambda = - J * v1 - b + // + // Now we define: + // + // K = J * M^-1 * J^T + softness + // + // So our new effective mass is K^-1 + outEffectiveMass = 1.0f / (inInvEffectiveMass + mSoftness); + } + +public: + /// Turn off the spring and set a bias only + /// + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateSpringPropertiesWithBias(float inBias) + { + mSoftness = 0.0f; + mBias = inBias; + } + + /// Calculate spring properties based on frequency and damping ratio + /// + /// @param inDeltaTime Time step + /// @param inInvEffectiveMass Inverse effective mass K + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inFrequency Oscillation frequency (Hz). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param outEffectiveMass On return, this contains the new effective mass K^-1 + inline void CalculateSpringPropertiesWithFrequencyAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inFrequency, float inDamping, float &outEffectiveMass) + { + outEffectiveMass = 1.0f / inInvEffectiveMass; + + if (inFrequency > 0.0f) + { + // Calculate angular frequency + float omega = 2.0f * JPH_PI * inFrequency; + + // Calculate spring stiffness k and damping constant c (page 45) + float k = outEffectiveMass * Square(omega); + float c = 2.0f * outEffectiveMass * inDamping * omega; + + CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, k, c, outEffectiveMass); + } + else + { + CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate spring properties with spring Stiffness (k) and damping (c), this is based on the spring equation: F = -k * x - c * v + /// + /// @param inDeltaTime Time step + /// @param inInvEffectiveMass Inverse effective mass K + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inStiffness Spring stiffness k. Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inDamping Spring damping coefficient c. Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param outEffectiveMass On return, this contains the new effective mass K^-1 + inline void CalculateSpringPropertiesWithStiffnessAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass) + { + if (inStiffness > 0.0f) + { + CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, inStiffness, inDamping, outEffectiveMass); + } + else + { + outEffectiveMass = 1.0f / inInvEffectiveMass; + + CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Returns if this spring is active + inline bool IsActive() const + { + return mSoftness != 0.0f; + } + + /// Get total bias b, including supplied bias and bias for spring: lambda = J v + b + inline float GetBias(float inTotalLambda) const + { + // Remainder of post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354 + // + // Each iteration we are not computing the whole impulse, we are computing an increment to the impulse and we are updating the velocity. + // Also, as we solve each constraint we get a perfect v2, but then some other constraint will come along and mess it up. + // So we want to patch up the constraint while acknowledging the accumulated impulse and the damaged velocity. + // To help with that we use P for the accumulated impulse and lambda as the update. Mathematically we have: + // + // M * (v2new - v2damaged) = J^T * lambda + // J * v2new + softness * (total_lambda + lambda) + b = 0 + // + // If we solve this we get: + // + // v2new = v2damaged + M^-1 * J^T * lambda + // J * (v2damaged + M^-1 * J^T * lambda) + softness * total_lambda + softness * lambda + b = 0 + // + // (J * M^-1 * J^T + softness) * lambda = -(J * v2damaged + softness * total_lambda + b) + // + // So our lagrange multiplier becomes: + // + // lambda = -K^-1 (J v + softness * total_lambda + b) + // + // So we return the bias: softness * total_lambda + b + return mSoftness * inTotalLambda + mBias; + } + +private: + float mBias = 0.0f; + float mSoftness = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h new file mode 100644 index 000000000000..c2a47547f5e7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h @@ -0,0 +1,597 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// How the swing limit behaves +enum class ESwingType : uint8 +{ + Cone, ///< Swing is limited by a cone shape, note that this cone starts to deform for larger swing angles. Cone limits only support limits that are symmetric around 0. + Pyramid, ///< Swing is limited by a pyramid shape, note that this pyramid starts to deform for larger swing angles. +}; + +/// Quaternion based constraint that decomposes the rotation in constraint space in swing and twist: q = q_swing * q_twist +/// where q_swing.x = 0 and where q_twist.y = q_twist.z = 0 +/// +/// - Rotation around the twist (x-axis) is within [inTwistMinAngle, inTwistMaxAngle]. +/// - Rotation around the swing axis (y and z axis) are limited to an ellipsoid in quaternion space formed by the equation: +/// +/// (q_swing.y / sin(inSwingYHalfAngle / 2))^2 + (q_swing.z / sin(inSwingZHalfAngle / 2))^2 <= 1 +/// +/// Which roughly corresponds to an elliptic cone shape with major axis (inSwingYHalfAngle, inSwingZHalfAngle). +/// +/// In case inSwingYHalfAngle = 0, the rotation around Y will be constrained to 0 and the rotation around Z +/// will be constrained between [-inSwingZHalfAngle, inSwingZHalfAngle]. Vice versa if inSwingZHalfAngle = 0. +class SwingTwistConstraintPart +{ +public: + /// Override the swing type + void SetSwingType(ESwingType inSwingType) + { + mSwingType = inSwingType; + } + + /// Get the swing type for this part + ESwingType GetSwingType() const + { + return mSwingType; + } + + /// Set limits for this constraint (see description above for parameters) + void SetLimits(float inTwistMinAngle, float inTwistMaxAngle, float inSwingYMinAngle, float inSwingYMaxAngle, float inSwingZMinAngle, float inSwingZMaxAngle) + { + constexpr float cLockedAngle = DegreesToRadians(0.5f); + constexpr float cFreeAngle = DegreesToRadians(179.5f); + + // Assume sane input + JPH_ASSERT(inTwistMinAngle <= inTwistMaxAngle); + JPH_ASSERT(inSwingYMinAngle <= inSwingYMaxAngle); + JPH_ASSERT(inSwingZMinAngle <= inSwingZMaxAngle); + JPH_ASSERT(inSwingYMinAngle >= -JPH_PI && inSwingYMaxAngle <= JPH_PI); + JPH_ASSERT(inSwingZMinAngle >= -JPH_PI && inSwingZMaxAngle <= JPH_PI); + + // Calculate the sine and cosine of the half angles + Vec4 half_twist = 0.5f * Vec4(inTwistMinAngle, inTwistMaxAngle, 0, 0); + Vec4 twist_s, twist_c; + half_twist.SinCos(twist_s, twist_c); + Vec4 half_swing = 0.5f * Vec4(inSwingYMinAngle, inSwingYMaxAngle, inSwingZMinAngle, inSwingZMaxAngle); + Vec4 swing_s, swing_c; + half_swing.SinCos(swing_s, swing_c); + + // Store half angles for pyramid limit + mSwingYHalfMinAngle = half_swing.GetX(); + mSwingYHalfMaxAngle = half_swing.GetY(); + mSwingZHalfMinAngle = half_swing.GetZ(); + mSwingZHalfMaxAngle = half_swing.GetW(); + + // Store axis flags which are used at runtime to quickly decided which constraints to apply + mRotationFlags = 0; + if (inTwistMinAngle > -cLockedAngle && inTwistMaxAngle < cLockedAngle) + { + mRotationFlags |= TwistXLocked; + mSinTwistHalfMinAngle = 0.0f; + mSinTwistHalfMaxAngle = 0.0f; + mCosTwistHalfMinAngle = 1.0f; + mCosTwistHalfMaxAngle = 1.0f; + } + else if (inTwistMinAngle < -cFreeAngle && inTwistMaxAngle > cFreeAngle) + { + mRotationFlags |= TwistXFree; + mSinTwistHalfMinAngle = -1.0f; + mSinTwistHalfMaxAngle = 1.0f; + mCosTwistHalfMinAngle = 0.0f; + mCosTwistHalfMaxAngle = 0.0f; + } + else + { + mSinTwistHalfMinAngle = twist_s.GetX(); + mSinTwistHalfMaxAngle = twist_s.GetY(); + mCosTwistHalfMinAngle = twist_c.GetX(); + mCosTwistHalfMaxAngle = twist_c.GetY(); + } + + if (inSwingYMinAngle > -cLockedAngle && inSwingYMaxAngle < cLockedAngle) + { + mRotationFlags |= SwingYLocked; + mSinSwingYHalfMinAngle = 0.0f; + mSinSwingYHalfMaxAngle = 0.0f; + mCosSwingYHalfMinAngle = 1.0f; + mCosSwingYHalfMaxAngle = 1.0f; + } + else if (inSwingYMinAngle < -cFreeAngle && inSwingYMaxAngle > cFreeAngle) + { + mRotationFlags |= SwingYFree; + mSinSwingYHalfMinAngle = -1.0f; + mSinSwingYHalfMaxAngle = 1.0f; + mCosSwingYHalfMinAngle = 0.0f; + mCosSwingYHalfMaxAngle = 0.0f; + } + else + { + mSinSwingYHalfMinAngle = swing_s.GetX(); + mSinSwingYHalfMaxAngle = swing_s.GetY(); + mCosSwingYHalfMinAngle = swing_c.GetX(); + mCosSwingYHalfMaxAngle = swing_c.GetY(); + JPH_ASSERT(mSinSwingYHalfMinAngle <= mSinSwingYHalfMaxAngle); + } + + if (inSwingZMinAngle > -cLockedAngle && inSwingZMaxAngle < cLockedAngle) + { + mRotationFlags |= SwingZLocked; + mSinSwingZHalfMinAngle = 0.0f; + mSinSwingZHalfMaxAngle = 0.0f; + mCosSwingZHalfMinAngle = 1.0f; + mCosSwingZHalfMaxAngle = 1.0f; + } + else if (inSwingZMinAngle < -cFreeAngle && inSwingZMaxAngle > cFreeAngle) + { + mRotationFlags |= SwingZFree; + mSinSwingZHalfMinAngle = -1.0f; + mSinSwingZHalfMaxAngle = 1.0f; + mCosSwingZHalfMinAngle = 0.0f; + mCosSwingZHalfMaxAngle = 0.0f; + } + else + { + mSinSwingZHalfMinAngle = swing_s.GetZ(); + mSinSwingZHalfMaxAngle = swing_s.GetW(); + mCosSwingZHalfMinAngle = swing_c.GetZ(); + mCosSwingZHalfMaxAngle = swing_c.GetW(); + JPH_ASSERT(mSinSwingZHalfMinAngle <= mSinSwingZHalfMaxAngle); + } + } + + /// Flags to indicate which axis got clamped by ClampSwingTwist + static constexpr uint cClampedTwistMin = 1 << 0; + static constexpr uint cClampedTwistMax = 1 << 1; + static constexpr uint cClampedSwingYMin = 1 << 2; + static constexpr uint cClampedSwingYMax = 1 << 3; + static constexpr uint cClampedSwingZMin = 1 << 4; + static constexpr uint cClampedSwingZMax = 1 << 5; + + /// Helper function to determine if we're clamped against the min or max limit + static JPH_INLINE bool sDistanceToMinShorter(float inDeltaMin, float inDeltaMax) + { + // We're outside of the limits, get actual delta to min/max range + // Note that a swing/twist of -1 and 1 represent the same angle, so if the difference is bigger than 1, the shortest angle is the other way around (2 - difference) + // We should actually be working with angles rather than sin(angle / 2). When the difference is small the approximation is accurate, but + // when working with extreme values the calculation is off and e.g. when the limit is between 0 and 180 a value of approx -60 will clamp + // to 180 rather than 0 (you'd expect anything > -90 to go to 0). + inDeltaMin = abs(inDeltaMin); + if (inDeltaMin > 1.0f) inDeltaMin = 2.0f - inDeltaMin; + inDeltaMax = abs(inDeltaMax); + if (inDeltaMax > 1.0f) inDeltaMax = 2.0f - inDeltaMax; + return inDeltaMin < inDeltaMax; + } + + /// Clamp twist and swing against the constraint limits, returns which parts were clamped (everything assumed in constraint space) + inline void ClampSwingTwist(Quat &ioSwing, Quat &ioTwist, uint &outClampedAxis) const + { + // Start with not clamped + outClampedAxis = 0; + + // Check that swing and twist quaternions don't contain rotations around the wrong axis + JPH_ASSERT(ioSwing.GetX() == 0.0f); + JPH_ASSERT(ioTwist.GetY() == 0.0f); + JPH_ASSERT(ioTwist.GetZ() == 0.0f); + + // Ensure quaternions have w > 0 + bool negate_swing = ioSwing.GetW() < 0.0f; + if (negate_swing) + ioSwing = -ioSwing; + bool negate_twist = ioTwist.GetW() < 0.0f; + if (negate_twist) + ioTwist = -ioTwist; + + if (mRotationFlags & TwistXLocked) + { + // Twist axis is locked, clamp whenever twist is not identity + outClampedAxis |= ioTwist.GetX() != 0.0f? (cClampedTwistMin | cClampedTwistMax) : 0; + ioTwist = Quat::sIdentity(); + } + else if ((mRotationFlags & TwistXFree) == 0) + { + // Twist axis has limit, clamp whenever out of range + float delta_min = mSinTwistHalfMinAngle - ioTwist.GetX(); + float delta_max = ioTwist.GetX() - mSinTwistHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the twist that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioTwist = Quat(mSinTwistHalfMinAngle, 0, 0, mCosTwistHalfMinAngle); + outClampedAxis |= cClampedTwistMin; + } + else + { + ioTwist = Quat(mSinTwistHalfMaxAngle, 0, 0, mCosTwistHalfMaxAngle); + outClampedAxis |= cClampedTwistMax; + } + } + } + + // Clamp swing + if (mRotationFlags & SwingYLocked) + { + if (mRotationFlags & SwingZLocked) + { + // Both swing Y and Z are disabled, no degrees of freedom in swing + outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0; + outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0; + ioSwing = Quat::sIdentity(); + } + else + { + // Swing Y angle disabled, only 1 degree of freedom in swing + outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0; + float delta_min = mSinSwingZHalfMinAngle - ioSwing.GetZ(); + float delta_max = ioSwing.GetZ() - mSinSwingZHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the swing that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioSwing = Quat(0, 0, mSinSwingZHalfMinAngle, mCosSwingZHalfMinAngle); + outClampedAxis |= cClampedSwingZMin; + } + else + { + ioSwing = Quat(0, 0, mSinSwingZHalfMaxAngle, mCosSwingZHalfMaxAngle); + outClampedAxis |= cClampedSwingZMax; + } + } + else if ((outClampedAxis & cClampedSwingYMin) != 0) + { + float z = ioSwing.GetZ(); + ioSwing = Quat(0, 0, z, sqrt(1.0f - Square(z))); + } + } + } + else if (mRotationFlags & SwingZLocked) + { + // Swing Z angle disabled, only 1 degree of freedom in swing + outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0; + float delta_min = mSinSwingYHalfMinAngle - ioSwing.GetY(); + float delta_max = ioSwing.GetY() - mSinSwingYHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the swing that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioSwing = Quat(0, mSinSwingYHalfMinAngle, 0, mCosSwingYHalfMinAngle); + outClampedAxis |= cClampedSwingYMin; + } + else + { + ioSwing = Quat(0, mSinSwingYHalfMaxAngle, 0, mCosSwingYHalfMaxAngle); + outClampedAxis |= cClampedSwingYMax; + } + } + else if ((outClampedAxis & cClampedSwingZMin) != 0) + { + float y = ioSwing.GetY(); + ioSwing = Quat(0, y, 0, sqrt(1.0f - Square(y))); + } + } + else + { + // Two degrees of freedom + if (mSwingType == ESwingType::Cone) + { + // Use ellipse to solve limits + Ellipse ellipse(mSinSwingYHalfMaxAngle, mSinSwingZHalfMaxAngle); + Float2 point(ioSwing.GetY(), ioSwing.GetZ()); + if (!ellipse.IsInside(point)) + { + Float2 closest = ellipse.GetClosestPoint(point); + ioSwing = Quat(0, closest.x, closest.y, sqrt(max(0.0f, 1.0f - Square(closest.x) - Square(closest.y)))); + outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here + } + } + else + { + // Use pyramid to solve limits + // The quaternion rotating by angle y around the Y axis then rotating by angle z around the Z axis is: + // q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y) + // [q.x, q.y, q.z, q.w] = [-sin(y / 2) * sin(z / 2), sin(y / 2) * cos(z / 2), cos(y / 2) * sin(z / 2), cos(y / 2) * cos(z / 2)] + // So we can calculate y / 2 = atan2(q.y, q.w) and z / 2 = atan2(q.z, q.w) + Vec4 half_angle = Vec4::sATan2(ioSwing.GetXYZW().Swizzle(), ioSwing.GetXYZW().SplatW()); + Vec4 min_half_angle(mSwingYHalfMinAngle, mSwingYHalfMinAngle, mSwingZHalfMinAngle, mSwingZHalfMinAngle); + Vec4 max_half_angle(mSwingYHalfMaxAngle, mSwingYHalfMaxAngle, mSwingZHalfMaxAngle, mSwingZHalfMaxAngle); + Vec4 clamped_half_angle = Vec4::sMin(Vec4::sMax(half_angle, min_half_angle), max_half_angle); + UVec4 unclamped = Vec4::sEquals(half_angle, clamped_half_angle); + if (!unclamped.TestAllTrue()) + { + // We now calculate the quaternion again using the formula for q above, + // but we leave out the x component in order to not introduce twist + Vec4 s, c; + clamped_half_angle.SinCos(s, c); + ioSwing = Quat(0, s.GetY() * c.GetZ(), c.GetY() * s.GetZ(), c.GetY() * c.GetZ()).Normalized(); + outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here + } + } + } + + // Flip sign back + if (negate_swing) + ioSwing = -ioSwing; + if (negate_twist) + ioTwist = -ioTwist; + + JPH_ASSERT(ioSwing.IsNormalized()); + JPH_ASSERT(ioTwist.IsNormalized()); + } + + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inConstraintRotation The current rotation of the constraint in constraint space + /// @param inConstraintToWorld Rotates from constraint space into world space + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, QuatArg inConstraintRotation, QuatArg inConstraintToWorld) + { + // Decompose into swing and twist + Quat q_swing, q_twist; + inConstraintRotation.GetSwingTwist(q_swing, q_twist); + + // Clamp against joint limits + Quat q_clamped_swing = q_swing, q_clamped_twist = q_twist; + uint clamped_axis; + ClampSwingTwist(q_clamped_swing, q_clamped_twist, clamped_axis); + + if (mRotationFlags & SwingYLocked) + { + Quat twist_to_world = inConstraintToWorld * q_swing; + mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY(); + mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ(); + + if (mRotationFlags & SwingZLocked) + { + // Swing fully locked + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else + { + // Swing only locked around Y + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + if ((clamped_axis & (cClampedSwingZMin | cClampedSwingZMax)) != 0) + { + if ((clamped_axis & cClampedSwingZMin) != 0) + mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else + mSwingLimitZConstraintPart.Deactivate(); + } + } + else if (mRotationFlags & SwingZLocked) + { + // Swing only locked around Z + Quat twist_to_world = inConstraintToWorld * q_swing; + mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY(); + mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ(); + + if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax)) != 0) + { + if ((clamped_axis & cClampedSwingYMin) != 0) + mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + } + else + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else if ((mRotationFlags & SwingYZFree) != SwingYZFree) + { + // Swing has limits around Y and Z + if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax)) != 0) + { + // Calculate axis of rotation from clamped swing to swing + Vec3 current = (inConstraintToWorld * q_swing).RotateAxisX(); + Vec3 desired = (inConstraintToWorld * q_clamped_swing).RotateAxisX(); + mWorldSpaceSwingLimitYRotationAxis = desired.Cross(current); + float len = mWorldSpaceSwingLimitYRotationAxis.Length(); + if (len != 0.0f) + { + mWorldSpaceSwingLimitYRotationAxis /= len; + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + } + else + mSwingLimitYConstraintPart.Deactivate(); + } + else + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + } + else + { + // No swing limits + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + } + + if (mRotationFlags & TwistXLocked) + { + // Twist locked, always activate constraint + mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX(); + mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis); + } + else if ((mRotationFlags & TwistXFree) == 0) + { + // Twist has limits + if ((clamped_axis & (cClampedTwistMin | cClampedTwistMax)) != 0) + { + mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX(); + if ((clamped_axis & cClampedTwistMin) != 0) + mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis); + } + else + mTwistLimitConstraintPart.Deactivate(); + } + else + { + // No twist limits + mTwistLimitConstraintPart.Deactivate(); + } + } + + /// Deactivate this constraint + void Deactivate() + { + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + mTwistLimitConstraintPart.Deactivate(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mSwingLimitYConstraintPart.IsActive() || mSwingLimitZConstraintPart.IsActive() || mTwistLimitConstraintPart.IsActive(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mSwingLimitYConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + mSwingLimitZConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + mTwistLimitConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + bool impulse = false; + + // Solve swing constraint + if (mSwingLimitYConstraintPart.IsActive()) + impulse |= mSwingLimitYConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitYRotationAxis, -FLT_MAX, mSinSwingYHalfMinAngle == mSinSwingYHalfMaxAngle? FLT_MAX : 0.0f); + + if (mSwingLimitZConstraintPart.IsActive()) + impulse |= mSwingLimitZConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitZRotationAxis, -FLT_MAX, mSinSwingZHalfMinAngle == mSinSwingZHalfMaxAngle? FLT_MAX : 0.0f); + + // Solve twist constraint + if (mTwistLimitConstraintPart.IsActive()) + impulse |= mTwistLimitConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceTwistLimitRotationAxis, -FLT_MAX, mSinTwistHalfMinAngle == mSinTwistHalfMaxAngle? FLT_MAX : 0.0f); + + return impulse; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inConstraintRotation The current rotation of the constraint in constraint space + /// @param inConstraintToBody1 , inConstraintToBody2 Rotates from constraint space to body 1/2 space + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inConstraintRotation, QuatArg inConstraintToBody1, QuatArg inConstraintToBody2, float inBaumgarte) const + { + Quat q_swing, q_twist; + inConstraintRotation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + ClampSwingTwist(q_swing, q_twist, clamped_axis); + + // Solve rotation violations + if (clamped_axis != 0) + { + RotationEulerConstraintPart part; + Quat inv_initial_orientation = inConstraintToBody2 * (inConstraintToBody1 * q_swing * q_twist).Conjugated(); + part.CalculateConstraintProperties(ioBody1, Mat44::sRotation(ioBody1.GetRotation()), ioBody2, Mat44::sRotation(ioBody2.GetRotation())); + return part.SolvePositionConstraint(ioBody1, ioBody2, inv_initial_orientation, inBaumgarte); + } + + return false; + } + + /// Return lagrange multiplier for swing + inline float GetTotalSwingYLambda() const + { + return mSwingLimitYConstraintPart.GetTotalLambda(); + } + + inline float GetTotalSwingZLambda() const + { + return mSwingLimitZConstraintPart.GetTotalLambda(); + } + + /// Return lagrange multiplier for twist + inline float GetTotalTwistLambda() const + { + return mTwistLimitConstraintPart.GetTotalLambda(); + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + mSwingLimitYConstraintPart.SaveState(inStream); + mSwingLimitZConstraintPart.SaveState(inStream); + mTwistLimitConstraintPart.SaveState(inStream); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + mSwingLimitYConstraintPart.RestoreState(inStream); + mSwingLimitZConstraintPart.RestoreState(inStream); + mTwistLimitConstraintPart.RestoreState(inStream); + } + +private: + // CONFIGURATION PROPERTIES FOLLOW + + enum ERotationFlags + { + /// Indicates that axis is completely locked (cannot rotate around this axis) + TwistXLocked = 1 << 0, + SwingYLocked = 1 << 1, + SwingZLocked = 1 << 2, + + /// Indicates that axis is completely free (can rotate around without limits) + TwistXFree = 1 << 3, + SwingYFree = 1 << 4, + SwingZFree = 1 << 5, + SwingYZFree = SwingYFree | SwingZFree + }; + + uint8 mRotationFlags; + + // Constants + ESwingType mSwingType = ESwingType::Cone; + float mSinTwistHalfMinAngle; + float mSinTwistHalfMaxAngle; + float mCosTwistHalfMinAngle; + float mCosTwistHalfMaxAngle; + float mSwingYHalfMinAngle; + float mSwingYHalfMaxAngle; + float mSwingZHalfMinAngle; + float mSwingZHalfMaxAngle; + float mSinSwingYHalfMinAngle; + float mSinSwingYHalfMaxAngle; + float mSinSwingZHalfMinAngle; + float mSinSwingZHalfMaxAngle; + float mCosSwingYHalfMinAngle; + float mCosSwingYHalfMaxAngle; + float mCosSwingZHalfMinAngle; + float mCosSwingZHalfMaxAngle; + + // RUN TIME PROPERTIES FOLLOW + + /// Rotation axis for the angle constraint parts + Vec3 mWorldSpaceSwingLimitYRotationAxis; + Vec3 mWorldSpaceSwingLimitZRotationAxis; + Vec3 mWorldSpaceTwistLimitRotationAxis; + + /// The constraint parts + AngleConstraintPart mSwingLimitYConstraintPart; + AngleConstraintPart mSwingLimitZConstraintPart; + AngleConstraintPart mTwistLimitConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp new file mode 100644 index 000000000000..928acdaa4221 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp @@ -0,0 +1,1783 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +#ifdef JPH_DEBUG_RENDERER +bool ContactConstraintManager::sDrawContactPoint = false; +bool ContactConstraintManager::sDrawSupportingFaces = false; +bool ContactConstraintManager::sDrawContactPointReduction = false; +bool ContactConstraintManager::sDrawContactManifolds = false; +#endif // JPH_DEBUG_RENDERER + +//#define JPH_MANIFOLD_CACHE_DEBUG + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::WorldContactPoint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal) +{ + // Calculate collision points relative to body + RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2); + Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition()); + Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition()); + + mNonPenetrationConstraint.CalculateConstraintPropertiesWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, r1, inBody2, inInvMass2, inInvInertiaScale2, r2, inWorldSpaceNormal); +} + +template +JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution) +{ + JPH_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2 + << " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2 + << " restitution: " << inSettings.mCombinedRestitution << " friction: " << inSettings.mCombinedFriction << " minv: " << inMinVelocityForRestitution + << " surface_vel: " << inSettings.mRelativeLinearSurfaceVelocity << " surface_ang: " << inSettings.mRelativeAngularSurfaceVelocity); + + // Calculate collision points relative to body + RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2); + Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition()); + Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition()); + + // The gravity is applied in the beginning of the time step. If we get here, there was a collision + // at the beginning of the time step, so we've applied too much gravity. This means that our + // calculated restitution can be too high, so when we apply restitution, we cancel the added + // velocity due to gravity. + float gravity_dt_dot_normal; + + // Calculate velocity of collision points + Vec3 relative_velocity; + if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + relative_velocity = mp2->GetPointVelocityCOM(r2) - mp1->GetPointVelocityCOM(r1); + gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * (mp2->GetGravityFactor() - mp1->GetGravityFactor()); + } + else if constexpr (Type1 != EMotionType::Static) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + relative_velocity = -mp1->GetPointVelocityCOM(r1); + gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp1->GetGravityFactor(); + } + else if constexpr (Type2 != EMotionType::Static) + { + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + relative_velocity = mp2->GetPointVelocityCOM(r2); + gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp2->GetGravityFactor(); + } + else + { + JPH_ASSERT(false); // Static vs static makes no sense + relative_velocity = Vec3::sZero(); + gravity_dt_dot_normal = 0.0f; + } + float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal); + + // How much the shapes are penetrating (> 0 if penetrating, < 0 if separated) + float penetration = Vec3(inWorldSpacePosition1 - inWorldSpacePosition2).Dot(inWorldSpaceNormal); + + // If there is no penetration, this is a speculative contact and we will apply a bias to the contact constraint + // so that the constraint becomes relative_velocity . contact normal > -penetration / delta_time + // instead of relative_velocity . contact normal > 0 + // See: GDC 2013: "Physics for Game Programmers; Continuous Collision" - Erin Catto + float speculative_contact_velocity_bias = max(0.0f, -penetration / inDeltaTime); + + // Determine if the velocity is big enough for restitution + float normal_velocity_bias; + if (inSettings.mCombinedRestitution > 0.0f && normal_velocity < -inMinVelocityForRestitution) + { + // We have a velocity that is big enough for restitution. This is where speculative contacts don't work + // great as we have to decide now if we're going to apply the restitution or not. If the relative + // velocity is big enough for a hit, we apply the restitution (in the end, due to other constraints, + // the objects may actually not collide and we will have applied restitution incorrectly). Another + // artifact that occurs because of this approximation is that the object will bounce from its current + // position rather than from a position where it is touching the other object. This causes the object + // to appear to move faster for 1 frame (the opposite of time stealing). + if (normal_velocity < -speculative_contact_velocity_bias) + normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - gravity_dt_dot_normal); + else + // In this case we have predicted that we don't hit the other object, but if we do (due to other constraints changing velocities) + // the speculative contact will prevent penetration but will not apply restitution leading to another artifact. + normal_velocity_bias = speculative_contact_velocity_bias; + } + else + { + // No restitution. We can safely apply our contact velocity bias. + normal_velocity_bias = speculative_contact_velocity_bias; + } + + mNonPenetrationConstraint.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias); + + // Calculate friction part + if (inSettings.mCombinedFriction > 0.0f) + { + // Get surface velocity relative to tangents + Vec3 ws_surface_velocity = inSettings.mRelativeLinearSurfaceVelocity + inSettings.mRelativeAngularSurfaceVelocity.Cross(r1); + float surface_velocity1 = inWorldSpaceTangent1.Dot(ws_surface_velocity); + float surface_velocity2 = inWorldSpaceTangent2.Dot(ws_surface_velocity); + + // Implement friction as 2 AxisContraintParts + mFrictionConstraint1.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent1, surface_velocity1); + mFrictionConstraint2.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent2, surface_velocity2); + } + else + { + // Turn off friction constraint + mFrictionConstraint1.Deactivate(); + mFrictionConstraint2.Deactivate(); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::ContactConstraint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +#ifdef JPH_DEBUG_RENDERER +void ContactConstraintManager::ContactConstraint::Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const +{ + if (mContactPoints.empty()) + return; + + // Get body transforms + RMat44 transform_body1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform_body2 = mBody2->GetCenterOfMassTransform(); + + RVec3 prev_point = transform_body1 * Vec3::sLoadFloat3Unsafe(mContactPoints.back().mContactPoint->mPosition1); + for (const WorldContactPoint &wcp : mContactPoints) + { + // Test if any lambda from the previous frame was transferred + float radius = wcp.mNonPenetrationConstraint.GetTotalLambda() == 0.0f + && wcp.mFrictionConstraint1.GetTotalLambda() == 0.0f + && wcp.mFrictionConstraint2.GetTotalLambda() == 0.0f? 0.1f : 0.2f; + + RVec3 next_point = transform_body1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + inRenderer->DrawMarker(next_point, Color::sCyan, radius); + inRenderer->DrawMarker(transform_body2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2), Color::sPurple, radius); + + // Draw edge + inRenderer->DrawArrow(prev_point, next_point, inManifoldColor, 0.05f); + prev_point = next_point; + } + + // Draw normal + RVec3 wp = transform_body1 * Vec3::sLoadFloat3Unsafe(mContactPoints[0].mContactPoint->mPosition1); + inRenderer->DrawArrow(wp, wp + GetWorldSpaceNormal(), Color::sRed, 0.05f); + + // Get tangents + Vec3 t1, t2; + GetTangents(t1, t2); + + // Draw tangents + inRenderer->DrawLine(wp, wp + t1, Color::sGreen); + inRenderer->DrawLine(wp, wp + t2, Color::sBlue); +} +#endif // JPH_DEBUG_RENDERER + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedContactPoint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedContactPoint::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mPosition1); + inStream.Write(mPosition2); + inStream.Write(mNonPenetrationLambda); + inStream.Write(mFrictionLambda); +} + +void ContactConstraintManager::CachedContactPoint::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition1); + inStream.Read(mPosition2); + inStream.Read(mNonPenetrationLambda); + inStream.Read(mFrictionLambda); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedManifold +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedManifold::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mContactNormal); +} + +void ContactConstraintManager::CachedManifold::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mContactNormal); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedBodyPair +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedBodyPair::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mDeltaPosition); + inStream.Write(mDeltaRotation); +} + +void ContactConstraintManager::CachedBodyPair::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mDeltaPosition); + inStream.Read(mDeltaRotation); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::ManifoldCache +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::ManifoldCache::Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize) +{ + mAllocator.Init(inMaxBodyPairs * sizeof(BodyPairMap::KeyValue) + inCachedManifoldsSize); + mCachedManifolds.Init(GetNextPowerOf2(inMaxContactConstraints)); + mCachedBodyPairs.Init(GetNextPowerOf2(inMaxBodyPairs)); +} + +void ContactConstraintManager::ManifoldCache::Clear() +{ + JPH_PROFILE_FUNCTION(); + + mCachedManifolds.Clear(); + mCachedBodyPairs.Clear(); + mAllocator.Clear(); + +#ifdef JPH_ENABLE_ASSERTS + // Mark as incomplete + mIsFinalized = false; +#endif +} + +void ContactConstraintManager::ManifoldCache::Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds) +{ + // Minimum amount of buckets to use in the hash map + constexpr uint32 cMinBuckets = 1024; + + // Use the next higher power of 2 of amount of objects in the cache from last frame to determine the amount of buckets in this frame + mCachedManifolds.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumManifolds)), mCachedManifolds.GetMaxBuckets())); + mCachedBodyPairs.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumBodyPairs)), mCachedBodyPairs.GetMaxBuckets())); +} + +const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedManifolds.Find(inKey, inKeyHash); +} + +ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints) +{ + JPH_ASSERT(!mIsFinalized); + MKeyValue *kv = mCachedManifolds.Create(ioContactAllocator, inKey, inKeyHash, CachedManifold::sGetRequiredExtraSize(inNumContactPoints)); + if (kv == nullptr) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ManifoldCacheFull; + return nullptr; + } + kv->GetValue().mNumContactPoints = uint16(inNumContactPoints); + ++ioContactAllocator.mNumManifolds; + return kv; +} + +ContactConstraintManager::MKVAndCreated ContactConstraintManager::ManifoldCache::FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints) +{ + MKeyValue *kv = const_cast(mCachedManifolds.Find(inKey, inKeyHash)); + if (kv != nullptr) + return { kv, false }; + + return { Create(ioContactAllocator, inKey, inKeyHash, inNumContactPoints), true }; +} + +uint32 ContactConstraintManager::ManifoldCache::ToHandle(const MKeyValue *inKeyValue) const +{ + JPH_ASSERT(!mIsFinalized); + return mCachedManifolds.ToHandle(inKeyValue); +} + +const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::FromHandle(uint32 inHandle) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedManifolds.FromHandle(inHandle); +} + +const ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Find(const BodyPair &inKey, uint64 inKeyHash) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedBodyPairs.Find(inKey, inKeyHash); +} + +ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash) +{ + JPH_ASSERT(!mIsFinalized); + BPKeyValue *kv = mCachedBodyPairs.Create(ioContactAllocator, inKey, inKeyHash, 0); + if (kv == nullptr) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::BodyPairCacheFull; + return nullptr; + } + ++ioContactAllocator.mNumBodyPairs; + return kv; +} + +void ContactConstraintManager::ManifoldCache::GetAllBodyPairsSorted(Array &outAll) const +{ + JPH_ASSERT(mIsFinalized); + mCachedBodyPairs.GetAllKeyValues(outAll); + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const BPKeyValue *inLHS, const BPKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array &outAll) const +{ + JPH_ASSERT(mIsFinalized); + + // Iterate through the attached manifolds + for (uint32 handle = inBodyPair.mFirstCachedManifold; handle != ManifoldMap::cInvalidHandle; handle = FromHandle(handle)->GetValue().mNextWithSameBodyPair) + { + const MKeyValue *kv = mCachedManifolds.FromHandle(handle); + outAll.push_back(kv); + } + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::GetAllCCDManifoldsSorted(Array &outAll) const +{ + mCachedManifolds.GetAllKeyValues(outAll); + + for (int i = (int)outAll.size() - 1; i >= 0; --i) + if ((outAll[i]->GetValue().mFlags & (uint16)CachedManifold::EFlags::CCDContact) == 0) + { + outAll[i] = outAll.back(); + outAll.pop_back(); + } + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::ContactPointRemovedCallbacks(ContactListener *inListener) +{ + JPH_PROFILE_FUNCTION(); + + for (MKeyValue &kv : mCachedManifolds) + if ((kv.GetValue().mFlags & uint16(CachedManifold::EFlags::ContactPersisted)) == 0) + inListener->OnContactRemoved(kv.GetKey()); +} + +#ifdef JPH_ENABLE_ASSERTS + +void ContactConstraintManager::ManifoldCache::Finalize() +{ + mIsFinalized = true; + +#ifdef JPH_MANIFOLD_CACHE_DEBUG + Trace("ManifoldMap:"); + mCachedManifolds.TraceStats(); + Trace("BodyPairMap:"); + mCachedBodyPairs.TraceStats(); +#endif // JPH_MANIFOLD_CACHE_DEBUG +} + +#endif + +void ContactConstraintManager::ManifoldCache::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + JPH_ASSERT(mIsFinalized); + + // Get contents of cache + Array all_bp; + GetAllBodyPairsSorted(all_bp); + + // Determine which ones to save + Array selected_bp; + if (inFilter == nullptr) + selected_bp = std::move(all_bp); + else + { + selected_bp.reserve(all_bp.size()); + for (const BPKeyValue *bp_kv : all_bp) + if (inFilter->ShouldSaveContact(bp_kv->GetKey().mBodyA, bp_kv->GetKey().mBodyB)) + selected_bp.push_back(bp_kv); + } + + // Write body pairs + uint32 num_body_pairs = uint32(selected_bp.size()); + inStream.Write(num_body_pairs); + for (const BPKeyValue *bp_kv : selected_bp) + { + // Write body pair key + inStream.Write(bp_kv->GetKey()); + + // Write body pair + const CachedBodyPair &bp = bp_kv->GetValue(); + bp.SaveState(inStream); + + // Get attached manifolds + Array all_m; + GetAllManifoldsSorted(bp, all_m); + + // Write num manifolds + uint32 num_manifolds = uint32(all_m.size()); + inStream.Write(num_manifolds); + + // Write all manifolds + for (const MKeyValue *m_kv : all_m) + { + // Write key + inStream.Write(m_kv->GetKey()); + const CachedManifold &cm = m_kv->GetValue(); + JPH_ASSERT((cm.mFlags & (uint16)CachedManifold::EFlags::CCDContact) == 0); + + // Write amount of contacts + inStream.Write(cm.mNumContactPoints); + + // Write manifold + cm.SaveState(inStream); + + // Write contact points + for (uint32 i = 0; i < cm.mNumContactPoints; ++i) + cm.mContactPoints[i].SaveState(inStream); + } + } + + // Get CCD manifolds + Array all_m; + GetAllCCDManifoldsSorted(all_m); + + // Determine which ones to save + Array selected_m; + if (inFilter == nullptr) + selected_m = std::move(all_m); + else + { + selected_m.reserve(all_m.size()); + for (const MKeyValue *m_kv : all_m) + if (inFilter->ShouldSaveContact(m_kv->GetKey().GetBody1ID(), m_kv->GetKey().GetBody2ID())) + selected_m.push_back(m_kv); + } + + // Write all CCD manifold keys + uint32 num_manifolds = uint32(selected_m.size()); + inStream.Write(num_manifolds); + for (const MKeyValue *m_kv : selected_m) + inStream.Write(m_kv->GetKey()); +} + +bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter) +{ + JPH_ASSERT(!mIsFinalized); + + bool success = true; + + // Create a contact allocator for restoring the contact cache + ContactAllocator contact_allocator(GetContactAllocator()); + + // When validating, get all existing body pairs + Array all_bp; + if (inStream.IsValidating()) + inReadCache.GetAllBodyPairsSorted(all_bp); + + // Read amount of body pairs + uint32 num_body_pairs; + if (inStream.IsValidating()) + num_body_pairs = uint32(all_bp.size()); + inStream.Read(num_body_pairs); + + // Read entire cache + for (uint32 i = 0; i < num_body_pairs; ++i) + { + // Read key + BodyPair body_pair_key; + if (inStream.IsValidating() && i < all_bp.size()) + body_pair_key = all_bp[i]->GetKey(); + inStream.Read(body_pair_key); + + // Check if we want to restore this contact + if (inFilter == nullptr || inFilter->ShouldRestoreContact(body_pair_key.mBodyA, body_pair_key.mBodyB)) + { + // Create new entry for this body pair + uint64 body_pair_hash = body_pair_key.GetHash(); + BPKeyValue *bp_kv = Create(contact_allocator, body_pair_key, body_pair_hash); + if (bp_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedBodyPair &bp = bp_kv->GetValue(); + + // Read body pair + if (inStream.IsValidating() && i < all_bp.size()) + memcpy(&bp, &all_bp[i]->GetValue(), sizeof(CachedBodyPair)); + bp.RestoreState(inStream); + + // When validating, get all existing manifolds + Array all_m; + if (inStream.IsValidating()) + inReadCache.GetAllManifoldsSorted(all_bp[i]->GetValue(), all_m); + + // Read amount of manifolds + uint32 num_manifolds = 0; + if (inStream.IsValidating()) + num_manifolds = uint32(all_m.size()); + inStream.Read(num_manifolds); + + uint32 handle = ManifoldMap::cInvalidHandle; + for (uint32 j = 0; j < num_manifolds; ++j) + { + // Read key + SubShapeIDPair sub_shape_key; + if (inStream.IsValidating() && j < all_m.size()) + sub_shape_key = all_m[j]->GetKey(); + inStream.Read(sub_shape_key); + uint64 sub_shape_key_hash = sub_shape_key.GetHash(); + + // Read amount of contact points + uint16 num_contact_points = 0; + if (inStream.IsValidating() && j < all_m.size()) + num_contact_points = all_m[j]->GetValue().mNumContactPoints; + inStream.Read(num_contact_points); + + // Read manifold + MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, num_contact_points); + if (m_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedManifold &cm = m_kv->GetValue(); + if (inStream.IsValidating() && j < all_m.size()) + { + memcpy(&cm, &all_m[j]->GetValue(), CachedManifold::sGetRequiredTotalSize(num_contact_points)); + cm.mNumContactPoints = uint16(num_contact_points); // Restore num contact points + } + cm.RestoreState(inStream); + cm.mNextWithSameBodyPair = handle; + handle = ToHandle(m_kv); + + // Read contact points + for (uint32 k = 0; k < num_contact_points; ++k) + cm.mContactPoints[k].RestoreState(inStream); + } + bp.mFirstCachedManifold = handle; + } + else + { + // Skip the contact + CachedBodyPair bp; + bp.RestoreState(inStream); + uint32 num_manifolds = 0; + inStream.Read(num_manifolds); + for (uint32 j = 0; j < num_manifolds; ++j) + { + SubShapeIDPair sub_shape_key; + inStream.Read(sub_shape_key); + uint16 num_contact_points; + inStream.Read(num_contact_points); + CachedManifold cm; + cm.RestoreState(inStream); + for (uint32 k = 0; k < num_contact_points; ++k) + cm.mContactPoints[0].RestoreState(inStream); + } + } + } + + // When validating, get all existing CCD manifolds + Array all_m; + if (inStream.IsValidating()) + inReadCache.GetAllCCDManifoldsSorted(all_m); + + // Read amount of CCD manifolds + uint32 num_manifolds; + if (inStream.IsValidating()) + num_manifolds = uint32(all_m.size()); + inStream.Read(num_manifolds); + + for (uint32 j = 0; j < num_manifolds; ++j) + { + // Read key + SubShapeIDPair sub_shape_key; + if (inStream.IsValidating() && j < all_m.size()) + sub_shape_key = all_m[j]->GetKey(); + inStream.Read(sub_shape_key); + + // Check if we want to restore this contact + if (inFilter == nullptr || inFilter->ShouldRestoreContact(sub_shape_key.GetBody1ID(), sub_shape_key.GetBody2ID())) + { + // Create CCD manifold + uint64 sub_shape_key_hash = sub_shape_key.GetHash(); + MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, 0); + if (m_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedManifold &cm = m_kv->GetValue(); + cm.mFlags |= (uint16)CachedManifold::EFlags::CCDContact; + } + } + +#ifdef JPH_ENABLE_ASSERTS + // We don't finalize until the last part is restored + if (inStream.IsLastPart()) + mIsFinalized = true; +#endif + + return success; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +ContactConstraintManager::ContactConstraintManager(const PhysicsSettings &inPhysicsSettings) : + mPhysicsSettings(inPhysicsSettings) +{ +#ifdef JPH_ENABLE_ASSERTS + // For the first frame mark this empty buffer as finalized + mCache[mCacheWriteIdx ^ 1].Finalize(); +#endif +} + +ContactConstraintManager::~ContactConstraintManager() +{ + JPH_ASSERT(mConstraints == nullptr); +} + +void ContactConstraintManager::Init(uint inMaxBodyPairs, uint inMaxContactConstraints) +{ + mMaxConstraints = inMaxContactConstraints; + + // Calculate worst case cache usage + uint cached_manifolds_size = inMaxContactConstraints * (sizeof(CachedManifold) + (MaxContactPoints - 1) * sizeof(CachedContactPoint)); + + // Init the caches + mCache[0].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size); + mCache[1].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size); +} + +void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inContext) +{ + // Store context + mUpdateContext = inContext; + + // Allocate temporary constraint buffer + JPH_ASSERT(mConstraints == nullptr); + mConstraints = (ContactConstraint *)inContext->mTempAllocator->Allocate(mMaxConstraints * sizeof(ContactConstraint)); +} + +template +JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2) +{ + // Calculate scaled mass and inertia + Mat44 inv_i1; + if constexpr (Type1 == EMotionType::Dynamic) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + inv_i1 = inSettings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inTransformBody1.GetRotation()); + } + else + { + inv_i1 = Mat44::sZero(); + } + + Mat44 inv_i2; + if constexpr (Type2 == EMotionType::Dynamic) + { + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + inv_i2 = inSettings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inTransformBody2.GetRotation()); + } + else + { + inv_i2 = Mat44::sZero(); + } + + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + // Calculate value for restitution correction + float gravity_dt_dot_normal = inGravityDeltaTime.Dot(ws_normal); + + // Setup velocity constraint properties + float min_velocity_for_restitution = mPhysicsSettings.mMinVelocityForRestitution; + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + RVec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + RVec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2); + wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(inDeltaTime, gravity_dt_dot_normal, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution); + } +} + +inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2) +{ + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Kinematic: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Static: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + default: + JPH_ASSERT(false); + break; + } +} + +void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated) +{ + JPH_PROFILE_FUNCTION(); + + // Start with nothing found and not handled + outConstraintCreated = false; + outPairHandled = false; + + // Swap bodies so that body 1 id < body 2 id + Body *body1, *body2; + if (inBody1.GetID() < inBody2.GetID()) + { + body1 = &inBody1; + body2 = &inBody2; + } + else + { + body1 = &inBody2; + body2 = &inBody1; + } + + // Find the cached body pair + BodyPair body_pair_key(body1->GetID(), body2->GetID()); + uint64 body_pair_hash = body_pair_key.GetHash(); + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const BPKeyValue *kv = read_cache.Find(body_pair_key, body_pair_hash); + if (kv == nullptr) + return; + const CachedBodyPair &input_cbp = kv->GetValue(); + + // Get relative translation + Quat inv_r1 = body1->GetRotation().Conjugated(); + Vec3 delta_position = inv_r1 * Vec3(body2->GetCenterOfMassPosition() - body1->GetCenterOfMassPosition()); + + // Get old position delta + Vec3 old_delta_position = Vec3::sLoadFloat3Unsafe(input_cbp.mDeltaPosition); + + // Check if bodies are still roughly in the same relative position + if ((delta_position - old_delta_position).LengthSq() > mPhysicsSettings.mBodyPairCacheMaxDeltaPositionSq) + return; + + // Determine relative orientation + Quat delta_rotation = inv_r1 * body2->GetRotation(); + + // Reconstruct old quaternion delta + Quat old_delta_rotation = Quat::sLoadFloat3Unsafe(input_cbp.mDeltaRotation); + + // Check if bodies are still roughly in the same relative orientation + // The delta between 2 quaternions p and q is: p q^* = [rotation_axis * sin(angle / 2), cos(angle / 2)] + // From the W component we can extract the angle: cos(angle / 2) = px * qx + py * qy + pz * qz + pw * qw = p . q + // Since we want to abort if the rotation is smaller than -angle or bigger than angle, we can write the comparison as |p . q| < cos(angle / 2) + if (abs(delta_rotation.Dot(old_delta_rotation)) < mPhysicsSettings.mBodyPairCacheCosMaxDeltaRotationDiv2) + return; + + // The cache is valid, return that we've handled this body pair + outPairHandled = true; + + // Copy the cached body pair to this frame + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + BPKeyValue *output_bp_kv = write_cache.Create(ioContactAllocator, body_pair_key, body_pair_hash); + if (output_bp_kv == nullptr) + return; // Out of cache space + CachedBodyPair *output_cbp = &output_bp_kv->GetValue(); + memcpy(output_cbp, &input_cbp, sizeof(CachedBodyPair)); + + // If there were no contacts, we have handled the contact + if (input_cbp.mFirstCachedManifold == ManifoldMap::cInvalidHandle) + return; + + // Get body transforms + RMat44 transform_body1 = body1->GetCenterOfMassTransform(); + RMat44 transform_body2 = body2->GetCenterOfMassTransform(); + + // Get time step + float delta_time = mUpdateContext->mStepDeltaTime; + + // Calculate value for restitution correction + Vec3 gravity_dt = mUpdateContext->mPhysicsSystem->GetGravity() * delta_time; + + // Copy manifolds + uint32 output_handle = ManifoldMap::cInvalidHandle; + uint32 input_handle = input_cbp.mFirstCachedManifold; + do + { + JPH_PROFILE("Add Constraint From Cached Manifold"); + + // Find the existing manifold + const MKeyValue *input_kv = read_cache.FromHandle(input_handle); + const SubShapeIDPair &input_key = input_kv->GetKey(); + const CachedManifold &input_cm = input_kv->GetValue(); + JPH_ASSERT(input_cm.mNumContactPoints > 0); // There should be contact points in this manifold! + + // Create room for manifold in write buffer and copy data + uint64 input_hash = input_key.GetHash(); + MKeyValue *output_kv = write_cache.Create(ioContactAllocator, input_key, input_hash, input_cm.mNumContactPoints); + if (output_kv == nullptr) + break; // Out of cache space + CachedManifold *output_cm = &output_kv->GetValue(); + memcpy(output_cm, &input_cm, CachedManifold::sGetRequiredTotalSize(input_cm.mNumContactPoints)); + + // Link the object under the body pairs + output_cm->mNextWithSameBodyPair = output_handle; + output_handle = write_cache.ToHandle(output_kv); + + // Calculate default contact settings + ContactSettings settings; + settings.mCombinedFriction = mCombineFriction(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2()); + settings.mCombinedRestitution = mCombineRestitution(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2()); + settings.mIsSensor = body1->IsSensor() || body2->IsSensor(); + + // Calculate world space contact normal + Vec3 world_space_normal = transform_body2.Multiply3x3(Vec3::sLoadFloat3Unsafe(output_cm->mContactNormal)).Normalized(); + + // Call contact listener to update settings + if (mContactListener != nullptr) + { + // Convert constraint to manifold structure for callback + ContactManifold manifold; + manifold.mWorldSpaceNormal = world_space_normal; + manifold.mSubShapeID1 = input_key.GetSubShapeID1(); + manifold.mSubShapeID2 = input_key.GetSubShapeID2(); + manifold.mBaseOffset = transform_body1.GetTranslation(); + manifold.mRelativeContactPointsOn1.resize(output_cm->mNumContactPoints); + manifold.mRelativeContactPointsOn2.resize(output_cm->mNumContactPoints); + Mat44 local_transform_body2 = transform_body2.PostTranslated(-manifold.mBaseOffset).ToMat44(); + float penetration_depth = -FLT_MAX; + for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i) + { + const CachedContactPoint &ccp = output_cm->mContactPoints[i]; + manifold.mRelativeContactPointsOn1[i] = transform_body1.Multiply3x3(Vec3::sLoadFloat3Unsafe(ccp.mPosition1)); + manifold.mRelativeContactPointsOn2[i] = local_transform_body2 * Vec3::sLoadFloat3Unsafe(ccp.mPosition2); + penetration_depth = max(penetration_depth, (manifold.mRelativeContactPointsOn1[0] - manifold.mRelativeContactPointsOn2[0]).Dot(world_space_normal)); + } + manifold.mPenetrationDepth = penetration_depth; // We don't have the penetration depth anymore, estimate it + + // Notify callback + mContactListener->OnContactPersisted(*body1, *body2, manifold, settings); + } + + JPH_ASSERT(settings.mIsSensor || !(body1->IsSensor() || body2->IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); + if (!settings.mIsSensor // If one of the bodies is a sensor, don't actually create the constraint + && ((body1->IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint + || (body2->IsDynamic() && settings.mInvMassScale2 != 0.0f))) + { + // Add contact constraint in world space for the solver + uint32 constraint_idx = mNumConstraints++; + if (constraint_idx >= mMaxConstraints) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull; + break; + } + + // A constraint will be created + outConstraintCreated = true; + + ContactConstraint &constraint = mConstraints[constraint_idx]; + new (&constraint) ContactConstraint(); + constraint.mBody1 = body1; + constraint.mBody2 = body2; + constraint.mSortKey = input_hash; + world_space_normal.StoreFloat3(&constraint.mWorldSpaceNormal); + constraint.mCombinedFriction = settings.mCombinedFriction; + constraint.mInvMass1 = body1->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * body1->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale1 = settings.mInvInertiaScale1; + constraint.mInvMass2 = body2->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * body2->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale2 = settings.mInvInertiaScale2; + constraint.mContactPoints.resize(output_cm->mNumContactPoints); + for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i) + { + CachedContactPoint &ccp = output_cm->mContactPoints[i]; + WorldContactPoint &wcp = constraint.mContactPoints[i]; + wcp.mNonPenetrationConstraint.SetTotalLambda(ccp.mNonPenetrationLambda); + wcp.mFrictionConstraint1.SetTotalLambda(ccp.mFrictionLambda[0]); + wcp.mFrictionConstraint2.SetTotalLambda(ccp.mFrictionLambda[1]); + wcp.mContactPoint = &ccp; + } + + JPH_DET_LOG("GetContactsFromCache: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey); + + // Calculate friction and non-penetration constraint properties for all contact points + CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity_dt, transform_body1, transform_body2, *body1, *body2); + + // Notify island builder + mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal()); + + #ifdef JPH_DEBUG_RENDERER + // Draw the manifold + if (sDrawContactManifolds) + constraint.Draw(DebugRenderer::sInstance, Color::sYellow); + #endif // JPH_DEBUG_RENDERER + } + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + input_cm.mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + + // Fetch the next manifold + input_handle = input_cm.mNextWithSameBodyPair; + } + while (input_handle != ManifoldMap::cInvalidHandle); + output_cbp->mFirstCachedManifold = output_handle; +} + +ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2) +{ + JPH_PROFILE_FUNCTION(); + + // Swap bodies so that body 1 id < body 2 id + const Body *body1, *body2; + if (inBody1.GetID() < inBody2.GetID()) + { + body1 = &inBody1; + body2 = &inBody2; + } + else + { + body1 = &inBody2; + body2 = &inBody1; + } + + // Add an entry + BodyPair body_pair_key(body1->GetID(), body2->GetID()); + uint64 body_pair_hash = body_pair_key.GetHash(); + BPKeyValue *body_pair_kv = mCache[mCacheWriteIdx].Create(ioContactAllocator, body_pair_key, body_pair_hash); + if (body_pair_kv == nullptr) + return nullptr; // Out of cache space + CachedBodyPair *cbp = &body_pair_kv->GetValue(); + cbp->mFirstCachedManifold = ManifoldMap::cInvalidHandle; + + // Get relative translation + Quat inv_r1 = body1->GetRotation().Conjugated(); + Vec3 delta_position = inv_r1 * Vec3(body2->GetCenterOfMassPosition() - body1->GetCenterOfMassPosition()); + + // Store it + delta_position.StoreFloat3(&cbp->mDeltaPosition); + + // Determine relative orientation + Quat delta_rotation = inv_r1 * body2->GetRotation(); + + // Store it + delta_rotation.StoreFloat3(&cbp->mDeltaRotation); + + return cbp; +} + +template +bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold) +{ + // Calculate hash + SubShapeIDPair key { inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2 }; + uint64 key_hash = key.GetHash(); + + // Determine number of contact points + int num_contact_points = (int)inManifold.mRelativeContactPointsOn1.size(); + JPH_ASSERT(num_contact_points <= MaxContactPoints); + JPH_ASSERT(num_contact_points == (int)inManifold.mRelativeContactPointsOn2.size()); + + // Reserve space for new contact cache entry + // Note that for dynamic vs dynamic we always require the first body to have a lower body id to get a consistent key + // under which to look up the contact + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + MKeyValue *new_manifold_kv = write_cache.Create(ioContactAllocator, key, key_hash, num_contact_points); + if (new_manifold_kv == nullptr) + return false; // Out of cache space + CachedManifold *new_manifold = &new_manifold_kv->GetValue(); + + // Transform the world space normal to the space of body 2 (this is usually the static body) + RMat44 inverse_transform_body2 = inBody2.GetInverseCenterOfMassTransform(); + inverse_transform_body2.Multiply3x3(inManifold.mWorldSpaceNormal).Normalized().StoreFloat3(&new_manifold->mContactNormal); + + // Settings object that gets passed to the callback + ContactSettings settings; + settings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + settings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + settings.mIsSensor = inBody1.IsSensor() || inBody2.IsSensor(); + + // Get the contact points for the old cache entry + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const MKeyValue *old_manifold_kv = read_cache.Find(key, key_hash); + const CachedContactPoint *ccp_start; + const CachedContactPoint *ccp_end; + if (old_manifold_kv != nullptr) + { + // Call point persisted listener + if (mContactListener != nullptr) + mContactListener->OnContactPersisted(inBody1, inBody2, inManifold, settings); + + // Fetch the contact points from the old manifold + const CachedManifold *old_manifold = &old_manifold_kv->GetValue(); + ccp_start = old_manifold->mContactPoints; + ccp_end = ccp_start + old_manifold->mNumContactPoints; + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + old_manifold->mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + } + else + { + // Call point added listener + if (mContactListener != nullptr) + mContactListener->OnContactAdded(inBody1, inBody2, inManifold, settings); + + // No contact points available from old manifold + ccp_start = nullptr; + ccp_end = nullptr; + } + + // Get inverse transform for body 1 + RMat44 inverse_transform_body1 = inBody1.GetInverseCenterOfMassTransform(); + + bool contact_constraint_created = false; + + // If one of the bodies is a sensor, don't actually create the constraint + JPH_ASSERT(settings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); + if (!settings.mIsSensor + && ((inBody1.IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint + || (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f))) + { + // Add contact constraint + uint32 constraint_idx = mNumConstraints++; + if (constraint_idx >= mMaxConstraints) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull; + + // Manifold has been created already, we're not filling it in, so we need to reset the contact number of points. + // Note that we don't hook it up to the body pair cache so that it won't be used as a cache during the next simulation. + new_manifold->mNumContactPoints = 0; + return false; + } + + // We will create a contact constraint + contact_constraint_created = true; + + ContactConstraint &constraint = mConstraints[constraint_idx]; + new (&constraint) ContactConstraint(); + constraint.mBody1 = &inBody1; + constraint.mBody2 = &inBody2; + constraint.mSortKey = key_hash; + inManifold.mWorldSpaceNormal.StoreFloat3(&constraint.mWorldSpaceNormal); + constraint.mCombinedFriction = settings.mCombinedFriction; + constraint.mInvMass1 = inBody1.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale1 = settings.mInvInertiaScale1; + constraint.mInvMass2 = inBody2.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale2 = settings.mInvInertiaScale2; + + JPH_DET_LOG("TemplatedAddContactConstraint: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey); + + // Notify island builder + mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, inBody1.GetIndexInActiveBodiesInternal(), inBody2.GetIndexInActiveBodiesInternal()); + + // Get time step + float delta_time = mUpdateContext->mStepDeltaTime; + + // Calculate value for restitution correction + float gravity_dt_dot_normal = inManifold.mWorldSpaceNormal.Dot(mUpdateContext->mPhysicsSystem->GetGravity() * delta_time); + + // Calculate scaled mass and inertia + float inv_m1; + Mat44 inv_i1; + if constexpr (Type1 == EMotionType::Dynamic) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + inv_m1 = settings.mInvMassScale1 * mp1->GetInverseMass(); + inv_i1 = settings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inverse_transform_body1.Transposed3x3()); + } + else + { + inv_m1 = 0.0f; + inv_i1 = Mat44::sZero(); + } + + float inv_m2; + Mat44 inv_i2; + if constexpr (Type2 == EMotionType::Dynamic) + { + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + inv_m2 = settings.mInvMassScale2 * mp2->GetInverseMass(); + inv_i2 = settings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inverse_transform_body2.Transposed3x3()); + } + else + { + inv_m2 = 0.0f; + inv_i2 = Mat44::sZero(); + } + + // Calculate tangents + Vec3 t1, t2; + constraint.GetTangents(t1, t2); + + constraint.mContactPoints.resize(num_contact_points); + for (int i = 0; i < num_contact_points; ++i) + { + // Convert to world space and set positions + WorldContactPoint &wcp = constraint.mContactPoints[i]; + RVec3 p1_ws = inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i]; + RVec3 p2_ws = inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i]; + + // Convert to local space to the body + Vec3 p1_ls = Vec3(inverse_transform_body1 * p1_ws); + Vec3 p2_ls = Vec3(inverse_transform_body2 * p2_ws); + + // Check if we have a close contact point from last update + bool lambda_set = false; + for (const CachedContactPoint *ccp = ccp_start; ccp < ccp_end; ccp++) + if (Vec3::sLoadFloat3Unsafe(ccp->mPosition1).IsClose(p1_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq) + && Vec3::sLoadFloat3Unsafe(ccp->mPosition2).IsClose(p2_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq)) + { + // Get lambdas from previous frame + wcp.mNonPenetrationConstraint.SetTotalLambda(ccp->mNonPenetrationLambda); + wcp.mFrictionConstraint1.SetTotalLambda(ccp->mFrictionLambda[0]); + wcp.mFrictionConstraint2.SetTotalLambda(ccp->mFrictionLambda[1]); + lambda_set = true; + break; + } + if (!lambda_set) + { + wcp.mNonPenetrationConstraint.SetTotalLambda(0.0f); + wcp.mFrictionConstraint1.SetTotalLambda(0.0f); + wcp.mFrictionConstraint2.SetTotalLambda(0.0f); + } + + // Create new contact point + CachedContactPoint &cp = new_manifold->mContactPoints[i]; + p1_ls.StoreFloat3(&cp.mPosition1); + p2_ls.StoreFloat3(&cp.mPosition2); + wcp.mContactPoint = &cp; + + // Setup velocity constraint + wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(delta_time, gravity_dt_dot_normal, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings, mPhysicsSettings.mMinVelocityForRestitution); + } + + #ifdef JPH_DEBUG_RENDERER + // Draw the manifold + if (sDrawContactManifolds) + constraint.Draw(DebugRenderer::sInstance, Color::sOrange); + #endif // JPH_DEBUG_RENDERER + } + else + { + // Store the contact manifold in the cache + for (int i = 0; i < num_contact_points; ++i) + { + // Convert to local space to the body + Vec3 p1 = Vec3(inverse_transform_body1 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i])); + Vec3 p2 = Vec3(inverse_transform_body2 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i])); + + // Create new contact point + CachedContactPoint &cp = new_manifold->mContactPoints[i]; + p1.StoreFloat3(&cp.mPosition1); + p2.StoreFloat3(&cp.mPosition2); + + // Reset contact impulses, we haven't applied any + cp.mNonPenetrationLambda = 0.0f; + cp.mFrictionLambda[0] = 0.0f; + cp.mFrictionLambda[1] = 0.0f; + } + } + + // Store cached contact point in body pair cache + CachedBodyPair *cbp = reinterpret_cast(inBodyPairHandle); + new_manifold->mNextWithSameBodyPair = cbp->mFirstCachedManifold; + cbp->mFirstCachedManifold = write_cache.ToHandle(new_manifold_kv); + + // A contact constraint was added + return contact_constraint_created; +} + +bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold) +{ + JPH_PROFILE_FUNCTION(); + + JPH_DET_LOG("AddContactConstraint: id1: " << inBody1.GetID() << " id2: " << inBody2.GetID() + << " subshape1: " << inManifold.mSubShapeID1 << " subshape2: " << inManifold.mSubShapeID2 + << " normal: " << inManifold.mWorldSpaceNormal << " pendepth: " << inManifold.mPenetrationDepth); + + JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized()); + + // Swap bodies so that body 1 id < body 2 id + const ContactManifold *manifold; + Body *body1, *body2; + ContactManifold temp; + if (inBody2.GetID() < inBody1.GetID()) + { + body1 = &inBody2; + body2 = &inBody1; + temp = inManifold.SwapShapes(); + manifold = &temp; + } + else + { + body1 = &inBody1; + body2 = &inBody2; + manifold = &inManifold; + } + + // Dispatch to the correct templated form + // Note: Non-dynamic vs non-dynamic can happen in this case due to one body being a sensor, so we need to have an extended switch case here + switch (body1->GetMotionType()) + { + case EMotionType::Dynamic: + { + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + default: + JPH_ASSERT(false); + break; + } + break; + } + + case EMotionType::Kinematic: + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Static: + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: // Static vs static not possible + default: + JPH_ASSERT(false); + break; + } + break; + + default: + JPH_ASSERT(false); + break; + } + + return false; +} + +void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings) +{ + JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized()); + + // Calculate contact settings + outSettings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + outSettings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + outSettings.mIsSensor = false; // For now, no sensors are supported during CCD + + // The remainder of this function only deals with calling contact callbacks, if there's no contact callback we also don't need to do this work + if (mContactListener != nullptr) + { + // Swap bodies so that body 1 id < body 2 id + const ContactManifold *manifold; + const Body *body1, *body2; + ContactManifold temp; + if (inBody2.GetID() < inBody1.GetID()) + { + body1 = &inBody2; + body2 = &inBody1; + temp = inManifold.SwapShapes(); + manifold = &temp; + } + else + { + body1 = &inBody1; + body2 = &inBody2; + manifold = &inManifold; + } + + // Calculate hash + SubShapeIDPair key { body1->GetID(), manifold->mSubShapeID1, body2->GetID(), manifold->mSubShapeID2 }; + uint64 key_hash = key.GetHash(); + + // Check if we already created this contact this physics update + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + MKVAndCreated new_manifold_kv = write_cache.FindOrCreate(ioContactAllocator, key, key_hash, 0); + if (new_manifold_kv.second) + { + // This contact is new for this physics update, check if previous update we already had this contact. + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const MKeyValue *old_manifold_kv = read_cache.Find(key, key_hash); + if (old_manifold_kv == nullptr) + { + // New contact + mContactListener->OnContactAdded(*body1, *body2, *manifold, outSettings); + } + else + { + // Existing contact + mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings); + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + old_manifold_kv->GetValue().mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + } + + // Check if the cache is full + if (new_manifold_kv.first != nullptr) + { + // We don't store any contact points in this manifold as it is not for caching impulses, we only need to know that the contact was created + CachedManifold &new_manifold = new_manifold_kv.first->GetValue(); + new_manifold.mContactNormal = { 0, 0, 0 }; + new_manifold.mFlags |= (uint16)CachedManifold::EFlags::CCDContact; + } + } + else + { + // Already found this contact this physics update. + // Note that we can trigger OnContactPersisted multiple times per physics update, but otherwise we have no way of obtaining the settings + mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings); + } + + // If we swapped body1 and body2 we need to swap the mass scales back + if (manifold == &temp) + { + std::swap(outSettings.mInvMassScale1, outSettings.mInvMassScale2); + std::swap(outSettings.mInvInertiaScale1, outSettings.mInvInertiaScale2); + // Note we do not need to negate the relative surface velocity as it is not applied by the CCD collision constraint + } + } + + JPH_ASSERT(outSettings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); +} + +void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const +{ + JPH_PROFILE_FUNCTION(); + + QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [this](uint32 inLHS, uint32 inRHS) { + const ContactConstraint &lhs = mConstraints[inLHS]; + const ContactConstraint &rhs = mConstraints[inRHS]; + + // Most of the time the sort key will be different so we sort on that + if (lhs.mSortKey != rhs.mSortKey) + return lhs.mSortKey < rhs.mSortKey; + + // If they're equal we use the IDs of body 1 to order + if (lhs.mBody1 != rhs.mBody1) + return lhs.mBody1->GetID() < rhs.mBody1->GetID(); + + // If they're still equal we use the IDs of body 2 to order + if (lhs.mBody2 != rhs.mBody2) + return lhs.mBody2->GetID() < rhs.mBody2->GetID(); + + JPH_ASSERT(inLHS == inRHS, "Hash collision, ordering will be inconsistent"); + return false; + }); +} + +void ContactConstraintManager::FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds) +{ + JPH_PROFILE_FUNCTION(); + +#ifdef JPH_ENABLE_ASSERTS + // Mark cache as finalized + ManifoldCache &old_write_cache = mCache[mCacheWriteIdx]; + old_write_cache.Finalize(); + + // Check that the count of body pairs and manifolds that we tracked outside of the cache (to avoid contention on an atomic) is correct + JPH_ASSERT(old_write_cache.GetNumBodyPairs() == inExpectedNumBodyPairs); + JPH_ASSERT(old_write_cache.GetNumManifolds() == inExpectedNumManifolds); +#endif + + // Buffers are now complete, make write buffer the read buffer + mCacheWriteIdx ^= 1; + + // Get the old read cache / new write cache + ManifoldCache &old_read_cache = mCache[mCacheWriteIdx]; + + // Call the contact point removal callbacks + if (mContactListener != nullptr) + old_read_cache.ContactPointRemovedCallbacks(mContactListener); + + // We're done with the old read cache now + old_read_cache.Clear(); + + // Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for the next iteration + old_read_cache.Prepare(inExpectedNumBodyPairs, inExpectedNumManifolds); +} + +bool ContactConstraintManager::WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const +{ + // The body pair needs to be in the cache and it needs to have a manifold (otherwise it's just a record indicating that there are no collisions) + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + BodyPair key; + if (inBody1ID < inBody2ID) + key = BodyPair(inBody1ID, inBody2ID); + else + key = BodyPair(inBody2ID, inBody1ID); + uint64 key_hash = key.GetHash(); + const BPKeyValue *kv = read_cache.Find(key, key_hash); + return kv != nullptr && kv->GetValue().mFirstCachedManifold != ManifoldMap::cInvalidHandle; +} + +template +JPH_INLINE void ContactConstraintManager::sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio) +{ + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Warm starting: Apply impulse from last frame + if (wcp.mFrictionConstraint1.IsActive() || wcp.mFrictionConstraint2.IsActive()) + { + wcp.mFrictionConstraint1.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, inWarmStartImpulseRatio); + wcp.mFrictionConstraint2.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, inWarmStartImpulseRatio); + } + wcp.mNonPenetrationConstraint.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, inWarmStartImpulseRatio); + } +} + +template +void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback) +{ + JPH_PROFILE_FUNCTION(); + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + EMotionType motion_type1 = body1.GetMotionType(); + MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked(); + + Body &body2 = *constraint.mBody2; + EMotionType motion_type2 = body2.GetMotionType(); + MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + // Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies + if (motion_type1 == EMotionType::Dynamic) + { + if (motion_type2 == EMotionType::Dynamic) + { + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties2); + } + else + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties1); + } + else + { + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties2); + } + } +} + +// Specialize for the two body callback types +template void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback); +template void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback); + +template +JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2) +{ + bool any_impulse_applied = false; + + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + // First apply all friction constraints (non-penetration is more important than friction) + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Check if friction is enabled + if (wcp.mFrictionConstraint1.IsActive() || wcp.mFrictionConstraint2.IsActive()) + { + // Calculate impulse to stop motion in tangential direction + float lambda1 = wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, t1); + float lambda2 = wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, t2); + float total_lambda_sq = Square(lambda1) + Square(lambda2); + + // Calculate max impulse that can be applied. Note that we're using the non-penetration impulse from the previous iteration here. + // We do this because non-penetration is more important so is solved last (the last things that are solved in an iterative solver + // contribute the most). + float max_lambda_f = ioConstraint.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda(); + + // If the total lambda that we will apply is too large, scale it back + if (total_lambda_sq > Square(max_lambda_f)) + { + float scale = max_lambda_f / sqrt(total_lambda_sq); + lambda1 *= scale; + lambda2 *= scale; + } + + // Apply the friction impulse + if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, lambda1)) + any_impulse_applied = true; + if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, lambda2)) + any_impulse_applied = true; + } + } + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + // Then apply all non-penetration constraints + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Solve non penetration velocities + if (wcp.mNonPenetrationConstraint.TemplatedSolveVelocityConstraint(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, 0.0f, FLT_MAX)) + any_impulse_applied = true; + } + + return any_impulse_applied; +} + +bool ContactConstraintManager::SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + EMotionType motion_type1 = body1.GetMotionType(); + MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked(); + + Body &body2 = *constraint.mBody2; + EMotionType motion_type2 = body2.GetMotionType(); + MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Kinematic: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Static: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + default: + JPH_ASSERT(false); + break; + } + } + + return any_impulse_applied; +} + +void ContactConstraintManager::StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const +{ + // Copy back total applied impulse to cache for the next frame + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + const ContactConstraint &constraint = mConstraints[*constraint_idx]; + + for (const WorldContactPoint &wcp : constraint.mContactPoints) + { + wcp.mContactPoint->mNonPenetrationLambda = wcp.mNonPenetrationConstraint.GetTotalLambda(); + wcp.mContactPoint->mFrictionLambda[0] = wcp.mFrictionConstraint1.GetTotalLambda(); + wcp.mContactPoint->mFrictionLambda[1] = wcp.mFrictionConstraint2.GetTotalLambda(); + } + } +} + +bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + Body &body2 = *constraint.mBody2; + + // Get transforms + RMat44 transform1 = body1.GetCenterOfMassTransform(); + RMat44 transform2 = body2.GetCenterOfMassTransform(); + + Vec3 ws_normal = constraint.GetWorldSpaceNormal(); + + for (WorldContactPoint &wcp : constraint.mContactPoints) + { + // Calculate new contact point positions in world space (the bodies may have moved) + RVec3 p1 = transform1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + RVec3 p2 = transform2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2); + + // Calculate separation along the normal (negative if interpenetrating) + // Allow a little penetration by default (PhysicsSettings::mPenetrationSlop) to avoid jittering between contact/no-contact which wipes out the contact cache and warm start impulses + // Clamp penetration to a max PhysicsSettings::mMaxPenetrationDistance so that we don't apply a huge impulse if we're penetrating a lot + float separation = max(Vec3(p2 - p1).Dot(ws_normal) + mPhysicsSettings.mPenetrationSlop, -mPhysicsSettings.mMaxPenetrationDistance); + + // Only enforce constraint when separation < 0 (otherwise we're apart) + if (separation < 0.0f) + { + // Update constraint properties (bodies may have moved) + wcp.CalculateNonPenetrationConstraintProperties(body1, constraint.mInvMass1, constraint.mInvInertiaScale1, body2, constraint.mInvMass2, constraint.mInvInertiaScale2, p1, p2, ws_normal); + + // Solve position errors + if (wcp.mNonPenetrationConstraint.SolvePositionConstraintWithMassOverride(body1, constraint.mInvMass1, body2, constraint.mInvMass2, ws_normal, separation, mPhysicsSettings.mBaumgarte)) + any_impulse_applied = true; + } + } + } + + return any_impulse_applied; +} + +void ContactConstraintManager::RecycleConstraintBuffer() +{ + // Reset constraint array + mNumConstraints = 0; +} + +void ContactConstraintManager::FinishConstraintBuffer() +{ + // Free constraints buffer + mUpdateContext->mTempAllocator->Free(mConstraints, mMaxConstraints * sizeof(ContactConstraint)); + mConstraints = nullptr; + mNumConstraints = 0; + + // Reset update context + mUpdateContext = nullptr; +} + +void ContactConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + mCache[mCacheWriteIdx ^ 1].SaveState(inStream, inFilter); +} + +bool ContactConstraintManager::RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter) +{ + bool success = mCache[mCacheWriteIdx].RestoreState(mCache[mCacheWriteIdx ^ 1], inStream, inFilter); + + // If this is the last part, the cache is finalized + if (inStream.IsLastPart()) + { + mCacheWriteIdx ^= 1; + mCache[mCacheWriteIdx].Clear(); + } + + return success; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h new file mode 100644 index 000000000000..3cb848cb5802 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h @@ -0,0 +1,513 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +struct PhysicsSettings; +class PhysicsUpdateContext; + +class JPH_EXPORT ContactConstraintManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ContactConstraintManager(const PhysicsSettings &inPhysicsSettings); + ~ContactConstraintManager(); + + /// Initialize the system. + /// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching + /// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world) + void Init(uint inMaxBodyPairs, uint inMaxContactConstraints); + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed + void SetContactListener(ContactListener *inListener) { mContactListener = inListener; } + ContactListener * GetContactListener() const { return mContactListener; } + + /// Callback function to combine the restitution or friction of two bodies + /// Note that when merging manifolds (when PhysicsSettings::mUseManifoldReduction is true) you will only get a callback for the merged manifold. + /// It is not possible in that case to get all sub shape ID pairs that were colliding, you'll get the first encountered pair. + using CombineFunction = float (*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2); + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(CombineFunction inCombineFriction) { mCombineFriction = inCombineFriction; } + CombineFunction GetCombineFriction() const { return mCombineFriction; } + + /// Set the function that combines the restitution of two bodies and returns it + /// Default method is max(restitution1, restitution1) + void SetCombineRestitution(CombineFunction inCombineRestitution) { mCombineRestitution = inCombineRestitution; } + CombineFunction GetCombineRestitution() const { return mCombineRestitution; } + + /// Get the max number of contact constraints that are allowed + uint32 GetMaxConstraints() const { return mMaxConstraints; } + + /// Check with the listener if inBody1 and inBody2 could collide, returns false if not + inline ValidateResult ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const + { + if (mContactListener == nullptr) + return ValidateResult::AcceptAllContactsForThisBodyPair; + + return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult); + } + + /// Sets up the constraint buffer. Should be called before starting collision detection. + void PrepareConstraintBuffer(PhysicsUpdateContext *inContext); + + /// Max 4 contact points are needed for a stable manifold + static const int MaxContactPoints = 4; + + /// Contacts are allocated in a lock free hash map + class ContactAllocator : public LFHMAllocatorContext + { + public: + using LFHMAllocatorContext::LFHMAllocatorContext; + + uint mNumBodyPairs = 0; ///< Total number of body pairs added using this allocator + uint mNumManifolds = 0; ///< Total number of manifolds added using this allocator + EPhysicsUpdateError mErrors = EPhysicsUpdateError::None; ///< Errors reported on this allocator + }; + + /// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context. + ContactAllocator GetContactAllocator() { return mCache[mCacheWriteIdx].GetContactAllocator(); } + + /// Check if the contact points from the previous frame are reusable and if so copy them. + /// When the cache was usable and the pair has been handled: outPairHandled = true. + /// When a contact constraint was produced: outConstraintCreated = true. + void GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated); + + /// Handle used to keep track of the current body pair + using BodyPairHandle = void *; + + /// Create a handle for a colliding body pair so that contact constraints can be added between them. + /// Needs to be called once per body pair per frame before calling AddContactConstraint. + BodyPairHandle AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2); + + /// Add a contact constraint for this frame. + /// + /// @param ioContactAllocator The allocator that reserves memory for the contacts + /// @param inBodyPair The handle for the contact cache for this body pair + /// @param inBody1 The first body that is colliding + /// @param inBody2 The second body that is colliding + /// @param inManifold The manifold that describes the collision + /// @return true if a contact constraint was created (can be false in the case of a sensor) + /// + /// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2009 (and later years with slight modifications). + /// We're using the formulas from slide 50 - 53 combined. + /// + /// Euler velocity integration: + /// + /// v1' = v1 + M^-1 P + /// + /// Impulse: + /// + /// P = J^T lambda + /// + /// Constraint force: + /// + /// lambda = -K^-1 J v1 + /// + /// Inverse effective mass: + /// + /// K = J M^-1 J^T + /// + /// Constraint equation (limits movement in 1 axis): + /// + /// C = (p2 - p1) . n + /// + /// Jacobian (for position constraint) + /// + /// J = [-n, -r1 x n, n, r2 x n] + /// + /// n = contact normal (pointing away from body 1). + /// p1, p2 = positions of collision on body 1 and 2. + /// r1, r2 = contact point relative to center of mass of body 1 and body 2 (r1 = p1 - x1, r2 = p2 - x2). + /// v1, v2 = (linear velocity, angular velocity): 6 vectors containing linear and angular velocity for body 1 and 2. + /// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2]. + bool AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold); + + /// Finalizes the contact cache, the contact cache that was generated during the calls to AddContactConstraint in this update + /// will be used from now on to read from. After finalizing the contact cache, the contact removed callbacks will be called. + /// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is + /// used to determine the amount of buckets the contact cache hash map will use in the next update. + void FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds); + + /// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, at least one of the bodies must be active. + /// Uses the read collision cache to determine if 2 bodies are in contact. + bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const; + + /// Get the number of contact constraints that were found + uint32 GetNumConstraints() const { return min(mNumConstraints, mMaxConstraints); } + + /// Sort contact constraints deterministically + void SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const; + + /// Get the affected bodies for a given constraint + inline void GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const + { + const ContactConstraint &constraint = mConstraints[inConstraintIdx]; + outBody1 = constraint.mBody1; + outBody2 = constraint.mBody2; + } + + /// Apply last frame's impulses as an initial guess for this frame's impulses + template + void WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback); + + /// Solve velocity constraints, when almost nothing changes this should only apply very small impulses + /// since we're warm starting with the total impulse applied in the last frame above. + /// + /// Friction wise we're using the Coulomb friction model which says that: + /// + /// |F_T| <= mu |F_N| + /// + /// Where F_T is the tangential force, F_N is the normal force and mu is the friction coefficient + /// + /// In impulse terms this becomes: + /// + /// |lambda_T| <= mu |lambda_N| + /// + /// And the constraint that needs to be applied is exactly the same as a non penetration constraint + /// except that we use a tangent instead of a normal. The tangent should point in the direction of the + /// tangential velocity of the point: + /// + /// J = [-T, -r1 x T, T, r2 x T] + /// + /// Where T is the tangent. + /// + /// See slide 42 and 43. + /// + /// Restitution is implemented as a velocity bias (see slide 41): + /// + /// b = e v_n^- + /// + /// e = the restitution coefficient, v_n^- is the normal velocity prior to the collision + /// + /// Restitution is only applied when v_n^- is large enough and the points are moving towards collision + bool SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd); + + /// Save back the lambdas to the contact cache for the next warm start + void StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const; + + /// Solve position constraints. + /// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2007. + /// On slide 78 it is suggested to split up the Baumgarte stabilization for positional drift so that it does not + /// actually add to the momentum. We combine an Euler velocity integrate + a position integrate and then discard the velocity + /// change. + /// + /// Constraint force: + /// + /// lambda = -K^-1 b + /// + /// Baumgarte stabilization: + /// + /// b = beta / dt C + /// + /// beta = baumgarte stabilization factor. + /// dt = delta time. + bool SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd); + + /// Recycle the constraint buffer. Should be called between collision simulation steps. + void RecycleConstraintBuffer(); + + /// Terminate the constraint buffer. Should be called after simulation ends. + void FinishConstraintBuffer(); + + /// Called by continuous collision detection to notify the contact listener that a contact was added + /// @param ioContactAllocator The allocator that reserves memory for the contacts + /// @param inBody1 The first body that is colliding + /// @param inBody2 The second body that is colliding + /// @param inManifold The manifold that describes the collision + /// @param outSettings The calculated contact settings (may be overridden by the contact listener) + void OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings); + +#ifdef JPH_DEBUG_RENDERER + // Drawing properties + static bool sDrawContactPoint; + static bool sDrawSupportingFaces; + static bool sDrawContactPointReduction; + static bool sDrawContactManifolds; +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restoring state for replay. Returns false when failed. + bool RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter); + +private: + /// Local space contact point, used for caching impulses + class CachedContactPoint + { + public: + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Local space positions on body 1 and 2. + /// Note: these values are read through sLoadFloat3Unsafe. + Float3 mPosition1; + Float3 mPosition2; + + /// Total applied impulse during the last update that it was used + float mNonPenetrationLambda; + Vector<2> mFrictionLambda; + }; + + static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size"); + static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned"); + + /// A single cached manifold + class CachedManifold + { + public: + /// Calculate size in bytes needed beyond the size of the class to store inNumContactPoints + static int sGetRequiredExtraSize(int inNumContactPoints) { return max(0, inNumContactPoints - 1) * sizeof(CachedContactPoint); } + + /// Calculate total class size needed for storing inNumContactPoints + static int sGetRequiredTotalSize(int inNumContactPoints) { return sizeof(CachedManifold) + sGetRequiredExtraSize(inNumContactPoints); } + + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Handle to next cached contact points in ManifoldCache::mCachedManifolds for the same body pair + uint32 mNextWithSameBodyPair; + + /// Contact normal in the space of 2. + /// Note: this value is read through sLoadFloat3Unsafe. + Float3 mContactNormal; + + /// Flags for this cached manifold + enum class EFlags : uint16 + { + ContactPersisted = 1, ///< If this cache entry was reused in the next simulation update + CCDContact = 2 ///< This is a cached manifold reported by continuous collision detection and was only used to create a contact callback + }; + + /// @see EFlags + mutable atomic mFlags { 0 }; + + /// Number of contact points in the array below + uint16 mNumContactPoints; + + /// Contact points that this manifold consists of + CachedContactPoint mContactPoints[1]; + }; + + static_assert(sizeof(CachedManifold) == 56, "This structure is expect to not contain any waste due to alignment"); + static_assert(alignof(CachedManifold) == 4, "Assuming 4 byte aligned"); + + /// Define a map that maps SubShapeIDPair -> manifold + using ManifoldMap = LockFreeHashMap; + using MKeyValue = ManifoldMap::KeyValue; + using MKVAndCreated = std::pair; + + /// Start of list of contact points for a particular pair of bodies + class CachedBodyPair + { + public: + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Local space position difference from Body A to Body B. + /// Note: this value is read through sLoadFloat3Unsafe + Float3 mDeltaPosition; + + /// Local space rotation difference from Body A to Body B, fourth component of quaternion is not stored but is guaranteed >= 0. + /// Note: this value is read through sLoadFloat3Unsafe + Float3 mDeltaRotation; + + /// Handle to first manifold in ManifoldCache::mCachedManifolds + uint32 mFirstCachedManifold; + }; + + static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size"); + static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned"); + + /// Define a map that maps BodyPair -> CachedBodyPair + using BodyPairMap = LockFreeHashMap; + using BPKeyValue = BodyPairMap::KeyValue; + + /// Holds all caches that are needed to quickly find cached body pairs / manifolds + class ManifoldCache + { + public: + /// Initialize the cache + void Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize); + + /// Reset all entries from the cache + void Clear(); + + /// Prepare cache before creating new contacts. + /// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use. + void Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds); + + /// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context. + ContactAllocator GetContactAllocator() { return ContactAllocator(mAllocator, cAllocatorBlockSize); } + + /// Find / create cached entry for SubShapeIDPair -> CachedManifold + const MKeyValue * Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const; + MKeyValue * Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints); + MKVAndCreated FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints); + uint32 ToHandle(const MKeyValue *inKeyValue) const; + const MKeyValue * FromHandle(uint32 inHandle) const; + + /// Find / create entry for BodyPair -> CachedBodyPair + const BPKeyValue * Find(const BodyPair &inKey, uint64 inKeyHash) const; + BPKeyValue * Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash); + void GetAllBodyPairsSorted(Array &outAll) const; + void GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array &outAll) const; + void GetAllCCDManifoldsSorted(Array &outAll) const; + void ContactPointRemovedCallbacks(ContactListener *inListener); + +#ifdef JPH_ENABLE_ASSERTS + /// Get the amount of manifolds in the cache + uint GetNumManifolds() const { return mCachedManifolds.GetNumKeyValues(); } + + /// Get the amount of body pairs in the cache + uint GetNumBodyPairs() const { return mCachedBodyPairs.GetNumKeyValues(); } + + /// Before a cache is finalized you can only do Create(), after only Find() or Clear() + void Finalize(); +#endif + + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + bool RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter); + + private: + /// Block size used when allocating new blocks in the contact cache + static constexpr uint32 cAllocatorBlockSize = 4096; + + /// Allocator used by both mCachedManifolds and mCachedBodyPairs, this makes it more likely that a body pair and its manifolds are close in memory + LFHMAllocator mAllocator; + + /// Simple hash map for SubShapeIDPair -> CachedManifold + ManifoldMap mCachedManifolds { mAllocator }; + + /// Simple hash map for BodyPair -> CachedBodyPair + BodyPairMap mCachedBodyPairs { mAllocator }; + +#ifdef JPH_ENABLE_ASSERTS + bool mIsFinalized = false; ///< Marks if this buffer is complete +#endif + }; + + ManifoldCache mCache[2]; ///< We have one cache to read from and one to write to + int mCacheWriteIdx = 0; ///< Which cache we're currently writing to + + /// World space contact point, used for solving penetrations + class WorldContactPoint + { + public: + /// Calculate constraint properties below + void CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal); + + template + JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution); + + /// The constraint parts + AxisConstraintPart mNonPenetrationConstraint; + AxisConstraintPart mFrictionConstraint1; + AxisConstraintPart mFrictionConstraint2; + + /// Contact cache + CachedContactPoint * mContactPoint; + }; + + using WorldContactPoints = StaticArray; + + /// Contact constraint class, used for solving penetrations + class ContactConstraint + { + public: + #ifdef JPH_DEBUG_RENDERER + /// Draw the state of the contact constraint + void Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const; + #endif // JPH_DEBUG_RENDERER + + /// Convert the world space normal to a Vec3 + JPH_INLINE Vec3 GetWorldSpaceNormal() const + { + return Vec3::sLoadFloat3Unsafe(mWorldSpaceNormal); + } + + /// Get the tangents for this contact constraint + JPH_INLINE void GetTangents(Vec3 &outTangent1, Vec3 &outTangent2) const + { + Vec3 ws_normal = GetWorldSpaceNormal(); + outTangent1 = ws_normal.GetNormalizedPerpendicular(); + outTangent2 = ws_normal.Cross(outTangent1); + } + + Body * mBody1; + Body * mBody2; + uint64 mSortKey; + Float3 mWorldSpaceNormal; + float mCombinedFriction; + float mInvMass1; + float mInvInertiaScale1; + float mInvMass2; + float mInvInertiaScale2; + WorldContactPoints mContactPoints; + }; + + /// Internal helper function to calculate the friction and non-penetration constraint properties. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2); + + /// Internal helper function to calculate the friction and non-penetration constraint properties. + inline void CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2); + + /// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + bool TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold); + + /// Internal helper function to warm start contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE static void sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio); + + /// Internal helper function to solve a single contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE static bool sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2); + + /// The main physics settings instance + const PhysicsSettings & mPhysicsSettings; + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed + ContactListener * mContactListener = nullptr; + + /// Functions that are used to combine friction and restitution of 2 bodies + CombineFunction mCombineFriction = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return sqrt(inBody1.GetFriction() * inBody2.GetFriction()); }; + CombineFunction mCombineRestitution = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return max(inBody1.GetRestitution(), inBody2.GetRestitution()); }; + + /// The constraints that were added this frame + ContactConstraint * mConstraints = nullptr; + uint32 mMaxConstraints = 0; + atomic mNumConstraints { 0 }; + + /// Context used for this physics update + PhysicsUpdateContext * mUpdateContext; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp new file mode 100644 index 000000000000..e70bfb24ced5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp @@ -0,0 +1,266 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(DistanceConstraintSettings) +{ + JPH_ADD_BASE_CLASS(DistanceConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(DistanceConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMaxDistance) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping") +} + +void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mPoint2); + inStream.Write(mMinDistance); + inStream.Write(mMaxDistance); + mLimitsSpringSettings.SaveBinaryState(inStream); +} + +void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mPoint2); + inStream.Read(mMinDistance); + inStream.Read(mMaxDistance); + mLimitsSpringSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new DistanceConstraint(inBody1, inBody2, *this); +} + +DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMinDistance(inSettings.mMinDistance), + mMaxDistance(inSettings.mMaxDistance) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + mWorldSpacePosition1 = inSettings.mPoint1; + mWorldSpacePosition2 = inSettings.mPoint2; + } + else + { + // If properties were specified in local space, we need to calculate world space positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mPoint1; + mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mPoint2; + } + + // Store distance we want to keep between the world space points + float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Length(); + float min_distance, max_distance; + if (mMinDistance < 0.0f && mMaxDistance < 0.0f) + { + min_distance = max_distance = distance; + } + else + { + min_distance = mMinDistance < 0.0f? min(distance, mMaxDistance) : mMinDistance; + max_distance = mMaxDistance < 0.0f? max(distance, mMinDistance) : mMaxDistance; + } + SetDistance(min_distance, max_distance); + + // Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0) + mWorldSpaceNormal = Vec3::sAxisY(); + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime) +{ + // Update world space positions (the bodies may have moved) + mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + + // Calculate world space normal + Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1); + float delta_len = delta.Length(); + if (delta_len > 0.0f) + mWorldSpaceNormal = delta / delta_len; + + // Calculate points relative to body + // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1 + Vec3 r1_plus_u = Vec3(mWorldSpacePosition2 - mBody1->GetCenterOfMassPosition()); + Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition()); + + if (mMinDistance == mMaxDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings); + + // Single distance, allow constraint forces in both directions + mMinLambda = -FLT_MAX; + mMaxLambda = FLT_MAX; + } + else if (delta_len <= mMinDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings); + + // Allow constraint forces to make distance bigger only + mMinLambda = 0; + mMaxLambda = FLT_MAX; + } + else if (delta_len >= mMaxDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMaxDistance, mLimitsSpringSettings); + + // Allow constraint forces to make distance smaller only + mMinLambda = -FLT_MAX; + mMaxLambda = 0; + } + else + mAxisConstraint.Deactivate(); +} + +void DistanceConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(inDeltaTime); +} + +void DistanceConstraint::ResetWarmStart() +{ + mAxisConstraint.Deactivate(); +} + +void DistanceConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + mAxisConstraint.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal, inWarmStartImpulseRatio); +} + +bool DistanceConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + if (mAxisConstraint.IsActive()) + return mAxisConstraint.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal, mMinLambda, mMaxLambda); + else + return false; +} + +bool DistanceConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mLimitsSpringSettings.mFrequency <= 0.0f) // When the spring is active, we don't need to solve the position constraint + { + float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Dot(mWorldSpaceNormal); + + // Calculate position error + float position_error = 0.0f; + if (distance < mMinDistance) + position_error = distance - mMinDistance; + else if (distance > mMaxDistance) + position_error = distance - mMaxDistance; + + if (position_error != 0.0f) + { + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(inDeltaTime); + + return mAxisConstraint.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal, position_error, inBaumgarte); + } + } + + return false; +} + +#ifdef JPH_DEBUG_RENDERER +void DistanceConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Draw constraint + Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1); + float len = delta.Length(); + if (len < mMinDistance) + { + RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMinDistance / len : Vec3(0, len, 0)); + inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen); + inRenderer->DrawLine(mWorldSpacePosition2, real_end_pos, Color::sYellow); + } + else if (len > mMaxDistance) + { + RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMaxDistance / len : Vec3(0, len, 0)); + inRenderer->DrawLine(mWorldSpacePosition1, real_end_pos, Color::sGreen); + inRenderer->DrawLine(real_end_pos, mWorldSpacePosition2, Color::sRed); + } + else + inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen); + + // Draw constraint end points + inRenderer->DrawMarker(mWorldSpacePosition1, Color::sWhite, 0.1f); + inRenderer->DrawMarker(mWorldSpacePosition2, Color::sWhite, 0.1f); + + // Draw current length + inRenderer->DrawText3D(0.5_r * (mWorldSpacePosition1 + mWorldSpacePosition2), StringFormat("%.2f", (double)len)); +} +#endif // JPH_DEBUG_RENDERER + +void DistanceConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mAxisConstraint.SaveState(inStream); + inStream.Write(mWorldSpaceNormal); // When distance = 0, the normal is used from last frame so we need to store it +} + +void DistanceConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mAxisConstraint.RestoreState(inStream); + inStream.Read(mWorldSpaceNormal); +} + +Ref DistanceConstraint::GetConstraintSettings() const +{ + DistanceConstraintSettings *settings = new DistanceConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mMinDistance = mMinDistance; + settings->mMaxDistance = mMaxDistance; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h new file mode 100644 index 000000000000..d237b8fd2b50 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h @@ -0,0 +1,120 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Distance constraint settings, used to create a distance constraint +class JPH_EXPORT DistanceConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DistanceConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Constraint will keep mPoint1 (a point on body 1) and mPoint2 (a point on body 2) at the same distance. + /// Note that this constraint can be used as a cheap PointConstraint by setting mPoint1 = mPoint2 (but this removes only 1 degree of freedom instead of 3). + RVec3 mPoint1 = RVec3::sZero(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + + /// Ability to override the distance range at which the two points are kept apart. If the value is negative, it will be replaced by the distance between mPoint1 and mPoint2 (works only if mSpace is world space). + float mMinDistance = -1.0f; + float mMaxDistance = -1.0f; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// This constraint is a stiff spring that holds 2 points at a fixed distance from each other +class JPH_EXPORT DistanceConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct distance constraint + DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Distance; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + /// Update the minimum and maximum distance for the constraint + void SetDistance(float inMinDistance, float inMaxDistance) { JPH_ASSERT(inMinDistance <= inMaxDistance); mMinDistance = inMinDistance; mMaxDistance = inMaxDistance; } + float GetMinDistance() const { return mMinDistance; } + float GetMaxDistance() const { return mMaxDistance; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline float GetTotalLambdaPosition() const { return mAxisConstraint.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Min/max distance that must be kept between the world space points + float mMinDistance; + float mMaxDistance; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // RUN TIME PROPERTIES FOLLOW + + // World space positions and normal + RVec3 mWorldSpacePosition1; + RVec3 mWorldSpacePosition2; + Vec3 mWorldSpaceNormal; + + // Depending on if the distance < min or distance > max we can apply forces to prevent further violations + float mMinLambda; + float mMaxLambda; + + // The constraint part + AxisConstraintPart mAxisConstraint; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp new file mode 100644 index 000000000000..b0639851672f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp @@ -0,0 +1,215 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(FixedConstraintSettings) +{ + JPH_ADD_BASE_CLASS(FixedConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(FixedConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAutoDetectPoint) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX2) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY2) +} + +void FixedConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mAutoDetectPoint); + inStream.Write(mPoint1); + inStream.Write(mAxisX1); + inStream.Write(mAxisY1); + inStream.Write(mPoint2); + inStream.Write(mAxisX2); + inStream.Write(mAxisY2); +} + +void FixedConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mAutoDetectPoint); + inStream.Read(mPoint1); + inStream.Read(mAxisX1); + inStream.Read(mAxisY1); + inStream.Read(mPoint2); + inStream.Read(mAxisX2); + inStream.Read(mAxisY2); +} + +TwoBodyConstraint *FixedConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new FixedConstraint(inBody1, inBody2, *this); +} + +FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mAxisX1, inSettings.mAxisY1, inSettings.mAxisX2, inSettings.mAxisY2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + if (inSettings.mAutoDetectPoint) + { + // Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point + RVec3 anchor; + if (!inBody1.CanBeKinematicOrDynamic()) + anchor = inBody2.GetCenterOfMassPosition(); + else if (!inBody2.CanBeKinematicOrDynamic()) + anchor = inBody1.GetCenterOfMassPosition(); + else + { + // Otherwise use weighted anchor point towards the lightest body + Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real total_inv_mass = inv_m1 + inv_m2; + if (total_inv_mass != 0.0_r) + anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2); + else + anchor = inBody1.GetCenterOfMassPosition(); + } + + // Store local positions + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * anchor); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * anchor); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + } + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + } +} + +void FixedConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void FixedConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint values that don't change when the bodies don't change position + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); +} + +void FixedConstraint::ResetWarmStart() +{ + mRotationConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); +} + +void FixedConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool FixedConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + return rot || pos; +} + +bool FixedConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Solve rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + + // Solve position constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + return rot || pos; +} + +#ifdef JPH_DEBUG_RENDERER +void FixedConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 com1 = mBody1->GetCenterOfMassTransform(); + RMat44 com2 = mBody2->GetCenterOfMassTransform(); + + RVec3 anchor1 = com1 * mLocalSpacePosition1; + RVec3 anchor2 = com2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawLine(com1.GetTranslation(), anchor1, Color::sGreen); + inRenderer->DrawLine(com2.GetTranslation(), anchor2, Color::sBlue); +} +#endif // JPH_DEBUG_RENDERER + +void FixedConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mRotationConstraintPart.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); +} + +void FixedConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mRotationConstraintPart.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); +} + +Ref FixedConstraint::GetConstraintSettings() const +{ + FixedConstraintSettings *settings = new FixedConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mAxisX1 = Vec3::sAxisX(); + settings->mAxisY1 = Vec3::sAxisY(); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mAxisX2 = mInvInitialOrientation.RotateAxisX(); + settings->mAxisY2 = mInvInitialOrientation.RotateAxisY(); + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h new file mode 100644 index 000000000000..114cf646e154 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h @@ -0,0 +1,96 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Fixed constraint settings, used to create a fixed constraint +class JPH_EXPORT FixedConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, FixedConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (they will be fixated in their current relative position/orientation). Set this to false if you want to supply the attachment points yourself. + bool mAutoDetectPoint = false; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mAxisX1 = Vec3::sAxisX(); + Vec3 mAxisY1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mAxisX2 = Vec3::sAxisX(); + Vec3 mAxisY2 = Vec3::sAxisY(); + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A fixed constraint welds two bodies together removing all degrees of freedom between them. +/// This variant uses Euler angles for the rotation constraint. +class JPH_EXPORT FixedConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Fixed; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mInvInitialOrientation, mLocalSpacePosition2); } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + +private: + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + Quat mInvInitialOrientation; + + // RUN TIME PROPERTIES FOLLOW + + // The constraint parts + RotationEulerConstraintPart mRotationConstraintPart; + PointConstraintPart mPointConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp new file mode 100644 index 000000000000..b2a7284c9edf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp @@ -0,0 +1,188 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GearConstraintSettings) +{ + JPH_ADD_BASE_CLASS(GearConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(GearConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis1) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis2) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mRatio) +} + +void GearConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mHingeAxis1); + inStream.Write(mHingeAxis2); + inStream.Write(mRatio); +} + +void GearConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mHingeAxis1); + inStream.Read(mHingeAxis2); + inStream.Read(mRatio); +} + +TwoBodyConstraint *GearConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new GearConstraint(inBody1, inBody2, *this); +} + +GearConstraint::GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mLocalSpaceHingeAxis1(inSettings.mHingeAxis1), + mLocalSpaceHingeAxis2(inSettings.mHingeAxis2), + mRatio(inSettings.mRatio) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpaceHingeAxis1 = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis1).Normalized(); + mLocalSpaceHingeAxis2 = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis2).Normalized(); + } +} + +void GearConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mWorldSpaceHingeAxis1 = inRotation1 * mLocalSpaceHingeAxis1; + mWorldSpaceHingeAxis2 = inRotation2 * mLocalSpaceHingeAxis2; + + mGearConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio); +} + +void GearConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); +} + +void GearConstraint::ResetWarmStart() +{ + mGearConstraintPart.Deactivate(); +} + +void GearConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mGearConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool GearConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mGearConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio); +} + +bool GearConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mGear1Constraint == nullptr || mGear2Constraint == nullptr) + return false; + + float gear1rot; + if (mGear1Constraint->GetSubType() == EConstraintSubType::Hinge) + { + gear1rot = StaticCast(mGear1Constraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float gear2rot; + if (mGear2Constraint->GetSubType() == EConstraintSubType::Hinge) + { + gear2rot = StaticCast(mGear2Constraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float error = CenterAngleAroundZero(fmod(gear1rot + mRatio * gear2rot, 2.0f * JPH_PI)); + if (error == 0.0f) + return false; + + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); + return mGearConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void GearConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint axis + inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis1, Color::sGreen, 0.01f); + inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceHingeAxis2, Color::sBlue, 0.01f); +} + +#endif // JPH_DEBUG_RENDERER + +void GearConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mGearConstraintPart.SaveState(inStream); +} + +void GearConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mGearConstraintPart.RestoreState(inStream); +} + +Ref GearConstraint::GetConstraintSettings() const +{ + GearConstraintSettings *settings = new GearConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mHingeAxis1 = mLocalSpaceHingeAxis1; + settings->mHingeAxis2 = mLocalSpaceHingeAxis2; + settings->mRatio = mRatio; + return settings; +} + +Mat44 GearConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis1.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis1.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 GearConstraint::GetConstraintToBody2Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis2.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis2.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h new file mode 100644 index 000000000000..134e74c7ccdf --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h @@ -0,0 +1,116 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Gear constraint settings +class JPH_EXPORT GearConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GearConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Defines the ratio between the rotation of both gears + /// The ratio is defined as: Gear1Rotation(t) = -ratio * Gear2Rotation(t) + /// @param inNumTeethGear1 Number of teeth that body 1 has + /// @param inNumTeethGear2 Number of teeth that body 2 has + void SetRatio(int inNumTeethGear1, int inNumTeethGear2) + { + mRatio = float(inNumTeethGear2) / float(inNumTeethGear1); + } + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + Vec3 mHingeAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + Vec3 mHingeAxis2 = Vec3::sAxisX(); + + /// Ratio between both gears, see SetRatio. + float mRatio = 1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A gear constraint constrains the rotation of body1 to the rotation of body 2 using a gear. +/// Note that this constraint needs to be used in conjunction with a two hinge constraints. +class JPH_EXPORT GearConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct gear constraint + GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Gear; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift. + void SetConstraints(const Constraint *inGear1, const Constraint *inGear2) { mGear1Constraint = inGear1; mGear2Constraint = inGear2; } + + ///@name Get Lagrange multiplier from last physics update (the angular impulse applied to satisfy the constraint) + inline float GetTotalLambda() const { return mGearConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space hinge axis for body 1 + Vec3 mLocalSpaceHingeAxis1; + + // Local space hinge axis for body 2 + Vec3 mLocalSpaceHingeAxis2; + + // Ratio between gear 1 and 2 + float mRatio; + + // The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift. + RefConst mGear1Constraint; + RefConst mGear2Constraint; + + // RUN TIME PROPERTIES FOLLOW + + // World space hinge axis for body 1 + Vec3 mWorldSpaceHingeAxis1; + + // World space hinge axis for body 2 + Vec3 mWorldSpaceHingeAxis2; + + // The constraint parts + GearConstraintPart mGearConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp new file mode 100644 index 000000000000..824657362959 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp @@ -0,0 +1,424 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HingeConstraintSettings) +{ + JPH_ADD_BASE_CLASS(HingeConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(HingeConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMin) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMax) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsSpringSettings) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMaxFrictionTorque) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMotorSettings) +} + +void HingeConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mHingeAxis1); + inStream.Write(mNormalAxis1); + inStream.Write(mPoint2); + inStream.Write(mHingeAxis2); + inStream.Write(mNormalAxis2); + inStream.Write(mLimitsMin); + inStream.Write(mLimitsMax); + inStream.Write(mMaxFrictionTorque); + mLimitsSpringSettings.SaveBinaryState(inStream); + mMotorSettings.SaveBinaryState(inStream); +} + +void HingeConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mHingeAxis1); + inStream.Read(mNormalAxis1); + inStream.Read(mPoint2); + inStream.Read(mHingeAxis2); + inStream.Read(mNormalAxis2); + inStream.Read(mLimitsMin); + inStream.Read(mLimitsMax); + inStream.Read(mMaxFrictionTorque); + mLimitsSpringSettings.RestoreBinaryState(inStream); + mMotorSettings.RestoreBinaryState(inStream);} + +TwoBodyConstraint *HingeConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new HingeConstraint(inBody1, inBody2, *this); +} + +HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMaxFrictionTorque(inSettings.mMaxFrictionTorque), + mMotorSettings(inSettings.mMotorSettings) +{ + // Store limits + JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint in this case"); + SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax); + + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXZ(inSettings.mNormalAxis1, inSettings.mHingeAxis1, inSettings.mNormalAxis2, inSettings.mHingeAxis2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpaceHingeAxis1 = inv_transform1.Multiply3x3(inSettings.mHingeAxis1).Normalized(); + mLocalSpaceNormalAxis1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized(); + + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + mLocalSpaceHingeAxis2 = inv_transform2.Multiply3x3(inSettings.mHingeAxis2).Normalized(); + mLocalSpaceNormalAxis2 = inv_transform2.Multiply3x3(inSettings.mNormalAxis2).Normalized(); + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpaceHingeAxis1 = inSettings.mHingeAxis1; + mLocalSpaceNormalAxis1 = inSettings.mNormalAxis1; + + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mLocalSpaceHingeAxis2 = inSettings.mHingeAxis2; + mLocalSpaceNormalAxis2 = inSettings.mNormalAxis2; + } + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void HingeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float HingeConstraint::GetCurrentAngle() const +{ + // See: CalculateA1AndTheta + Quat rotation1 = mBody1->GetRotation(); + Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated(); + return diff.GetRotationAngle(rotation1 * mLocalSpaceHingeAxis1); +} + +void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax) +{ + JPH_ASSERT(inLimitsMin <= 0.0f && inLimitsMin >= -JPH_PI); + JPH_ASSERT(inLimitsMax >= 0.0f && inLimitsMax <= JPH_PI); + mLimitsMin = inLimitsMin; + mLimitsMax = inLimitsMax; + mHasLimits = mLimitsMin > -JPH_PI && mLimitsMax < JPH_PI; +} + +void HingeConstraint::CalculateA1AndTheta() +{ + if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f) + { + Quat rotation1 = mBody1->GetRotation(); + + // Calculate relative rotation in world space + // + // The rest rotation is: + // + // q2 = q1 r0 + // + // But the actual rotation is + // + // q2 = diff q1 r0 + // <=> diff = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // diff = relative rotation in world space + Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated(); + + // Calculate hinge axis in world space + mA1 = rotation1 * mLocalSpaceHingeAxis1; + + // Get rotation angle around the hinge axis + mTheta = diff.GetRotationAngle(mA1); + } +} + +void HingeConstraint::CalculateRotationLimitsConstraintProperties(float inDeltaTime) +{ + // Apply constraint if outside of limits + if (mHasLimits && (mTheta <= mLimitsMin || mTheta >= mLimitsMax)) + mRotationLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, GetSmallestAngleToLimit(), mLimitsSpringSettings); + else + mRotationLimitsConstraintPart.Deactivate(); +} + +void HingeConstraint::CalculateMotorConstraintProperties(float inDeltaTime) +{ + switch (mMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1); + else + mMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1, -mTargetAngularVelocity); + break; + + case EMotorState::Position: + if (mMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mSpringSettings); + else + mMotorConstraintPart.Deactivate(); + break; + } +} + +void HingeConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Cache constraint values that are valid until the bodies move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2)); + CalculateA1AndTheta(); + CalculateRotationLimitsConstraintProperties(inDeltaTime); + CalculateMotorConstraintProperties(inDeltaTime); +} + +void HingeConstraint::ResetWarmStart() +{ + mMotorConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + mRotationLimitsConstraintPart.Deactivate(); +} + +void HingeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mMotorConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mRotationLimitsConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +float HingeConstraint::GetSmallestAngleToLimit() const +{ + float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin); + float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax); + return abs(dist_to_min) < abs(dist_to_max)? dist_to_min : dist_to_max; +} + +bool HingeConstraint::IsMinLimitClosest() const +{ + float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin); + float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax); + return abs(dist_to_min) < abs(dist_to_max); +} + +bool HingeConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mMotorConstraintPart.IsActive()) + { + switch (mMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionTorque * inDeltaTime; + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, inDeltaTime * mMotorSettings.mMinTorqueLimit, inDeltaTime * mMotorSettings.mMaxTorqueLimit); + break; + } + } + + // Solve point constraint + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve rotation limits + bool limit = false; + if (mRotationLimitsConstraintPart.IsActive()) + { + float min_lambda, max_lambda; + if (mLimitsMin == mLimitsMax) + { + min_lambda = -FLT_MAX; + max_lambda = FLT_MAX; + } + else if (IsMinLimitClosest()) + { + min_lambda = 0.0f; + max_lambda = FLT_MAX; + } + else + { + min_lambda = -FLT_MAX; + max_lambda = 0.0f; + } + limit = mRotationLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, min_lambda, max_lambda); + } + + return motor || pos || rot || limit; +} + +bool HingeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Motor operates on velocities only, don't call SolvePositionConstraint + + // Solve point constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + // Solve rotation constraint + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); // Note that previous call to GetRotation() is out of date since the rotation has changed + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2)); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + // Solve rotation limits + bool limit = false; + if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f) + { + CalculateA1AndTheta(); + CalculateRotationLimitsConstraintProperties(inDeltaTime); + if (mRotationLimitsConstraintPart.IsActive()) + limit = mRotationLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, GetSmallestAngleToLimit(), inBaumgarte); + } + + return pos || rot || limit; +} + +#ifdef JPH_DEBUG_RENDERER +void HingeConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint + RVec3 constraint_pos1 = transform1 * mLocalSpacePosition1; + inRenderer->DrawMarker(constraint_pos1, Color::sRed, 0.1f); + inRenderer->DrawLine(constraint_pos1, transform1 * (mLocalSpacePosition1 + mDrawConstraintSize * mLocalSpaceHingeAxis1), Color::sRed); + + RVec3 constraint_pos2 = transform2 * mLocalSpacePosition2; + inRenderer->DrawMarker(constraint_pos2, Color::sGreen, 0.1f); + inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceHingeAxis2), Color::sGreen); + inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceNormalAxis2), Color::sWhite); +} + +void HingeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + if (mHasLimits && mLimitsMax > mLimitsMin) + { + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Vec3 hinge_axis1 = transform1.Multiply3x3(mLocalSpaceHingeAxis1); + Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceNormalAxis1); + + inRenderer->DrawPie(position1, mDrawConstraintSize, hinge_axis1, normal_axis1, mLimitsMin, mLimitsMax, Color::sPurple, DebugRenderer::ECastShadow::Off); + } +} +#endif // JPH_DEBUG_RENDERER + +void HingeConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mMotorConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); + mRotationLimitsConstraintPart.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetAngle); +} + +void HingeConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mMotorConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); + mRotationLimitsConstraintPart.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetAngle); +} + + +Ref HingeConstraint::GetConstraintSettings() const +{ + HingeConstraintSettings *settings = new HingeConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mHingeAxis1 = mLocalSpaceHingeAxis1; + settings->mNormalAxis1 = mLocalSpaceNormalAxis1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mHingeAxis2 = mLocalSpaceHingeAxis2; + settings->mNormalAxis2 = mLocalSpaceNormalAxis2; + settings->mLimitsMin = mLimitsMin; + settings->mLimitsMax = mLimitsMax; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + settings->mMaxFrictionTorque = mMaxFrictionTorque; + settings->mMotorSettings = mMotorSettings; + return settings; +} + +Mat44 HingeConstraint::GetConstraintToBody1Matrix() const +{ + return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(mLocalSpaceNormalAxis1, 0), Vec4(mLocalSpaceHingeAxis1.Cross(mLocalSpaceNormalAxis1), 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 HingeConstraint::GetConstraintToBody2Matrix() const +{ + return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(mLocalSpaceNormalAxis2, 0), Vec4(mLocalSpaceHingeAxis2.Cross(mLocalSpaceNormalAxis2), 0), Vec4(mLocalSpacePosition2, 1)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h new file mode 100644 index 000000000000..691bb1f5f4b5 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h @@ -0,0 +1,200 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Hinge constraint settings, used to create a hinge constraint +class JPH_EXPORT HingeConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HingeConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Hinge axis is the axis where rotation is allowed. + /// When the normal axis of both bodies align in world space, the hinge angle is defined to be 0. + /// mHingeAxis1 and mNormalAxis1 should be perpendicular. mHingeAxis2 and mNormalAxis2 should also be perpendicular. + /// If you configure the joint in world space and create both bodies with a relative rotation you want to be defined as zero, + /// you can simply set mHingeAxis1 = mHingeAxis2 and mNormalAxis1 = mNormalAxis2. + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mHingeAxis1 = Vec3::sAxisY(); + Vec3 mNormalAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mHingeAxis2 = Vec3::sAxisY(); + Vec3 mNormalAxis2 = Vec3::sAxisX(); + + /// Rotation around the hinge axis will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-pi, 0] and mLimitsMax e [0, pi]. + /// Both angles are in radians. + float mLimitsMin = -JPH_PI; + float mLimitsMax = JPH_PI; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + + /// Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor + float mMaxFrictionTorque = 0.0f; + + /// In case the constraint is powered, this determines the motor settings around the hinge axis + MotorSettings mMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A hinge constraint constrains 2 bodies on a single point and allows only a single axis of rotation +class JPH_EXPORT HingeConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct hinge constraint + HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Hinge; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Get the attachment point for body 1 relative to body 1 COM (transform by Body::GetCenterOfMassTransform to take to world space) + inline Vec3 GetLocalSpacePoint1() const { return mLocalSpacePosition1; } + + /// Get the attachment point for body 2 relative to body 2 COM (transform by Body::GetCenterOfMassTransform to take to world space) + inline Vec3 GetLocalSpacePoint2() const { return mLocalSpacePosition2; } + + // Local space hinge directions (transform direction by Body::GetCenterOfMassTransform to take to world space) + Vec3 GetLocalSpaceHingeAxis1() const { return mLocalSpaceHingeAxis1; } + Vec3 GetLocalSpaceHingeAxis2() const { return mLocalSpaceHingeAxis2; } + + // Local space normal directions (transform direction by Body::GetCenterOfMassTransform to take to world space) + Vec3 GetLocalSpaceNormalAxis1() const { return mLocalSpaceNormalAxis1; } + Vec3 GetLocalSpaceNormalAxis2() const { return mLocalSpaceNormalAxis2; } + + /// Get the current rotation angle from the rest position + float GetCurrentAngle() const; + + // Friction control + void SetMaxFrictionTorque(float inFrictionTorque) { mMaxFrictionTorque = inFrictionTorque; } + float GetMaxFrictionTorque() const { return mMaxFrictionTorque; } + + // Motor settings + MotorSettings & GetMotorSettings() { return mMotorSettings; } + const MotorSettings & GetMotorSettings() const { return mMotorSettings; } + + // Motor controls + void SetMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; } + EMotorState GetMotorState() const { return mMotorState; } + void SetTargetAngularVelocity(float inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } ///< rad/s + float GetTargetAngularVelocity() const { return mTargetAngularVelocity; } + void SetTargetAngle(float inAngle) { mTargetAngle = mHasLimits? Clamp(inAngle, mLimitsMin, mLimitsMax) : inAngle; } ///< rad + float GetTargetAngle() const { return mTargetAngle; } + + /// Update the rotation limits of the hinge, value in radians (see HingeConstraintSettings) + void SetLimits(float inLimitsMin, float inLimitsMax); + float GetLimitsMin() const { return mLimitsMin; } + float GetLimitsMax() const { return mLimitsMax; } + bool HasLimits() const { return mHasLimits; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline Vector<2> GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaRotationLimits() const { return mRotationLimitsConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mMotorConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateA1AndTheta(); + void CalculateRotationLimitsConstraintProperties(float inDeltaTime); + void CalculateMotorConstraintProperties(float inDeltaTime); + inline float GetSmallestAngleToLimit() const; + inline bool IsMinLimitClosest() const; + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space hinge directions + Vec3 mLocalSpaceHingeAxis1; + Vec3 mLocalSpaceHingeAxis2; + + // Local space normal direction (direction relative to which to draw constraint limits) + Vec3 mLocalSpaceNormalAxis1; + Vec3 mLocalSpaceNormalAxis2; + + // Inverse of initial relative orientation between bodies (which defines hinge angle = 0) + Quat mInvInitialOrientation; + + // Hinge limits + bool mHasLimits; + float mLimitsMin; + float mLimitsMax; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // Friction + float mMaxFrictionTorque; + + // Motor controls + MotorSettings mMotorSettings; + EMotorState mMotorState = EMotorState::Off; + float mTargetAngularVelocity = 0.0f; + float mTargetAngle = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Current rotation around the hinge axis + float mTheta = 0.0f; + + // World space hinge axis for body 1 + Vec3 mA1; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + HingeRotationConstraintPart mRotationConstraintPart; + AngleConstraintPart mRotationLimitsConstraintPart; + AngleConstraintPart mMotorConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp new file mode 100644 index 000000000000..e4daecdd7d52 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp @@ -0,0 +1,43 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MotorSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mDamping, "mDamping") + JPH_ADD_ATTRIBUTE(MotorSettings, mMinForceLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMaxForceLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMinTorqueLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMaxTorqueLimit) +} + +void MotorSettings::SaveBinaryState(StreamOut &inStream) const +{ + mSpringSettings.SaveBinaryState(inStream); + inStream.Write(mMinForceLimit); + inStream.Write(mMaxForceLimit); + inStream.Write(mMinTorqueLimit); + inStream.Write(mMaxTorqueLimit); +} + +void MotorSettings::RestoreBinaryState(StreamIn &inStream) +{ + mSpringSettings.RestoreBinaryState(inStream); + inStream.Read(mMinForceLimit); + inStream.Read(mMaxForceLimit); + inStream.Read(mMinTorqueLimit); + inStream.Read(mMaxTorqueLimit); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h new file mode 100644 index 000000000000..601fa3dc3f9b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +enum class EMotorState +{ + Off, ///< Motor is off + Velocity, ///< Motor will drive to target velocity + Position ///< Motor will drive to target position +}; + +/// Class that contains the settings for a constraint motor. +/// See the main page of the API documentation for more information on how to configure a motor. +class JPH_EXPORT MotorSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MotorSettings) + +public: + /// Constructor + MotorSettings() = default; + MotorSettings(const MotorSettings &) = default; + MotorSettings & operator = (const MotorSettings &) = default; + MotorSettings(float inFrequency, float inDamping) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping) { JPH_ASSERT(IsValid()); } + MotorSettings(float inFrequency, float inDamping, float inForceLimit, float inTorqueLimit) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping), mMinForceLimit(-inForceLimit), mMaxForceLimit(inForceLimit), mMinTorqueLimit(-inTorqueLimit), mMaxTorqueLimit(inTorqueLimit) { JPH_ASSERT(IsValid()); } + + /// Set asymmetric force limits + void SetForceLimits(float inMin, float inMax) { JPH_ASSERT(inMin <= inMax); mMinForceLimit = inMin; mMaxForceLimit = inMax; } + + /// Set asymmetric torque limits + void SetTorqueLimits(float inMin, float inMax) { JPH_ASSERT(inMin <= inMax); mMinTorqueLimit = inMin; mMaxTorqueLimit = inMax; } + + /// Set symmetric force limits + void SetForceLimit(float inLimit) { mMinForceLimit = -inLimit; mMaxForceLimit = inLimit; } + + /// Set symmetric torque limits + void SetTorqueLimit(float inLimit) { mMinTorqueLimit = -inLimit; mMaxTorqueLimit = inLimit; } + + /// Check if settings are valid + bool IsValid() const { return mSpringSettings.mFrequency >= 0.0f && mSpringSettings.mDamping >= 0.0f && mMinForceLimit <= mMaxForceLimit && mMinTorqueLimit <= mMaxTorqueLimit; } + + /// Saves the contents of the motor settings in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores contents from the binary stream inStream. + void RestoreBinaryState(StreamIn &inStream); + + // Settings + SpringSettings mSpringSettings { ESpringMode::FrequencyAndDamping, 2.0f, 1.0f }; ///< Settings for the spring that is used to drive to the position target (not used when motor is a velocity motor). + float mMinForceLimit = -FLT_MAX; ///< Minimum force to apply in case of a linear constraint (N). Usually this is -mMaxForceLimit unless you want a motor that can e.g. push but not pull. Not used when motor is an angular motor. + float mMaxForceLimit = FLT_MAX; ///< Maximum force to apply in case of a linear constraint (N). Not used when motor is an angular motor. + float mMinTorqueLimit = -FLT_MAX; ///< Minimum torque to apply in case of a angular constraint (N m). Usually this is -mMaxTorqueLimit unless you want a motor that can e.g. push but not pull. Not used when motor is a position motor. + float mMaxTorqueLimit = FLT_MAX; ///< Maximum torque to apply in case of a angular constraint (N m). Not used when motor is a position motor. +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp new file mode 100644 index 000000000000..e5b6eb7da037 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp @@ -0,0 +1,458 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PathConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPath) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathPosition) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathRotation) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathFraction) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mMaxFrictionForce) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPositionMotorSettings) + JPH_ADD_ENUM_ATTRIBUTE(PathConstraintSettings, mRotationConstraintType) +} + +void PathConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + mPath->SaveBinaryState(inStream); + inStream.Write(mPathPosition); + inStream.Write(mPathRotation); + inStream.Write(mPathFraction); + inStream.Write(mMaxFrictionForce); + inStream.Write(mRotationConstraintType); + mPositionMotorSettings.SaveBinaryState(inStream); +} + +void PathConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + PathConstraintPath::PathResult result = PathConstraintPath::sRestoreFromBinaryState(inStream); + if (!result.HasError()) + mPath = result.Get(); + inStream.Read(mPathPosition); + inStream.Read(mPathRotation); + inStream.Read(mPathFraction); + inStream.Read(mMaxFrictionForce); + inStream.Read(mRotationConstraintType); + mPositionMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *PathConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PathConstraint(inBody1, inBody2, *this); +} + +PathConstraint::PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mRotationConstraintType(inSettings.mRotationConstraintType), + mMaxFrictionForce(inSettings.mMaxFrictionForce), + mPositionMotorSettings(inSettings.mPositionMotorSettings) +{ + // Calculate transform that takes us from the path start to center of mass space of body 1 + mPathToBody1 = Mat44::sRotationTranslation(inSettings.mPathRotation, inSettings.mPathPosition - inBody1.GetShape()->GetCenterOfMass()); + + SetPath(inSettings.mPath, inSettings.mPathFraction); +} + +void PathConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mPathToBody1.SetTranslation(mPathToBody1.GetTranslation() - inDeltaCOM); + else if (mBody2->GetID() == inBodyID) + mPathToBody2.SetTranslation(mPathToBody2.GetTranslation() - inDeltaCOM); +} + +void PathConstraint::SetPath(const PathConstraintPath *inPath, float inPathFraction) +{ + mPath = inPath; + mPathFraction = inPathFraction; + + if (mPath != nullptr) + { + // Get the point on the path for this fraction + Vec3 path_point, path_tangent, path_normal, path_binormal; + mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal); + + // Construct the matrix that takes us from the closest point on the path to body 2 center of mass space + Mat44 closest_point_to_path(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4(path_point, 1)); + Mat44 cp_to_body1 = mPathToBody1 * closest_point_to_path; + mPathToBody2 = (mBody2->GetInverseCenterOfMassTransform() * mBody1->GetCenterOfMassTransform()).ToMat44() * cp_to_body1; + + // Calculate initial orientation + if (mRotationConstraintType == EPathRotationConstraintType::FullyConstrained) + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientation(*mBody1, *mBody2); + } +} + +void PathConstraint::CalculateConstraintProperties(float inDeltaTime) +{ + // Get transforms of body 1 and 2 + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Get the transform of the path transform as seen from body 1 in world space + RMat44 path_to_world_1 = transform1 * mPathToBody1; + + // Get the transform of from the point on path that body 2 is attached to in world space + RMat44 path_to_world_2 = transform2 * mPathToBody2; + + // Calculate new closest point on path + RVec3 position2 = path_to_world_2.GetTranslation(); + Vec3 position2_local_to_path = Vec3(path_to_world_1.InversedRotationTranslation() * position2); + mPathFraction = mPath->GetClosestPoint(position2_local_to_path, mPathFraction); + + // Get the point on the path for this fraction + Vec3 path_point, path_tangent, path_normal, path_binormal; + mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal); + + // Calculate R1 and R2 + RVec3 path_point_ws = path_to_world_1 * path_point; + mR1 = Vec3(path_point_ws - mBody1->GetCenterOfMassPosition()); + mR2 = Vec3(position2 - mBody2->GetCenterOfMassPosition()); + + // Calculate U = X2 + R2 - X1 - R1 + mU = Vec3(position2 - path_point_ws); + + // Calculate world space normals + mPathNormal = path_to_world_1.Multiply3x3(path_normal); + mPathBinormal = path_to_world_1.Multiply3x3(path_binormal); + + // Calculate slide axis + mPathTangent = path_to_world_1.Multiply3x3(path_tangent); + + // Prepare constraint part for position constraint to slide along the path + mPositionConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mR1 + mU, *mBody2, transform2.GetRotation(), mR2, mPathNormal, mPathBinormal); + + // Check if closest point is on the boundary of the path and if so apply limit + if (!mPath->IsLooping() && (mPathFraction <= 0.0f || mPathFraction >= mPath->GetPathMaxFraction())) + mPositionLimitsConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent); + else + mPositionLimitsConstraintPart.Deactivate(); + + // Prepare rotation constraint part + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathTangent, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisX()); + break; + + case EPathRotationConstraintType::ConstrainAroundNormal: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathNormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisZ()); + break; + + case EPathRotationConstraintType::ConstrainAroundBinormal: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathBinormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisY()); + break; + + case EPathRotationConstraintType::ConstrainToPath: + // We need to calculate the inverse of the rotation from body 1 to body 2 for the current path position (see: RotationEulerConstraintPart::sGetInvInitialOrientation) + // RotationBody2 = RotationBody1 * InitialOrientation <=> InitialOrientation^-1 = RotationBody2^-1 * RotationBody1 + // We can express RotationBody2 in terms of RotationBody1: RotationBody2 = RotationBody1 * PathToBody1 * RotationClosestPointOnPath * PathToBody2^-1 + // Combining these two: InitialOrientation^-1 = PathToBody2 * (PathToBody1 * RotationClosestPointOnPath)^-1 + mInvInitialOrientation = mPathToBody2.Multiply3x3RightTransposed(mPathToBody1.Multiply3x3(Mat44(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4::sZero()))).GetQuaternion(); + [[fallthrough]]; + + case EPathRotationConstraintType::FullyConstrained: + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), *mBody2, transform2.GetRotation()); + break; + } + + // Motor properties + switch (mPositionMotorState) + { + case EMotorState::Off: + if (mMaxFrictionForce > 0.0f) + mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent); + else + mPositionMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, -mTargetVelocity); + break; + + case EMotorState::Position: + if (mPositionMotorSettings.mSpringSettings.HasStiffness()) + { + // Calculate constraint value to drive to + float c; + if (mPath->IsLooping()) + { + float max_fraction = mPath->GetPathMaxFraction(); + c = fmod(mPathFraction - mTargetPathFraction, max_fraction); + float half_max_fraction = 0.5f * max_fraction; + if (c > half_max_fraction) + c -= max_fraction; + else if (c < -half_max_fraction) + c += max_fraction; + } + else + c = mPathFraction - mTargetPathFraction; + mPositionMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mSpringSettings); + } + else + mPositionMotorConstraintPart.Deactivate(); + break; + } +} + +void PathConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(inDeltaTime); +} + +void PathConstraint::ResetWarmStart() +{ + mPositionMotorConstraintPart.Deactivate(); + mPositionConstraintPart.Deactivate(); + mPositionLimitsConstraintPart.Deactivate(); + mHingeConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); +} + +void PathConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPositionMotorConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio); + mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mPathNormal, mPathBinormal, inWarmStartImpulseRatio); + mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio); + + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + mHingeConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + break; + } +} + +bool PathConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mPositionMotorConstraintPart.IsActive()) + { + switch (mPositionMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionForce * inDeltaTime; + motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, inDeltaTime * mPositionMotorSettings.mMinForceLimit, inDeltaTime * mPositionMotorSettings.mMaxForceLimit); + break; + } + } + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathNormal, mPathBinormal); + + // Solve limits along path axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mPathFraction <= 0.0f) + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, 0, FLT_MAX); + else + { + JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction()); + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -FLT_MAX, 0); + } + } + + // Solve rotational constraint + // Note, this is not entirely correct, we should apply a velocity constraint so that the body will actually follow the path + // by looking at the derivative of the tangent, normal or binormal but we don't. This means the position constraint solver + // will need to correct the orientation error that builds up, which in turn means that the simulation is not physically correct. + bool rot = false; + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + rot = mHingeConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + break; + } + + return motor || pos || limit || rot; +} + +bool PathConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(inDeltaTime); + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mPathNormal, mPathBinormal, inBaumgarte); + + // Solve limits along path axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mPathFraction <= 0.0f) + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte); + else + { + JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction()); + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte); + } + } + + // Solve rotational constraint + bool rot = false; + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + rot = mHingeConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + break; + } + + return pos || limit || rot; +} + +#ifdef JPH_DEBUG_RENDERER +void PathConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + if (mPath != nullptr) + { + // Draw the path in world space + RMat44 path_to_world = mBody1->GetCenterOfMassTransform() * mPathToBody1; + mPath->DrawPath(inRenderer, path_to_world); + + // Draw anchor point of both bodies in world space + RVec3 x1 = mBody1->GetCenterOfMassPosition() + mR1; + RVec3 x2 = mBody2->GetCenterOfMassPosition() + mR2; + inRenderer->DrawMarker(x1, Color::sYellow, 0.1f); + inRenderer->DrawMarker(x2, Color::sYellow, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathTangent, Color::sBlue, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathNormal, Color::sRed, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathBinormal, Color::sGreen, 0.1f); + inRenderer->DrawText3D(x1, StringFormat("%.1f", (double)mPathFraction)); + + // Draw motor + switch (mPositionMotorState) + { + case EMotorState::Position: + { + // Draw target marker + Vec3 position, tangent, normal, binormal; + mPath->GetPointOnPath(mTargetPathFraction, position, tangent, normal, binormal); + inRenderer->DrawMarker(path_to_world * position, Color::sYellow, 1.0f); + break; + } + + case EMotorState::Velocity: + { + RVec3 position = mBody2->GetCenterOfMassPosition() + mR2; + inRenderer->DrawArrow(position, position + mPathTangent * mTargetVelocity, Color::sRed, 0.1f); + break; + } + + case EMotorState::Off: + break; + } + } +} +#endif // JPH_DEBUG_RENDERER + +void PathConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPositionConstraintPart.SaveState(inStream); + mPositionLimitsConstraintPart.SaveState(inStream); + mPositionMotorConstraintPart.SaveState(inStream); + mHingeConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + + inStream.Write(mMaxFrictionForce); + inStream.Write(mPositionMotorSettings); + inStream.Write(mPositionMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetPathFraction); + inStream.Write(mPathFraction); +} + +void PathConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPositionConstraintPart.RestoreState(inStream); + mPositionLimitsConstraintPart.RestoreState(inStream); + mPositionMotorConstraintPart.RestoreState(inStream); + mHingeConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + + inStream.Read(mMaxFrictionForce); + inStream.Read(mPositionMotorSettings); + inStream.Read(mPositionMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetPathFraction); + inStream.Read(mPathFraction); +} + +Ref PathConstraint::GetConstraintSettings() const +{ + JPH_ASSERT(false); // Not implemented yet + return nullptr; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h new file mode 100644 index 000000000000..3d15438bc9f8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h @@ -0,0 +1,191 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_SUPPRESS_WARNING_PUSH +JPH_GCC_SUPPRESS_WARNING("-Wshadow") // GCC complains about the 'Free' value conflicting with the 'Free' method + +/// How to constrain the rotation of the body to a PathConstraint +enum class EPathRotationConstraintType +{ + Free, ///< Do not constrain the rotation of the body at all + ConstrainAroundTangent, ///< Only allow rotation around the tangent vector (following the path) + ConstrainAroundNormal, ///< Only allow rotation around the normal vector (perpendicular to the path) + ConstrainAroundBinormal, ///< Only allow rotation around the binormal vector (perpendicular to the path) + ConstrainToPath, ///< Fully constrain the rotation of body 2 to the path (following the tangent and normal of the path) + FullyConstrained, ///< Fully constrain the rotation of the body 2 to the rotation of body 1 +}; + +JPH_SUPPRESS_WARNING_POP + +/// Path constraint settings, used to constrain the degrees of freedom between two bodies to a path +/// +/// The requirements of the path are that: +/// * Tangent, normal and bi-normal form an orthonormal basis with: tangent cross bi-normal = normal +/// * The path points along the tangent vector +/// * The path is continuous so doesn't contain any sharp corners +/// +/// The reason for all this is that the constraint acts like a slider constraint with the sliding axis being the tangent vector (the assumption here is that delta time will be small enough so that the path is linear for that delta time). +class JPH_EXPORT PathConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// The path that constrains the two bodies + RefConst mPath; + + /// The position of the path start relative to world transform of body 1 + Vec3 mPathPosition = Vec3::sZero(); + + /// The rotation of the path start relative to world transform of body 1 + Quat mPathRotation = Quat::sIdentity(); + + /// The fraction along the path that corresponds to the initial position of body 2. Usually this is 0, the beginning of the path. But if you want to start an object halfway the path you can calculate this with mPath->GetClosestPoint(point on path to attach body to). + float mPathFraction = 0.0f; + + /// Maximum amount of friction force to apply (N) when not driven by a motor. + float mMaxFrictionForce = 0.0f; + + /// In case the constraint is powered, this determines the motor settings along the path + MotorSettings mPositionMotorSettings; + + /// How to constrain the rotation of the body to the path + EPathRotationConstraintType mRotationConstraintType = EPathRotationConstraintType::Free; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// Path constraint, used to constrain the degrees of freedom between two bodies to a path +class JPH_EXPORT PathConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct point constraint + PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Path; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual bool IsActive() const override { return TwoBodyConstraint::IsActive() && mPath != nullptr; } + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return mPathToBody1; } + virtual Mat44 GetConstraintToBody2Matrix() const override { return mPathToBody2; } + + /// Update the path for this constraint + void SetPath(const PathConstraintPath *inPath, float inPathFraction); + + /// Access to the current path + const PathConstraintPath * GetPath() const { return mPath; } + + /// Access to the current fraction along the path e [0, GetPath()->GetMaxPathFraction()] + float GetPathFraction() const { return mPathFraction; } + + /// Friction control + void SetMaxFrictionForce(float inFrictionForce) { mMaxFrictionForce = inFrictionForce; } + float GetMaxFrictionForce() const { return mMaxFrictionForce; } + + /// Position motor settings + MotorSettings & GetPositionMotorSettings() { return mPositionMotorSettings; } + const MotorSettings & GetPositionMotorSettings() const { return mPositionMotorSettings; } + + // Position motor controls (drives body 2 along the path) + void SetPositionMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mPositionMotorSettings.IsValid()); mPositionMotorState = inState; } + EMotorState GetPositionMotorState() const { return mPositionMotorState; } + void SetTargetVelocity(float inVelocity) { mTargetVelocity = inVelocity; } + float GetTargetVelocity() const { return mTargetVelocity; } + void SetTargetPathFraction(float inFraction) { JPH_ASSERT(mPath->IsLooping() || (inFraction >= 0.0f && inFraction <= mPath->GetPathMaxFraction())); mTargetPathFraction = inFraction; } + float GetTargetPathFraction() const { return mTargetPathFraction; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vector<2> GetTotalLambdaPosition() const { return mPositionConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaPositionLimits() const { return mPositionLimitsConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mPositionMotorConstraintPart.GetTotalLambda(); } + inline Vector<2> GetTotalLambdaRotationHinge() const { return mHingeConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + RefConst mPath; ///< The path that attaches the two bodies + Mat44 mPathToBody1; ///< Transform that takes a quantity from path space to body 1 center of mass space + Mat44 mPathToBody2; ///< Transform that takes a quantity from path space to body 2 center of mass space + EPathRotationConstraintType mRotationConstraintType; ///< How to constrain the rotation of the path + + // Friction + float mMaxFrictionForce; + + // Motor controls + MotorSettings mPositionMotorSettings; + EMotorState mPositionMotorState = EMotorState::Off; + float mTargetVelocity = 0.0f; + float mTargetPathFraction = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Positions where the point constraint acts on in world space + Vec3 mR1; + Vec3 mR2; + + // X2 + R2 - X1 - R1 + Vec3 mU; + + // World space path tangent + Vec3 mPathTangent; + + // Normals to the path tangent + Vec3 mPathNormal; + Vec3 mPathBinormal; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space (only used when rotation constraint type is FullyConstrained) + Quat mInvInitialOrientation; + + // Current fraction along the path where body 2 is attached + float mPathFraction = 0.0f; + + // Translation constraint parts + DualAxisConstraintPart mPositionConstraintPart; ///< Constraint part that keeps the movement along the tangent of the path + AxisConstraintPart mPositionLimitsConstraintPart; ///< Constraint part that prevents movement beyond the beginning and end of the path + AxisConstraintPart mPositionMotorConstraintPart; ///< Constraint to drive the object along the path or to apply friction + + // Rotation constraint parts + HingeRotationConstraintPart mHingeConstraintPart; ///< Constraint part that removes 2 degrees of rotation freedom + RotationEulerConstraintPart mRotationConstraintPart; ///< Constraint part that removes all rotational freedom +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp new file mode 100644 index 000000000000..69c0a82f3bd8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp @@ -0,0 +1,85 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(PathConstraintPath) +{ + JPH_ADD_BASE_CLASS(PathConstraintPath, SerializableObject) +} + +#ifdef JPH_DEBUG_RENDERER +// Helper function to transform the results of GetPointOnPath to world space +static inline void sTransformPathPoint(RMat44Arg inTransform, Vec3Arg inPosition, RVec3 &outPosition, Vec3 &ioNormal, Vec3 &ioBinormal) +{ + outPosition = inTransform * inPosition; + ioNormal = inTransform.Multiply3x3(ioNormal); + ioBinormal = inTransform.Multiply3x3(ioBinormal); +} + +// Helper function to draw a path segment +static inline void sDrawPathSegment(DebugRenderer *inRenderer, RVec3Arg inPrevPosition, RVec3Arg inPosition, Vec3Arg inNormal, Vec3Arg inBinormal) +{ + inRenderer->DrawLine(inPrevPosition, inPosition, Color::sWhite); + inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inNormal, Color::sRed, 0.02f); + inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inBinormal, Color::sGreen, 0.02f); +} + +void PathConstraintPath::DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const +{ + // Calculate first point + Vec3 lfirst_pos, first_tangent, first_normal, first_binormal; + GetPointOnPath(0.0f, lfirst_pos, first_tangent, first_normal, first_binormal); + RVec3 first_pos; + sTransformPathPoint(inBaseTransform, lfirst_pos, first_pos, first_normal, first_binormal); + + float t_max = GetPathMaxFraction(); + + // Draw the segments + RVec3 prev_pos = first_pos; + for (float t = 0.1f; t < t_max; t += 0.1f) + { + Vec3 lpos, tangent, normal, binormal; + GetPointOnPath(t, lpos, tangent, normal, binormal); + RVec3 pos; + sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal); + sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal); + prev_pos = pos; + } + + // Draw last point + Vec3 lpos, tangent, normal, binormal; + GetPointOnPath(t_max, lpos, tangent, normal, binormal); + RVec3 pos; + sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal); + sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal); +} +#endif // JPH_DEBUG_RENDERER + +void PathConstraintPath::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); + inStream.Write(mIsLooping); +} + +void PathConstraintPath::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mIsLooping); +} + +PathConstraintPath::PathResult PathConstraintPath::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &PathConstraintPath::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h new file mode 100644 index 000000000000..2e05c2ce6f6f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h @@ -0,0 +1,71 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// The path for a path constraint. It allows attaching two bodies to each other while giving the second body the freedom to move along a path relative to the first. +class JPH_EXPORT PathConstraintPath : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, PathConstraintPath) + +public: + using PathResult = Result>; + + /// Virtual destructor to ensure that derived types get their destructors called + virtual ~PathConstraintPath() override = default; + + /// Gets the max fraction along the path. I.e. sort of the length of the path. + virtual float GetPathMaxFraction() const = 0; + + /// Get the globally closest point on the curve (Could be slow!) + /// @param inPosition Position to find closest point for + /// @param inFractionHint Last known fraction along the path (can be used to speed up the search) + /// @return Fraction of closest point along the path + virtual float GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const = 0; + + /// Given the fraction along the path, get the point, tangent and normal. + /// @param inFraction Fraction along the path [0, GetPathMaxFraction()]. + /// @param outPathPosition Returns the closest position to inSearchPosition on the path. + /// @param outPathTangent Returns the tangent to the path at outPathPosition (the vector that follows the direction of the path) + /// @param outPathNormal Return the normal to the path at outPathPosition (a vector that's perpendicular to outPathTangent) + /// @param outPathBinormal Returns the binormal to the path at outPathPosition (a vector so that normal cross tangent = binormal) + virtual void GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const = 0; + + /// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points. + void SetIsLooping(bool inIsLooping) { mIsLooping = inIsLooping; } + bool IsLooping() const { return mIsLooping; } + +#ifdef JPH_DEBUG_RENDERER + /// Draw the path relative to inBaseTransform. Used for debug purposes. + void DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const; +#endif // JPH_DEBUG_RENDERER + + /// Saves the contents of the path in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a Shape of the correct type and restores its contents from the binary stream inStream. + static PathResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); + +private: + /// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points. + bool mIsLooping = false; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp new file mode 100644 index 000000000000..132d3b25fcca --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp @@ -0,0 +1,308 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PathConstraintPathHermite::Point) +{ + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mPosition) + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mTangent) + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mNormal) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintPathHermite) +{ + JPH_ADD_BASE_CLASS(PathConstraintPathHermite, PathConstraintPath) + + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite, mPoints) +} + +// Calculate position and tangent for a Cubic Hermite Spline segment +static inline void sCalculatePositionAndTangent(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inT, Vec3 &outPosition, Vec3 &outTangent) +{ + // Calculate factors for Cubic Hermite Spline + // See: https://en.wikipedia.org/wiki/Cubic_Hermite_spline + float t2 = inT * inT; + float t3 = inT * t2; + float h00 = 2.0f * t3 - 3.0f * t2 + 1.0f; + float h10 = t3 - 2.0f * t2 + inT; + float h01 = -2.0f * t3 + 3.0f * t2; + float h11 = t3 - t2; + + // Calculate d/dt for factors to calculate the tangent + float ddt_h00 = 6.0f * (t2 - inT); + float ddt_h10 = 3.0f * t2 - 4.0f * inT + 1.0f; + float ddt_h01 = -ddt_h00; + float ddt_h11 = 3.0f * t2 - 2.0f * inT; + + outPosition = h00 * inP1 + h10 * inM1 + h01 * inP2 + h11 * inM2; + outTangent = ddt_h00 * inP1 + ddt_h10 * inM1 + ddt_h01 * inP2 + ddt_h11 * inM2; +} + +// Calculate the closest point to the origin for a Cubic Hermite Spline segment +// This is used to get an estimate for the interval in which the closest point can be found, +// the interval [0, 1] is too big for Newton Raphson to work on because it is solving a 5th degree polynomial which may +// have multiple local minima that are not the root. This happens especially when the path is straight (tangents aligned with inP2 - inP1). +// Based on the bisection method: https://en.wikipedia.org/wiki/Bisection_method +static inline void sCalculateClosestPointThroughBisection(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float &outTMin, float &outTMax) +{ + outTMin = 0.0f; + outTMax = 1.0f; + + // To get the closest point of the curve to the origin we need to solve: + // d/dt P(t) . P(t) = 0 for t, where P(t) is the point on the curve segment + // Using d/dt (a(t) . b(t)) = d/dt a(t) . b(t) + a(t) . d/dt b(t) + // See: https://proofwiki.org/wiki/Derivative_of_Dot_Product_of_Vector-Valued_Functions + // d/dt P(t) . P(t) = 2 P(t) d/dt P(t) = 2 P(t) . Tangent(t) + + // Calculate the derivative at t = 0, we know P(0) = inP1 and Tangent(0) = inM1 + float ddt_min = inP1.Dot(inM1); // Leaving out factor 2, we're only interested in the root + if (abs(ddt_min) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMax = 0.0f; + return; + } + bool ddt_min_negative = ddt_min < 0.0f; + + // Calculate derivative at t = 1, we know P(1) = inP2 and Tangent(1) = inM2 + float ddt_max = inP2.Dot(inM2); + if (abs(ddt_max) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMin = 1.0f; + return; + } + bool ddt_max_negative = ddt_max < 0.0f; + + // If the signs of the derivative are not different, this algorithm can't find the root + if (ddt_min_negative == ddt_max_negative) + return; + + // With 4 iterations we'll get a result accurate to 1 / 2^4 = 0.0625 + for (int iteration = 0; iteration < 4; ++iteration) + { + float t_mid = 0.5f * (outTMin + outTMax); + Vec3 position, tangent; + sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t_mid, position, tangent); + float ddt_mid = position.Dot(tangent); + if (abs(ddt_mid) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMin = outTMax = t_mid; + return; + } + bool ddt_mid_negative = ddt_mid < 0.0f; + + // Update the search interval so that the signs of the derivative at both ends of the interval are still different + if (ddt_mid_negative == ddt_min_negative) + outTMin = t_mid; + else + outTMax = t_mid; + } +} + +// Calculate the closest point to the origin for a Cubic Hermite Spline segment +// Only considers the range t e [inTMin, inTMax] and will stop as soon as the closest point falls outside of that range +static inline float sCalculateClosestPointThroughNewtonRaphson(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inTMin, float inTMax, float &outDistanceSq) +{ + // This is the closest position on the curve to the origin that we found + Vec3 position; + + // Calculate the size of the interval + float interval = inTMax - inTMin; + + // Start in the middle of the interval + float t = 0.5f * (inTMin + inTMax); + + // Do max 10 iterations to prevent taking too much CPU time + for (int iteration = 0; iteration < 10; ++iteration) + { + // Calculate derivative at t, see comment at sCalculateClosestPointThroughBisection for derivation of the equations + Vec3 tangent; + sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t, position, tangent); + float ddt = position.Dot(tangent); // Leaving out factor 2, we're only interested in the root + + // Calculate derivative of ddt: d^2/dt P(t) . P(t) = d/dt (2 P(t) . Tangent(t)) + // = 2 (d/dt P(t)) . Tangent(t) + P(t) . d/dt Tangent(t)) = 2 (Tangent(t) . Tangent(t) + P(t) . d/dt Tangent(t)) + float d2dt_h00 = 12.0f * t - 6.0f; + float d2dt_h10 = 6.0f * t - 4.0f; + float d2dt_h01 = -d2dt_h00; + float d2dt_h11 = 6.0f * t - 2.0f; + Vec3 ddt_tangent = d2dt_h00 * inP1 + d2dt_h10 * inM1 + d2dt_h01 * inP2 + d2dt_h11 * inM2; + float d2dt = tangent.Dot(tangent) + position.Dot(ddt_tangent); // Leaving out factor 2, because we left it out above too + + // If d2dt is zero, the curve is flat and there are multiple t's for which we are closest to the origin, stop now + if (d2dt == 0.0f) + break; + + // Do a Newton Raphson step + // See: https://en.wikipedia.org/wiki/Newton%27s_method + // Clamp against [-interval, interval] to avoid overshooting too much, we're not interested outside the interval + float delta = Clamp(-ddt / d2dt, -interval, interval); + + // If we're stepping away further from t e [inTMin, inTMax] stop now + if ((t > inTMax && delta > 0.0f) || (t < inTMin && delta < 0.0f)) + break; + + // If we've converged, stop now + t += delta; + if (abs(delta) < 1.0e-4f) + break; + } + + // Calculate the distance squared for the origin to the curve + outDistanceSq = position.LengthSq(); + return t; +} + +void PathConstraintPathHermite::GetIndexAndT(float inFraction, int &outIndex, float &outT) const +{ + int num_points = int(mPoints.size()); + + // Start by truncating the fraction to get the index and storing the remainder in t + int index = int(trunc(inFraction)); + float t = inFraction - float(index); + + if (IsLooping()) + { + JPH_ASSERT(!mPoints.front().mPosition.IsClose(mPoints.back().mPosition), "A looping path should have a different first and last point!"); + + // Make sure index is positive by adding a multiple of num_points + if (index < 0) + index += (-index / num_points + 1) * num_points; + + // Index needs to be modulo num_points + index = index % num_points; + } + else + { + // Clamp against range of points + if (index < 0) + { + index = 0; + t = 0.0f; + } + else if (index >= num_points - 1) + { + index = num_points - 2; + t = 1.0f; + } + } + + outIndex = index; + outT = t; +} + +float PathConstraintPathHermite::GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const +{ + JPH_PROFILE_FUNCTION(); + + int num_points = int(mPoints.size()); + + // Start with last point on the path, in the non-looping case we won't be visiting this point + float best_dist_sq = (mPoints[num_points - 1].mPosition - inPosition).LengthSq(); + float best_t = float(num_points - 1); + + // Loop over all points + for (int i = 0, max_i = IsLooping()? num_points : num_points - 1; i < max_i; ++i) + { + const Point &p1 = mPoints[i]; + const Point &p2 = mPoints[(i + 1) % num_points]; + + // Make the curve relative to inPosition + Vec3 p1_pos = p1.mPosition - inPosition; + Vec3 p2_pos = p2.mPosition - inPosition; + + // Get distance to p1 + float dist_sq = p1_pos.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_t = float(i); + best_dist_sq = dist_sq; + } + + // First find an interval for the closest point so that we can start doing Newton Raphson steps + float t_min, t_max; + sCalculateClosestPointThroughBisection(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max); + + if (t_min == t_max) + { + // If the function above returned no interval then it found the root already and we can just calculate the distance + Vec3 position, tangent; + sCalculatePositionAndTangent(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, position, tangent); + dist_sq = position.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_t = float(i) + t_min; + best_dist_sq = dist_sq; + } + } + else + { + // Get closest distance along curve segment + float t = sCalculateClosestPointThroughNewtonRaphson(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max, dist_sq); + if (t >= 0.0f && t <= 1.0f && dist_sq < best_dist_sq) + { + best_t = float(i) + t; + best_dist_sq = dist_sq; + } + } + } + + return best_t; +} + +void PathConstraintPathHermite::GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const +{ + JPH_PROFILE_FUNCTION(); + + // Determine which hermite spline segment we need + int index; + float t; + GetIndexAndT(inFraction, index, t); + + // Get the points on the segment + const Point &p1 = mPoints[index]; + const Point &p2 = mPoints[(index + 1) % int(mPoints.size())]; + + // Calculate the position and tangent on the path + Vec3 tangent; + sCalculatePositionAndTangent(p1.mPosition, p1.mTangent, p2.mPosition, p2.mTangent, t, outPathPosition, tangent); + outPathTangent = tangent.Normalized(); + + // Just linearly interpolate the normal + Vec3 normal = (1.0f - t) * p1.mNormal + t * p2.mNormal; + + // Calculate binormal + outPathBinormal = normal.Cross(outPathTangent).Normalized(); + + // Recalculate normal so it is perpendicular to both (linear interpolation will cause it not to be) + outPathNormal = outPathTangent.Cross(outPathBinormal); + JPH_ASSERT(outPathNormal.IsNormalized()); +} + +void PathConstraintPathHermite::SaveBinaryState(StreamOut &inStream) const +{ + PathConstraintPath::SaveBinaryState(inStream); + + inStream.Write(mPoints); +} + +void PathConstraintPathHermite::RestoreBinaryState(StreamIn &inStream) +{ + PathConstraintPath::RestoreBinaryState(inStream); + + inStream.Read(mPoints); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h new file mode 100644 index 000000000000..839909be8b90 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h @@ -0,0 +1,54 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A path that follows a Hermite spline +class JPH_EXPORT PathConstraintPathHermite final : public PathConstraintPath +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintPathHermite) + +public: + // See PathConstraintPath::GetPathMaxFraction + virtual float GetPathMaxFraction() const override { return float(IsLooping()? mPoints.size() : mPoints.size() - 1); } + + // See PathConstraintPath::GetClosestPoint + virtual float GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const override; + + // See PathConstraintPath::GetPointOnPath + virtual void GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const override; + + /// Adds a point to the path + void AddPoint(Vec3Arg inPosition, Vec3Arg inTangent, Vec3Arg inNormal) { mPoints.push_back({ inPosition, inTangent, inNormal}); } + + // See: PathConstraintPath::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + struct Point + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Point) + + Vec3 mPosition; ///< Position on the path + Vec3 mTangent; ///< Tangent of the path, does not need to be normalized (in the direction of the path) + Vec3 mNormal; ///< Normal of the path (together with the tangent along the curve this forms a basis for the constraint) + }; + +protected: + // See: PathConstraintPath::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + /// Helper function that returns the index of the path segment and the fraction t on the path segment based on the full path fraction + inline void GetIndexAndT(float inFraction, int &outIndex, float &outT) const; + + using Points = Array; + + Points mPoints; ///< Points on the Hermite spline +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp new file mode 100644 index 000000000000..74d0ecd7c74f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp @@ -0,0 +1,157 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PointConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PointConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(PointConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint2) +} + +void PointConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mPoint2); +} + +void PointConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mPoint2); +} + +TwoBodyConstraint *PointConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PointConstraint(inBody1, inBody2, *this); +} + +PointConstraint::PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + } +} + +void PointConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void PointConstraint::SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1) +{ + if (inSpace == EConstraintSpace::WorldSpace) + mLocalSpacePosition1 = Vec3(mBody1->GetInverseCenterOfMassTransform() * inPoint1); + else + mLocalSpacePosition1 = Vec3(inPoint1); +} + +void PointConstraint::SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2) +{ + if (inSpace == EConstraintSpace::WorldSpace) + mLocalSpacePosition2 = Vec3(mBody2->GetInverseCenterOfMassTransform() * inPoint2); + else + mLocalSpacePosition2 = Vec3(inPoint2); +} + +void PointConstraint::CalculateConstraintProperties() +{ + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); +} + +void PointConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(); +} + +void PointConstraint::ResetWarmStart() +{ + mPointConstraintPart.Deactivate(); +} + +void PointConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool PointConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); +} + +bool PointConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(); + + return mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void PointConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Draw constraint + inRenderer->DrawMarker(mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1, Color::sRed, 0.1f); + inRenderer->DrawMarker(mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2, Color::sGreen, 0.1f); +} +#endif // JPH_DEBUG_RENDERER + +void PointConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); +} + +void PointConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); +} + +Ref PointConstraint::GetConstraintSettings() const +{ + PointConstraintSettings *settings = new PointConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h new file mode 100644 index 000000000000..8ab943eec81f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h @@ -0,0 +1,94 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Point constraint settings, used to create a point constraint +class JPH_EXPORT PointConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PointConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint position (space determined by mSpace). + RVec3 mPoint1 = RVec3::sZero(); + + /// Body 2 constraint position (space determined by mSpace). + /// Note: Normally you would set mPoint1 = mPoint2 if the bodies are already placed how you want to constrain them (if mSpace = world space). + RVec3 mPoint2 = RVec3::sZero(); + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A point constraint constrains 2 bodies on a single point (removing 3 degrees of freedom) +class JPH_EXPORT PointConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct point constraint + PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Point; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + /// Update the attachment point for body 1 + void SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1); + + /// Update the attachment point for body 2 + void SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2); + + /// Get the attachment point for body 1 relative to body 1 COM (transform by Body::GetCenterOfMassTransform to take to world space) + inline Vec3 GetLocalSpacePoint1() const { return mLocalSpacePosition1; } + + /// Get the attachment point for body 2 relative to body 2 COM (transform by Body::GetCenterOfMassTransform to take to world space) + inline Vec3 GetLocalSpacePoint2() const { return mLocalSpacePosition2; } + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(); + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // The constraint part + PointConstraintPart mPointConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp new file mode 100644 index 000000000000..9d15c1d4dc1e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp @@ -0,0 +1,253 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PulleyConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PulleyConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(PulleyConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint1) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint1) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint2) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint2) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mRatio) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMinLength) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMaxLength) +} + +void PulleyConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mBodyPoint1); + inStream.Write(mFixedPoint1); + inStream.Write(mBodyPoint2); + inStream.Write(mFixedPoint2); + inStream.Write(mRatio); + inStream.Write(mMinLength); + inStream.Write(mMaxLength); +} + +void PulleyConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mBodyPoint1); + inStream.Read(mFixedPoint1); + inStream.Read(mBodyPoint2); + inStream.Read(mFixedPoint2); + inStream.Read(mRatio); + inStream.Read(mMinLength); + inStream.Read(mMaxLength); +} + +TwoBodyConstraint *PulleyConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PulleyConstraint(inBody1, inBody2, *this); +} + +PulleyConstraint::PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mFixedPosition1(inSettings.mFixedPoint1), + mFixedPosition2(inSettings.mFixedPoint2), + mRatio(inSettings.mRatio), + mMinLength(inSettings.mMinLength), + mMaxLength(inSettings.mMaxLength) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint2); + mWorldSpacePosition1 = inSettings.mBodyPoint1; + mWorldSpacePosition2 = inSettings.mBodyPoint2; + } + else + { + // If properties were specified in local space, we need to calculate world space positions + mLocalSpacePosition1 = Vec3(inSettings.mBodyPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mBodyPoint2); + mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mBodyPoint1; + mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mBodyPoint2; + } + + // Calculate min/max length if it was not provided + float current_length = GetCurrentLength(); + if (mMinLength < 0.0f) + mMinLength = current_length; + if (mMaxLength < 0.0f) + mMaxLength = current_length; + + // Initialize the normals to a likely valid axis in case the fixed points overlap with the attachment points (most likely the fixed points are above both bodies) + mWorldSpaceNormal1 = mWorldSpaceNormal2 = -Vec3::sAxisY(); +} + +void PulleyConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float PulleyConstraint::CalculatePositionsNormalsAndLength() +{ + // Update world space positions (the bodies may have moved) + mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + + // Calculate world space normals + Vec3 delta1 = Vec3(mWorldSpacePosition1 - mFixedPosition1); + float delta1_len = delta1.Length(); + if (delta1_len > 0.0f) + mWorldSpaceNormal1 = delta1 / delta1_len; + + Vec3 delta2 = Vec3(mWorldSpacePosition2 - mFixedPosition2); + float delta2_len = delta2.Length(); + if (delta2_len > 0.0f) + mWorldSpaceNormal2 = delta2 / delta2_len; + + // Calculate length + return delta1_len + mRatio * delta2_len; +} + +void PulleyConstraint::CalculateConstraintProperties() +{ + // Calculate attachment points relative to COM + Vec3 r1 = Vec3(mWorldSpacePosition1 - mBody1->GetCenterOfMassPosition()); + Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition()); + + mIndependentAxisConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, r1, mWorldSpaceNormal1, r2, mWorldSpaceNormal2, mRatio); +} + +void PulleyConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Determine if the constraint is active + float current_length = CalculatePositionsNormalsAndLength(); + bool min_length_violation = current_length <= mMinLength; + bool max_length_violation = current_length >= mMaxLength; + if (min_length_violation || max_length_violation) + { + // Determine max lambda based on if the length is too big or small + mMinLambda = max_length_violation? -FLT_MAX : 0.0f; + mMaxLambda = min_length_violation? FLT_MAX : 0.0f; + + CalculateConstraintProperties(); + } + else + mIndependentAxisConstraintPart.Deactivate(); +} + +void PulleyConstraint::ResetWarmStart() +{ + mIndependentAxisConstraintPart.Deactivate(); +} + +void PulleyConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + mIndependentAxisConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, inWarmStartImpulseRatio); +} + +bool PulleyConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + if (mIndependentAxisConstraintPart.IsActive()) + return mIndependentAxisConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, mMinLambda, mMaxLambda); + else + return false; +} + +bool PulleyConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Calculate new length (bodies may have changed) + float current_length = CalculatePositionsNormalsAndLength(); + + float position_error = 0.0f; + if (current_length < mMinLength) + position_error = current_length - mMinLength; + else if (current_length > mMaxLength) + position_error = current_length - mMaxLength; + + if (position_error != 0.0f) + { + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(); + + return mIndependentAxisConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, position_error, inBaumgarte); + } + + return false; +} + +#ifdef JPH_DEBUG_RENDERER +void PulleyConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Color according to length vs min/max length + float current_length = GetCurrentLength(); + Color color = Color::sGreen; + if (current_length < mMinLength) + color = Color::sYellow; + else if (current_length > mMaxLength) + color = Color::sRed; + + // Draw constraint + inRenderer->DrawLine(mWorldSpacePosition1, mFixedPosition1, color); + inRenderer->DrawLine(mFixedPosition1, mFixedPosition2, color); + inRenderer->DrawLine(mFixedPosition2, mWorldSpacePosition2, color); + + // Draw current length + inRenderer->DrawText3D(0.5_r * (mFixedPosition1 + mFixedPosition2), StringFormat("%.2f", (double)current_length)); +} +#endif // JPH_DEBUG_RENDERER + +void PulleyConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mIndependentAxisConstraintPart.SaveState(inStream); + inStream.Write(mWorldSpaceNormal1); // When distance to fixed point = 0, the normal is used from last frame so we need to store it + inStream.Write(mWorldSpaceNormal2); +} + +void PulleyConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mIndependentAxisConstraintPart.RestoreState(inStream); + inStream.Read(mWorldSpaceNormal1); + inStream.Read(mWorldSpaceNormal2); +} + +Ref PulleyConstraint::GetConstraintSettings() const +{ + PulleyConstraintSettings *settings = new PulleyConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mBodyPoint1 = RVec3(mLocalSpacePosition1); + settings->mFixedPoint1 = mFixedPosition1; + settings->mBodyPoint2 = RVec3(mLocalSpacePosition2); + settings->mFixedPoint2 = mFixedPosition2; + settings->mRatio = mRatio; + settings->mMinLength = mMinLength; + settings->mMaxLength = mMaxLength; + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h new file mode 100644 index 000000000000..092be68c1b46 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h @@ -0,0 +1,137 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Pulley constraint settings, used to create a pulley constraint. +/// A pulley connects two bodies via two fixed world points to each other similar to a distance constraint. +/// We define Length1 = |BodyPoint1 - FixedPoint1| where Body1 is a point on body 1 in world space and FixedPoint1 a fixed point in world space +/// Length2 = |BodyPoint2 - FixedPoint2| +/// The constraint keeps the two line segments constrained so that +/// MinDistance <= Length1 + Ratio * Length2 <= MaxDistance +class JPH_EXPORT PulleyConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PulleyConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, specified properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint attachment point (space determined by mSpace). + RVec3 mBodyPoint1 = RVec3::sZero(); + + /// Fixed world point to which body 1 is connected (always world space) + RVec3 mFixedPoint1 = RVec3::sZero(); + + /// Body 2 constraint attachment point (space determined by mSpace) + RVec3 mBodyPoint2 = RVec3::sZero(); + + /// Fixed world point to which body 2 is connected (always world space) + RVec3 mFixedPoint2 = RVec3::sZero(); + + /// Ratio between the two line segments (see formula above), can be used to create a block and tackle + float mRatio = 1.0f; + + /// The minimum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created. + float mMinLength = 0.0f; + + /// The maximum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created. + float mMaxLength = -1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A pulley constraint. +class JPH_EXPORT PulleyConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct distance constraint + PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Pulley; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + /// Update the minimum and maximum length for the constraint + void SetLength(float inMinLength, float inMaxLength) { JPH_ASSERT(inMinLength >= 0.0f && inMinLength <= inMaxLength); mMinLength = inMinLength; mMaxLength = inMaxLength; } + float GetMinLength() const { return mMinLength; } + float GetMaxLength() const { return mMaxLength; } + + /// Get the current length of both segments (multiplied by the ratio for segment 2) + float GetCurrentLength() const { return Vec3(mWorldSpacePosition1 - mFixedPosition1).Length() + mRatio * Vec3(mWorldSpacePosition2 - mFixedPosition2).Length(); } + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline float GetTotalLambdaPosition() const { return mIndependentAxisConstraintPart.GetTotalLambda(); } + +private: + // Calculates world positions and normals and returns current length + float CalculatePositionsNormalsAndLength(); + + // Internal helper function to calculate the values below + void CalculateConstraintProperties(); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions on the bodies + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // World space fixed positions + RVec3 mFixedPosition1; + RVec3 mFixedPosition2; + + /// Ratio between the two line segments + float mRatio; + + // The minimum/maximum length of the line segments + float mMinLength; + float mMaxLength; + + // RUN TIME PROPERTIES FOLLOW + + // World space positions and normal + RVec3 mWorldSpacePosition1; + RVec3 mWorldSpacePosition2; + Vec3 mWorldSpaceNormal1; + Vec3 mWorldSpaceNormal2; + + // Depending on if the length < min or length > max we can apply forces to prevent further violations + float mMinLambda; + float mMaxLambda; + + // The constraint part + IndependentAxisConstraintPart mIndependentAxisConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp new file mode 100644 index 000000000000..d0bcb62febd7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp @@ -0,0 +1,189 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RackAndPinionConstraintSettings) +{ + JPH_ADD_BASE_CLASS(RackAndPinionConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(RackAndPinionConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mHingeAxis) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mSliderAxis) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mRatio) +} + +void RackAndPinionConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mHingeAxis); + inStream.Write(mSliderAxis); + inStream.Write(mRatio); +} + +void RackAndPinionConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mHingeAxis); + inStream.Read(mSliderAxis); + inStream.Read(mRatio); +} + +TwoBodyConstraint *RackAndPinionConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new RackAndPinionConstraint(inBody1, inBody2, *this); +} + +RackAndPinionConstraint::RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mLocalSpaceHingeAxis(inSettings.mHingeAxis), + mLocalSpaceSliderAxis(inSettings.mSliderAxis), + mRatio(inSettings.mRatio) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpaceHingeAxis = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis).Normalized(); + mLocalSpaceSliderAxis = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceSliderAxis).Normalized(); + } +} + +void RackAndPinionConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mWorldSpaceHingeAxis = inRotation1 * mLocalSpaceHingeAxis; + mWorldSpaceSliderAxis = inRotation2 * mLocalSpaceSliderAxis; + + mRackAndPinionConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio); +} + +void RackAndPinionConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); +} + +void RackAndPinionConstraint::ResetWarmStart() +{ + mRackAndPinionConstraintPart.Deactivate(); +} + +void RackAndPinionConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mRackAndPinionConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool RackAndPinionConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mRackAndPinionConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio); +} + +bool RackAndPinionConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mRackConstraint == nullptr || mPinionConstraint == nullptr) + return false; + + float rotation; + if (mPinionConstraint->GetSubType() == EConstraintSubType::Hinge) + { + rotation = StaticCast(mPinionConstraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float translation; + if (mRackConstraint->GetSubType() == EConstraintSubType::Slider) + { + translation = StaticCast(mRackConstraint)->GetCurrentPosition(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float error = CenterAngleAroundZero(fmod(rotation - mRatio * translation, 2.0f * JPH_PI)); + if (error == 0.0f) + return false; + + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); + return mRackAndPinionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void RackAndPinionConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint axis + inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis, Color::sGreen, 0.01f); + inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceSliderAxis, Color::sBlue, 0.01f); +} + +#endif // JPH_DEBUG_RENDERER + +void RackAndPinionConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mRackAndPinionConstraintPart.SaveState(inStream); +} + +void RackAndPinionConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mRackAndPinionConstraintPart.RestoreState(inStream); +} + +Ref RackAndPinionConstraint::GetConstraintSettings() const +{ + RackAndPinionConstraintSettings *settings = new RackAndPinionConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mHingeAxis = mLocalSpaceHingeAxis; + settings->mSliderAxis = mLocalSpaceSliderAxis; + settings->mRatio = mRatio; + return settings; +} + +Mat44 RackAndPinionConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 RackAndPinionConstraint::GetConstraintToBody2Matrix() const +{ + Vec3 perp = mLocalSpaceSliderAxis.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceSliderAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceSliderAxis.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h new file mode 100644 index 000000000000..f26af31f1b79 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h @@ -0,0 +1,118 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Rack and pinion constraint (slider & gear) settings +class JPH_EXPORT RackAndPinionConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RackAndPinionConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + /// Body1 should be the pinion (gear) and body 2 the rack (slider). + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Defines the ratio between the rotation of the pinion and the translation of the rack. + /// The ratio is defined as: PinionRotation(t) = ratio * RackTranslation(t) + /// @param inNumTeethRack Number of teeth that the rack has + /// @param inRackLength Length of the rack + /// @param inNumTeethPinion Number of teeth the pinion has + void SetRatio(int inNumTeethRack, float inRackLength, int inNumTeethPinion) + { + mRatio = 2.0f * JPH_PI * inNumTeethRack / (inRackLength * inNumTeethPinion); + } + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 (pinion) constraint reference frame (space determined by mSpace). + Vec3 mHingeAxis = Vec3::sAxisX(); + + /// Body 2 (rack) constraint reference frame (space determined by mSpace) + Vec3 mSliderAxis = Vec3::sAxisX(); + + /// Ratio between the rack and pinion, see SetRatio. + float mRatio = 1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A rack and pinion constraint constrains the rotation of body1 to the translation of body 2. +/// Note that this constraint needs to be used in conjunction with a hinge constraint for body 1 and a slider constraint for body 2. +class JPH_EXPORT RackAndPinionConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct gear constraint + RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::RackAndPinion; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift. + void SetConstraints(const Constraint *inPinion, const Constraint *inRack) { mPinionConstraint = inPinion; mRackConstraint = inRack; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline float GetTotalLambda() const { return mRackAndPinionConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space hinge axis + Vec3 mLocalSpaceHingeAxis; + + // Local space sliding direction + Vec3 mLocalSpaceSliderAxis; + + // Ratio between rack and pinion + float mRatio; + + // The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift. + RefConst mPinionConstraint; + RefConst mRackConstraint; + + // RUN TIME PROPERTIES FOLLOW + + // World space hinge axis + Vec3 mWorldSpaceHingeAxis; + + // World space sliding direction + Vec3 mWorldSpaceSliderAxis; + + // The constraint parts + RackAndPinionConstraintPart mRackAndPinionConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp new file mode 100644 index 000000000000..070a45e213d4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp @@ -0,0 +1,900 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SixDOFConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SixDOFConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction) + JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSwingType) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitsSpringSettings) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings) +} + +void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPosition1); + inStream.Write(mAxisX1); + inStream.Write(mAxisY1); + inStream.Write(mPosition2); + inStream.Write(mAxisX2); + inStream.Write(mAxisY2); + inStream.Write(mMaxFriction); + inStream.Write(mSwingType); + inStream.Write(mLimitMin); + inStream.Write(mLimitMax); + for (const SpringSettings &s : mLimitsSpringSettings) + s.SaveBinaryState(inStream); + for (const MotorSettings &m : mMotorSettings) + m.SaveBinaryState(inStream); +} + +void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPosition1); + inStream.Read(mAxisX1); + inStream.Read(mAxisY1); + inStream.Read(mPosition2); + inStream.Read(mAxisX2); + inStream.Read(mAxisY2); + inStream.Read(mMaxFriction); + inStream.Read(mSwingType); + inStream.Read(mLimitMin); + inStream.Read(mLimitMax); + for (SpringSettings &s : mLimitsSpringSettings) + s.RestoreBinaryState(inStream); + for (MotorSettings &m : mMotorSettings) + m.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SixDOFConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SixDOFConstraint(inBody1, inBody2, *this); +} + +void SixDOFConstraint::UpdateTranslationLimits() +{ + // Set to zero if the limits are inversed + for (int i = EAxis::TranslationX; i <= EAxis::TranslationZ; ++i) + if (mLimitMin[i] > mLimitMax[i]) + mLimitMin[i] = mLimitMax[i] = 0.0f; +} + +void SixDOFConstraint::UpdateRotationLimits() +{ + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Cone) + { + // Cone swing upper limit needs to be positive + mLimitMax[EAxis::RotationY] = max(0.0f, mLimitMax[EAxis::RotationY]); + mLimitMax[EAxis::RotationZ] = max(0.0f, mLimitMax[EAxis::RotationZ]); + + // Cone swing limits only support symmetric ranges + mLimitMin[EAxis::RotationY] = -mLimitMax[EAxis::RotationY]; + mLimitMin[EAxis::RotationZ] = -mLimitMax[EAxis::RotationZ]; + } + + for (int i = EAxis::RotationX; i <= EAxis::RotationZ; ++i) + { + // Clamp to [-PI, PI] range + mLimitMin[i] = Clamp(mLimitMin[i], -JPH_PI, JPH_PI); + mLimitMax[i] = Clamp(mLimitMax[i], -JPH_PI, JPH_PI); + + // Set to zero if the limits are inversed + if (mLimitMin[i] > mLimitMax[i]) + mLimitMin[i] = mLimitMax[i] = 0.0f; + } + + // Pass limits on to constraint part + mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]); +} + +void SixDOFConstraint::UpdateFixedFreeAxis() +{ + uint8 old_free_axis = mFreeAxis; + uint8 old_fixed_axis = mFixedAxis; + + // Cache which axis are fixed and which ones are free + mFreeAxis = 0; + mFixedAxis = 0; + for (int a = 0; a < EAxis::Num; ++a) + { + float limit = a >= EAxis::RotationX? JPH_PI : FLT_MAX; + + if (mLimitMin[a] >= mLimitMax[a]) + mFixedAxis |= 1 << a; + else if (mLimitMin[a] <= -limit && mLimitMax[a] >= limit) + mFreeAxis |= 1 << a; + } + + // On change we deactivate all constraints to reset warm starting + if (old_free_axis != mFreeAxis || old_fixed_axis != mFixedAxis) + { + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.Deactivate(); + mPointConstraintPart.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.Deactivate(); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.Deactivate(); + } +} + +SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Override swing type + mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType); + + // Calculate rotation needed to go from constraint space to body1 local space + Vec3 axis_z1 = inSettings.mAxisX1.Cross(inSettings.mAxisY1); + Mat44 c_to_b1(Vec4(inSettings.mAxisX1, 0), Vec4(inSettings.mAxisY1, 0), Vec4(axis_z1, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody1 = c_to_b1.GetQuaternion(); + + // Calculate rotation needed to go from constraint space to body2 local space + Vec3 axis_z2 = inSettings.mAxisX2.Cross(inSettings.mAxisY2); + Mat44 c_to_b2(Vec4(inSettings.mAxisX2, 0), Vec4(inSettings.mAxisY2, 0), Vec4(axis_z2, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody2 = c_to_b2.GetQuaternion(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1); + mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1; + + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2); + mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2; + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPosition1); + mLocalSpacePosition2 = Vec3(inSettings.mPosition2); + } + + // Copy translation and rotation limits + memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin)); + memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax)); + memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings)); + UpdateTranslationLimits(); + UpdateRotationLimits(); + UpdateFixedFreeAxis(); + CacheHasSpringLimits(); + + // Store friction settings + memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction)); + + // Store motor settings + for (int i = 0; i < EAxis::Num; ++i) + mMotorSettings[i] = inSettings.mMotorSettings[i]; + + // Cache if motors are active (motors are off initially, but we may have friction) + CacheTranslationMotorActive(); + CacheRotationMotorActive(); +} + +void SixDOFConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax) +{ + mLimitMin[EAxis::TranslationX] = inLimitMin.GetX(); + mLimitMin[EAxis::TranslationY] = inLimitMin.GetY(); + mLimitMin[EAxis::TranslationZ] = inLimitMin.GetZ(); + mLimitMax[EAxis::TranslationX] = inLimitMax.GetX(); + mLimitMax[EAxis::TranslationY] = inLimitMax.GetY(); + mLimitMax[EAxis::TranslationZ] = inLimitMax.GetZ(); + + UpdateTranslationLimits(); + UpdateFixedFreeAxis(); +} + +void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax) +{ + mLimitMin[EAxis::RotationX] = inLimitMin.GetX(); + mLimitMin[EAxis::RotationY] = inLimitMin.GetY(); + mLimitMin[EAxis::RotationZ] = inLimitMin.GetZ(); + mLimitMax[EAxis::RotationX] = inLimitMax.GetX(); + mLimitMax[EAxis::RotationY] = inLimitMax.GetY(); + mLimitMax[EAxis::RotationZ] = inLimitMax.GetZ(); + + UpdateRotationLimits(); + UpdateFixedFreeAxis(); +} + +void SixDOFConstraint::SetMaxFriction(EAxis inAxis, float inFriction) +{ + mMaxFriction[inAxis] = inFriction; + + if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ) + CacheTranslationMotorActive(); + else + CacheRotationMotorActive(); +} + +void SixDOFConstraint::GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const +{ + RVec3 p1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + RVec3 p2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + outR1PlusU = Vec3(p2 - mBody1->GetCenterOfMassPosition()); // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1 + outR2 = Vec3(p2 - mBody2->GetCenterOfMassPosition()); + outU = Vec3(p2 - p1); +} + +Quat SixDOFConstraint::GetRotationInConstraintSpace() const +{ + // Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform()) + // Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1)) + // Let q be the rotation of the constraint in constraint space + // b2 takes a vector from the local space of body2 to world space + // To express this in terms of b1: b2 = b1 * c1 * q * c2^-1 + // c2^-1 goes from local body 2 space to constraint space + // q rotates the constraint + // c1 goes from constraint space to body 1 local space + // b1 goes from body 1 local space to world space + // So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2 + // Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations + return (mBody1->GetRotation() * mConstraintToBody1).Conjugated() * mBody2->GetRotation() * mConstraintToBody2; +} + +void SixDOFConstraint::CacheTranslationMotorActive() +{ + mTranslationMotorActive = mMotorState[EAxis::TranslationX] != EMotorState::Off + || mMotorState[EAxis::TranslationY] != EMotorState::Off + || mMotorState[EAxis::TranslationZ] != EMotorState::Off + || HasFriction(EAxis::TranslationX) + || HasFriction(EAxis::TranslationY) + || HasFriction(EAxis::TranslationZ); +} + +void SixDOFConstraint::CacheRotationMotorActive() +{ + mRotationMotorActive = mMotorState[EAxis::RotationX] != EMotorState::Off + || mMotorState[EAxis::RotationY] != EMotorState::Off + || mMotorState[EAxis::RotationZ] != EMotorState::Off + || HasFriction(EAxis::RotationX) + || HasFriction(EAxis::RotationY) + || HasFriction(EAxis::RotationZ); +} + +void SixDOFConstraint::CacheRotationPositionMotorActive() +{ + mRotationPositionMotorActive = 0; + for (int i = 0; i < 3; ++i) + if (mMotorState[EAxis::RotationX + i] == EMotorState::Position) + mRotationPositionMotorActive |= 1 << i; +} + +void SixDOFConstraint::CacheHasSpringLimits() +{ + mHasSpringLimits = mLimitsSpringSettings[EAxis::TranslationX].mFrequency > 0.0f + || mLimitsSpringSettings[EAxis::TranslationY].mFrequency > 0.0f + || mLimitsSpringSettings[EAxis::TranslationZ].mFrequency > 0.0f; +} + +void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid()); + + if (mMotorState[inAxis] != inState) + { + mMotorState[inAxis] = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ) + { + mMotorTranslationConstraintPart[inAxis - EAxis::TranslationX].Deactivate(); + + CacheTranslationMotorActive(); + } + else + { + JPH_ASSERT(inAxis >= EAxis::RotationX && inAxis <= EAxis::RotationZ); + + mMotorRotationConstraintPart[inAxis - EAxis::RotationX].Deactivate(); + + CacheRotationMotorActive(); + CacheRotationPositionMotorActive(); + } + } +} + +void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation) +{ + Quat q_swing, q_twist; + inOrientation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis); + + if (clamped_axis != 0) + mTargetOrientation = q_swing * q_twist; + else + mTargetOrientation = inOrientation; +} + +void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Get body rotations + Quat rotation1 = mBody1->GetRotation(); + Quat rotation2 = mBody2->GetRotation(); + + // Quaternion that rotates from body1's constraint space to world space + Quat constraint_body1_to_world = rotation1 * mConstraintToBody1; + + // Store world space axis of constraint space + Mat44 translation_axis_mat = Mat44::sRotation(constraint_body1_to_world); + for (int i = 0; i < 3; ++i) + mTranslationAxis[i] = translation_axis_mat.GetColumn3(i); + + if (IsTranslationFullyConstrained()) + { + // All translation locked: Setup point constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), mLocalSpacePosition1, *mBody2, Mat44::sRotation(rotation2), mLocalSpacePosition2); + } + else if (IsTranslationConstrained() || mTranslationMotorActive) + { + // Update world space positions (the bodies may have moved) + Vec3 r1_plus_u, r2, u; + GetPositionConstraintProperties(r1_plus_u, r2, u); + + // Setup axis constraint parts + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::TranslationX + i); + + Vec3 translation_axis = mTranslationAxis[i]; + + // Calculate displacement along this axis + float d = translation_axis.Dot(u); + mDisplacement[i] = d; // Store for SolveVelocityConstraint + + // Setup limit constraint + bool constraint_active = false; + float constraint_value = 0.0f; + if (IsFixedAxis(axis)) + { + // When constraint is fixed it is always active + constraint_value = d - mLimitMin[i]; + constraint_active = true; + } + else if (!IsFreeAxis(axis)) + { + // When constraint is limited, it is only active when outside of the allowed range + if (d <= mLimitMin[i]) + { + constraint_value = d - mLimitMin[i]; + constraint_active = true; + } + else if (d >= mLimitMax[i]) + { + constraint_value = d - mLimitMax[i]; + constraint_active = true; + } + } + + if (constraint_active) + mTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, constraint_value, mLimitsSpringSettings[i]); + else + mTranslationConstraintPart[i].Deactivate(); + + // Setup motor constraint + switch (mMotorState[i]) + { + case EMotorState::Off: + if (HasFriction(axis)) + mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis); + else + mMotorTranslationConstraintPart[i].Deactivate(); + break; + + case EMotorState::Velocity: + mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]); + break; + + case EMotorState::Position: + { + const SpringSettings &spring_settings = mMotorSettings[i].mSpringSettings; + if (spring_settings.HasStiffness()) + mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], spring_settings); + else + mMotorTranslationConstraintPart[i].Deactivate(); + break; + } + } + } + } + + // Setup rotation constraints + if (IsRotationFullyConstrained()) + { + // All rotation locked: Setup rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + } + else if (IsRotationConstrained() || mRotationMotorActive) + { + // GetRotationInConstraintSpace without redoing the calculation of constraint_body1_to_world + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world; + + // Use swing twist constraint part + if (IsRotationConstrained()) + mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world); + else + mSwingTwistConstraintPart.Deactivate(); + + if (mRotationMotorActive) + { + // Calculate rotation motor axis + Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world); + for (int i = 0; i < 3; ++i) + mRotationAxis[i] = ws_axis.GetColumn3(i); + + // Get target orientation along the shortest path from q + Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation; + + // The definition of the constraint rotation q: + // R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1) + // + // R2' is the rotation of body 2 when reaching the target_orientation: + // R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2) + // + // The difference in body 2 space: + // R2' = R2 * diff_body2 (3) + // + // We want to specify the difference in the constraint space of body 2: + // diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4) + // + // Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5) + // Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6) + // Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7) + // Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* + // <=> target_orientation = q * diff + // <=> diff = q^* * target_orientation + Quat diff = q.Conjugated() * target_orientation; + + // Project diff so that only rotation around axis that have a position motor are remaining + Quat projected_diff; + switch (mRotationPositionMotorActive) + { + case 0b001: + // Keep only rotation around X + projected_diff = diff.GetTwist(Vec3::sAxisX()); + break; + + case 0b010: + // Keep only rotation around Y + projected_diff = diff.GetTwist(Vec3::sAxisY()); + break; + + case 0b100: + // Keep only rotation around Z + projected_diff = diff.GetTwist(Vec3::sAxisZ()); + break; + + case 0b011: + // Remove rotation around Z + // q = swing_xy * twist_z <=> swing_xy = q * twist_z^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisZ()).Conjugated(); + break; + + case 0b101: + // Remove rotation around Y + // q = swing_xz * twist_y <=> swing_xz = q * twist_y^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisY()).Conjugated(); + break; + + case 0b110: + // Remove rotation around X + // q = swing_yz * twist_x <=> swing_yz = q * twist_x^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisX()).Conjugated(); + break; + + case 0b111: + default: // All motors off is handled here but the results are unused + // Keep entire rotation + projected_diff = diff; + break; + } + + // Approximate error angles + // The imaginary part of a quaternion is rotation_axis * sin(angle / 2) + // If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i] + // We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction + Vec3 rotation_error = -2.0f * projected_diff.GetXYZ(); + + // Setup motors + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::RotationX + i); + + Vec3 rotation_axis = mRotationAxis[i]; + + switch (mMotorState[axis]) + { + case EMotorState::Off: + if (HasFriction(axis)) + mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis); + else + mMotorRotationConstraintPart[i].Deactivate(); + break; + + case EMotorState::Velocity: + mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]); + break; + + case EMotorState::Position: + { + const SpringSettings &spring_settings = mMotorSettings[axis].mSpringSettings; + if (spring_settings.HasStiffness()) + mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], spring_settings); + else + mMotorRotationConstraintPart[i].Deactivate(); + break; + } + } + } + } + } +} + +void SixDOFConstraint::ResetWarmStart() +{ + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.Deactivate(); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.Deactivate(); + mRotationConstraintPart.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.Deactivate(); +} + +void SixDOFConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm start translation motors + if (mTranslationMotorActive) + for (int i = 0; i < 3; ++i) + if (mMotorTranslationConstraintPart[i].IsActive()) + mMotorTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio); + + // Warm start rotation motors + if (mRotationMotorActive) + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + if (c.IsActive()) + c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + + // Warm start rotation constraints + if (IsRotationFullyConstrained()) + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + else if (IsRotationConstrained()) + mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + + // Warm start translation constraints + if (IsTranslationFullyConstrained()) + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + else if (IsTranslationConstrained()) + for (int i = 0; i < 3; ++i) + if (mTranslationConstraintPart[i].IsActive()) + mTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio); +} + +bool SixDOFConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve translation motor + if (mTranslationMotorActive) + for (int i = 0; i < 3; ++i) + if (mMotorTranslationConstraintPart[i].IsActive()) + switch (mMotorState[i]) + { + case EMotorState::Off: + { + // Apply friction only + float max_lambda = mMaxFriction[i] * inDeltaTime; + impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + // Drive motor + impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], inDeltaTime * mMotorSettings[i].mMinForceLimit, inDeltaTime * mMotorSettings[i].mMaxForceLimit); + break; + } + + // Solve rotation motor + if (mRotationMotorActive) + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::RotationX + i); + if (mMotorRotationConstraintPart[i].IsActive()) + switch (mMotorState[axis]) + { + case EMotorState::Off: + { + // Apply friction only + float max_lambda = mMaxFriction[axis] * inDeltaTime; + impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + // Drive motor + impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], inDeltaTime * mMotorSettings[axis].mMinTorqueLimit, inDeltaTime * mMotorSettings[axis].mMaxTorqueLimit); + break; + } + } + + // Solve rotation constraint + if (IsRotationFullyConstrained()) + impulse |= mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + else if (IsRotationConstrained()) + impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + if (IsTranslationFullyConstrained()) + impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + else if (IsTranslationConstrained()) + for (int i = 0; i < 3; ++i) + if (mTranslationConstraintPart[i].IsActive()) + { + // If the axis is not fixed it must be limited (or else the constraint would not be active) + // Calculate the min and max constraint force based on on which side we're limited + float limit_min = -FLT_MAX, limit_max = FLT_MAX; + if (!IsFixedAxis(EAxis(EAxis::TranslationX + i))) + { + JPH_ASSERT(!IsFreeAxis(EAxis(EAxis::TranslationX + i))); + if (mDisplacement[i] <= mLimitMin[i]) + limit_min = 0; + else if (mDisplacement[i] >= mLimitMax[i]) + limit_max = 0; + } + + impulse |= mTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], limit_min, limit_max); + } + + return impulse; +} + +bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + if (IsRotationFullyConstrained()) + { + // Rotation locked: Solve rotation constraint + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + // Definition of initial orientation r0: q2 = q1 r0 + // Initial rotation (see: GetRotationInConstraintSpace): q2 = q1 c1 c2^-1 + // So: r0^-1 = (c1 c2^-1)^-1 = c2 * c1^-1 + Quat constraint_to_body1 = mConstraintToBody1 * Quat::sEulerAngles(GetRotationLimitsMin()); + Quat inv_initial_orientation = mConstraintToBody2 * constraint_to_body1.Conjugated(); + + // Solve rotation violations + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + impulse |= mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inv_initial_orientation, inBaumgarte); + } + else if (IsRotationConstrained()) + { + // Rotation partially constraint + + // Solve rotation violations + Quat q = GetRotationInConstraintSpace(); + impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte); + } + + // Solve position violations + if (IsTranslationFullyConstrained()) + { + // Translation locked: Solve point constraint + Vec3 local_space_position1 = mLocalSpacePosition1 + mConstraintToBody1 * GetTranslationLimitsMin(); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), local_space_position1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + } + else if (IsTranslationConstrained()) + { + // Translation partially locked: Solve per axis + for (int i = 0; i < 3; ++i) + if (mLimitsSpringSettings[i].mFrequency <= 0.0f) // If not soft limit + { + // Update world space positions (the bodies may have moved) + Vec3 r1_plus_u, r2, u; + GetPositionConstraintProperties(r1_plus_u, r2, u); + + // Quaternion that rotates from body1's constraint space to world space + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + + // Calculate axis + Vec3 translation_axis; + switch (i) + { + case 0: translation_axis = constraint_body1_to_world.RotateAxisX(); break; + case 1: translation_axis = constraint_body1_to_world.RotateAxisY(); break; + default: JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break; + } + + // Determine position error + float error = 0.0f; + EAxis axis(EAxis(EAxis::TranslationX + i)); + if (IsFixedAxis(axis)) + error = u.Dot(translation_axis) - mLimitMin[axis]; + else if (!IsFreeAxis(axis)) + { + float displacement = u.Dot(translation_axis); + if (displacement <= mLimitMin[axis]) + error = displacement - mLimitMin[axis]; + else if (displacement >= mLimitMax[axis]) + error = displacement - mLimitMax[axis]; + } + + if (error != 0.0f) + { + // Setup axis constraint part and solve it + mTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis); + impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte); + } + } + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER +void SixDOFConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RVec3 position1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1; + Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2; + + // Draw constraint orientation + inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize); + + if ((IsRotationConstrained() || mRotationPositionMotorActive != 0) && !IsRotationFullyConstrained()) + { + // Draw current swing and twist + Quat q = GetRotationInConstraintSpace(); + Quat q_swing, q_twist; + q.GetSwingTwist(q_swing, q_twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite); + } + + // Draw target rotation + Quat m_swing, m_twist; + mTargetOrientation.GetSwingTwist(m_swing, m_twist); + if (mMotorState[EAxis::RotationX] == EMotorState::Position) + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_twist).RotateAxisY(), Color::sYellow); + if (mMotorState[EAxis::RotationY] == EMotorState::Position || mMotorState[EAxis::RotationZ] == EMotorState::Position) + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_swing).RotateAxisX(), Color::sYellow); + + // Draw target angular velocity + Vec3 target_angular_velocity = Vec3::sZero(); + for (int i = 0; i < 3; ++i) + if (mMotorState[EAxis::RotationX + i] == EMotorState::Velocity) + target_angular_velocity.SetComponent(i, mTargetAngularVelocity[i]); + if (target_angular_velocity != Vec3::sZero()) + inRenderer->DrawArrow(position1, position1 + rotation2 * target_angular_velocity, Color::sRed, 0.1f); +} + +void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get matrix that transforms from constraint space to world space + RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1); + + // Draw limits + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid) + inRenderer->DrawSwingPyramidLimits(constraint_body1_to_world, mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + else + inRenderer->DrawSwingConeLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + inRenderer->DrawPie(constraint_body1_to_world.GetTranslation(), mDrawConstraintSize, constraint_body1_to_world.GetAxisX(), constraint_body1_to_world.GetAxisY(), mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void SixDOFConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + for (const AxisConstraintPart &c : mTranslationConstraintPart) + c.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); + mSwingTwistConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + for (const AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.SaveState(inStream); + for (const AngleConstraintPart &c : mMotorRotationConstraintPart) + c.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetPosition); + inStream.Write(mTargetOrientation); +} + +void SixDOFConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); + mSwingTwistConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.RestoreState(inStream); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetPosition); + inStream.Read(mTargetOrientation); + + CacheTranslationMotorActive(); + CacheRotationMotorActive(); + CacheRotationPositionMotorActive(); +} + +Ref SixDOFConstraint::GetConstraintSettings() const +{ + SixDOFConstraintSettings *settings = new SixDOFConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPosition1 = RVec3(mLocalSpacePosition1); + settings->mAxisX1 = mConstraintToBody1.RotateAxisX(); + settings->mAxisY1 = mConstraintToBody1.RotateAxisY(); + settings->mPosition2 = RVec3(mLocalSpacePosition2); + settings->mAxisX2 = mConstraintToBody2.RotateAxisX(); + settings->mAxisY2 = mConstraintToBody2.RotateAxisY(); + settings->mSwingType = mSwingTwistConstraintPart.GetSwingType(); + memcpy(settings->mLimitMin, mLimitMin, sizeof(mLimitMin)); + memcpy(settings->mLimitMax, mLimitMax, sizeof(mLimitMax)); + memcpy(settings->mMaxFriction, mMaxFriction, sizeof(mMaxFriction)); + for (int i = 0; i < EAxis::Num; ++i) + settings->mMotorSettings[i] = mMotorSettings[i]; + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h new file mode 100644 index 000000000000..2ebb9856bda7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h @@ -0,0 +1,289 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// 6 Degree Of Freedom Constraint setup structure. Allows control over each of the 6 degrees of freedom. +class JPH_EXPORT SixDOFConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SixDOFConstraintSettings) + +public: + /// Constraint is split up into translation/rotation around X, Y and Z axis. + enum EAxis + { + TranslationX, + TranslationY, + TranslationZ, + + RotationX, + RotationY, + RotationZ, + + Num, + NumTranslation = TranslationZ + 1, + }; + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPosition1 = RVec3::sZero(); + Vec3 mAxisX1 = Vec3::sAxisX(); + Vec3 mAxisY1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPosition2 = RVec3::sZero(); + Vec3 mAxisX2 = Vec3::sAxisX(); + Vec3 mAxisY2 = Vec3::sAxisY(); + + /// Friction settings. + /// For translation: Max friction force in N. 0 = no friction. + /// For rotation: Max friction torque in Nm. 0 = no friction. + float mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 }; + + /// The type of swing constraint that we want to use. + ESwingType mSwingType = ESwingType::Cone; + + /// Limits. + /// For translation: Min and max linear limits in m (0 is frame of body 1 and 2 coincide). + /// For rotation: Min and max angular limits in rad (0 is frame of body 1 and 2 coincide). See comments at Axis enum for limit ranges. + /// + /// Remove degree of freedom by setting min = FLT_MAX and max = -FLT_MAX. The constraint will be driven to 0 for this axis. + /// + /// Free movement over an axis is allowed when min = -FLT_MAX and max = FLT_MAX. + /// + /// Rotation limit around X-Axis: When limited, should be \f$\in [-\pi, \pi]\f$. Can be asymmetric around zero. + /// + /// Rotation limit around Y-Z Axis: Forms a pyramid or cone shaped limit: + /// * For pyramid, should be \f$\in [-\pi, \pi]\f$ and does not need to be symmetrical around zero. + /// * For cone should be \f$\in [0, \pi]\f$ and needs to be symmetrical around zero (min limit is assumed to be -max limit). + float mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX }; + float mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX }; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + /// Only soft translation limits are supported, soft rotation limits are not currently supported. + SpringSettings mLimitsSpringSettings[EAxis::NumTranslation]; + + /// Make axis free (unconstrained) + void MakeFreeAxis(EAxis inAxis) { mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; } + bool IsFreeAxis(EAxis inAxis) const { return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; } + + /// Make axis fixed (fixed at value 0) + void MakeFixedAxis(EAxis inAxis) { mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; } + bool IsFixedAxis(EAxis inAxis) const { return mLimitMin[inAxis] >= mLimitMax[inAxis]; } + + /// Set a valid range for the constraint (if inMax < inMin, the axis will become fixed) + void SetLimitedAxis(EAxis inAxis, float inMin, float inMax) { mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; } + + /// Motor settings for each axis + MotorSettings mMotorSettings[EAxis::Num]; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// 6 Degree Of Freedom Constraint. Allows control over each of the 6 degrees of freedom. +class JPH_EXPORT SixDOFConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Get Axis from settings class + using EAxis = SixDOFConstraintSettings::EAxis; + + /// Construct six DOF constraint + SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings); + + /// Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SixDOF; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); } + + /// Update the translation limits for this constraint + void SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax); + + /// Update the rotational limits for this constraint + void SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax); + + /// Get constraint Limits + float GetLimitsMin(EAxis inAxis) const { return mLimitMin[inAxis]; } + float GetLimitsMax(EAxis inAxis) const { return mLimitMax[inAxis]; } + Vec3 GetTranslationLimitsMin() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMin[EAxis::TranslationX])); } + Vec3 GetTranslationLimitsMax() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMax[EAxis::TranslationX])); } + Vec3 GetRotationLimitsMin() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMin[EAxis::RotationX])); } + Vec3 GetRotationLimitsMax() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMax[EAxis::RotationX])); } + + /// Check which axis are fixed/free + inline bool IsFixedAxis(EAxis inAxis) const { return (mFixedAxis & (1 << inAxis)) != 0; } + inline bool IsFreeAxis(EAxis inAxis) const { return (mFreeAxis & (1 << inAxis)) != 0; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings(EAxis inAxis) const { JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; } + void SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); } + + /// Set the max friction for each axis + void SetMaxFriction(EAxis inAxis, float inFriction); + float GetMaxFriction(EAxis inAxis) const { return mMaxFriction[inAxis]; } + + /// Get rotation of constraint in constraint space + Quat GetRotationInConstraintSpace() const; + + /// Motor settings + MotorSettings & GetMotorSettings(EAxis inAxis) { return mMotorSettings[inAxis]; } + const MotorSettings & GetMotorSettings(EAxis inAxis) const { return mMotorSettings[inAxis]; } + + /// Motor controls. + /// Translation motors work in constraint space of body 1. + /// Rotation motors work in constraint space of body 2 (!). + void SetMotorState(EAxis inAxis, EMotorState inState); + EMotorState GetMotorState(EAxis inAxis) const { return mMotorState[inAxis]; } + + /// Set the target velocity in body 1 constraint space + Vec3 GetTargetVelocityCS() const { return mTargetVelocity; } + void SetTargetVelocityCS(Vec3Arg inVelocity) { mTargetVelocity = inVelocity; } + + /// Set the target angular velocity in body 2 constraint space (!) + void SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } + Vec3 GetTargetAngularVelocityCS() const { return mTargetAngularVelocity; } + + /// Set the target position in body 1 constraint space + Vec3 GetTargetPositionCS() const { return mTargetPosition; } + void SetTargetPositionCS(Vec3Arg inPosition) { mTargetPosition = inPosition; } + + /// Set the target orientation in body 1 constraint space + void SetTargetOrientationCS(QuatArg inOrientation); + Quat GetTargetOrientationCS() const { return mTargetOrientation; } + + /// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2). + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q. + void SetTargetOrientationBS(QuatArg inOrientation) { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return IsTranslationFullyConstrained()? mPointConstraintPart.GetTotalLambda() : Vec3(mTranslationConstraintPart[0].GetTotalLambda(), mTranslationConstraintPart[1].GetTotalLambda(), mTranslationConstraintPart[2].GetTotalLambda()); } + inline Vec3 GetTotalLambdaRotation() const { return IsRotationFullyConstrained()? mRotationConstraintPart.GetTotalLambda() : Vec3(mSwingTwistConstraintPart.GetTotalTwistLambda(), mSwingTwistConstraintPart.GetTotalSwingYLambda(), mSwingTwistConstraintPart.GetTotalSwingZLambda()); } + inline Vec3 GetTotalLambdaMotorTranslation() const { return Vec3(mMotorTranslationConstraintPart[0].GetTotalLambda(), mMotorTranslationConstraintPart[1].GetTotalLambda(), mMotorTranslationConstraintPart[2].GetTotalLambda()); } + inline Vec3 GetTotalLambdaMotorRotation() const { return Vec3(mMotorRotationConstraintPart[0].GetTotalLambda(), mMotorRotationConstraintPart[1].GetTotalLambda(), mMotorRotationConstraintPart[2].GetTotalLambda()); } + +private: + // Calculate properties needed for the position constraint + inline void GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const; + + // Sanitize the translation limits + inline void UpdateTranslationLimits(); + + // Propagate the rotation limits to the constraint part + inline void UpdateRotationLimits(); + + // Update the cached state of which axis are free and which ones are fixed + inline void UpdateFixedFreeAxis(); + + // Cache the state of mTranslationMotorActive + void CacheTranslationMotorActive(); + + // Cache the state of mRotationMotorActive + void CacheRotationMotorActive(); + + // Cache the state of mRotationPositionMotorActive + void CacheRotationPositionMotorActive(); + + /// Cache the state of mHasSpringLimits + void CacheHasSpringLimits(); + + // Constraint settings helper functions + inline bool IsTranslationConstrained() const { return (mFreeAxis & 0b111) != 0b111; } + inline bool IsTranslationFullyConstrained() const { return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; } + inline bool IsRotationConstrained() const { return (mFreeAxis & 0b111000) != 0b111000; } + inline bool IsRotationFullyConstrained() const { return (mFixedAxis & 0b111000) == 0b111000; } + inline bool HasFriction(EAxis inAxis) const { return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; } + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Transforms from constraint space to body space + Quat mConstraintToBody1; + Quat mConstraintToBody2; + + // Limits + uint8 mFreeAxis = 0; // Bitmask of free axis (bit 0 = TranslationX) + uint8 mFixedAxis = 0; // Bitmask of fixed axis (bit 0 = TranslationX) + bool mTranslationMotorActive = false; // If any of the translational frictions / motors are active + bool mRotationMotorActive = false; // If any of the rotational frictions / motors are active + uint8 mRotationPositionMotorActive = 0; // Bitmask of axis that have position motor active (bit 0 = RotationX) + bool mHasSpringLimits = false; // If any of the limit springs have a non-zero frequency/stiffness + float mLimitMin[EAxis::Num]; + float mLimitMax[EAxis::Num]; + SpringSettings mLimitsSpringSettings[EAxis::NumTranslation]; + + // Motor settings for each axis + MotorSettings mMotorSettings[EAxis::Num]; + + // Friction settings for each axis + float mMaxFriction[EAxis::Num]; + + // Motor controls + EMotorState mMotorState[EAxis::Num] = { EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off }; + Vec3 mTargetVelocity = Vec3::sZero(); + Vec3 mTargetAngularVelocity = Vec3::sZero(); + Vec3 mTargetPosition = Vec3::sZero(); + Quat mTargetOrientation = Quat::sIdentity(); + + // RUN TIME PROPERTIES FOLLOW + + // Constraint space axis in world space + Vec3 mTranslationAxis[3]; + Vec3 mRotationAxis[3]; + + // Translation displacement (valid when translation axis has a range limit) + float mDisplacement[3]; + + // Individual constraint parts for translation, or a combined point constraint part if all axis are fixed + AxisConstraintPart mTranslationConstraintPart[3]; + PointConstraintPart mPointConstraintPart; + + // Individual constraint parts for rotation or a combined constraint part if rotation is fixed + SwingTwistConstraintPart mSwingTwistConstraintPart; + RotationEulerConstraintPart mRotationConstraintPart; + + // Motor or friction constraints + AxisConstraintPart mMotorTranslationConstraintPart[3]; + AngleConstraintPart mMotorRotationConstraintPart[3]; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp new file mode 100644 index 000000000000..75335c32cba3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp @@ -0,0 +1,501 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mAutoDetectPoint) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping") + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings) +} + +void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis) +{ + JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace); + + mSliderAxis1 = mSliderAxis2 = inSliderAxis; + mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular(); +} + +void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mAutoDetectPoint); + inStream.Write(mPoint1); + inStream.Write(mSliderAxis1); + inStream.Write(mNormalAxis1); + inStream.Write(mPoint2); + inStream.Write(mSliderAxis2); + inStream.Write(mNormalAxis2); + inStream.Write(mLimitsMin); + inStream.Write(mLimitsMax); + inStream.Write(mMaxFrictionForce); + mLimitsSpringSettings.SaveBinaryState(inStream); + mMotorSettings.SaveBinaryState(inStream); +} + +void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mAutoDetectPoint); + inStream.Read(mPoint1); + inStream.Read(mSliderAxis1); + inStream.Read(mNormalAxis1); + inStream.Read(mPoint2); + inStream.Read(mSliderAxis2); + inStream.Read(mNormalAxis2); + inStream.Read(mLimitsMin); + inStream.Read(mLimitsMax); + inStream.Read(mMaxFrictionForce); + mLimitsSpringSettings.RestoreBinaryState(inStream); + mMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SliderConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SliderConstraint(inBody1, inBody2, *this); +} + +SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMaxFrictionForce(inSettings.mMaxFrictionForce), + mMotorSettings(inSettings.mMotorSettings) +{ + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mSliderAxis1, inSettings.mNormalAxis1, inSettings.mSliderAxis2, inSettings.mNormalAxis2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + + if (inSettings.mAutoDetectPoint) + { + // Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point + RVec3 anchor; + if (!inBody1.CanBeKinematicOrDynamic()) + anchor = inBody2.GetCenterOfMassPosition(); + else if (!inBody2.CanBeKinematicOrDynamic()) + anchor = inBody1.GetCenterOfMassPosition(); + else + { + // Otherwise use weighted anchor point towards the lightest body + Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real total_inv_mass = inv_m1 + inv_m2; + if (total_inv_mass != 0.0_r) + anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / total_inv_mass; + else + anchor = inBody1.GetCenterOfMassPosition(); + } + + // Store local positions + mLocalSpacePosition1 = Vec3(inv_transform1 * anchor); + mLocalSpacePosition2 = Vec3(inv_transform2 * anchor); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + } + + // If all properties were specified in world space, take them to local space now + mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized(); + mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized(); + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + + // Store local space axis + mLocalSpaceSliderAxis1 = inSettings.mSliderAxis1; + mLocalSpaceNormal1 = inSettings.mNormalAxis1; + } + + // Calculate 2nd local space normal + mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1); + + // Store limits + JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint"); + SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax); + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float SliderConstraint::GetCurrentPosition() const +{ + // See: CalculateR1R2U and CalculateSlidingAxisAndPosition + Vec3 r1 = mBody1->GetRotation() * mLocalSpacePosition1; + Vec3 r2 = mBody2->GetRotation() * mLocalSpacePosition2; + Vec3 u = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + r2 - r1; + return u.Dot(mBody1->GetRotation() * mLocalSpaceSliderAxis1); +} + +void SliderConstraint::SetLimits(float inLimitsMin, float inLimitsMax) +{ + JPH_ASSERT(inLimitsMin <= 0.0f); + JPH_ASSERT(inLimitsMax >= 0.0f); + mLimitsMin = inLimitsMin; + mLimitsMax = inLimitsMax; + mHasLimits = mLimitsMin != -FLT_MAX || mLimitsMax != FLT_MAX; +} + +void SliderConstraint::CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate points relative to body + mR1 = inRotation1 * mLocalSpacePosition1; + mR2 = inRotation2 * mLocalSpacePosition2; + + // Calculate X2 + R2 - X1 - R1 + mU = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + mR2 - mR1; +} + +void SliderConstraint::CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mN1 = inRotation1 * mLocalSpaceNormal1; + mN2 = inRotation1 * mLocalSpaceNormal2; + + mPositionConstraintPart.CalculateConstraintProperties(*mBody1, inRotation1, mR1 + mU, *mBody2, inRotation2, mR2, mN1, mN2); +} + +void SliderConstraint::CalculateSlidingAxisAndPosition(Mat44Arg inRotation1) +{ + if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionForce > 0.0f) + { + // Calculate world space slider axis + mWorldSpaceSliderAxis = inRotation1 * mLocalSpaceSliderAxis1; + + // Calculate slide distance along axis + mD = mU.Dot(mWorldSpaceSliderAxis); + } +} + +void SliderConstraint::CalculatePositionLimitsConstraintProperties(float inDeltaTime) +{ + // Check if distance is within limits + bool below_min = mD <= mLimitsMin; + if (mHasLimits && (below_min || mD >= mLimitsMax)) + mPositionLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mLimitsSpringSettings); + else + mPositionLimitsConstraintPart.Deactivate(); +} + +void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime) +{ + switch (mMotorState) + { + case EMotorState::Off: + if (mMaxFrictionForce > 0.0f) + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis); + else + mMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity); + break; + + case EMotorState::Position: + if (mMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings); + else + mMotorConstraintPart.Deactivate(); + break; + } +} + +void SliderConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculatePositionConstraintProperties(rotation1, rotation2); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2); + CalculateSlidingAxisAndPosition(rotation1); + CalculatePositionLimitsConstraintProperties(inDeltaTime); + CalculateMotorConstraintProperties(inDeltaTime); +} + +void SliderConstraint::ResetWarmStart() +{ + mMotorConstraintPart.Deactivate(); + mPositionConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + mPositionLimitsConstraintPart.Deactivate(); +} + +void SliderConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mMotorConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio); + mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mN1, mN2, inWarmStartImpulseRatio); + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio); +} + +bool SliderConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mMotorConstraintPart.IsActive()) + { + switch (mMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionForce * inDeltaTime; + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, inDeltaTime * mMotorSettings.mMinForceLimit, inDeltaTime * mMotorSettings.mMaxForceLimit); + break; + } + } + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mN1, mN2); + + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve limits along slider axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + float min_lambda, max_lambda; + if (mLimitsMin == mLimitsMax) + { + min_lambda = -FLT_MAX; + max_lambda = FLT_MAX; + } + else if (mD <= mLimitsMin) + { + min_lambda = 0.0f; + max_lambda = FLT_MAX; + } + else + { + min_lambda = -FLT_MAX; + max_lambda = 0.0f; + } + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, min_lambda, max_lambda); + } + + return motor || pos || rot || limit; +} + +bool SliderConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Motor operates on velocities only, don't call SolvePositionConstraint + + // Solve position constraint along 2 axis + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculatePositionConstraintProperties(rotation1, rotation2); + bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mN1, mN2, inBaumgarte); + + // Solve rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + + // Solve limits along slider axis + bool limit = false; + if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f) + { + rotation1 = Mat44::sRotation(mBody1->GetRotation()); + rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculateSlidingAxisAndPosition(rotation1); + CalculatePositionLimitsConstraintProperties(inDeltaTime); + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mD <= mLimitsMin) + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMin, inBaumgarte); + else + { + JPH_ASSERT(mD >= mLimitsMax); + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMax, inBaumgarte); + } + } + } + + return pos || rot || limit; +} + +#ifdef JPH_DEBUG_RENDERER +void SliderConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Transform the local positions into world space + Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1); + RVec3 position1 = transform1 * mLocalSpacePosition1; + RVec3 position2 = transform2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawMarker(position1, Color::sRed, 0.1f); + inRenderer->DrawMarker(position2, Color::sGreen, 0.1f); + inRenderer->DrawLine(position1, position2, Color::sGreen); + + // Draw motor + switch (mMotorState) + { + case EMotorState::Position: + inRenderer->DrawMarker(position1 + mTargetPosition * slider_axis, Color::sYellow, 1.0f); + break; + + case EMotorState::Velocity: + { + Vec3 cur_vel = (mBody2->GetLinearVelocity() - mBody1->GetLinearVelocity()).Dot(slider_axis) * slider_axis; + inRenderer->DrawLine(position2, position2 + cur_vel, Color::sBlue); + inRenderer->DrawArrow(position2 + cur_vel, position2 + mTargetVelocity * slider_axis, Color::sRed, 0.1f); + break; + } + + case EMotorState::Off: + break; + } +} + +void SliderConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + if (mHasLimits) + { + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Transform the local positions into world space + Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1); + RVec3 position1 = transform1 * mLocalSpacePosition1; + RVec3 position2 = transform2 * mLocalSpacePosition2; + + // Calculate the limits in world space + RVec3 limits_min = position1 + mLimitsMin * slider_axis; + RVec3 limits_max = position1 + mLimitsMax * slider_axis; + + inRenderer->DrawLine(limits_min, position1, Color::sWhite); + inRenderer->DrawLine(position2, limits_max, Color::sWhite); + + inRenderer->DrawMarker(limits_min, Color::sWhite, 0.1f); + inRenderer->DrawMarker(limits_max, Color::sWhite, 0.1f); + } +} +#endif // JPH_DEBUG_RENDERER + +void SliderConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mMotorConstraintPart.SaveState(inStream); + mPositionConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + mPositionLimitsConstraintPart.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetPosition); +} + +void SliderConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mMotorConstraintPart.RestoreState(inStream); + mPositionConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + mPositionLimitsConstraintPart.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetPosition); +} + +Ref SliderConstraint::GetConstraintSettings() const +{ + SliderConstraintSettings *settings = new SliderConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mSliderAxis1 = mLocalSpaceSliderAxis1; + settings->mNormalAxis1 = mLocalSpaceNormal1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + Mat44 inv_initial_rotation = Mat44::sRotation(mInvInitialOrientation); + settings->mSliderAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceSliderAxis1); + settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1); + settings->mLimitsMin = mLimitsMin; + settings->mLimitsMax = mLimitsMax; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + settings->mMaxFrictionForce = mMaxFrictionForce; + settings->mMotorSettings = mMotorSettings; + return settings; +} + +Mat44 SliderConstraint::GetConstraintToBody1Matrix() const +{ + return Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 SliderConstraint::GetConstraintToBody2Matrix() const +{ + Mat44 mat = Mat44::sRotation(mInvInitialOrientation).Multiply3x3(Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(0, 0, 0, 1))); + mat.SetTranslation(mLocalSpacePosition2); + return mat; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h new file mode 100644 index 000000000000..1e126e57df5f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h @@ -0,0 +1,198 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Slider constraint settings, used to create a slider constraint +class JPH_EXPORT SliderConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SliderConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + /// Note that the rotation constraint will be solved from body 1. This means that if body 1 and body 2 have different masses / inertias (kinematic body = infinite mass / inertia), body 1 should be the heaviest body. + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Simple way of setting the slider and normal axis in world space (assumes the bodies are already oriented correctly when the constraint is created) + void SetSliderAxis(Vec3Arg inSliderAxis); + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (the current relative position/orientation is chosen as the '0' position). Set this to false if you want to supply the attachment points yourself. + bool mAutoDetectPoint = false; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Slider axis is the axis along which movement is possible (direction), normal axis is a perpendicular vector to define the frame. + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mSliderAxis1 = Vec3::sAxisX(); + Vec3 mNormalAxis1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mSliderAxis2 = Vec3::sAxisX(); + Vec3 mNormalAxis2 = Vec3::sAxisY(); + + /// When the bodies move so that mPoint1 coincides with mPoint2 the slider position is defined to be 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-inf, 0] and mLimitsMax e [0, inf] + float mLimitsMin = -FLT_MAX; + float mLimitsMax = FLT_MAX; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + + /// Maximum amount of friction force to apply (N) when not driven by a motor. + float mMaxFrictionForce = 0.0f; + + /// In case the constraint is powered, this determines the motor settings around the sliding axis + MotorSettings mMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A slider constraint allows movement in only 1 axis (and no rotation). Also known as a prismatic constraint. +class JPH_EXPORT SliderConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct slider constraint + SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Slider; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Get the current distance from the rest position + float GetCurrentPosition() const; + + /// Friction control + void SetMaxFrictionForce(float inFrictionForce) { mMaxFrictionForce = inFrictionForce; } + float GetMaxFrictionForce() const { return mMaxFrictionForce; } + + /// Motor settings + MotorSettings & GetMotorSettings() { return mMotorSettings; } + const MotorSettings & GetMotorSettings() const { return mMotorSettings; } + + // Motor controls + void SetMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; } + EMotorState GetMotorState() const { return mMotorState; } + void SetTargetVelocity(float inVelocity) { mTargetVelocity = inVelocity; } + float GetTargetVelocity() const { return mTargetVelocity; } + void SetTargetPosition(float inPosition) { mTargetPosition = mHasLimits? Clamp(inPosition, mLimitsMin, mLimitsMax) : inPosition; } + float GetTargetPosition() const { return mTargetPosition; } + + /// Update the limits of the slider constraint (see SliderConstraintSettings) + void SetLimits(float inLimitsMin, float inLimitsMax); + float GetLimitsMin() const { return mLimitsMin; } + float GetLimitsMax() const { return mLimitsMax; } + bool HasLimits() const { return mHasLimits; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vector<2> GetTotalLambdaPosition() const { return mPositionConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaPositionLimits() const { return mPositionLimitsConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mMotorConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2); + void CalculateSlidingAxisAndPosition(Mat44Arg inRotation1); + void CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + void CalculatePositionLimitsConstraintProperties(float inDeltaTime); + void CalculateMotorConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space sliding direction + Vec3 mLocalSpaceSliderAxis1; + + // Local space normals to the sliding direction (in body 1 space) + Vec3 mLocalSpaceNormal1; + Vec3 mLocalSpaceNormal2; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + Quat mInvInitialOrientation; + + // Slider limits + bool mHasLimits; + float mLimitsMin; + float mLimitsMax; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // Friction + float mMaxFrictionForce; + + // Motor controls + MotorSettings mMotorSettings; + EMotorState mMotorState = EMotorState::Off; + float mTargetVelocity = 0.0f; + float mTargetPosition = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Positions where the point constraint acts on (middle point between center of masses) + Vec3 mR1; + Vec3 mR2; + + // X2 + R2 - X1 - R1 + Vec3 mU; + + // World space sliding direction + Vec3 mWorldSpaceSliderAxis; + + // Normals to the slider axis + Vec3 mN1; + Vec3 mN2; + + // Distance along the slide axis + float mD = 0.0f; + + // The constraint parts + DualAxisConstraintPart mPositionConstraintPart; + RotationEulerConstraintPart mRotationConstraintPart; + AxisConstraintPart mPositionLimitsConstraintPart; + AxisConstraintPart mMotorConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp new file mode 100644 index 000000000000..c2c32400fed1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SpringSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE(SpringSettings, mMode) + JPH_ADD_ATTRIBUTE(SpringSettings, mFrequency) + JPH_ADD_ATTRIBUTE(SpringSettings, mDamping) +} + +void SpringSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMode); + inStream.Write(mFrequency); + inStream.Write(mDamping); +} + +void SpringSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMode); + inStream.Read(mFrequency); + inStream.Read(mDamping); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h new file mode 100644 index 000000000000..bfa49caac08d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h @@ -0,0 +1,70 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Enum used by constraints to specify how the spring is defined +enum class ESpringMode : uint8 +{ + FrequencyAndDamping, ///< Frequency and damping are specified + StiffnessAndDamping, ///< Stiffness and damping are specified +}; + +/// Settings for a linear or angular spring +class JPH_EXPORT SpringSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SpringSettings) + +public: + /// Constructor + SpringSettings() = default; + SpringSettings(const SpringSettings &) = default; + SpringSettings & operator = (const SpringSettings &) = default; + SpringSettings(ESpringMode inMode, float inFrequencyOrStiffness, float inDamping) : mMode(inMode), mFrequency(inFrequencyOrStiffness), mDamping(inDamping) { } + + /// Saves the contents of the spring settings in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores contents from the binary stream inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Check if the spring has a valid frequency / stiffness, if not the spring will be hard + inline bool HasStiffness() const { return mFrequency > 0.0f; } + + /// Selects the way in which the spring is defined + /// If the mode is StiffnessAndDamping then mFrequency becomes the stiffness (k) and mDamping becomes the damping ratio (c) in the spring equation F = -k * x - c * v. Otherwise the properties are as documented. + ESpringMode mMode = ESpringMode::FrequencyAndDamping; + + union + { + /// Valid when mSpringMode = ESpringMode::FrequencyAndDamping. + /// If mFrequency > 0 the constraint will be soft and mFrequency specifies the oscillation frequency in Hz. + /// If mFrequency <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows). + float mFrequency = 0.0f; + + /// Valid when mSpringMode = ESpringMode::StiffnessAndDamping. + /// If mStiffness > 0 the constraint will be soft and mStiffness specifies the stiffness (k) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring. + /// If mStiffness <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows). + /// + /// Note that stiffness values are large numbers. To calculate a ballpark value for the needed stiffness you can use: + /// force = stiffness * delta_spring_length = mass * gravity <=> stiffness = mass * gravity / delta_spring_length. + /// So if your object weighs 1500 kg and the spring compresses by 2 meters, you need a stiffness in the order of 1500 * 9.81 / 2 ~ 7500 N/m. + float mStiffness; + }; + + /// When mSpringMode = ESpringMode::FrequencyAndDamping mDamping is the damping ratio (0 = no damping, 1 = critical damping). + /// When mSpringMode = ESpringMode::StiffnessAndDamping mDamping is the damping (c) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring. + /// Note that if you set mDamping = 0, you will not get an infinite oscillation. Because we integrate physics using an explicit Euler scheme, there is always energy loss. + /// This is done to keep the simulation from exploding, because with a damping of 0 and even the slightest rounding error, the oscillation could become bigger and bigger until the simulation explodes. + float mDamping = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp new file mode 100644 index 000000000000..bcd74ff305a2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp @@ -0,0 +1,524 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SwingTwistConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SwingTwistConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition2) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis2) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis2) + JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSwingType) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mNormalHalfConeAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneHalfConeAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMinAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMaxAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mMaxFrictionTorque) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mSwingMotorSettings) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMotorSettings) +} + +void SwingTwistConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPosition1); + inStream.Write(mTwistAxis1); + inStream.Write(mPlaneAxis1); + inStream.Write(mPosition2); + inStream.Write(mTwistAxis2); + inStream.Write(mPlaneAxis2); + inStream.Write(mSwingType); + inStream.Write(mNormalHalfConeAngle); + inStream.Write(mPlaneHalfConeAngle); + inStream.Write(mTwistMinAngle); + inStream.Write(mTwistMaxAngle); + inStream.Write(mMaxFrictionTorque); + mSwingMotorSettings.SaveBinaryState(inStream); + mTwistMotorSettings.SaveBinaryState(inStream); +} + +void SwingTwistConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPosition1); + inStream.Read(mTwistAxis1); + inStream.Read(mPlaneAxis1); + inStream.Read(mPosition2); + inStream.Read(mTwistAxis2); + inStream.Read(mPlaneAxis2); + inStream.Read(mSwingType); + inStream.Read(mNormalHalfConeAngle); + inStream.Read(mPlaneHalfConeAngle); + inStream.Read(mTwistMinAngle); + inStream.Read(mTwistMaxAngle); + inStream.Read(mMaxFrictionTorque); + mSwingMotorSettings.RestoreBinaryState(inStream); + mTwistMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SwingTwistConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SwingTwistConstraint(inBody1, inBody2, *this); +} + +void SwingTwistConstraint::UpdateLimits() +{ + // Pass limits on to swing twist constraint part + mSwingTwistConstraintPart.SetLimits(mTwistMinAngle, mTwistMaxAngle, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle); +} + +SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mNormalHalfConeAngle(inSettings.mNormalHalfConeAngle), + mPlaneHalfConeAngle(inSettings.mPlaneHalfConeAngle), + mTwistMinAngle(inSettings.mTwistMinAngle), + mTwistMaxAngle(inSettings.mTwistMaxAngle), + mMaxFrictionTorque(inSettings.mMaxFrictionTorque), + mSwingMotorSettings(inSettings.mSwingMotorSettings), + mTwistMotorSettings(inSettings.mTwistMotorSettings) +{ + // Override swing type + mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType); + + // Calculate rotation needed to go from constraint space to body1 local space + Vec3 normal_axis1 = inSettings.mPlaneAxis1.Cross(inSettings.mTwistAxis1); + Mat44 c_to_b1(Vec4(inSettings.mTwistAxis1, 0), Vec4(normal_axis1, 0), Vec4(inSettings.mPlaneAxis1, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody1 = c_to_b1.GetQuaternion(); + + // Calculate rotation needed to go from constraint space to body2 local space + Vec3 normal_axis2 = inSettings.mPlaneAxis2.Cross(inSettings.mTwistAxis2); + Mat44 c_to_b2(Vec4(inSettings.mTwistAxis2, 0), Vec4(normal_axis2, 0), Vec4(inSettings.mPlaneAxis2, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody2 = c_to_b2.GetQuaternion(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1); + mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1; + + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2); + mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2; + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPosition1); + mLocalSpacePosition2 = Vec3(inSettings.mPosition2); + } + + UpdateLimits(); +} + +void SwingTwistConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +Quat SwingTwistConstraint::GetRotationInConstraintSpace() const +{ + // Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform()) + // Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1)) + // Let q be the rotation of the constraint in constraint space + // b2 takes a vector from the local space of body2 to world space + // To express this in terms of b1: b2 = b1 * c1 * q * c2^-1 + // c2^-1 goes from local body 2 space to constraint space + // q rotates the constraint + // c1 goes from constraint space to body 1 local space + // b1 goes from body 1 local space to world space + // So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2 + // Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + return constraint_body1_to_world.Conjugated() * constraint_body2_to_world; +} + +void SwingTwistConstraint::SetSwingMotorState(EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mSwingMotorSettings.IsValid()); + + if (mSwingMotorState != inState) + { + mSwingMotorState = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } +} + +void SwingTwistConstraint::SetTwistMotorState(EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mTwistMotorSettings.IsValid()); + + if (mTwistMotorState != inState) + { + mTwistMotorState = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + mMotorConstraintPart[0].Deactivate(); + } +} + +void SwingTwistConstraint::SetTargetOrientationCS(QuatArg inOrientation) +{ + Quat q_swing, q_twist; + inOrientation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis); + + if (clamped_axis != 0) + mTargetOrientation = q_swing * q_twist; + else + mTargetOrientation = inOrientation; +} + +void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Setup point constraint + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + + // GetRotationInConstraintSpace written out since we reuse the sub expressions + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world; + + // Calculate constraint properties for the swing twist limit + mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world); + + if (mSwingMotorState != EMotorState::Off || mTwistMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f) + { + // Calculate rotation motor axis + Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world); + for (int i = 0; i < 3; ++i) + mWorldSpaceMotorAxis[i] = ws_axis.GetColumn3(i); + + Vec3 rotation_error; + if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position) + { + // Get target orientation along the shortest path from q + Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation; + + // The definition of the constraint rotation q: + // R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1) + // + // R2' is the rotation of body 2 when reaching the target_orientation: + // R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2) + // + // The difference in body 2 space: + // R2' = R2 * diff_body2 (3) + // + // We want to specify the difference in the constraint space of body 2: + // diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4) + // + // Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5) + // Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6) + // Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7) + // Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* + // <=> target_orientation = q * diff + // <=> diff = q^* * target_orientation + Quat diff = q.Conjugated() * target_orientation; + + // Approximate error angles + // The imaginary part of a quaternion is rotation_axis * sin(angle / 2) + // If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i] + // We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction + rotation_error = -2.0f * diff.GetXYZ(); + } + + // Swing motor + switch (mSwingMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + { + // Enable friction + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f); + } + else + { + // Disable friction + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } + break; + + case EMotorState::Velocity: + // Use motor to create angular velocity around desired axis + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], -mTargetAngularVelocity[i]); + break; + + case EMotorState::Position: + // Use motor to drive rotation error to zero + if (mSwingMotorSettings.mSpringSettings.HasStiffness()) + { + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings); + } + else + { + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].Deactivate(); + } + break; + } + + // Twist motor + switch (mTwistMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + { + // Enable friction + mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f); + } + else + { + // Disable friction + mMotorConstraintPart[0].Deactivate(); + } + break; + + case EMotorState::Velocity: + // Use motor to create angular velocity around desired axis + mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], -mTargetAngularVelocity[0]); + break; + + case EMotorState::Position: + // Use motor to drive rotation error to zero + if (mTwistMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings); + else + mMotorConstraintPart[0].Deactivate(); + break; + } + } + else + { + // Disable rotation motor + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } +} + +void SwingTwistConstraint::ResetWarmStart() +{ + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); +} + +void SwingTwistConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + for (AngleConstraintPart &c : mMotorConstraintPart) + c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool SwingTwistConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve twist rotation motor + if (mMotorConstraintPart[0].IsActive()) + { + // Twist limits + float min_twist_limit, max_twist_limit; + if (mTwistMotorState == EMotorState::Off) + { + max_twist_limit = inDeltaTime * mMaxFrictionTorque; + min_twist_limit = -max_twist_limit; + } + else + { + min_twist_limit = inDeltaTime * mTwistMotorSettings.mMinTorqueLimit; + max_twist_limit = inDeltaTime * mTwistMotorSettings.mMaxTorqueLimit; + } + + impulse |= mMotorConstraintPart[0].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], min_twist_limit, max_twist_limit); + } + + // Solve swing rotation motor + if (mMotorConstraintPart[1].IsActive()) + { + // Swing parts should turn on / off together + JPH_ASSERT(mMotorConstraintPart[2].IsActive()); + + // Swing limits + float min_swing_limit, max_swing_limit; + if (mSwingMotorState == EMotorState::Off) + { + max_swing_limit = inDeltaTime * mMaxFrictionTorque; + min_swing_limit = -max_swing_limit; + } + else + { + min_swing_limit = inDeltaTime * mSwingMotorSettings.mMinTorqueLimit; + max_swing_limit = inDeltaTime * mSwingMotorSettings.mMaxTorqueLimit; + } + + for (int i = 1; i < 3; ++i) + impulse |= mMotorConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], min_swing_limit, max_swing_limit); + } + else + { + // Swing parts should turn on / off together + JPH_ASSERT(!mMotorConstraintPart[2].IsActive()); + } + + // Solve rotation limits + impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + return impulse; +} + +bool SwingTwistConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + // Solve rotation violations + Quat q = GetRotationInConstraintSpace(); + impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte); + + // Solve position violations + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER +void SwingTwistConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1; + Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2; + + // Draw constraint orientation + inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize); + + // Draw current swing and twist + Quat q = GetRotationInConstraintSpace(); + Quat q_swing, q_twist; + q.GetSwingTwist(q_swing, q_twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite); + + if (mSwingMotorState == EMotorState::Velocity || mTwistMotorState == EMotorState::Velocity) + { + // Draw target angular velocity + inRenderer->DrawArrow(position1, position1 + rotation2 * mTargetAngularVelocity, Color::sRed, 0.1f); + } + if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position) + { + // Draw motor swing and twist + Quat swing, twist; + mTargetOrientation.GetSwingTwist(swing, twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * twist).RotateAxisY(), Color::sYellow); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * swing).RotateAxisX(), Color::sCyan); + } +} + +void SwingTwistConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get matrix that transforms from constraint space to world space + RMat44 constraint_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1); + + // Draw limits + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid) + inRenderer->DrawSwingPyramidLimits(constraint_to_world, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + else + inRenderer->DrawSwingConeLimits(constraint_to_world, mPlaneHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + inRenderer->DrawPie(constraint_to_world.GetTranslation(), mDrawConstraintSize, constraint_to_world.GetAxisX(), constraint_to_world.GetAxisY(), mTwistMinAngle, mTwistMaxAngle, Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void SwingTwistConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); + mSwingTwistConstraintPart.SaveState(inStream); + for (const AngleConstraintPart &c : mMotorConstraintPart) + c.SaveState(inStream); + + inStream.Write(mSwingMotorState); + inStream.Write(mTwistMotorState); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetOrientation); +} + +void SwingTwistConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); + mSwingTwistConstraintPart.RestoreState(inStream); + for (AngleConstraintPart &c : mMotorConstraintPart) + c.RestoreState(inStream); + + inStream.Read(mSwingMotorState); + inStream.Read(mTwistMotorState); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetOrientation); +} + +Ref SwingTwistConstraint::GetConstraintSettings() const +{ + SwingTwistConstraintSettings *settings = new SwingTwistConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPosition1 = RVec3(mLocalSpacePosition1); + settings->mTwistAxis1 = mConstraintToBody1.RotateAxisX(); + settings->mPlaneAxis1 = mConstraintToBody1.RotateAxisZ(); + settings->mPosition2 = RVec3(mLocalSpacePosition2); + settings->mTwistAxis2 = mConstraintToBody2.RotateAxisX(); + settings->mPlaneAxis2 = mConstraintToBody2.RotateAxisZ(); + settings->mSwingType = mSwingTwistConstraintPart.GetSwingType(); + settings->mNormalHalfConeAngle = mNormalHalfConeAngle; + settings->mPlaneHalfConeAngle = mPlaneHalfConeAngle; + settings->mTwistMinAngle = mTwistMinAngle; + settings->mTwistMaxAngle = mTwistMaxAngle; + settings->mMaxFrictionTorque = mMaxFrictionTorque; + settings->mSwingMotorSettings = mSwingMotorSettings; + settings->mTwistMotorSettings = mTwistMotorSettings; + return settings; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h new file mode 100644 index 000000000000..5e3e896f44cd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h @@ -0,0 +1,197 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Swing twist constraint settings, used to create a swing twist constraint +/// All values in this structure are copied to the swing twist constraint and the settings object is no longer needed afterwards. +/// +/// This image describes the limit settings: +/// @image html Docs/SwingTwistConstraint.png +class JPH_EXPORT SwingTwistConstraintSettings final : public TwoBodyConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SwingTwistConstraintSettings) + +public: + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + ///@name Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPosition1 = RVec3::sZero(); + Vec3 mTwistAxis1 = Vec3::sAxisX(); + Vec3 mPlaneAxis1 = Vec3::sAxisY(); + + ///@name Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPosition2 = RVec3::sZero(); + Vec3 mTwistAxis2 = Vec3::sAxisX(); + Vec3 mPlaneAxis2 = Vec3::sAxisY(); + + /// The type of swing constraint that we want to use. + ESwingType mSwingType = ESwingType::Cone; + + ///@name Swing rotation limits + float mNormalHalfConeAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. + float mPlaneHalfConeAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. + + ///@name Twist rotation limits + float mTwistMinAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$. + float mTwistMaxAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$. + + ///@name Friction + float mMaxFrictionTorque = 0.0f; ///< Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor + + ///@name In case the constraint is powered, this determines the motor settings around the swing and twist axis + MotorSettings mSwingMotorSettings; + MotorSettings mTwistMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A swing twist constraint is a specialized constraint for humanoid ragdolls that allows limited rotation only +/// +/// @see SwingTwistConstraintSettings for a description of the limits +class JPH_EXPORT SwingTwistConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct swing twist constraint + SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings); + + ///@name Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SwingTwist; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); } + + ///@name Constraint reference frame + inline Vec3 GetLocalSpacePosition1() const { return mLocalSpacePosition1; } + inline Vec3 GetLocalSpacePosition2() const { return mLocalSpacePosition2; } + inline Quat GetConstraintToBody1() const { return mConstraintToBody1; } + inline Quat GetConstraintToBody2() const { return mConstraintToBody2; } + + ///@name Constraint limits + inline float GetNormalHalfConeAngle() const { return mNormalHalfConeAngle; } + inline void SetNormalHalfConeAngle(float inAngle) { mNormalHalfConeAngle = inAngle; UpdateLimits(); } + inline float GetPlaneHalfConeAngle() const { return mPlaneHalfConeAngle; } + inline void SetPlaneHalfConeAngle(float inAngle) { mPlaneHalfConeAngle = inAngle; UpdateLimits(); } + inline float GetTwistMinAngle() const { return mTwistMinAngle; } + inline void SetTwistMinAngle(float inAngle) { mTwistMinAngle = inAngle; UpdateLimits(); } + inline float GetTwistMaxAngle() const { return mTwistMaxAngle; } + inline void SetTwistMaxAngle(float inAngle) { mTwistMaxAngle = inAngle; UpdateLimits(); } + + ///@name Motor settings + const MotorSettings & GetSwingMotorSettings() const { return mSwingMotorSettings; } + MotorSettings & GetSwingMotorSettings() { return mSwingMotorSettings; } + const MotorSettings & GetTwistMotorSettings() const { return mTwistMotorSettings; } + MotorSettings & GetTwistMotorSettings() { return mTwistMotorSettings; } + + ///@name Friction control + void SetMaxFrictionTorque(float inFrictionTorque) { mMaxFrictionTorque = inFrictionTorque; } + float GetMaxFrictionTorque() const { return mMaxFrictionTorque; } + + ///@name Motor controls + + /// Controls if the motors are on or off + void SetSwingMotorState(EMotorState inState); + EMotorState GetSwingMotorState() const { return mSwingMotorState; } + void SetTwistMotorState(EMotorState inState); + EMotorState GetTwistMotorState() const { return mTwistMotorState; } + + /// Set the target angular velocity of body 2 in constraint space of body 2 + void SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } + Vec3 GetTargetAngularVelocityCS() const { return mTargetAngularVelocity; } + + /// Set the target orientation in constraint space (drives constraint to: GetRotationInConstraintSpace() == inOrientation) + void SetTargetOrientationCS(QuatArg inOrientation); + Quat GetTargetOrientationCS() const { return mTargetOrientation; } + + /// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2). + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q. + void SetTargetOrientationBS(QuatArg inOrientation) { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); } + + /// Get current rotation of constraint in constraint space. + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q for q. + Quat GetRotationInConstraintSpace() const; + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaTwist() const { return mSwingTwistConstraintPart.GetTotalTwistLambda(); } + inline float GetTotalLambdaSwingY() const { return mSwingTwistConstraintPart.GetTotalSwingYLambda(); } + inline float GetTotalLambdaSwingZ() const { return mSwingTwistConstraintPart.GetTotalSwingZLambda(); } + inline Vec3 GetTotalLambdaMotor() const { return Vec3(mMotorConstraintPart[0].GetTotalLambda(), mMotorConstraintPart[1].GetTotalLambda(), mMotorConstraintPart[2].GetTotalLambda()); } + +private: + // Update the limits in the swing twist constraint part + void UpdateLimits(); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Transforms from constraint space to body space + Quat mConstraintToBody1; + Quat mConstraintToBody2; + + // Limits + float mNormalHalfConeAngle; + float mPlaneHalfConeAngle; + float mTwistMinAngle; + float mTwistMaxAngle; + + // Friction + float mMaxFrictionTorque; + + // Motor controls + MotorSettings mSwingMotorSettings; + MotorSettings mTwistMotorSettings; + EMotorState mSwingMotorState = EMotorState::Off; + EMotorState mTwistMotorState = EMotorState::Off; + Vec3 mTargetAngularVelocity = Vec3::sZero(); + Quat mTargetOrientation = Quat::sIdentity(); + + // RUN TIME PROPERTIES FOLLOW + + // Rotation axis for motor constraint parts + Vec3 mWorldSpaceMotorAxis[3]; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + SwingTwistConstraintPart mSwingTwistConstraintPart; + AngleConstraintPart mMotorConstraintPart[3]; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp new file mode 100644 index 000000000000..9ee7cc5d892d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(TwoBodyConstraintSettings) +{ + JPH_ADD_BASE_CLASS(TwoBodyConstraintSettings, ConstraintSettings) +} + +void TwoBodyConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + // Activate bodies + BodyID body_ids[2]; + int num_bodies = 0; + if (mBody1->IsDynamic() && !mBody1->IsActive()) + body_ids[num_bodies++] = mBody1->GetID(); + if (mBody2->IsDynamic() && !mBody2->IsActive()) + body_ids[num_bodies++] = mBody2->GetID(); + if (num_bodies > 0) + inBodyManager.ActivateBodies(body_ids, num_bodies); + + // Link the bodies into the same island + ioBuilder.LinkConstraint(inConstraintIndex, mBody1->GetIndexInActiveBodiesInternal(), mBody2->GetIndexInActiveBodiesInternal()); +} + +uint TwoBodyConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const +{ + return ioSplitter.AssignSplit(mBody1, mBody2); +} + +#ifdef JPH_DEBUG_RENDERER + +void TwoBodyConstraint::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform() * GetConstraintToBody1Matrix(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform() * GetConstraintToBody2Matrix(); + inRenderer->DrawCoordinateSystem(transform1, 1.1f * mDrawConstraintSize); + inRenderer->DrawCoordinateSystem(transform2, mDrawConstraintSize); +} + +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h new file mode 100644 index 000000000000..24630eb6de3c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h @@ -0,0 +1,65 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class TwoBodyConstraint; + +/// Base class for settings for all constraints that involve 2 bodies +class JPH_EXPORT TwoBodyConstraintSettings : public ConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, TwoBodyConstraintSettings) + +public: + /// Create an instance of this constraint + /// You can use Body::sFixedToWorld for inBody1 if you want to attach inBody2 to the world + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const = 0; +}; + +/// Base class for all constraints that involve 2 bodies. Body1 is usually considered the parent, Body2 the child. +class JPH_EXPORT TwoBodyConstraint : public Constraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TwoBodyConstraint(Body &inBody1, Body &inBody2, const TwoBodyConstraintSettings &inSettings) : Constraint(inSettings), mBody1(&inBody1), mBody2(&inBody2) { } + + /// Get the type of a constraint + virtual EConstraintType GetType() const override { return EConstraintType::TwoBodyConstraint; } + + /// Solver interface + virtual bool IsActive() const override { return Constraint::IsActive() && (mBody1->IsActive() || mBody2->IsActive()) && (mBody2->IsDynamic() || mBody1->IsDynamic()); } +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + /// Access to the connected bodies + Body * GetBody1() const { return mBody1; } + Body * GetBody2() const { return mBody2; } + + /// Calculates the transform that transforms from constraint space to body 1 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc. + virtual Mat44 GetConstraintToBody1Matrix() const = 0; + + /// Calculates the transform that transforms from constraint space to body 2 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc. + virtual Mat44 GetConstraintToBody2Matrix() const = 0; + + /// Link bodies that are connected by this constraint in the island builder + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override; + + /// Link bodies that are connected by this constraint in the same split. Returns the split index. + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override; + +protected: + /// The two bodies involved + Body * mBody1; + Body * mBody2; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp new file mode 100644 index 000000000000..7985a36bf971 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_ENABLE_DETERMINISM_LOG + +JPH_NAMESPACE_BEGIN + +DeterminismLog DeterminismLog::sLog; + +JPH_NAMESPACE_END + +#endif // JPH_ENABLE_DETERMINISM_LOG diff --git a/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h new file mode 100644 index 000000000000..e2930ff3ceaa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h @@ -0,0 +1,159 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +//#define JPH_ENABLE_DETERMINISM_LOG +#ifdef JPH_ENABLE_DETERMINISM_LOG + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +/// A simple class that logs the state of the simulation. The resulting text file can be used to diff between platforms and find issues in determinism. +class DeterminismLog +{ +private: + JPH_INLINE uint32 Convert(float inValue) const + { + return *(uint32 *)&inValue; + } + + JPH_INLINE uint64 Convert(double inValue) const + { + return *(uint64 *)&inValue; + } + +public: + DeterminismLog() + { + mLog.open("detlog.txt", std::ios::out | std::ios::trunc | std::ios::binary); // Binary because we don't want a difference between Unix and Windows line endings. + mLog.fill('0'); + } + + DeterminismLog & operator << (char inValue) + { + mLog << inValue; + return *this; + } + + DeterminismLog & operator << (const char *inValue) + { + mLog << std::dec << inValue; + return *this; + } + + DeterminismLog & operator << (const string &inValue) + { + mLog << std::dec << inValue; + return *this; + } + + DeterminismLog & operator << (const BodyID &inValue) + { + mLog << std::hex << std::setw(8) << inValue.GetIndexAndSequenceNumber(); + return *this; + } + + DeterminismLog & operator << (const SubShapeID &inValue) + { + mLog << std::hex << std::setw(8) << inValue.GetValue(); + return *this; + } + + DeterminismLog & operator << (float inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue); + return *this; + } + + DeterminismLog & operator << (int inValue) + { + mLog << inValue; + return *this; + } + + DeterminismLog & operator << (uint32 inValue) + { + mLog << std::hex << std::setw(8) << inValue; + return *this; + } + + DeterminismLog & operator << (uint64 inValue) + { + mLog << std::hex << std::setw(16) << inValue; + return *this; + } + + DeterminismLog & operator << (Vec3Arg inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ()); + return *this; + } + + DeterminismLog & operator << (DVec3Arg inValue) + { + mLog << std::hex << std::setw(16) << Convert(inValue.GetX()) << " " << std::setw(16) << Convert(inValue.GetY()) << " " << std::setw(16) << Convert(inValue.GetZ()); + return *this; + } + + DeterminismLog & operator << (Vec4Arg inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ()) << " " << std::setw(8) << Convert(inValue.GetW()); + return *this; + } + + DeterminismLog & operator << (const Float3 &inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.x) << " " << std::setw(8) << Convert(inValue.y) << " " << std::setw(8) << Convert(inValue.z); + return *this; + } + + DeterminismLog & operator << (Mat44Arg inValue) + { + *this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetColumn4(3); + return *this; + } + + DeterminismLog & operator << (DMat44Arg inValue) + { + *this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetTranslation(); + return *this; + } + + DeterminismLog & operator << (QuatArg inValue) + { + *this << inValue.GetXYZW(); + return *this; + } + + // Singleton instance + static DeterminismLog sLog; + +private: + std::ofstream mLog; +}; + +/// Will log something to the determinism log, usage: JPH_DET_LOG("label " << value); +#define JPH_DET_LOG(...) DeterminismLog::sLog << __VA_ARGS__ << '\n' + +JPH_NAMESPACE_END + +#else + +JPH_SUPPRESS_WARNING_PUSH +JPH_SUPPRESS_WARNINGS + +/// By default we log nothing +#define JPH_DET_LOG(...) + +JPH_SUPPRESS_WARNING_POP + +#endif // JPH_ENABLE_DETERMINISM_LOG diff --git a/thirdparty/jolt_physics/Jolt/Physics/EActivation.h b/thirdparty/jolt_physics/Jolt/Physics/EActivation.h new file mode 100644 index 000000000000..08c10c20dd42 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/EActivation.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used by AddBody to determine if the body needs to be initially active +enum class EActivation +{ + Activate, ///< Activate the body, making it part of the simulation + DontActivate ///< Leave activation state as it is (will not deactivate an active body) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h b/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h new file mode 100644 index 000000000000..c9edd6deda08 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used by PhysicsSystem to report error conditions during the PhysicsSystem::Update call. This is a bit field, multiple errors can trigger in the same update. +enum class EPhysicsUpdateError : uint32 +{ + None = 0, ///< No errors + ManifoldCacheFull = 1 << 0, ///< The manifold cache is full, this means that the total number of contacts between bodies is too high. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init. + BodyPairCacheFull = 1 << 1, ///< The body pair cache is full, this means that too many bodies contacted. Some contacts were ignored. Increase inMaxBodyPairs in PhysicsSystem::Init. + ContactConstraintsFull = 1 << 2, ///< The contact constraints buffer is full. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init. +}; + +/// OR operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator | (EPhysicsUpdateError inA, EPhysicsUpdateError inB) +{ + return static_cast(static_cast(inA) | static_cast(inB)); +} + +/// OR operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator |= (EPhysicsUpdateError &ioA, EPhysicsUpdateError inB) +{ + ioA = ioA | inB; + return ioA; +} + +/// AND operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator & (EPhysicsUpdateError inA, EPhysicsUpdateError inB) +{ + return static_cast(static_cast(inA) & static_cast(inB)); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp new file mode 100644 index 000000000000..ed1064df9965 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp @@ -0,0 +1,484 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +IslandBuilder::~IslandBuilder() +{ + JPH_ASSERT(mConstraintLinks == nullptr); + JPH_ASSERT(mContactLinks == nullptr); + JPH_ASSERT(mBodyIslands == nullptr); + JPH_ASSERT(mBodyIslandEnds == nullptr); + JPH_ASSERT(mConstraintIslands == nullptr); + JPH_ASSERT(mConstraintIslandEnds == nullptr); + JPH_ASSERT(mContactIslands == nullptr); + JPH_ASSERT(mContactIslandEnds == nullptr); + JPH_ASSERT(mIslandsSorted == nullptr); + + delete [] mBodyLinks; +} + +void IslandBuilder::Init(uint32 inMaxActiveBodies) +{ + mMaxActiveBodies = inMaxActiveBodies; + + // Link each body to itself, BuildBodyIslands() will restore this so that we don't need to do this each step + JPH_ASSERT(mBodyLinks == nullptr); + mBodyLinks = new BodyLink [mMaxActiveBodies]; + for (uint32 i = 0; i < mMaxActiveBodies; ++i) + mBodyLinks[i].mLinkedTo.store(i, memory_order_relaxed); +} + +void IslandBuilder::PrepareContactConstraints(uint32 inMaxContacts, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Need to call Init first + JPH_ASSERT(mBodyLinks != nullptr); + + // Check that the builder has been reset + JPH_ASSERT(mNumContacts == 0); + JPH_ASSERT(mNumIslands == 0); + + // Create contact link buffer, not initialized so each contact needs to be explicitly set + JPH_ASSERT(mContactLinks == nullptr); + mContactLinks = (uint32 *)inTempAllocator->Allocate(inMaxContacts * sizeof(uint32)); + mMaxContacts = inMaxContacts; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + // Create validation structures + JPH_ASSERT(mLinkValidation == nullptr); + mLinkValidation = (LinkValidation *)inTempAllocator->Allocate(inMaxContacts * sizeof(LinkValidation)); + mNumLinkValidation = 0; +#endif +} + +void IslandBuilder::PrepareNonContactConstraints(uint32 inNumConstraints, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Need to call Init first + JPH_ASSERT(mBodyLinks != nullptr); + + // Check that the builder has been reset + JPH_ASSERT(mNumIslands == 0); + + // Store number of constraints + mNumConstraints = inNumConstraints; + + // Create constraint link buffer, not initialized so each constraint needs to be explicitly set + JPH_ASSERT(mConstraintLinks == nullptr); + mConstraintLinks = (uint32 *)inTempAllocator->Allocate(inNumConstraints * sizeof(uint32)); +} + +uint32 IslandBuilder::GetLowestBodyIndex(uint32 inActiveBodyIndex) const +{ + uint32 index = inActiveBodyIndex; + for (;;) + { + uint32 link_to = mBodyLinks[index].mLinkedTo.load(memory_order_relaxed); + if (link_to == index) + break; + index = link_to; + } + return index; +} + +void IslandBuilder::LinkBodies(uint32 inFirst, uint32 inSecond) +{ + JPH_PROFILE_FUNCTION(); + + // Both need to be active, we don't want to create an island with static objects + if (inFirst >= mMaxActiveBodies || inSecond >= mMaxActiveBodies) + return; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + // Add link to the validation list + if (mNumLinkValidation < uint32(mMaxContacts)) + mLinkValidation[mNumLinkValidation++] = { inFirst, inSecond }; + else + JPH_ASSERT(false, "Out of links"); +#endif + + // Start the algorithm with the two bodies + uint32 first_link_to = inFirst; + uint32 second_link_to = inSecond; + + for (;;) + { + // Follow the chain until we get to the body with lowest index + // If the swap compare below fails, we'll keep searching from the lowest index for the new lowest index + first_link_to = GetLowestBodyIndex(first_link_to); + second_link_to = GetLowestBodyIndex(second_link_to); + + // If the targets are the same, the bodies are already connected + if (first_link_to != second_link_to) + { + // We always link the highest to the lowest + if (first_link_to < second_link_to) + { + // Attempt to link the second to the first + // Since we found this body to be at the end of the chain it must point to itself, and if it + // doesn't it has been reparented and we need to retry the algorithm + if (!mBodyLinks[second_link_to].mLinkedTo.compare_exchange_weak(second_link_to, first_link_to, memory_order_relaxed)) + continue; + } + else + { + // Attempt to link the first to the second + // Since we found this body to be at the end of the chain it must point to itself, and if it + // doesn't it has been reparented and we need to retry the algorithm + if (!mBodyLinks[first_link_to].mLinkedTo.compare_exchange_weak(first_link_to, second_link_to, memory_order_relaxed)) + continue; + } + } + + // Linking succeeded! + // Chains of bodies can become really long, resulting in an O(N) loop to find the lowest body index + // to prevent this we attempt to update the link of the bodies that were passed in to directly point + // to the lowest index that we found. If the value became lower than our lowest link, some other + // thread must have relinked these bodies in the mean time so we won't update the value. + uint32 lowest_link_to = min(first_link_to, second_link_to); + AtomicMin(mBodyLinks[inFirst].mLinkedTo, lowest_link_to, memory_order_relaxed); + AtomicMin(mBodyLinks[inSecond].mLinkedTo, lowest_link_to, memory_order_relaxed); + break; + } +} + +void IslandBuilder::LinkConstraint(uint32 inConstraintIndex, uint32 inFirst, uint32 inSecond) +{ + LinkBodies(inFirst, inSecond); + + JPH_ASSERT(inConstraintIndex < mNumConstraints); + uint32 min_value = min(inFirst, inSecond); // Use fact that invalid index is 0xffffffff, we want the active body of two + JPH_ASSERT(min_value != Body::cInactiveIndex); // At least one of the bodies must be active + mConstraintLinks[inConstraintIndex] = min_value; +} + +void IslandBuilder::LinkContact(uint32 inContactIndex, uint32 inFirst, uint32 inSecond) +{ + JPH_ASSERT(inContactIndex < mMaxContacts); + mContactLinks[inContactIndex] = min(inFirst, inSecond); // Use fact that invalid index is 0xffffffff, we want the active body of two +} + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + +void IslandBuilder::ValidateIslands(uint32 inNumActiveBodies) const +{ + JPH_PROFILE_FUNCTION(); + + // Go through all links so far + for (uint32 i = 0; i < mNumLinkValidation; ++i) + { + // If the bodies in this link ended up in different groups we have a problem + if (mBodyLinks[mLinkValidation[i].mFirst].mIslandIndex != mBodyLinks[mLinkValidation[i].mSecond].mIslandIndex) + { + Trace("Fail: %u, %u", mLinkValidation[i].mFirst, mLinkValidation[i].mSecond); + Trace("Num Active: %u", inNumActiveBodies); + + for (uint32 j = 0; j < mNumLinkValidation; ++j) + Trace("builder.Link(%u, %u);", mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + + IslandBuilder tmp; + tmp.Init(inNumActiveBodies); + for (uint32 j = 0; j < mNumLinkValidation; ++j) + { + Trace("Link %u -> %u", mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + tmp.LinkBodies(mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + for (uint32 t = 0; t < inNumActiveBodies; ++t) + Trace("%u -> %u", t, (uint32)tmp.mBodyLinks[t].mLinkedTo); + } + + JPH_ASSERT(false, "IslandBuilder validation failed"); + } + } +} + +#endif + +void IslandBuilder::BuildBodyIslands(const BodyID *inActiveBodies, uint32 inNumActiveBodies, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Store the amount of active bodies + mNumActiveBodies = inNumActiveBodies; + + // Create output arrays for body ID's, don't call constructors + JPH_ASSERT(mBodyIslands == nullptr); + mBodyIslands = (BodyID *)inTempAllocator->Allocate(inNumActiveBodies * sizeof(BodyID)); + + // Create output array for start index of each island. At this point we don't know how many islands there will be, but we know it cannot be more than inNumActiveBodies. + // Note: We allocate 1 extra entry because we always increment the count of the next island. + uint32 *body_island_starts = (uint32 *)inTempAllocator->Allocate((inNumActiveBodies + 1) * sizeof(uint32)); + + // First island always starts at 0 + body_island_starts[0] = 0; + + // Calculate island index for all bodies + JPH_ASSERT(mNumIslands == 0); + for (uint32 i = 0; i < inNumActiveBodies; ++i) + { + BodyLink &link = mBodyLinks[i]; + uint32 s = link.mLinkedTo.load(memory_order_relaxed); + if (s != i) + { + // Links to another body, take island index from other body (this must have been filled in already since we're looping from low to high) + JPH_ASSERT(s < uint32(i)); + uint32 island_index = mBodyLinks[s].mIslandIndex; + link.mIslandIndex = island_index; + + // Increment the start of the next island + body_island_starts[island_index + 1]++; + } + else + { + // Does not link to other body, this is the start of a new island + link.mIslandIndex = mNumIslands; + ++mNumIslands; + + // Set the start of the next island to 1 + body_island_starts[mNumIslands] = 1; + } + } + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + ValidateIslands(inNumActiveBodies); +#endif + + // Make the start array absolute (so far we only counted) + for (uint32 island = 1; island < mNumIslands; ++island) + body_island_starts[island] += body_island_starts[island - 1]; + + // Convert the to a linear list grouped by island + for (uint32 i = 0; i < inNumActiveBodies; ++i) + { + BodyLink &link = mBodyLinks[i]; + + // Copy the body to the correct location in the array and increment it + uint32 &start = body_island_starts[link.mIslandIndex]; + mBodyIslands[start] = inActiveBodies[i]; + start++; + + // Reset linked to field for the next update + link.mLinkedTo.store(i, memory_order_relaxed); + } + + // We should now have a full array + JPH_ASSERT(mNumIslands == 0 || body_island_starts[mNumIslands - 1] == inNumActiveBodies); + + // We've incremented all body indices so that they now point at the end instead of the starts + JPH_ASSERT(mBodyIslandEnds == nullptr); + mBodyIslandEnds = body_island_starts; +} + +void IslandBuilder::BuildConstraintIslands(const uint32 *inConstraintToBody, uint32 inNumConstraints, uint32 *&outConstraints, uint32 *&outConstraintsEnd, TempAllocator *inTempAllocator) const +{ + JPH_PROFILE_FUNCTION(); + + // Check if there's anything to do + if (inNumConstraints == 0) + return; + + // Create output arrays for constraints + // Note: For the end indices we allocate 1 extra entry so we don't have to do an if in the inner loop + uint32 *constraints = (uint32 *)inTempAllocator->Allocate(inNumConstraints * sizeof(uint32)); + uint32 *constraint_ends = (uint32 *)inTempAllocator->Allocate((mNumIslands + 1) * sizeof(uint32)); + + // Reset sizes + for (uint32 island = 0; island < mNumIslands; ++island) + constraint_ends[island] = 0; + + // Loop over array and increment start relative position for the next island + for (uint32 constraint = 0; constraint < inNumConstraints; ++constraint) + { + uint32 body_idx = inConstraintToBody[constraint]; + uint32 next_island_idx = mBodyLinks[body_idx].mIslandIndex + 1; + JPH_ASSERT(next_island_idx <= mNumIslands); + constraint_ends[next_island_idx]++; + } + + // Make start positions absolute + for (uint32 island = 1; island < mNumIslands; ++island) + constraint_ends[island] += constraint_ends[island - 1]; + + // Loop over array and collect constraints + for (uint32 constraint = 0; constraint < inNumConstraints; ++constraint) + { + uint32 body_idx = inConstraintToBody[constraint]; + uint32 island_idx = mBodyLinks[body_idx].mIslandIndex; + constraints[constraint_ends[island_idx]++] = constraint; + } + + JPH_ASSERT(outConstraints == nullptr); + outConstraints = constraints; + JPH_ASSERT(outConstraintsEnd == nullptr); + outConstraintsEnd = constraint_ends; +} + +void IslandBuilder::SortIslands(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + if (mNumContacts > 0 || mNumConstraints > 0) + { + // Allocate mapping table + JPH_ASSERT(mIslandsSorted == nullptr); + mIslandsSorted = (uint32 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint32)); + + // Initialize index + for (uint32 island = 0; island < mNumIslands; ++island) + mIslandsSorted[island] = island; + + // Determine the sum of contact constraints / constraints per island + uint32 *num_constraints = (uint32 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint32)); + if (mNumContacts > 0 && mNumConstraints > 0) + { + num_constraints[0] = mConstraintIslandEnds[0] + mContactIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mConstraintIslandEnds[island] - mConstraintIslandEnds[island - 1] + + mContactIslandEnds[island] - mContactIslandEnds[island - 1]; + } + else if (mNumContacts > 0) + { + num_constraints[0] = mContactIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mContactIslandEnds[island] - mContactIslandEnds[island - 1]; + } + else + { + num_constraints[0] = mConstraintIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mConstraintIslandEnds[island] - mConstraintIslandEnds[island - 1]; + } + + // Sort so the biggest islands go first, this means that the jobs that take longest will be running + // first which improves the chance that all jobs finish at the same time. + QuickSort(mIslandsSorted, mIslandsSorted + mNumIslands, [num_constraints](uint32 inLHS, uint32 inRHS) { + return num_constraints[inLHS] > num_constraints[inRHS]; + }); + + inTempAllocator->Free(num_constraints, mNumIslands * sizeof(uint32)); + } +} + +void IslandBuilder::Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBodies, uint32 inNumContacts, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + mNumContacts = inNumContacts; + + BuildBodyIslands(inActiveBodies, inNumActiveBodies, inTempAllocator); + BuildConstraintIslands(mConstraintLinks, mNumConstraints, mConstraintIslands, mConstraintIslandEnds, inTempAllocator); + BuildConstraintIslands(mContactLinks, mNumContacts, mContactIslands, mContactIslandEnds, inTempAllocator); + SortIslands(inTempAllocator); + + mNumPositionSteps = (uint8 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint8)); +} + +void IslandBuilder::GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + uint32 sorted_index = mIslandsSorted != nullptr? mIslandsSorted[inIslandIndex] : inIslandIndex; + outBodiesBegin = sorted_index > 0? mBodyIslands + mBodyIslandEnds[sorted_index - 1] : mBodyIslands; + outBodiesEnd = mBodyIslands + mBodyIslandEnds[sorted_index]; +} + +bool IslandBuilder::GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + if (mNumConstraints == 0) + { + outConstraintsBegin = nullptr; + outConstraintsEnd = nullptr; + return false; + } + else + { + uint32 sorted_index = mIslandsSorted[inIslandIndex]; + outConstraintsBegin = sorted_index > 0? mConstraintIslands + mConstraintIslandEnds[sorted_index - 1] : mConstraintIslands; + outConstraintsEnd = mConstraintIslands + mConstraintIslandEnds[sorted_index]; + return outConstraintsBegin != outConstraintsEnd; + } +} + +bool IslandBuilder::GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + if (mNumContacts == 0) + { + outContactsBegin = nullptr; + outContactsEnd = nullptr; + return false; + } + else + { + uint32 sorted_index = mIslandsSorted[inIslandIndex]; + outContactsBegin = sorted_index > 0? mContactIslands + mContactIslandEnds[sorted_index - 1] : mContactIslands; + outContactsEnd = mContactIslands + mContactIslandEnds[sorted_index]; + return outContactsBegin != outContactsEnd; + } +} + +void IslandBuilder::ResetIslands(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + inTempAllocator->Free(mNumPositionSteps, mNumIslands * sizeof(uint8)); + + if (mIslandsSorted != nullptr) + { + inTempAllocator->Free(mIslandsSorted, mNumIslands * sizeof(uint32)); + mIslandsSorted = nullptr; + } + + if (mContactIslands != nullptr) + { + inTempAllocator->Free(mContactIslandEnds, (mNumIslands + 1) * sizeof(uint32)); + mContactIslandEnds = nullptr; + inTempAllocator->Free(mContactIslands, mNumContacts * sizeof(uint32)); + mContactIslands = nullptr; + } + + if (mConstraintIslands != nullptr) + { + inTempAllocator->Free(mConstraintIslandEnds, (mNumIslands + 1) * sizeof(uint32)); + mConstraintIslandEnds = nullptr; + inTempAllocator->Free(mConstraintIslands, mNumConstraints * sizeof(uint32)); + mConstraintIslands = nullptr; + } + + inTempAllocator->Free(mBodyIslandEnds, (mNumActiveBodies + 1) * sizeof(uint32)); + mBodyIslandEnds = nullptr; + inTempAllocator->Free(mBodyIslands, mNumActiveBodies * sizeof(uint32)); + mBodyIslands = nullptr; + + inTempAllocator->Free(mConstraintLinks, mNumConstraints * sizeof(uint32)); + mConstraintLinks = nullptr; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + inTempAllocator->Free(mLinkValidation, mMaxContacts * sizeof(LinkValidation)); + mLinkValidation = nullptr; +#endif + + inTempAllocator->Free(mContactLinks, mMaxContacts * sizeof(uint32)); + mContactLinks = nullptr; + + mNumActiveBodies = 0; + mNumConstraints = 0; + mMaxContacts = 0; + mNumContacts = 0; + mNumIslands = 0; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h new file mode 100644 index 000000000000..4c2f097d60c3 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h @@ -0,0 +1,125 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class TempAllocator; + +//#define JPH_VALIDATE_ISLAND_BUILDER + +/// Keeps track of connected bodies and builds islands for multithreaded velocity/position update +class IslandBuilder : public NonCopyable +{ +public: + /// Destructor + ~IslandBuilder(); + + /// Initialize the island builder with the maximum amount of bodies that could be active + void Init(uint32 inMaxActiveBodies); + + /// Prepare for simulation step by allocating space for the contact constraints + void PrepareContactConstraints(uint32 inMaxContactConstraints, TempAllocator *inTempAllocator); + + /// Prepare for simulation step by allocating space for the non-contact constraints + void PrepareNonContactConstraints(uint32 inNumConstraints, TempAllocator *inTempAllocator); + + /// Link two bodies by their index in the BodyManager::mActiveBodies list to form islands + void LinkBodies(uint32 inFirst, uint32 inSecond); + + /// Link a constraint to a body by their index in the BodyManager::mActiveBodies + void LinkConstraint(uint32 inConstraintIndex, uint32 inFirst, uint32 inSecond); + + /// Link a contact to a body by their index in the BodyManager::mActiveBodies + void LinkContact(uint32 inContactIndex, uint32 inFirst, uint32 inSecond); + + /// Finalize the islands after all bodies have been Link()-ed + void Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBodies, uint32 inNumContacts, TempAllocator *inTempAllocator); + + /// Get the amount of islands formed + uint32 GetNumIslands() const { return mNumIslands; } + + /// Get iterator for a particular island, return false if there are no constraints + void GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const; + bool GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const; + bool GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const; + + /// The number of position iterations for each island + void SetNumPositionSteps(uint32 inIslandIndex, uint inNumPositionSteps) { JPH_ASSERT(inIslandIndex < mNumIslands); JPH_ASSERT(inNumPositionSteps < 256); mNumPositionSteps[inIslandIndex] = uint8(inNumPositionSteps); } + uint GetNumPositionSteps(uint32 inIslandIndex) const { JPH_ASSERT(inIslandIndex < mNumIslands); return mNumPositionSteps[inIslandIndex]; } + + /// After you're done calling the three functions above, call this function to free associated data + void ResetIslands(TempAllocator *inTempAllocator); + +private: + /// Returns the index of the lowest body in the group + uint32 GetLowestBodyIndex(uint32 inActiveBodyIndex) const; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + /// Helper function to validate all islands so far generated + void ValidateIslands(uint32 inNumActiveBodies) const; +#endif + + // Helper functions to build various islands + void BuildBodyIslands(const BodyID *inActiveBodies, uint32 inNumActiveBodies, TempAllocator *inTempAllocator); + void BuildConstraintIslands(const uint32 *inConstraintToBody, uint32 inNumConstraints, uint32 *&outConstraints, uint32 *&outConstraintsEnd, TempAllocator *inTempAllocator) const; + + /// Sorts the islands so that the islands with most constraints go first + void SortIslands(TempAllocator *inTempAllocator); + + /// Intermediate data structure that for each body keeps track what the lowest index of the body is that it is connected to + struct BodyLink + { + JPH_OVERRIDE_NEW_DELETE + + atomic mLinkedTo; ///< An index in mBodyLinks pointing to another body in this island with a lower index than this body + uint32 mIslandIndex; ///< The island index of this body (filled in during Finalize) + }; + + // Intermediate data + BodyLink * mBodyLinks = nullptr; ///< Maps bodies to the first body in the island + uint32 * mConstraintLinks = nullptr; ///< Maps constraint index to body index (which maps to island index) + uint32 * mContactLinks = nullptr; ///< Maps contact constraint index to body index (which maps to island index) + + // Final data + BodyID * mBodyIslands = nullptr; ///< Bodies ordered by island + uint32 * mBodyIslandEnds = nullptr; ///< End index of each body island + + uint32 * mConstraintIslands = nullptr; ///< Constraints ordered by island + uint32 * mConstraintIslandEnds = nullptr; ///< End index of each constraint island + + uint32 * mContactIslands = nullptr; ///< Contacts ordered by island + uint32 * mContactIslandEnds = nullptr; ///< End index of each contact island + + uint32 * mIslandsSorted = nullptr; ///< A list of island indices in order of most constraints first + + uint8 * mNumPositionSteps = nullptr; ///< Number of position steps for each island + + // Counters + uint32 mMaxActiveBodies; ///< Maximum size of the active bodies list (see BodyManager::mActiveBodies) + uint32 mNumActiveBodies = 0; ///< Number of active bodies passed to + uint32 mNumConstraints = 0; ///< Size of the constraint list (see ConstraintManager::mConstraints) + uint32 mMaxContacts = 0; ///< Maximum amount of contacts supported + uint32 mNumContacts = 0; ///< Size of the contacts list (see ContactConstraintManager::mNumConstraints) + uint32 mNumIslands = 0; ///< Final number of islands + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + /// Structure to keep track of all added links to validate that islands were generated correctly + struct LinkValidation + { + uint32 mFirst; + uint32 mSecond; + }; + + LinkValidation * mLinkValidation = nullptr; + atomic mNumLinkValidation; +#endif +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp new file mode 100644 index 000000000000..50ab1c583fad --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp @@ -0,0 +1,582 @@ +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +//#define JPH_LARGE_ISLAND_SPLITTER_DEBUG + +JPH_NAMESPACE_BEGIN + +LargeIslandSplitter::EStatus LargeIslandSplitter::Splits::FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration) +{ + { + // First check if we can get a new batch (doing a read to avoid hammering an atomic with an atomic subtract) + // Note this also avoids overflowing the status counter if we're done but there's still one thread processing items + uint64 status = mStatus.load(memory_order_acquire); + + // Check for special value that indicates that the splits are still being built + // (note we do not check for this condition again below as we reset all splits before kicking off jobs that fetch batches of work) + if (status == StatusItemMask) + return EStatus::WaitingForBatch; + + // Next check if all items have been processed. Note that we do this after checking if the job can be started + // as mNumIterations is not initialized until the split is started. + if (sGetIteration(status) >= mNumIterations) + return EStatus::AllBatchesDone; + + uint item = sGetItem(status); + uint split_index = sGetSplit(status); + if (split_index == cNonParallelSplitIdx) + { + // Non parallel split needs to be taken as a single batch, only the thread that takes element 0 will do it + if (item != 0) + return EStatus::WaitingForBatch; + } + else + { + // Parallel split is split into batches + JPH_ASSERT(split_index < mNumSplits); + const Split &split = mSplits[split_index]; + if (item >= split.GetNumItems()) + return EStatus::WaitingForBatch; + } + } + + // Then try to actually get the batch + uint64 status = mStatus.fetch_add(cBatchSize, memory_order_acquire); + int iteration = sGetIteration(status); + if (iteration >= mNumIterations) + return EStatus::AllBatchesDone; + + uint split_index = sGetSplit(status); + JPH_ASSERT(split_index < mNumSplits || split_index == cNonParallelSplitIdx); + const Split &split = mSplits[split_index]; + uint item_begin = sGetItem(status); + if (split_index == cNonParallelSplitIdx) + { + if (item_begin == 0) + { + // Non-parallel split always goes as a single batch + outConstraintsBegin = split.mConstraintBufferBegin; + outConstraintsEnd = split.mConstraintBufferEnd; + outContactsBegin = split.mContactBufferBegin; + outContactsEnd = split.mContactBufferEnd; + outFirstIteration = iteration == 0; + return EStatus::BatchRetrieved; + } + else + { + // Otherwise we're done with this split + return EStatus::WaitingForBatch; + } + } + + // Parallel split is split into batches + uint num_constraints = split.GetNumConstraints(); + uint num_contacts = split.GetNumContacts(); + uint num_items = num_constraints + num_contacts; + if (item_begin >= num_items) + return EStatus::WaitingForBatch; + + uint item_end = min(item_begin + cBatchSize, num_items); + if (item_end >= num_constraints) + { + if (item_begin < num_constraints) + { + // Partially from constraints and partially from contacts + outConstraintsBegin = split.mConstraintBufferBegin + item_begin; + outConstraintsEnd = split.mConstraintBufferEnd; + } + else + { + // Only contacts + outConstraintsBegin = 0; + outConstraintsEnd = 0; + } + + outContactsBegin = split.mContactBufferBegin + (max(item_begin, num_constraints) - num_constraints); + outContactsEnd = split.mContactBufferBegin + (item_end - num_constraints); + } + else + { + // Only constraints + outConstraintsBegin = split.mConstraintBufferBegin + item_begin; + outConstraintsEnd = split.mConstraintBufferBegin + item_end; + + outContactsBegin = 0; + outContactsEnd = 0; + } + + outFirstIteration = iteration == 0; + return EStatus::BatchRetrieved; +} + +void LargeIslandSplitter::Splits::MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch) +{ + // We fetched this batch, nobody should change the split and or iteration until we mark the last batch as processed so we can safely get the current status + uint64 status = mStatus.load(memory_order_relaxed); + uint split_index = sGetSplit(status); + JPH_ASSERT(split_index < mNumSplits || split_index == cNonParallelSplitIdx); + const Split &split = mSplits[split_index]; + uint num_items_in_split = split.GetNumItems(); + + // Determine if this is the last iteration before possibly incrementing it + int iteration = sGetIteration(status); + outLastIteration = iteration == mNumIterations - 1; + + // Add the number of items we processed to the total number of items processed + // Note: This needs to happen after we read the status as other threads may update the status after we mark items as processed + JPH_ASSERT(inNumProcessed > 0); // Logic will break if we mark a block of 0 items as processed + uint total_items_processed = mItemsProcessed.fetch_add(inNumProcessed, memory_order_acq_rel) + inNumProcessed; + + // Check if we're at the end of the split + if (total_items_processed >= num_items_in_split) + { + JPH_ASSERT(total_items_processed == num_items_in_split); // Should not overflow, that means we're retiring more items than we should process + + // Set items processed back to 0 for the next split/iteration + mItemsProcessed.store(0, memory_order_release); + + // Determine next split + do + { + if (split_index == cNonParallelSplitIdx) + { + // At start of next iteration + split_index = 0; + ++iteration; + } + else + { + // At start of next split + ++split_index; + } + + // If we're beyond the end of splits, go to the non-parallel split + if (split_index >= mNumSplits) + split_index = cNonParallelSplitIdx; + } + while (iteration < mNumIterations + && mSplits[split_index].GetNumItems() == 0); // We don't support processing empty splits, skip to the next split in this case + + mStatus.store((uint64(iteration) << StatusIterationShift) | (uint64(split_index) << StatusSplitShift), memory_order_release); + } + + // Track if this is the final batch + outFinalBatch = iteration >= mNumIterations; +} + +LargeIslandSplitter::~LargeIslandSplitter() +{ + JPH_ASSERT(mSplitMasks == nullptr); + JPH_ASSERT(mContactAndConstraintsSplitIdx == nullptr); + JPH_ASSERT(mContactAndConstraintIndices == nullptr); + JPH_ASSERT(mSplitIslands == nullptr); +} + +void LargeIslandSplitter::Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Count the total number of constraints and contacts that we will be putting in splits + mContactAndConstraintsSize = 0; + for (uint32 island = 0; island < inIslandBuilder.GetNumIslands(); ++island) + { + // Get the contacts in this island + uint32 *contacts_start, *contacts_end; + inIslandBuilder.GetContactsInIsland(island, contacts_start, contacts_end); + uint num_contacts_in_island = uint(contacts_end - contacts_start); + + // Get the constraints in this island + uint32 *constraints_start, *constraints_end; + inIslandBuilder.GetConstraintsInIsland(island, constraints_start, constraints_end); + uint num_constraints_in_island = uint(constraints_end - constraints_start); + + uint island_size = num_contacts_in_island + num_constraints_in_island; + if (island_size >= cLargeIslandTreshold) + { + mNumSplitIslands++; + mContactAndConstraintsSize += island_size; + } + else + break; // If this island doesn't have enough constraints, the next islands won't either since they're sorted from big to small + } + + if (mContactAndConstraintsSize > 0) + { + mNumActiveBodies = inNumActiveBodies; + + // Allocate split mask buffer + mSplitMasks = (SplitMask *)inTempAllocator->Allocate(mNumActiveBodies * sizeof(SplitMask)); + + // Allocate contact and constraint buffer + uint contact_and_constraint_indices_size = mContactAndConstraintsSize * sizeof(uint32); + mContactAndConstraintsSplitIdx = (uint32 *)inTempAllocator->Allocate(contact_and_constraint_indices_size); + mContactAndConstraintIndices = (uint32 *)inTempAllocator->Allocate(contact_and_constraint_indices_size); + + // Allocate island split buffer + mSplitIslands = (Splits *)inTempAllocator->Allocate(mNumSplitIslands * sizeof(Splits)); + + // Prevent any of the splits from being picked up as work + for (uint i = 0; i < mNumSplitIslands; ++i) + mSplitIslands[i].ResetStatus(); + } +} + +uint LargeIslandSplitter::AssignSplit(const Body *inBody1, const Body *inBody2) +{ + uint32 idx1 = inBody1->GetIndexInActiveBodiesInternal(); + uint32 idx2 = inBody2->GetIndexInActiveBodiesInternal(); + + // Test if either index is negative + if (idx1 == Body::cInactiveIndex || !inBody1->IsDynamic()) + { + // Body 1 is not active or a kinematic body, so we only need to set 1 body + JPH_ASSERT(idx2 < mNumActiveBodies); + SplitMask &mask = mSplitMasks[idx2]; + uint split = min(CountTrailingZeros(~uint32(mask)), cNonParallelSplitIdx); + mask |= SplitMask(1U << split); + return split; + } + else if (idx2 == Body::cInactiveIndex || !inBody2->IsDynamic()) + { + // Body 2 is not active or a kinematic body, so we only need to set 1 body + JPH_ASSERT(idx1 < mNumActiveBodies); + SplitMask &mask = mSplitMasks[idx1]; + uint split = min(CountTrailingZeros(~uint32(mask)), cNonParallelSplitIdx); + mask |= SplitMask(1U << split); + return split; + } + else + { + // If both bodies are active, we need to set 2 bodies + JPH_ASSERT(idx1 < mNumActiveBodies); + JPH_ASSERT(idx2 < mNumActiveBodies); + SplitMask &mask1 = mSplitMasks[idx1]; + SplitMask &mask2 = mSplitMasks[idx2]; + uint split = min(CountTrailingZeros((~uint32(mask1)) & (~uint32(mask2))), cNonParallelSplitIdx); + SplitMask mask = SplitMask(1U << split); + mask1 |= mask; + mask2 |= mask; + return split; + } +} + +uint LargeIslandSplitter::AssignToNonParallelSplit(const Body *inBody) +{ + uint32 idx = inBody->GetIndexInActiveBodiesInternal(); + if (idx != Body::cInactiveIndex) + { + JPH_ASSERT(idx < mNumActiveBodies); + mSplitMasks[idx] |= 1U << cNonParallelSplitIdx; + } + + return cNonParallelSplitIdx; +} + +bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator) +{ + JPH_PROFILE_FUNCTION(); + + // Get the contacts in this island + uint32 *contacts_start, *contacts_end; + inIslandBuilder.GetContactsInIsland(inIslandIndex, contacts_start, contacts_end); + uint num_contacts_in_island = uint(contacts_end - contacts_start); + + // Get the constraints in this island + uint32 *constraints_start, *constraints_end; + inIslandBuilder.GetConstraintsInIsland(inIslandIndex, constraints_start, constraints_end); + uint num_constraints_in_island = uint(constraints_end - constraints_start); + + // Check if it exceeds the threshold + uint island_size = num_contacts_in_island + num_constraints_in_island; + if (island_size < cLargeIslandTreshold) + return false; + + // Get bodies in this island + BodyID *bodies_start, *bodies_end; + inIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_start, bodies_end); + + // Reset the split mask for all bodies in this island + Body const * const *bodies = inBodyManager.GetBodies().data(); + for (const BodyID *b = bodies_start; b < bodies_end; ++b) + mSplitMasks[bodies[b->GetIndex()]->GetIndexInActiveBodiesInternal()] = 0; + + // Count the number of contacts and constraints per split + uint num_contacts_in_split[cNumSplits] = { }; + uint num_constraints_in_split[cNumSplits] = { }; + + // Get space to store split indices + uint offset = mContactAndConstraintsNextFree.fetch_add(island_size, memory_order_relaxed); + uint32 *contact_split_idx = mContactAndConstraintsSplitIdx + offset; + uint32 *constraint_split_idx = contact_split_idx + num_contacts_in_island; + + // Assign the contacts to a split + uint32 *cur_contact_split_idx = contact_split_idx; + for (const uint32 *c = contacts_start; c < contacts_end; ++c) + { + const Body *body1, *body2; + inContactManager.GetAffectedBodies(*c, body1, body2); + uint split = AssignSplit(body1, body2); + num_contacts_in_split[split]++; + *cur_contact_split_idx++ = split; + + if (body1->IsDynamic()) + ioStepsCalculator(body1->GetMotionPropertiesUnchecked()); + if (body2->IsDynamic()) + ioStepsCalculator(body2->GetMotionPropertiesUnchecked()); + } + + // Assign the constraints to a split + uint32 *cur_constraint_split_idx = constraint_split_idx; + for (const uint32 *c = constraints_start; c < constraints_end; ++c) + { + const Constraint *constraint = inActiveConstraints[*c]; + uint split = constraint->BuildIslandSplits(*this); + num_constraints_in_split[split]++; + *cur_constraint_split_idx++ = split; + + ioStepsCalculator(constraint); + } + + ioStepsCalculator.Finalize(); + + // Start with 0 splits + uint split_remap_table[cNumSplits]; + uint new_split_idx = mNextSplitIsland.fetch_add(1, memory_order_relaxed); + JPH_ASSERT(new_split_idx < mNumSplitIslands); + Splits &splits = mSplitIslands[new_split_idx]; + splits.mIslandIndex = inIslandIndex; + splits.mNumSplits = 0; + splits.mNumIterations = ioStepsCalculator.GetNumVelocitySteps() + 1; // Iteration 0 is used for warm starting + splits.mNumVelocitySteps = ioStepsCalculator.GetNumVelocitySteps(); + splits.mNumPositionSteps = ioStepsCalculator.GetNumPositionSteps(); + splits.mItemsProcessed.store(0, memory_order_release); + + // Allocate space to store the sorted constraint and contact indices per split + uint32 *constraint_buffer_cur[cNumSplits], *contact_buffer_cur[cNumSplits]; + for (uint s = 0; s < cNumSplits; ++s) + { + // If this split doesn't contain enough constraints and contacts, we will combine it with the non parallel split + if (num_constraints_in_split[s] + num_contacts_in_split[s] < cSplitCombineTreshold + && s < cNonParallelSplitIdx) // The non-parallel split cannot merge into itself + { + // Remap it + split_remap_table[s] = cNonParallelSplitIdx; + + // Add the counts to the non parallel split + num_contacts_in_split[cNonParallelSplitIdx] += num_contacts_in_split[s]; + num_constraints_in_split[cNonParallelSplitIdx] += num_constraints_in_split[s]; + } + else + { + // This split is valid, map it to the next empty slot + uint target_split; + if (s < cNonParallelSplitIdx) + target_split = splits.mNumSplits++; + else + target_split = cNonParallelSplitIdx; + Split &split = splits.mSplits[target_split]; + split_remap_table[s] = target_split; + + // Allocate space for contacts + split.mContactBufferBegin = offset; + split.mContactBufferEnd = split.mContactBufferBegin + num_contacts_in_split[s]; + + // Allocate space for constraints + split.mConstraintBufferBegin = split.mContactBufferEnd; + split.mConstraintBufferEnd = split.mConstraintBufferBegin + num_constraints_in_split[s]; + + // Store start for each split + contact_buffer_cur[target_split] = mContactAndConstraintIndices + split.mContactBufferBegin; + constraint_buffer_cur[target_split] = mContactAndConstraintIndices + split.mConstraintBufferBegin; + + // Update offset + offset = split.mConstraintBufferEnd; + } + } + + // Split the contacts + for (uint c = 0; c < num_contacts_in_island; ++c) + { + uint split = split_remap_table[contact_split_idx[c]]; + *contact_buffer_cur[split]++ = contacts_start[c]; + } + + // Split the constraints + for (uint c = 0; c < num_constraints_in_island; ++c) + { + uint split = split_remap_table[constraint_split_idx[c]]; + *constraint_buffer_cur[split]++ = constraints_start[c]; + } + +#ifdef JPH_LARGE_ISLAND_SPLITTER_DEBUG + // Trace the size of all splits + uint sum = 0; + String stats; + for (uint s = 0; s < cNumSplits; ++s) + { + // If we've processed all splits, jump to the non-parallel split + if (s >= splits.GetNumSplits()) + s = cNonParallelSplitIdx; + + const Split &split = splits.mSplits[s]; + stats += StringFormat("g:%d:%d:%d, ", s, split.GetNumContacts(), split.GetNumConstraints()); + sum += split.GetNumItems(); + } + stats += StringFormat("sum: %d", sum); + Trace(stats.c_str()); +#endif // JPH_LARGE_ISLAND_SPLITTER_DEBUG + +#ifdef JPH_ENABLE_ASSERTS + for (uint s = 0; s < cNumSplits; ++s) + { + // If there are no more splits, process the non-parallel split + if (s >= splits.mNumSplits) + s = cNonParallelSplitIdx; + + // Check that we wrote all elements + Split &split = splits.mSplits[s]; + JPH_ASSERT(contact_buffer_cur[s] == mContactAndConstraintIndices + split.mContactBufferEnd); + JPH_ASSERT(constraint_buffer_cur[s] == mContactAndConstraintIndices + split.mConstraintBufferEnd); + } + +#ifdef JPH_DEBUG + // Validate that the splits are indeed not touching the same body + for (uint s = 0; s < splits.mNumSplits; ++s) + { + Array body_used(mNumActiveBodies, false); + + // Validate contacts + uint32 split_contacts_begin, split_contacts_end; + splits.GetContactsInSplit(s, split_contacts_begin, split_contacts_end); + for (uint32 *c = mContactAndConstraintIndices + split_contacts_begin; c < mContactAndConstraintIndices + split_contacts_end; ++c) + { + const Body *body1, *body2; + inContactManager.GetAffectedBodies(*c, body1, body2); + + uint32 idx1 = body1->GetIndexInActiveBodiesInternal(); + if (idx1 != Body::cInactiveIndex && body1->IsDynamic()) + { + JPH_ASSERT(!body_used[idx1]); + body_used[idx1] = true; + } + + uint32 idx2 = body2->GetIndexInActiveBodiesInternal(); + if (idx2 != Body::cInactiveIndex && body2->IsDynamic()) + { + JPH_ASSERT(!body_used[idx2]); + body_used[idx2] = true; + } + } + } +#endif // JPH_DEBUG +#endif // JPH_ENABLE_ASSERTS + + // Allow other threads to pick up this split island now + splits.StartFirstBatch(); + return true; +} + +LargeIslandSplitter::EStatus LargeIslandSplitter::FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration) +{ + // We can't be done when all islands haven't been submitted yet + uint num_splits_created = mNextSplitIsland.load(memory_order_acquire); + bool all_done = num_splits_created == mNumSplitIslands; + + // Loop over all split islands to find work + uint32 constraints_begin, constraints_end, contacts_begin, contacts_end; + for (Splits *s = mSplitIslands; s < mSplitIslands + num_splits_created; ++s) + switch (s->FetchNextBatch(constraints_begin, constraints_end, contacts_begin, contacts_end, outFirstIteration)) + { + case EStatus::AllBatchesDone: + break; + + case EStatus::WaitingForBatch: + all_done = false; + break; + + case EStatus::BatchRetrieved: + outSplitIslandIndex = uint(s - mSplitIslands); + outConstraintsBegin = mContactAndConstraintIndices + constraints_begin; + outConstraintsEnd = mContactAndConstraintIndices + constraints_end; + outContactsBegin = mContactAndConstraintIndices + contacts_begin; + outContactsEnd = mContactAndConstraintIndices + contacts_end; + return EStatus::BatchRetrieved; + } + + return all_done? EStatus::AllBatchesDone : EStatus::WaitingForBatch; +} + +void LargeIslandSplitter::MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch) +{ + uint num_items_processed = uint(inConstraintsEnd - inConstraintsBegin) + uint(inContactsEnd - inContactsBegin); + + JPH_ASSERT(inSplitIslandIndex < mNextSplitIsland.load(memory_order_relaxed)); + Splits &splits = mSplitIslands[inSplitIslandIndex]; + splits.MarkBatchProcessed(num_items_processed, outLastIteration, outFinalBatch); +} + +void LargeIslandSplitter::PrepareForSolvePositions() +{ + for (Splits *s = mSplitIslands, *s_end = mSplitIslands + mNumSplitIslands; s < s_end; ++s) + { + // Set the number of iterations to the number of position steps + s->mNumIterations = s->mNumPositionSteps; + + // We can start again from the first batch + s->StartFirstBatch(); + } +} + +void LargeIslandSplitter::Reset(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Everything should have been used + JPH_ASSERT(mContactAndConstraintsNextFree.load(memory_order_relaxed) == mContactAndConstraintsSize); + JPH_ASSERT(mNextSplitIsland.load(memory_order_relaxed) == mNumSplitIslands); + + // Free split islands + if (mNumSplitIslands > 0) + { + inTempAllocator->Free(mSplitIslands, mNumSplitIslands * sizeof(Splits)); + mSplitIslands = nullptr; + + mNumSplitIslands = 0; + mNextSplitIsland.store(0, memory_order_relaxed); + } + + // Free contact and constraint buffers + if (mContactAndConstraintsSize > 0) + { + inTempAllocator->Free(mContactAndConstraintIndices, mContactAndConstraintsSize * sizeof(uint32)); + mContactAndConstraintIndices = nullptr; + + inTempAllocator->Free(mContactAndConstraintsSplitIdx, mContactAndConstraintsSize * sizeof(uint32)); + mContactAndConstraintsSplitIdx = nullptr; + + mContactAndConstraintsSize = 0; + mContactAndConstraintsNextFree.store(0, memory_order_relaxed); + } + + // Free split masks + if (mSplitMasks != nullptr) + { + inTempAllocator->Free(mSplitMasks, mNumActiveBodies * sizeof(SplitMask)); + mSplitMasks = nullptr; + + mNumActiveBodies = 0; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h new file mode 100644 index 000000000000..36f0d6140126 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h @@ -0,0 +1,185 @@ +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class BodyID; +class IslandBuilder; +class TempAllocator; +class Constraint; +class BodyManager; +class ContactConstraintManager; +class CalculateSolverSteps; + +/// Assigns bodies in large islands to multiple groups that can run in parallel +/// +/// This basically implements what is described in: High-Performance Physical Simulations on Next-Generation Architecture with Many Cores by Chen et al. +/// See: http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf section "PARALLELIZATION METHODOLOGY" +class LargeIslandSplitter : public NonCopyable +{ +private: + using SplitMask = uint32; + +public: + static constexpr uint cNumSplits = sizeof(SplitMask) * 8; + static constexpr uint cNonParallelSplitIdx = cNumSplits - 1; + static constexpr uint cLargeIslandTreshold = 128; ///< If the number of constraints + contacts in an island is larger than this, we will try to split the island + + /// Status code for retrieving a batch + enum class EStatus + { + WaitingForBatch, ///< Work is expected to be available later + BatchRetrieved, ///< Work is being returned + AllBatchesDone, ///< No further work is expected from this + }; + + /// Describes a split of constraints and contacts + struct Split + { + inline uint GetNumContacts() const { return mContactBufferEnd - mContactBufferBegin; } + inline uint GetNumConstraints() const { return mConstraintBufferEnd - mConstraintBufferBegin; } + inline uint GetNumItems() const { return GetNumContacts() + GetNumConstraints(); } + + uint32 mContactBufferBegin; ///< Begin of the contact buffer (offset relative to mContactAndConstraintIndices) + uint32 mContactBufferEnd; ///< End of the contact buffer + + uint32 mConstraintBufferBegin; ///< Begin of the constraint buffer (offset relative to mContactAndConstraintIndices) + uint32 mConstraintBufferEnd; ///< End of the constraint buffer + }; + + /// Structure that describes the resulting splits from the large island splitter + class Splits + { + public: + inline uint GetNumSplits() const + { + return mNumSplits; + } + + inline void GetConstraintsInSplit(uint inSplitIndex, uint32 &outConstraintsBegin, uint32 &outConstraintsEnd) const + { + const Split &split = mSplits[inSplitIndex]; + outConstraintsBegin = split.mConstraintBufferBegin; + outConstraintsEnd = split.mConstraintBufferEnd; + } + + inline void GetContactsInSplit(uint inSplitIndex, uint32 &outContactsBegin, uint32 &outContactsEnd) const + { + const Split &split = mSplits[inSplitIndex]; + outContactsBegin = split.mContactBufferBegin; + outContactsEnd = split.mContactBufferEnd; + } + + /// Reset current status so that no work can be picked up from this split + inline void ResetStatus() + { + mStatus.store(StatusItemMask, memory_order_relaxed); + } + + /// Make the first batch available to other threads + inline void StartFirstBatch() + { + uint split_index = mNumSplits > 0? 0 : cNonParallelSplitIdx; + mStatus.store(uint64(split_index) << StatusSplitShift, memory_order_release); + } + + /// Fetch the next batch to process + EStatus FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration); + + /// Mark a batch as processed + void MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch); + + enum EIterationStatus : uint64 + { + StatusIterationMask = 0xffff000000000000, + StatusIterationShift = 48, + StatusSplitMask = 0x0000ffff00000000, + StatusSplitShift = 32, + StatusItemMask = 0x00000000ffffffff, + }; + + static inline int sGetIteration(uint64 inStatus) + { + return int((inStatus & StatusIterationMask) >> StatusIterationShift); + } + + static inline uint sGetSplit(uint64 inStatus) + { + return uint((inStatus & StatusSplitMask) >> StatusSplitShift); + } + + static inline uint sGetItem(uint64 inStatus) + { + return uint(inStatus & StatusItemMask); + } + + Split mSplits[cNumSplits]; ///< Data per split + uint32 mIslandIndex; ///< Index of the island that was split + uint mNumSplits; ///< Number of splits that were created (excluding the non-parallel split) + int mNumIterations; ///< Number of iterations to do + int mNumVelocitySteps; ///< Number of velocity steps to do (cached for 2nd sub step) + int mNumPositionSteps; ///< Number of position steps to do + atomic mStatus; ///< Status of the split, see EIterationStatus + atomic mItemsProcessed; ///< Number of items that have been marked as processed + }; + +public: + /// Destructor + ~LargeIslandSplitter(); + + /// Prepare the island splitter by allocating memory + void Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator); + + /// Assign two bodies to a split. Returns the split index. + uint AssignSplit(const Body *inBody1, const Body *inBody2); + + /// Force a body to be in a non parallel split. Returns the split index. + uint AssignToNonParallelSplit(const Body *inBody); + + /// Splits up an island, the created splits will be added to the list of batches and can be fetched with FetchNextBatch. Returns false if the island did not need splitting. + bool SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator); + + /// Fetch the next batch to process, returns a handle in outSplitIslandIndex that must be provided to MarkBatchProcessed when complete + EStatus FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration); + + /// Mark a batch as processed + void MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch); + + /// Get the island index of the island that was split for a particular split island index + inline uint32 GetIslandIndex(uint inSplitIslandIndex) const + { + JPH_ASSERT(inSplitIslandIndex < mNumSplitIslands); + return mSplitIslands[inSplitIslandIndex].mIslandIndex; + } + + /// Prepare the island splitter for iterating over the split islands again for position solving. Marks all batches as startable. + void PrepareForSolvePositions(); + + /// Reset the island splitter + void Reset(TempAllocator *inTempAllocator); + +private: + static constexpr uint cSplitCombineTreshold = 32; ///< If the number of constraints + contacts in a split is lower than this, we will merge this split into the 'non-parallel split' + static constexpr uint cBatchSize = 16; ///< Number of items to process in a constraint batch + + uint32 mNumActiveBodies = 0; ///< Cached number of active bodies + + SplitMask * mSplitMasks = nullptr; ///< Bits that indicate for each body in the BodyManager::mActiveBodies list which split they already belong to + + uint32 * mContactAndConstraintsSplitIdx = nullptr; ///< Buffer to store the split index per constraint or contact + uint32 * mContactAndConstraintIndices = nullptr; ///< Buffer to store the ordered constraint indices per split + uint mContactAndConstraintsSize = 0; ///< Total size of mContactAndConstraintsSplitIdx and mContactAndConstraintIndices + atomic mContactAndConstraintsNextFree { 0 }; ///< Next element that is free in both buffers + + uint mNumSplitIslands = 0; ///< Total number of islands that required splitting + Splits * mSplitIslands = nullptr; ///< List of islands that required splitting + atomic mNextSplitIsland = 0; ///< Next split island to pick from mSplitIslands +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h new file mode 100644 index 000000000000..1e83d1881011 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_ENABLE_ASSERTS + +/// This is the list of locks used by the physics engine, they need to be locked in a particular order (from top of the list to bottom of the list) in order to prevent deadlocks +enum class EPhysicsLockTypes +{ + BroadPhaseQuery = 1 << 0, + PerBody = 1 << 1, + BodiesList = 1 << 2, + BroadPhaseUpdate = 1 << 3, + ConstraintsList = 1 << 4, + ActiveBodiesList = 1 << 5, +}; + +/// A token that indicates the context of a lock (we use 1 per physics system and we use the body manager pointer because it's convenient) +class BodyManager; +using PhysicsLockContext = const BodyManager *; + +#endif // !JPH_ENABLE_ASSERTS + +/// Helpers to safely lock the different mutexes that are part of the physics system while preventing deadlock +/// Class that keeps track per thread which lock are taken and if the order of locking is correct +class JPH_EXPORT PhysicsLock +{ +public: +#ifdef JPH_ENABLE_ASSERTS + /// Call before taking the lock + static inline void sCheckLock(PhysicsLockContext inContext, EPhysicsLockTypes inType) + { + uint32 &mutexes = sGetLockedMutexes(inContext); + JPH_ASSERT(uint32(inType) > mutexes, "A lock of same or higher priority was already taken, this can create a deadlock!"); + mutexes = mutexes | uint32(inType); + } + + /// Call after releasing the lock + static inline void sCheckUnlock(PhysicsLockContext inContext, EPhysicsLockTypes inType) + { + uint32 &mutexes = sGetLockedMutexes(inContext); + JPH_ASSERT((mutexes & uint32(inType)) != 0, "Mutex was not locked!"); + mutexes = mutexes & ~uint32(inType); + } +#endif // !JPH_ENABLE_ASSERTS + + template + static inline void sLock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);) + inMutex.lock(); + } + + template + static inline void sUnlock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);) + inMutex.unlock(); + } + + template + static inline void sLockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);) + inMutex.lock_shared(); + } + + template + static inline void sUnlockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);) + inMutex.unlock_shared(); + } + +#ifdef JPH_ENABLE_ASSERTS +private: + struct LockData + { + uint32 mLockedMutexes = 0; + PhysicsLockContext mContext = nullptr; + }; + + // Helper function to find the locked mutexes for a particular context + static uint32 & sGetLockedMutexes(PhysicsLockContext inContext) + { + static thread_local LockData sLocks[4]; + + // If we find a matching context we can use it + for (LockData &l : sLocks) + if (l.mContext == inContext) + return l.mLockedMutexes; + + // Otherwise we look for an entry that is not in use + for (LockData &l : sLocks) + if (l.mLockedMutexes == 0) + { + l.mContext = inContext; + return l.mLockedMutexes; + } + + JPH_ASSERT(false, "Too many physics systems locked at the same time!"); + return sLocks[0].mLockedMutexes; + } +#endif // !JPH_ENABLE_ASSERTS +}; + +/// Helper class that is similar to std::unique_lock +template +class UniqueLock : public NonCopyable +{ +public: + explicit UniqueLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) : + mLock(inLock) +#ifdef JPH_ENABLE_ASSERTS + , mContext(inContext), + mType(inType) +#endif // JPH_ENABLE_ASSERTS + { + PhysicsLock::sLock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + + ~UniqueLock() + { + PhysicsLock::sUnlock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + +private: + LockType & mLock; +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mContext; + EPhysicsLockTypes mType; +#endif // JPH_ENABLE_ASSERTS +}; + +/// Helper class that is similar to std::shared_lock +template +class SharedLock : public NonCopyable +{ +public: + explicit SharedLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) : + mLock(inLock) +#ifdef JPH_ENABLE_ASSERTS + , mContext(inContext) + , mType(inType) +#endif // JPH_ENABLE_ASSERTS + { + PhysicsLock::sLockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + + ~SharedLock() + { + PhysicsLock::sUnlockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + +private: + LockType & mLock; +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mContext; + EPhysicsLockTypes mType; +#endif // JPH_ENABLE_ASSERTS +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp new file mode 100644 index 000000000000..1e9c60d230ef --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp @@ -0,0 +1,261 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene) +{ + JPH_ADD_ATTRIBUTE(PhysicsScene, mBodies) + JPH_ADD_ATTRIBUTE(PhysicsScene, mConstraints) + JPH_ADD_ATTRIBUTE(PhysicsScene, mSoftBodies) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene::ConnectedConstraint) +{ + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mSettings) + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody1) + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody2) +} + +void PhysicsScene::AddBody(const BodyCreationSettings &inBody) +{ + mBodies.push_back(inBody); +} + +void PhysicsScene::AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2) +{ + mConstraints.emplace_back(inConstraint, inBody1, inBody2); +} + +void PhysicsScene::AddSoftBody(const SoftBodyCreationSettings &inSoftBody) +{ + mSoftBodies.push_back(inSoftBody); +} + +bool PhysicsScene::FixInvalidScales() +{ + const Vec3 unit_scale = Vec3::sReplicate(1.0f); + + bool success = true; + for (BodyCreationSettings &b : mBodies) + { + // Test if there is an invalid scale in the shape hierarchy + const Shape *shape = b.GetShape(); + if (!shape->IsValidScale(unit_scale)) + { + // Fix it up + Shape::ShapeResult result = shape->ScaleShape(unit_scale); + if (result.IsValid()) + b.SetShape(result.Get()); + else + success = false; + } + } + return success; +} + +bool PhysicsScene::CreateBodies(PhysicsSystem *inSystem) const +{ + BodyInterface &bi = inSystem->GetBodyInterface(); + + BodyIDVector body_ids; + body_ids.reserve(mBodies.size() + mSoftBodies.size()); + + // Create bodies + for (const BodyCreationSettings &b : mBodies) + { + const Body *body = bi.CreateBody(b); + if (body == nullptr) + break; + body_ids.push_back(body->GetID()); + } + + // Create soft bodies + for (const SoftBodyCreationSettings &b : mSoftBodies) + { + const Body *body = bi.CreateSoftBody(b); + if (body == nullptr) + break; + body_ids.push_back(body->GetID()); + } + + // Batch add bodies + BodyIDVector temp_body_ids = body_ids; // Body ID's get shuffled by AddBodiesPrepare + BodyInterface::AddState add_state = bi.AddBodiesPrepare(temp_body_ids.data(), (int)temp_body_ids.size()); + bi.AddBodiesFinalize(temp_body_ids.data(), (int)temp_body_ids.size(), add_state, EActivation::Activate); + + // If not all bodies are created, creating constraints will be unreliable + if (body_ids.size() != mBodies.size() + mSoftBodies.size()) + return false; + + // Create constraints + for (const ConnectedConstraint &cc : mConstraints) + { + BodyID body1_id = cc.mBody1 == cFixedToWorld? BodyID() : body_ids[cc.mBody1]; + BodyID body2_id = cc.mBody2 == cFixedToWorld? BodyID() : body_ids[cc.mBody2]; + Constraint *c = bi.CreateConstraint(cc.mSettings, body1_id, body2_id); + inSystem->AddConstraint(c); + } + + // Everything was created + return true; +} + +void PhysicsScene::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const +{ + BodyCreationSettings::ShapeToIDMap shape_to_id; + BodyCreationSettings::MaterialToIDMap material_to_id; + BodyCreationSettings::GroupFilterToIDMap group_filter_to_id; + SoftBodyCreationSettings::SharedSettingsToIDMap settings_to_id; + + // Save bodies + inStream.Write((uint32)mBodies.size()); + for (const BodyCreationSettings &b : mBodies) + b.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr); + + // Save constraints + inStream.Write((uint32)mConstraints.size()); + for (const ConnectedConstraint &cc : mConstraints) + { + cc.mSettings->SaveBinaryState(inStream); + inStream.Write(cc.mBody1); + inStream.Write(cc.mBody2); + } + + // Save soft bodies + inStream.Write((uint32)mSoftBodies.size()); + for (const SoftBodyCreationSettings &b : mSoftBodies) + b.SaveWithChildren(inStream, &settings_to_id, &material_to_id, inSaveGroupFilter? &group_filter_to_id : nullptr); +} + +PhysicsScene::PhysicsSceneResult PhysicsScene::sRestoreFromBinaryState(StreamIn &inStream) +{ + PhysicsSceneResult result; + + // Create scene + Ref scene = new PhysicsScene(); + + BodyCreationSettings::IDToShapeMap id_to_shape; + BodyCreationSettings::IDToMaterialMap id_to_material; + BodyCreationSettings::IDToGroupFilterMap id_to_group_filter; + SoftBodyCreationSettings::IDToSharedSettingsMap id_to_settings; + + // Reserve some memory to avoid frequent reallocations + id_to_shape.reserve(1024); + id_to_material.reserve(128); + id_to_group_filter.reserve(128); + + // Read bodies + uint32 len = 0; + inStream.Read(len); + scene->mBodies.resize(len); + for (BodyCreationSettings &b : scene->mBodies) + { + // Read creation settings + BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter); + if (bcs_result.HasError()) + { + result.SetError(bcs_result.GetError()); + return result; + } + b = bcs_result.Get(); + } + + // Read constraints + len = 0; + inStream.Read(len); + scene->mConstraints.resize(len); + for (ConnectedConstraint &cc : scene->mConstraints) + { + ConstraintSettings::ConstraintResult c_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (c_result.HasError()) + { + result.SetError(c_result.GetError()); + return result; + } + cc.mSettings = StaticCast(c_result.Get()); + inStream.Read(cc.mBody1); + inStream.Read(cc.mBody2); + } + + // Read soft bodies + len = 0; + inStream.Read(len); + scene->mSoftBodies.resize(len); + for (SoftBodyCreationSettings &b : scene->mSoftBodies) + { + // Read creation settings + SoftBodyCreationSettings::SBCSResult sbcs_result = SoftBodyCreationSettings::sRestoreWithChildren(inStream, id_to_settings, id_to_material, id_to_group_filter); + if (sbcs_result.HasError()) + { + result.SetError(sbcs_result.GetError()); + return result; + } + b = sbcs_result.Get(); + } + + result.Set(scene); + return result; +} + +void PhysicsScene::FromPhysicsSystem(const PhysicsSystem *inSystem) +{ + // This map will track where each body went in mBodies + using BodyIDToIdxMap = UnorderedMap; + BodyIDToIdxMap body_id_to_idx; + + // Map invalid ID + body_id_to_idx[BodyID()] = cFixedToWorld; + + // Get all bodies + BodyIDVector body_ids; + inSystem->GetBodies(body_ids); + + // Loop over all bodies + const BodyLockInterface &bli = inSystem->GetBodyLockInterface(); + for (const BodyID &id : body_ids) + { + BodyLockRead lock(bli, id); + if (lock.Succeeded()) + { + // Store location of body + body_id_to_idx[id] = (uint32)mBodies.size(); + + const Body &body = lock.GetBody(); + + // Convert to body creation settings + if (body.IsRigidBody()) + AddBody(body.GetBodyCreationSettings()); + else + AddSoftBody(body.GetSoftBodyCreationSettings()); + } + } + + // Loop over all constraints + Constraints constraints = inSystem->GetConstraints(); + for (const Constraint *c : constraints) + if (c->GetType() == EConstraintType::TwoBodyConstraint) + { + // Cast to two body constraint + const TwoBodyConstraint *tbc = static_cast(c); + + // Find the body indices + BodyIDToIdxMap::const_iterator b1 = body_id_to_idx.find(tbc->GetBody1()->GetID()); + BodyIDToIdxMap::const_iterator b2 = body_id_to_idx.find(tbc->GetBody2()->GetID()); + JPH_ASSERT(b1 != body_id_to_idx.end() && b2 != body_id_to_idx.end()); + + // Create constraint settings and add the constraint + Ref settings = c->GetConstraintSettings(); + AddConstraint(StaticCast(settings), b1->second, b2->second); + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h new file mode 100644 index 000000000000..f974f83bd585 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h @@ -0,0 +1,104 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// Contains the creation settings of a set of bodies +class JPH_EXPORT PhysicsScene : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, PhysicsScene) + +public: + /// Add a body to the scene + void AddBody(const BodyCreationSettings &inBody); + + /// Body constant to use to indicate that the constraint is attached to the fixed world + static constexpr uint32 cFixedToWorld = 0xffffffff; + + /// Add a constraint to the scene + /// @param inConstraint Constraint settings + /// @param inBody1 Index in the bodies list of first body to attach constraint to + /// @param inBody2 Index in the bodies list of the second body to attach constraint to + void AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2); + + /// Add a soft body to the scene + void AddSoftBody(const SoftBodyCreationSettings &inSoftBody); + + /// Get number of bodies in this scene + size_t GetNumBodies() const { return mBodies.size(); } + + /// Access to the body settings for this scene + const Array & GetBodies() const { return mBodies; } + Array & GetBodies() { return mBodies; } + + /// A constraint and how it is connected to the bodies in the scene + class ConnectedConstraint + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, ConnectedConstraint) + + public: + ConnectedConstraint() = default; + ConnectedConstraint(const TwoBodyConstraintSettings *inSettings, uint inBody1, uint inBody2) : mSettings(inSettings), mBody1(inBody1), mBody2(inBody2) { } + + RefConst mSettings; ///< Constraint settings + uint32 mBody1; ///< Index of first body (in mBodies) + uint32 mBody2; ///< Index of second body (in mBodies) + }; + + /// Get number of constraints in this scene + size_t GetNumConstraints() const { return mConstraints.size(); } + + /// Access to the constraints for this scene + const Array & GetConstraints() const { return mConstraints; } + Array & GetConstraints() { return mConstraints; } + + /// Get number of bodies in this scene + size_t GetNumSoftBodies() const { return mSoftBodies.size(); } + + /// Access to the soft body settings for this scene + const Array & GetSoftBodies() const { return mSoftBodies; } + Array & GetSoftBodies() { return mSoftBodies; } + + /// Instantiate all bodies, returns false if not all bodies could be created + bool CreateBodies(PhysicsSystem *inSystem) const; + + /// Go through all body creation settings and fix shapes that are scaled incorrectly (note this will change the scene a bit). + /// @return False when not all scales could be fixed. + bool FixInvalidScales(); + + /// Saves the state of this object in binary form to inStream. + /// @param inStream The stream to save the state to + /// @param inSaveShapes If the shapes should be saved as well (these could be shared between physics scenes, in which case the calling application may want to write custom code to restore them) + /// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared) + void SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const; + + using PhysicsSceneResult = Result>; + + /// Restore a saved scene from inStream + static PhysicsSceneResult sRestoreFromBinaryState(StreamIn &inStream); + + /// For debugging purposes: Construct a scene from the current state of the physics system + void FromPhysicsSystem(const PhysicsSystem *inSystem); + +private: + /// The bodies that are part of this scene + Array mBodies; + + /// Constraints that are part of this scene + Array mConstraints; + + /// Soft bodies that are part of this scene + Array mSoftBodies; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h new file mode 100644 index 000000000000..eb8ecbbeaf81 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h @@ -0,0 +1,119 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter) +constexpr float cDefaultCollisionTolerance = 1.0e-4f; + +/// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless) +constexpr float cDefaultPenetrationTolerance = 1.0e-4f; ///< Stop when there's less than 1% change + +/// How much padding to add around objects +constexpr float cDefaultConvexRadius = 0.05f; + +/// Used by (Tapered)CapsuleShape to determine when supporting face is an edge rather than a point (unit: meter) +static constexpr float cCapsuleProjectionSlop = 0.02f; + +/// Maximum amount of jobs to allow +constexpr int cMaxPhysicsJobs = 2048; + +/// Maximum amount of barriers to allow +constexpr int cMaxPhysicsBarriers = 8; + +struct PhysicsSettings +{ + JPH_OVERRIDE_NEW_DELETE + + /// Size of body pairs array, corresponds to the maximum amount of potential body pairs that can be in flight at any time. + /// Setting this to a low value will use less memory but slow down simulation as threads may run out of narrow phase work. + int mMaxInFlightBodyPairs = 16384; + + /// How many PhysicsStepListeners to notify in 1 batch + int mStepListenersBatchSize = 8; + + /// How many step listener batches are needed before spawning another job (set to INT_MAX if no parallelism is desired) + int mStepListenerBatchesPerJob = 1; + + /// Baumgarte stabilization factor (how much of the position error to 'fix' in 1 update) (unit: dimensionless, 0 = nothing, 1 = 100%) + float mBaumgarte = 0.2f; + + /// Radius around objects inside which speculative contact points will be detected. Note that if this is too big + /// you will get ghost collisions as speculative contacts are based on the closest points during the collision detection + /// step which may not be the actual closest points by the time the two objects hit (unit: meters) + float mSpeculativeContactDistance = 0.02f; + + /// How much bodies are allowed to sink into each other (unit: meters) + float mPenetrationSlop = 0.02f; + + /// Fraction of its inner radius a body must move per step to enable casting for the LinearCast motion quality + float mLinearCastThreshold = 0.75f; + + /// Fraction of its inner radius a body may penetrate another body for the LinearCast motion quality + float mLinearCastMaxPenetration = 0.25f; + + /// Max squared distance to use to determine if two points are on the same plane for determining the contact manifold between two shape faces (unit: meter^2) + float mManifoldToleranceSq = 1.0e-6f; + + /// Maximum distance to correct in a single iteration when solving position constraints (unit: meters) + float mMaxPenetrationDistance = 0.2f; + + /// Maximum relative delta position for body pairs to be able to reuse collision results from last frame (units: meter^2) + float mBodyPairCacheMaxDeltaPositionSq = Square(0.001f); ///< 1 mm + + /// Maximum relative delta orientation for body pairs to be able to reuse collision results from last frame, stored as cos(max angle / 2) + float mBodyPairCacheCosMaxDeltaRotationDiv2 = 0.99984769515639123915701155881391f; ///< cos(2 degrees / 2) + + /// Maximum angle between normals that allows manifolds between different sub shapes of the same body pair to be combined + float mContactNormalCosMaxDeltaRotation = 0.99619469809174553229501040247389f; ///< cos(5 degree) + + /// Maximum allowed distance between old and new contact point to preserve contact forces for warm start (units: meter^2) + float mContactPointPreserveLambdaMaxDistSq = Square(0.01f); ///< 1 cm + + /// Number of solver velocity iterations to run + /// Note that this needs to be >= 2 in order for friction to work (friction is applied using the non-penetration impulse from the previous iteration) + uint mNumVelocitySteps = 10; + + /// Number of solver position iterations to run + uint mNumPositionSteps = 2; + + /// Minimal velocity needed before a collision can be elastic (unit: m) + float mMinVelocityForRestitution = 1.0f; + + /// Time before object is allowed to go to sleep (unit: seconds) + float mTimeBeforeSleep = 0.5f; + + /// Velocity of points on bounding box of object below which an object can be considered sleeping (unit: m/s) + float mPointVelocitySleepThreshold = 0.03f; + + /// By default the simulation is deterministic, it is possible to turn this off by setting this setting to false. This will make the simulation run faster but it will no longer be deterministic. + bool mDeterministicSimulation = true; + + ///@name These variables are mainly for debugging purposes, they allow turning on/off certain subsystems. You probably want to leave them alone. + ///@{ + + /// Whether or not to use warm starting for constraints (initially applying previous frames impulses) + bool mConstraintWarmStart = true; + + /// Whether or not to use the body pair cache, which removes the need for narrow phase collision detection when orientation between two bodies didn't change + bool mUseBodyPairContactCache = true; + + /// Whether or not to reduce manifolds with similar contact normals into one contact manifold (see description at Body::SetUseManifoldReduction) + bool mUseManifoldReduction = true; + + /// If we split up large islands into smaller parallel batches of work (to improve performance) + bool mUseLargeIslandSplitter = true; + + /// If objects can go to sleep or not + bool mAllowSleeping = true; + + /// When false, we prevent collision against non-active (shared) edges. Mainly for debugging the algorithm. + bool mCheckActiveEdges = true; + + ///@} +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h new file mode 100644 index 000000000000..26c8eec34aa4 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// Context information for the step listener +class JPH_EXPORT PhysicsStepListenerContext +{ +public: + float mDeltaTime; ///< Delta time of the current step + bool mIsFirstStep; ///< True if this is the first step + bool mIsLastStep; ///< True if this is the last step + PhysicsSystem * mPhysicsSystem; ///< The physics system that is being stepped +}; + +/// A listener class that receives a callback before every physics simulation step +class JPH_EXPORT PhysicsStepListener +{ +public: + /// Ensure virtual destructor + virtual ~PhysicsStepListener() = default; + + /// Called before every simulation step (received inCollisionSteps times for every PhysicsSystem::Update(...) call) + /// This is called while all body and constraint mutexes are locked. You can read/write bodies and constraints but not add/remove them. + /// Multiple listeners can be executed in parallel and it is the responsibility of the listener to avoid race conditions. + /// The best way to do this is to have each step listener operate on a subset of the bodies and constraints + /// and making sure that these bodies and constraints are not touched by any other step listener. + /// Note that this function is not called if there aren't any active bodies or when the physics system is updated with 0 delta time. + virtual void OnStep(const PhysicsStepListenerContext &inContext) = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp new file mode 100644 index 000000000000..3ea6beabfab6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp @@ -0,0 +1,2754 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool PhysicsSystem::sDrawMotionQualityLinearCast = false; +#endif // JPH_DEBUG_RENDERER + +//#define BROAD_PHASE BroadPhaseBruteForce +#define BROAD_PHASE BroadPhaseQuadTree + +static const Color cColorUpdateBroadPhaseFinalize = Color::sGetDistinctColor(1); +static const Color cColorUpdateBroadPhasePrepare = Color::sGetDistinctColor(2); +static const Color cColorFindCollisions = Color::sGetDistinctColor(3); +static const Color cColorApplyGravity = Color::sGetDistinctColor(4); +static const Color cColorSetupVelocityConstraints = Color::sGetDistinctColor(5); +static const Color cColorBuildIslandsFromConstraints = Color::sGetDistinctColor(6); +static const Color cColorDetermineActiveConstraints = Color::sGetDistinctColor(7); +static const Color cColorFinalizeIslands = Color::sGetDistinctColor(8); +static const Color cColorContactRemovedCallbacks = Color::sGetDistinctColor(9); +static const Color cColorBodySetIslandIndex = Color::sGetDistinctColor(10); +static const Color cColorStartNextStep = Color::sGetDistinctColor(11); +static const Color cColorSolveVelocityConstraints = Color::sGetDistinctColor(12); +static const Color cColorPreIntegrateVelocity = Color::sGetDistinctColor(13); +static const Color cColorIntegrateVelocity = Color::sGetDistinctColor(14); +static const Color cColorPostIntegrateVelocity = Color::sGetDistinctColor(15); +static const Color cColorResolveCCDContacts = Color::sGetDistinctColor(16); +static const Color cColorSolvePositionConstraints = Color::sGetDistinctColor(17); +static const Color cColorFindCCDContacts = Color::sGetDistinctColor(18); +static const Color cColorStepListeners = Color::sGetDistinctColor(19); +static const Color cColorSoftBodyPrepare = Color::sGetDistinctColor(20); +static const Color cColorSoftBodyCollide = Color::sGetDistinctColor(21); +static const Color cColorSoftBodySimulate = Color::sGetDistinctColor(22); +static const Color cColorSoftBodyFinalize = Color::sGetDistinctColor(23); + +PhysicsSystem::~PhysicsSystem() +{ + // Remove broadphase + delete mBroadPhase; +} + +void PhysicsSystem::Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter) +{ + JPH_ASSERT(inMaxBodies <= BodyID::cMaxBodyIndex, "Cannot support this many bodies"); + + mObjectVsBroadPhaseLayerFilter = &inObjectVsBroadPhaseLayerFilter; + mObjectLayerPairFilter = &inObjectLayerPairFilter; + + // Initialize body manager + mBodyManager.Init(inMaxBodies, inNumBodyMutexes, inBroadPhaseLayerInterface); + + // Create broadphase + mBroadPhase = new BROAD_PHASE(); + mBroadPhase->Init(&mBodyManager, inBroadPhaseLayerInterface); + + // Init contact constraint manager + mContactManager.Init(inMaxBodyPairs, inMaxContactConstraints); + + // Init islands builder + mIslandBuilder.Init(inMaxBodies); + + // Initialize body interface + mBodyInterfaceLocking.Init(mBodyLockInterfaceLocking, mBodyManager, *mBroadPhase); + mBodyInterfaceNoLock.Init(mBodyLockInterfaceNoLock, mBodyManager, *mBroadPhase); + + // Initialize narrow phase query + mNarrowPhaseQueryLocking.Init(mBodyLockInterfaceLocking, *mBroadPhase); + mNarrowPhaseQueryNoLock.Init(mBodyLockInterfaceNoLock, *mBroadPhase); +} + +void PhysicsSystem::OptimizeBroadPhase() +{ + mBroadPhase->Optimize(); +} + +void PhysicsSystem::AddStepListener(PhysicsStepListener *inListener) +{ + lock_guard lock(mStepListenersMutex); + + JPH_ASSERT(std::find(mStepListeners.begin(), mStepListeners.end(), inListener) == mStepListeners.end()); + mStepListeners.push_back(inListener); +} + +void PhysicsSystem::RemoveStepListener(PhysicsStepListener *inListener) +{ + lock_guard lock(mStepListenersMutex); + + StepListeners::iterator i = std::find(mStepListeners.begin(), mStepListeners.end(), inListener); + JPH_ASSERT(i != mStepListeners.end()); + *i = mStepListeners.back(); + mStepListeners.pop_back(); +} + +EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem) +{ + JPH_PROFILE_FUNCTION(); + + JPH_DET_LOG("PhysicsSystem::Update: dt: " << inDeltaTime << " steps: " << inCollisionSteps); + + JPH_ASSERT(inCollisionSteps > 0); + JPH_ASSERT(inDeltaTime >= 0.0f); + + // Sync point for the broadphase. This will allow it to do clean up operations without having any mutexes locked yet. + mBroadPhase->FrameSync(); + + // If there are no active bodies or there's no time delta + uint32 num_active_rigid_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + uint32 num_active_soft_bodies = mBodyManager.GetNumActiveBodies(EBodyType::SoftBody); + if ((num_active_rigid_bodies == 0 && num_active_soft_bodies == 0) || inDeltaTime <= 0.0f) + { + mBodyManager.LockAllBodies(); + + // Update broadphase + mBroadPhase->LockModifications(); + BroadPhase::UpdateState update_state = mBroadPhase->UpdatePrepare(); + mBroadPhase->UpdateFinalize(update_state); + mBroadPhase->UnlockModifications(); + + // Call contact removal callbacks from contacts that existed in the previous update + mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0); + + mBodyManager.UnlockAllBodies(); + return EPhysicsUpdateError::None; + } + + // Calculate ratio between current and previous frame delta time to scale initial constraint forces + float step_delta_time = inDeltaTime / inCollisionSteps; + float warm_start_impulse_ratio = mPhysicsSettings.mConstraintWarmStart && mPreviousStepDeltaTime > 0.0f? step_delta_time / mPreviousStepDeltaTime : 0.0f; + mPreviousStepDeltaTime = step_delta_time; + + // Create the context used for passing information between jobs + PhysicsUpdateContext context(*inTempAllocator); + context.mPhysicsSystem = this; + context.mJobSystem = inJobSystem; + context.mBarrier = inJobSystem->CreateBarrier(); + context.mIslandBuilder = &mIslandBuilder; + context.mStepDeltaTime = step_delta_time; + context.mWarmStartImpulseRatio = warm_start_impulse_ratio; + context.mSteps.resize(inCollisionSteps); + + // Allocate space for body pairs + JPH_ASSERT(context.mBodyPairs == nullptr); + context.mBodyPairs = static_cast(inTempAllocator->Allocate(sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs)); + + // Lock all bodies for write so that we can freely touch them + mStepListenersMutex.lock(); + mBodyManager.LockAllBodies(); + mBroadPhase->LockModifications(); + + // Get max number of concurrent jobs + int max_concurrency = context.GetMaxConcurrency(); + + // Calculate how many step listener jobs we spawn + int num_step_listener_jobs = mStepListeners.empty()? 0 : max(1, min((int)mStepListeners.size() / mPhysicsSettings.mStepListenersBatchSize / mPhysicsSettings.mStepListenerBatchesPerJob, max_concurrency)); + + // Number of gravity jobs depends on the amount of active bodies. + // Launch max 1 job per batch of active bodies + // Leave 1 thread for update broadphase prepare and 1 for determine active constraints + int num_apply_gravity_jobs = max(1, min(((int)num_active_rigid_bodies + cApplyGravityBatchSize - 1) / cApplyGravityBatchSize, max_concurrency - 2)); + + // Number of determine active constraints jobs to run depends on number of constraints. + // Leave 1 thread for update broadphase prepare and 1 for apply gravity + int num_determine_active_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cDetermineActiveConstraintsBatchSize - 1) / cDetermineActiveConstraintsBatchSize, max_concurrency - 2)); + + // Number of setup velocity constraints jobs to run depends on number of constraints. + int num_setup_velocity_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cSetupVelocityConstraintsBatchSize - 1) / cSetupVelocityConstraintsBatchSize, max_concurrency)); + + // Number of find collisions jobs to run depends on number of active bodies. + // Note that when we have more than 1 thread, we always spawn at least 2 find collisions jobs so that the first job can wait for build islands from constraints + // (which may activate additional bodies that need to be processed) while the second job can start processing collision work. + int num_find_collisions_jobs = max(max_concurrency == 1? 1 : 2, min(((int)num_active_rigid_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_concurrency)); + + // Number of integrate velocity jobs depends on number of active bodies. + int num_integrate_velocity_jobs = max(1, min(((int)num_active_rigid_bodies + cIntegrateVelocityBatchSize - 1) / cIntegrateVelocityBatchSize, max_concurrency)); + + { + JPH_PROFILE("Build Jobs"); + + // Iterate over collision steps + for (int step_idx = 0; step_idx < inCollisionSteps; ++step_idx) + { + bool is_first_step = step_idx == 0; + bool is_last_step = step_idx == inCollisionSteps - 1; + + PhysicsUpdateContext::Step &step = context.mSteps[step_idx]; + step.mContext = &context; + step.mIsFirst = is_first_step; + step.mIsLast = is_last_step; + + // Create job to do broadphase finalization + // This job must finish before integrating velocities. Until then the positions will not be updated neither will bodies be added / removed. + step.mUpdateBroadphaseFinalize = inJobSystem->CreateJob("UpdateBroadPhaseFinalize", cColorUpdateBroadPhaseFinalize, [&context, &step]() + { + // Validate that all find collision jobs have stopped + JPH_ASSERT(step.mActiveFindCollisionJobs.load(memory_order_relaxed) == 0); + + // Finalize the broadphase update + context.mPhysicsSystem->mBroadPhase->UpdateFinalize(step.mBroadPhaseUpdateState); + + // Signal that it is done + step.mPreIntegrateVelocity.RemoveDependency(); + }, num_find_collisions_jobs + 2); // depends on: find collisions, broadphase prepare update, finish building jobs + + // The immediate jobs below are only immediate for the first step, the all finished job will kick them for the next step + int previous_step_dependency_count = is_first_step? 0 : 1; + + // Start job immediately: Start the prepare broadphase + // Must be done under body lock protection since the order is body locks then broadphase mutex + // If this is turned around the RemoveBody call will hang since it locks in that order + step.mBroadPhasePrepare = inJobSystem->CreateJob("UpdateBroadPhasePrepare", cColorUpdateBroadPhasePrepare, [&context, &step]() + { + // Prepare the broadphase update + step.mBroadPhaseUpdateState = context.mPhysicsSystem->mBroadPhase->UpdatePrepare(); + + // Now the finalize can run (if other dependencies are met too) + step.mUpdateBroadphaseFinalize.RemoveDependency(); + }, previous_step_dependency_count); + + // This job will find all collisions + step.mBodyPairQueues.resize(max_concurrency); + step.mMaxBodyPairsPerQueue = mPhysicsSettings.mMaxInFlightBodyPairs / max_concurrency; + step.mActiveFindCollisionJobs.store(~PhysicsUpdateContext::JobMask(0) >> (sizeof(PhysicsUpdateContext::JobMask) * 8 - num_find_collisions_jobs), memory_order_release); + step.mFindCollisions.resize(num_find_collisions_jobs); + for (int i = 0; i < num_find_collisions_jobs; ++i) + { + // Build islands from constraints may activate additional bodies, so the first job will wait for this to finish in order to not miss any active bodies + int num_dep_build_islands_from_constraints = i == 0? 1 : 0; + step.mFindCollisions[i] = inJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [&step, i]() + { + step.mContext->mPhysicsSystem->JobFindCollisions(&step, i); + }, num_apply_gravity_jobs + num_determine_active_constraints_jobs + 1 + num_dep_build_islands_from_constraints); // depends on: apply gravity, determine active constraints, finish building jobs, build islands from constraints + } + + if (is_first_step) + { + #ifdef JPH_ENABLE_ASSERTS + // Don't allow write operations to the active bodies list + mBodyManager.SetActiveBodiesLocked(true); + #endif + + // Store the number of active bodies at the start of the step + step.mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + + // Lock all constraints + mConstraintManager.LockAllConstraints(); + + // Allocate memory for storing the active constraints + JPH_ASSERT(context.mActiveConstraints == nullptr); + context.mActiveConstraints = static_cast(inTempAllocator->Allocate(mConstraintManager.GetNumConstraints() * sizeof(Constraint *))); + + // Prepare contact buffer + mContactManager.PrepareConstraintBuffer(&context); + + // Setup island builder + mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), context.mTempAllocator); + } + + // This job applies gravity to all active bodies + step.mApplyGravity.resize(num_apply_gravity_jobs); + for (int i = 0; i < num_apply_gravity_jobs; ++i) + step.mApplyGravity[i] = inJobSystem->CreateJob("ApplyGravity", cColorApplyGravity, [&context, &step]() + { + context.mPhysicsSystem->JobApplyGravity(&context, &step); + + JobHandle::sRemoveDependencies(step.mFindCollisions); + }, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners) + + // This job will setup velocity constraints for non-collision constraints + step.mSetupVelocityConstraints.resize(num_setup_velocity_constraints_jobs); + for (int i = 0; i < num_setup_velocity_constraints_jobs; ++i) + step.mSetupVelocityConstraints[i] = inJobSystem->CreateJob("SetupVelocityConstraints", cColorSetupVelocityConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSetupVelocityConstraints(context.mStepDeltaTime, &step); + + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + }, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs + + // This job will build islands from constraints + step.mBuildIslandsFromConstraints = inJobSystem->CreateJob("BuildIslandsFromConstraints", cColorBuildIslandsFromConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobBuildIslandsFromConstraints(&context, &step); + + step.mFindCollisions[0].RemoveDependency(); // The first collisions job cannot start running until we've finished building islands and activated all bodies + step.mFinalizeIslands.RemoveDependency(); + }, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs + + // This job determines active constraints + step.mDetermineActiveConstraints.resize(num_determine_active_constraints_jobs); + for (int i = 0; i < num_determine_active_constraints_jobs; ++i) + step.mDetermineActiveConstraints[i] = inJobSystem->CreateJob("DetermineActiveConstraints", cColorDetermineActiveConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobDetermineActiveConstraints(&step); + + step.mBuildIslandsFromConstraints.RemoveDependency(); + + // Kick these jobs last as they will use up all CPU cores leaving no space for the previous job, we prefer setup velocity constraints to finish first so we kick it first + JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints); + JobHandle::sRemoveDependencies(step.mFindCollisions); + }, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners) + + // This job calls the step listeners + step.mStepListeners.resize(num_step_listener_jobs); + for (int i = 0; i < num_step_listener_jobs; ++i) + step.mStepListeners[i] = inJobSystem->CreateJob("StepListeners", cColorStepListeners, [&context, &step]() + { + // Call the step listeners + context.mPhysicsSystem->JobStepListeners(&step); + + // Kick apply gravity and determine active constraint jobs + JobHandle::sRemoveDependencies(step.mApplyGravity); + JobHandle::sRemoveDependencies(step.mDetermineActiveConstraints); + }, previous_step_dependency_count); + + // Unblock the previous step + if (!is_first_step) + context.mSteps[step_idx - 1].mStartNextStep.RemoveDependency(); + + // This job will finalize the simulation islands + step.mFinalizeIslands = inJobSystem->CreateJob("FinalizeIslands", cColorFinalizeIslands, [&context, &step]() + { + // Validate that all find collision jobs have stopped + JPH_ASSERT(step.mActiveFindCollisionJobs.load(memory_order_relaxed) == 0); + + context.mPhysicsSystem->JobFinalizeIslands(&context); + + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + step.mBodySetIslandIndex.RemoveDependency(); + }, num_find_collisions_jobs + 2); // depends on: find collisions, build islands from constraints, finish building jobs + + // Unblock previous job + // Note: technically we could release find collisions here but we don't want to because that could make them run before 'setup velocity constraints' which means that job won't have a thread left + step.mBuildIslandsFromConstraints.RemoveDependency(); + + // This job will call the contact removed callbacks + step.mContactRemovedCallbacks = inJobSystem->CreateJob("ContactRemovedCallbacks", cColorContactRemovedCallbacks, [&context, &step]() + { + context.mPhysicsSystem->JobContactRemovedCallbacks(&step); + + if (step.mStartNextStep.IsValid()) + step.mStartNextStep.RemoveDependency(); + }, 1); // depends on the find ccd contacts + + // This job will set the island index on each body (only used for debug drawing purposes) + // It will also delete any bodies that have been destroyed in the last frame + step.mBodySetIslandIndex = inJobSystem->CreateJob("BodySetIslandIndex", cColorBodySetIslandIndex, [&context, &step]() + { + context.mPhysicsSystem->JobBodySetIslandIndex(); + + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + }, 2); // depends on: finalize islands, finish building jobs + + // Job to start the next collision step + if (!is_last_step) + { + PhysicsUpdateContext::Step *next_step = &context.mSteps[step_idx + 1]; + step.mStartNextStep = inJobSystem->CreateJob("StartNextStep", cColorStartNextStep, [this, next_step]() + { + #ifdef JPH_DEBUG + // Validate that the cached bounds are correct + mBodyManager.ValidateActiveBodyBounds(); + #endif // JPH_DEBUG + + // Store the number of active bodies at the start of the step + next_step->mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + + // Clear the large island splitter + TempAllocator *temp_allocator = next_step->mContext->mTempAllocator; + mLargeIslandSplitter.Reset(temp_allocator); + + // Clear the island builder + mIslandBuilder.ResetIslands(temp_allocator); + + // Setup island builder + mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), temp_allocator); + + // Restart the contact manager + mContactManager.RecycleConstraintBuffer(); + + // Kick the jobs of the next step (in the same order as the first step) + next_step->mBroadPhasePrepare.RemoveDependency(); + if (next_step->mStepListeners.empty()) + { + // Kick the gravity and active constraints jobs immediately + JobHandle::sRemoveDependencies(next_step->mApplyGravity); + JobHandle::sRemoveDependencies(next_step->mDetermineActiveConstraints); + } + else + { + // Kick the step listeners job first + JobHandle::sRemoveDependencies(next_step->mStepListeners); + } + }, 3); // depends on: update soft bodies, contact removed callbacks, finish building the previous step + } + + // This job will solve the velocity constraints + step.mSolveVelocityConstraints.resize(max_concurrency); + for (int i = 0; i < max_concurrency; ++i) + step.mSolveVelocityConstraints[i] = inJobSystem->CreateJob("SolveVelocityConstraints", cColorSolveVelocityConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSolveVelocityConstraints(&context, &step); + + step.mPreIntegrateVelocity.RemoveDependency(); + }, num_setup_velocity_constraints_jobs + 2); // depends on: finalize islands, setup velocity constraints, finish building jobs. + + // We prefer setup velocity constraints to finish first so we kick it first + JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints); + JobHandle::sRemoveDependencies(step.mFindCollisions); + + // Finalize islands is a dependency on find collisions so it can go last + step.mFinalizeIslands.RemoveDependency(); + + // This job will prepare the position update of all active bodies + step.mPreIntegrateVelocity = inJobSystem->CreateJob("PreIntegrateVelocity", cColorPreIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobPreIntegrateVelocity(&context, &step); + + JobHandle::sRemoveDependencies(step.mIntegrateVelocity); + }, 2 + max_concurrency); // depends on: broadphase update finalize, solve velocity constraints, finish building jobs. + + // Unblock previous jobs + step.mUpdateBroadphaseFinalize.RemoveDependency(); + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + + // This job will update the positions of all active bodies + step.mIntegrateVelocity.resize(num_integrate_velocity_jobs); + for (int i = 0; i < num_integrate_velocity_jobs; ++i) + step.mIntegrateVelocity[i] = inJobSystem->CreateJob("IntegrateVelocity", cColorIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobIntegrateVelocity(&context, &step); + + step.mPostIntegrateVelocity.RemoveDependency(); + }, 2); // depends on: pre integrate velocity, finish building jobs. + + // Unblock previous job + step.mPreIntegrateVelocity.RemoveDependency(); + + // This job will finish the position update of all active bodies + step.mPostIntegrateVelocity = inJobSystem->CreateJob("PostIntegrateVelocity", cColorPostIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobPostIntegrateVelocity(&context, &step); + + step.mResolveCCDContacts.RemoveDependency(); + }, num_integrate_velocity_jobs + 1); // depends on: integrate velocity, finish building jobs + + // Unblock previous jobs + JobHandle::sRemoveDependencies(step.mIntegrateVelocity); + + // This job will update the positions and velocities for all bodies that need continuous collision detection + step.mResolveCCDContacts = inJobSystem->CreateJob("ResolveCCDContacts", cColorResolveCCDContacts, [&context, &step]() + { + context.mPhysicsSystem->JobResolveCCDContacts(&context, &step); + + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + }, 2); // depends on: integrate velocities, detect ccd contacts (added dynamically), finish building jobs. + + // Unblock previous job + step.mPostIntegrateVelocity.RemoveDependency(); + + // Fixes up drift in positions and updates the broadphase with new body positions + step.mSolvePositionConstraints.resize(max_concurrency); + for (int i = 0; i < max_concurrency; ++i) + step.mSolvePositionConstraints[i] = inJobSystem->CreateJob("SolvePositionConstraints", cColorSolvePositionConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSolvePositionConstraints(&context, &step); + + // Kick the next step + if (step.mSoftBodyPrepare.IsValid()) + step.mSoftBodyPrepare.RemoveDependency(); + }, 3); // depends on: resolve ccd contacts, body set island index, finish building jobs. + + // Unblock previous jobs. + step.mResolveCCDContacts.RemoveDependency(); + step.mBodySetIslandIndex.RemoveDependency(); + + // The soft body prepare job will create other jobs if needed + step.mSoftBodyPrepare = inJobSystem->CreateJob("SoftBodyPrepare", cColorSoftBodyPrepare, [&context, &step]() + { + context.mPhysicsSystem->JobSoftBodyPrepare(&context, &step); + }, max_concurrency); // depends on: solve position constraints. + + // Unblock previous jobs + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + } + } + + // Build the list of jobs to wait for + JobSystem::Barrier *barrier = context.mBarrier; + { + JPH_PROFILE("Build job barrier"); + + StaticArray handles; + for (const PhysicsUpdateContext::Step &step : context.mSteps) + { + if (step.mBroadPhasePrepare.IsValid()) + handles.push_back(step.mBroadPhasePrepare); + for (const JobHandle &h : step.mStepListeners) + handles.push_back(h); + for (const JobHandle &h : step.mDetermineActiveConstraints) + handles.push_back(h); + for (const JobHandle &h : step.mApplyGravity) + handles.push_back(h); + for (const JobHandle &h : step.mFindCollisions) + handles.push_back(h); + if (step.mUpdateBroadphaseFinalize.IsValid()) + handles.push_back(step.mUpdateBroadphaseFinalize); + for (const JobHandle &h : step.mSetupVelocityConstraints) + handles.push_back(h); + handles.push_back(step.mBuildIslandsFromConstraints); + handles.push_back(step.mFinalizeIslands); + handles.push_back(step.mBodySetIslandIndex); + for (const JobHandle &h : step.mSolveVelocityConstraints) + handles.push_back(h); + handles.push_back(step.mPreIntegrateVelocity); + for (const JobHandle &h : step.mIntegrateVelocity) + handles.push_back(h); + handles.push_back(step.mPostIntegrateVelocity); + handles.push_back(step.mResolveCCDContacts); + for (const JobHandle &h : step.mSolvePositionConstraints) + handles.push_back(h); + handles.push_back(step.mContactRemovedCallbacks); + if (step.mSoftBodyPrepare.IsValid()) + handles.push_back(step.mSoftBodyPrepare); + if (step.mStartNextStep.IsValid()) + handles.push_back(step.mStartNextStep); + } + barrier->AddJobs(handles.data(), handles.size()); + } + + // Wait until all jobs finish + // Note we don't just wait for the last job. If we would and another job + // would be scheduled in between there is the possibility of a deadlock. + // The other job could try to e.g. add/remove a body which would try to + // lock a body mutex while this thread has already locked the mutex + inJobSystem->WaitForJobs(barrier); + + // We're done with the barrier for this update + inJobSystem->DestroyBarrier(barrier); + +#ifdef JPH_DEBUG + // Validate that the cached bounds are correct + mBodyManager.ValidateActiveBodyBounds(); +#endif // JPH_DEBUG + + // Clear the large island splitter + mLargeIslandSplitter.Reset(inTempAllocator); + + // Clear the island builder + mIslandBuilder.ResetIslands(inTempAllocator); + + // Clear the contact manager + mContactManager.FinishConstraintBuffer(); + + // Free active constraints + inTempAllocator->Free(context.mActiveConstraints, mConstraintManager.GetNumConstraints() * sizeof(Constraint *)); + context.mActiveConstraints = nullptr; + + // Free body pairs + inTempAllocator->Free(context.mBodyPairs, sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs); + context.mBodyPairs = nullptr; + + // Unlock the broadphase + mBroadPhase->UnlockModifications(); + + // Unlock all constraints + mConstraintManager.UnlockAllConstraints(); + +#ifdef JPH_ENABLE_ASSERTS + // Allow write operations to the active bodies list + mBodyManager.SetActiveBodiesLocked(false); +#endif + + // Unlock all bodies + mBodyManager.UnlockAllBodies(); + + // Unlock step listeners + mStepListenersMutex.unlock(); + + // Return any errors + EPhysicsUpdateError errors = static_cast(context.mErrors.load(memory_order_acquire)); + JPH_ASSERT(errors == EPhysicsUpdateError::None, "An error occurred during the physics update, see EPhysicsUpdateError for more information"); + return errors; +} + +void PhysicsSystem::JobStepListeners(PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // Read positions (broadphase updates concurrently so we can't write), read/write velocities + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); + + // Can activate bodies only (we cache the amount of active bodies at the beginning of the step in mNumActiveBodiesAtStepStart so we cannot deactivate here) + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + PhysicsStepListenerContext context; + context.mDeltaTime = ioStep->mContext->mStepDeltaTime; + context.mIsFirstStep = ioStep->mIsFirst; + context.mIsLastStep = ioStep->mIsLast; + context.mPhysicsSystem = this; + + uint32 batch_size = mPhysicsSettings.mStepListenersBatchSize; + for (;;) + { + // Get the start of a new batch + uint32 batch = ioStep->mStepListenerReadIdx.fetch_add(batch_size); + if (batch >= mStepListeners.size()) + break; + + // Call the listeners + for (uint32 i = batch, i_end = min((uint32)mStepListeners.size(), batch + batch_size); i < i_end; ++i) + mStepListeners[i]->OnStep(context); + } +} + +void PhysicsSystem::JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const +{ +#ifdef JPH_ENABLE_ASSERTS + // No body access + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + uint32 num_constraints = mConstraintManager.GetNumConstraints(); + uint32 num_active_constraints; + Constraint **active_constraints = (Constraint **)JPH_STACK_ALLOC(cDetermineActiveConstraintsBatchSize * sizeof(Constraint *)); + + for (;;) + { + // Atomically fetch a batch of constraints + uint32 constraint_idx = ioStep->mDetermineActiveConstraintReadIdx.fetch_add(cDetermineActiveConstraintsBatchSize); + if (constraint_idx >= num_constraints) + break; + + // Calculate the end of the batch + uint32 constraint_idx_end = min(num_constraints, constraint_idx + cDetermineActiveConstraintsBatchSize); + + // Store the active constraints at the start of the step (bodies get activated during the step which in turn may activate constraints leading to an inconsistent shapshot) + mConstraintManager.GetActiveConstraints(constraint_idx, constraint_idx_end, active_constraints, num_active_constraints); + + // Copy the block of active constraints to the global list of active constraints + if (num_active_constraints > 0) + { + uint32 active_constraint_idx = ioStep->mNumActiveConstraints.fetch_add(num_active_constraints); + memcpy(ioStep->mContext->mActiveConstraints + active_constraint_idx, active_constraints, num_active_constraints * sizeof(Constraint *)); + } + } +} + +void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update velocities and need the rotation to do so + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + // Get list of active bodies that we had at the start of the physics update. + // Any body that is activated as part of the simulation step does not receive gravity this frame. + // Note that bodies may be activated during this job but not deactivated, this means that only elements + // will be added to the array. Since the array is made to not reallocate, this is a safe operation. + const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody); + uint32 num_active_bodies_at_step_start = ioStep->mNumActiveBodiesAtStepStart; + + // Fetch delta time once outside the loop + float delta_time = ioContext->mStepDeltaTime; + + // Update velocities from forces + for (;;) + { + // Atomically fetch a batch of bodies + uint32 active_body_idx = ioStep->mApplyGravityReadIdx.fetch_add(cApplyGravityBatchSize); + if (active_body_idx >= num_active_bodies_at_step_start) + break; + + // Calculate the end of the batch + uint32 active_body_idx_end = min(num_active_bodies_at_step_start, active_body_idx + cApplyGravityBatchSize); + + // Process the batch + while (active_body_idx < active_body_idx_end) + { + Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]); + if (body.IsDynamic()) + { + MotionProperties *mp = body.GetMotionProperties(); + Quat rotation = body.GetRotation(); + + if (body.GetApplyGyroscopicForce()) + mp->ApplyGyroscopicForceInternal(rotation, delta_time); + + mp->ApplyForceTorqueAndDragInternal(rotation, mGravity, delta_time); + } + active_body_idx++; + } + } +} + +void PhysicsSystem::JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); +#endif + + uint32 num_constraints = ioStep->mNumActiveConstraints; + + for (;;) + { + // Atomically fetch a batch of constraints + uint32 constraint_idx = ioStep->mSetupVelocityConstraintsReadIdx.fetch_add(cSetupVelocityConstraintsBatchSize); + if (constraint_idx >= num_constraints) + break; + + ConstraintManager::sSetupVelocityConstraints(ioStep->mContext->mActiveConstraints + constraint_idx, min(cSetupVelocityConstraintsBatchSize, num_constraints - constraint_idx), inDeltaTime); + } +} + +void PhysicsSystem::JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We read constraints and positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); + + // Can only activate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + // Prepare the island builder + mIslandBuilder.PrepareNonContactConstraints(ioStep->mNumActiveConstraints, ioContext->mTempAllocator); + + // Build the islands + ConstraintManager::sBuildIslands(ioStep->mContext->mActiveConstraints, ioStep->mNumActiveConstraints, mIslandBuilder, mBodyManager); +} + +void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const +{ + // Get how many jobs we can spawn and check if we can spawn more + uint max_jobs = ioStep->mBodyPairQueues.size(); + if (CountBits(ioStep->mActiveFindCollisionJobs.load(memory_order_relaxed)) >= max_jobs) + return; + + // Count how many body pairs we have waiting + uint32 num_body_pairs = 0; + for (const PhysicsUpdateContext::BodyPairQueue &queue : ioStep->mBodyPairQueues) + num_body_pairs += queue.mWriteIdx - queue.mReadIdx; + + // Count how many active bodies we have waiting + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody) - ioStep->mActiveBodyReadIdx; + + // Calculate how many jobs we would like + uint desired_num_jobs = min((num_body_pairs + cNarrowPhaseBatchSize - 1) / cNarrowPhaseBatchSize + (num_active_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_jobs); + + for (;;) + { + // Get the bit mask of active jobs and see if we can spawn more + PhysicsUpdateContext::JobMask current_active_jobs = ioStep->mActiveFindCollisionJobs.load(memory_order_relaxed); + uint job_index = CountTrailingZeros(~current_active_jobs); + if (job_index >= desired_num_jobs) + break; + + // Try to claim the job index + PhysicsUpdateContext::JobMask job_mask = PhysicsUpdateContext::JobMask(1) << job_index; + PhysicsUpdateContext::JobMask prev_value = ioStep->mActiveFindCollisionJobs.fetch_or(job_mask, memory_order_acquire); + if ((prev_value & job_mask) == 0) + { + // Add dependencies from the find collisions job to the next jobs + ioStep->mUpdateBroadphaseFinalize.AddDependency(); + ioStep->mFinalizeIslands.AddDependency(); + + // Start the job + JobHandle job = ioStep->mContext->mJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [step = ioStep, job_index]() + { + step->mContext->mPhysicsSystem->JobFindCollisions(step, job_index); + }); + + // Add the job to the job barrier so the main updating thread can execute the job too + ioStep->mContext->mBarrier->AddJob(job); + + // Spawn only 1 extra job at a time + return; + } + } +} + +static void sFinalizeContactAllocator(PhysicsUpdateContext::Step &ioStep, const ContactConstraintManager::ContactAllocator &inAllocator) +{ + // Atomically accumulate the number of found manifolds and body pairs + ioStep.mNumBodyPairs.fetch_add(inAllocator.mNumBodyPairs, memory_order_relaxed); + ioStep.mNumManifolds.fetch_add(inAllocator.mNumManifolds, memory_order_relaxed); + + // Combine update errors + ioStep.mContext->mErrors.fetch_or((uint32)inAllocator.mErrors, memory_order_relaxed); +} + +// Disable TSAN for this function. It detects a false positive race condition on mBodyPairs. +// We have written mBodyPairs before doing mWriteIdx++ and we check mWriteIdx before reading mBodyPairs, so this should be safe. +JPH_TSAN_NO_SANITIZE +void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex) +{ +#ifdef JPH_ENABLE_ASSERTS + // We read positions and read velocities (for elastic collisions) + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); + + // Can only activate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + // Allocation context for allocating new contact points + ContactAllocator contact_allocator(mContactManager.GetContactAllocator()); + + // Determine initial queue to read pairs from if no broadphase work can be done + // (always start looking at results from the next job) + int read_queue_idx = (inJobIndex + 1) % ioStep->mBodyPairQueues.size(); + + for (;;) + { + // Check if there are active bodies to be processed + uint32 active_bodies_read_idx = ioStep->mActiveBodyReadIdx; + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + if (active_bodies_read_idx < num_active_bodies) + { + // Take a batch of active bodies + uint32 active_bodies_read_idx_end = min(num_active_bodies, active_bodies_read_idx + cActiveBodiesBatchSize); + if (ioStep->mActiveBodyReadIdx.compare_exchange_strong(active_bodies_read_idx, active_bodies_read_idx_end)) + { + // Callback when a new body pair is found + class MyBodyPairCallback : public BodyPairCollector + { + public: + // Constructor + MyBodyPairCallback(PhysicsUpdateContext::Step *inStep, ContactAllocator &ioContactAllocator, int inJobIndex) : + mStep(inStep), + mContactAllocator(ioContactAllocator), + mJobIndex(inJobIndex) + { + } + + // Callback function when a body pair is found + virtual void AddHit(const BodyPair &inPair) override + { + // Check if we have space in our write queue + PhysicsUpdateContext::BodyPairQueue &queue = mStep->mBodyPairQueues[mJobIndex]; + uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx; + if (body_pairs_in_queue >= mStep->mMaxBodyPairsPerQueue) + { + // Buffer full, process the pair now + mStep->mContext->mPhysicsSystem->ProcessBodyPair(mContactAllocator, inPair); + } + else + { + // Store the pair in our own queue + mStep->mContext->mBodyPairs[mJobIndex * mStep->mMaxBodyPairsPerQueue + queue.mWriteIdx % mStep->mMaxBodyPairsPerQueue] = inPair; + ++queue.mWriteIdx; + } + } + + private: + PhysicsUpdateContext::Step * mStep; + ContactAllocator & mContactAllocator; + int mJobIndex; + }; + MyBodyPairCallback add_pair(ioStep, contact_allocator, inJobIndex); + + // Copy active bodies to temporary array, broadphase will reorder them + uint32 batch_size = active_bodies_read_idx_end - active_bodies_read_idx; + BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(batch_size * sizeof(BodyID)); + memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody) + active_bodies_read_idx, batch_size * sizeof(BodyID)); + + // Find pairs in the broadphase + mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, *mObjectVsBroadPhaseLayerFilter, *mObjectLayerPairFilter, add_pair); + + // Check if we have enough pairs in the buffer to start a new job + const PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[inJobIndex]; + uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx; + if (body_pairs_in_queue >= cNarrowPhaseBatchSize) + TrySpawnJobFindCollisions(ioStep); + } + } + else + { + // Lockless loop to get the next body pair from the pairs buffer + const PhysicsUpdateContext *context = ioStep->mContext; + int first_read_queue_idx = read_queue_idx; + for (;;) + { + PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[read_queue_idx]; + + // Get the next pair to process + uint32 pair_idx = queue.mReadIdx; + + // If the pair hasn't been written yet + if (pair_idx >= queue.mWriteIdx) + { + // Go to the next queue + read_queue_idx = (read_queue_idx + 1) % ioStep->mBodyPairQueues.size(); + + // If we're back at the first queue, we've looked at all of them and found nothing + if (read_queue_idx == first_read_queue_idx) + { + // Collect information from the contact allocator and accumulate it in the step. + sFinalizeContactAllocator(*ioStep, contact_allocator); + + // Mark this job as inactive + ioStep->mActiveFindCollisionJobs.fetch_and(~PhysicsUpdateContext::JobMask(1 << inJobIndex), memory_order_release); + + // Trigger the next jobs + ioStep->mUpdateBroadphaseFinalize.RemoveDependency(); + ioStep->mFinalizeIslands.RemoveDependency(); + return; + } + + // Try again reading from the next queue + continue; + } + + // Copy the body pair out of the buffer + const BodyPair bp = context->mBodyPairs[read_queue_idx * ioStep->mMaxBodyPairsPerQueue + pair_idx % ioStep->mMaxBodyPairsPerQueue]; + + // Mark this pair as taken + if (queue.mReadIdx.compare_exchange_strong(pair_idx, pair_idx + 1)) + { + // Process the actual body pair + ProcessBodyPair(contact_allocator, bp); + break; + } + } + } + } +} + +void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair) +{ + JPH_PROFILE_FUNCTION(); + + // Fetch body pair + Body *body1 = &mBodyManager.GetBody(inBodyPair.mBodyA); + Body *body2 = &mBodyManager.GetBody(inBodyPair.mBodyB); + JPH_ASSERT(body1->IsActive()); + + JPH_DET_LOG("ProcessBodyPair: id1: " << inBodyPair.mBodyA << " id2: " << inBodyPair.mBodyB << " p1: " << body1->GetCenterOfMassPosition() << " p2: " << body2->GetCenterOfMassPosition() << " r1: " << body1->GetRotation() << " r2: " << body2->GetRotation()); + + // Check for soft bodies + if (body2->IsSoftBody()) + { + // If the 2nd body is a soft body and not active, we activate it now + if (!body2->IsActive()) + mBodyManager.ActivateBodies(&inBodyPair.mBodyB, 1); + + // Soft body processing is done later in the pipeline + return; + } + + // Ensure that body1 has the higher motion type (i.e. dynamic trumps kinematic), this ensures that we do the collision detection in the space of a moving body, + // which avoids accuracy problems when testing a very large static object against a small dynamic object + // Ensure that body1 id < body2 id when motion types are the same. + if (body1->GetMotionType() < body2->GetMotionType() + || (body1->GetMotionType() == body2->GetMotionType() && inBodyPair.mBodyB < inBodyPair.mBodyA)) + std::swap(body1, body2); + + // Check if the contact points from the previous frame are reusable and if so copy them + bool pair_handled = false, constraint_created = false; + if (mPhysicsSettings.mUseBodyPairContactCache && !(body1->IsCollisionCacheInvalid() || body2->IsCollisionCacheInvalid())) + mContactManager.GetContactsFromCache(ioContactAllocator, *body1, *body2, pair_handled, constraint_created); + + // If the cache hasn't handled this body pair do actual collision detection + if (!pair_handled) + { + // Create entry in the cache for this body pair + // Needs to happen irrespective if we found a collision or not (we want to remember that no collision was found too) + ContactConstraintManager::BodyPairHandle body_pair_handle = mContactManager.AddBodyPair(ioContactAllocator, *body1, *body2); + if (body_pair_handle == nullptr) + return; // Out of cache space + + // If we want enhanced active edge detection for this body pair + bool enhanced_active_edges = body1->GetEnhancedInternalEdgeRemovalWithBody(*body2); + + // Create the query settings + CollideShapeSettings settings; + settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges && !enhanced_active_edges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll; + settings.mMaxSeparationDistance = body1->IsSensor() || body2->IsSensor()? 0.0f : mPhysicsSettings.mSpeculativeContactDistance; + settings.mActiveEdgeMovementDirection = body1->GetLinearVelocity() - body2->GetLinearVelocity(); + + // Create shape filter + SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, body1); + SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper(); + shape_filter.SetBody2(body2); + + // Get transforms relative to body1 + RVec3 offset = body1->GetCenterOfMassPosition(); + Mat44 transform1 = Mat44::sRotation(body1->GetRotation()); + Mat44 transform2 = body2->GetCenterOfMassTransform().PostTranslated(-offset).ToMat44(); + + if (mPhysicsSettings.mUseManifoldReduction // Check global flag + && body1->GetUseManifoldReductionWithBody(*body2)) // Check body flag + { + // Version WITH contact manifold reduction + + class MyManifold : public ContactManifold + { + public: + Vec3 mFirstWorldSpaceNormal; + }; + + // A temporary structure that allows us to keep track of the all manifolds between this body pair + using Manifolds = StaticArray; + + // Create collector + class ReductionCollideShapeCollector : public CollideShapeCollector + { + public: + ReductionCollideShapeCollector(PhysicsSystem *inSystem, const Body *inBody1, const Body *inBody2) : + mSystem(inSystem), + mBody1(inBody1), + mBody2(inBody2) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // The first body should be the one with the highest motion type + JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType()); + JPH_ASSERT(!ShouldEarlyOut()); + + // Test if we want to accept this hit + if (mValidateBodyPair) + { + switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult)) + { + case ValidateResult::AcceptContact: + // We're just accepting this one, nothing to do + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept and stop calling the validate callback + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + // Skip this contact + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Skip this and early out + ForceEarlyOut(); + return; + } + } + + // Calculate normal + Vec3 world_space_normal = inResult.mPenetrationAxis.Normalized(); + + // Check if we can add it to an existing manifold + Manifolds::iterator manifold; + float contact_normal_cos_max_delta_rot = mSystem->mPhysicsSettings.mContactNormalCosMaxDeltaRotation; + for (manifold = mManifolds.begin(); manifold != mManifolds.end(); ++manifold) + if (world_space_normal.Dot(manifold->mFirstWorldSpaceNormal) >= contact_normal_cos_max_delta_rot) + { + // Update average normal + manifold->mWorldSpaceNormal += world_space_normal; + manifold->mPenetrationDepth = max(manifold->mPenetrationDepth, inResult.mPenetrationDepth); + break; + } + if (manifold == mManifolds.end()) + { + // Check if array is full + if (mManifolds.size() == mManifolds.capacity()) + { + // Full, find manifold with least amount of penetration + manifold = mManifolds.begin(); + for (Manifolds::iterator m = mManifolds.begin() + 1; m < mManifolds.end(); ++m) + if (m->mPenetrationDepth < manifold->mPenetrationDepth) + manifold = m; + + // If this contacts penetration is smaller than the smallest manifold, we skip this contact + if (inResult.mPenetrationDepth < manifold->mPenetrationDepth) + return; + + // Replace the manifold + *manifold = { { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal }; + } + else + { + // Not full, create new manifold + mManifolds.push_back({ { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal }); + manifold = mManifolds.end() - 1; + } + } + + // Determine contact points + const PhysicsSettings &settings = mSystem->mPhysicsSettings; + ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition())); + + // Prune if we have more than 32 points (this means we could run out of space in the next iteration) + if (manifold->mRelativeContactPointsOn1.size() > 32) + PruneContactPoints(manifold->mFirstWorldSpaceNormal, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold->mBaseOffset)); + } + + PhysicsSystem * mSystem; + const Body * mBody1; + const Body * mBody2; + bool mValidateBodyPair = true; + Manifolds mManifolds; + }; + ReductionCollideShapeCollector collector(this, body1, body2); + + // Perform collision detection between the two shapes + SubShapeIDCreator part1, part2; + auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape; + f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, shape_filter); + + // Add the contacts + for (ContactManifold &manifold : collector.mManifolds) + { + // Normalize the normal (is a sum of all normals from merged manifolds) + manifold.mWorldSpaceNormal = manifold.mWorldSpaceNormal.Normalized(); + + // If we still have too many points, prune them now + if (manifold.mRelativeContactPointsOn1.size() > 4) + PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Actually add the contact points to the manager + constraint_created |= mContactManager.AddContactConstraint(ioContactAllocator, body_pair_handle, *body1, *body2, manifold); + } + } + else + { + // Version WITHOUT contact manifold reduction + + // Create collector + class NonReductionCollideShapeCollector : public CollideShapeCollector + { + public: + NonReductionCollideShapeCollector(PhysicsSystem *inSystem, ContactAllocator &ioContactAllocator, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) : + mSystem(inSystem), + mContactAllocator(ioContactAllocator), + mBody1(inBody1), + mBody2(inBody2), + mBodyPairHandle(inPairHandle) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // The first body should be the one with the highest motion type + JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType()); + JPH_ASSERT(!ShouldEarlyOut()); + + // Test if we want to accept this hit + if (mValidateBodyPair) + { + switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult)) + { + case ValidateResult::AcceptContact: + // We're just accepting this one, nothing to do + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept and stop calling the validate callback + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + // Skip this contact + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Skip this and early out + ForceEarlyOut(); + return; + } + } + + // Determine contact points + ContactManifold manifold; + manifold.mBaseOffset = mBody1->GetCenterOfMassPosition(); + const PhysicsSettings &settings = mSystem->mPhysicsSettings; + ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Calculate normal + manifold.mWorldSpaceNormal = inResult.mPenetrationAxis.Normalized(); + + // Store penetration depth + manifold.mPenetrationDepth = inResult.mPenetrationDepth; + + // Prune if we have more than 4 points + if (manifold.mRelativeContactPointsOn1.size() > 4) + PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Set other properties + manifold.mSubShapeID1 = inResult.mSubShapeID1; + manifold.mSubShapeID2 = inResult.mSubShapeID2; + + // Actually add the contact points to the manager + mConstraintCreated |= mSystem->mContactManager.AddContactConstraint(mContactAllocator, mBodyPairHandle, *mBody1, *mBody2, manifold); + } + + PhysicsSystem * mSystem; + ContactAllocator & mContactAllocator; + Body * mBody1; + Body * mBody2; + ContactConstraintManager::BodyPairHandle mBodyPairHandle; + bool mValidateBodyPair = true; + bool mConstraintCreated = false; + }; + NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle); + + // Perform collision detection between the two shapes + SubShapeIDCreator part1, part2; + auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape; + f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, shape_filter); + + constraint_created = collector.mConstraintCreated; + } + } + + // If a contact constraint was created, we need to do some extra work + if (constraint_created) + { + // Wake up sleeping bodies + BodyID body_ids[2]; + int num_bodies = 0; + if (body1->IsDynamic() && !body1->IsActive()) + body_ids[num_bodies++] = body1->GetID(); + if (body2->IsDynamic() && !body2->IsActive()) + body_ids[num_bodies++] = body2->GetID(); + if (num_bodies > 0) + mBodyManager.ActivateBodies(body_ids, num_bodies); + + // Link the two bodies + mIslandBuilder.LinkBodies(body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal()); + } +} + +void PhysicsSystem::JobFinalizeIslands(PhysicsUpdateContext *ioContext) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only touch island data + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Finish collecting the islands, at this point the active body list doesn't change so it's safe to access + mIslandBuilder.Finalize(mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody), mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), mContactManager.GetNumConstraints(), ioContext->mTempAllocator); + + // Prepare the large island splitter + if (mPhysicsSettings.mUseLargeIslandSplitter) + mLargeIslandSplitter.Prepare(mIslandBuilder, mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), ioContext->mTempAllocator); +} + +void PhysicsSystem::JobBodySetIslandIndex() +{ +#ifdef JPH_ENABLE_ASSERTS + // We only touch island data + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Loop through the result and tag all bodies with an island index + for (uint32 island_idx = 0, n = mIslandBuilder.GetNumIslands(); island_idx < n; ++island_idx) + { + BodyID *body_start, *body_end; + mIslandBuilder.GetBodiesInIsland(island_idx, body_start, body_end); + for (const BodyID *body = body_start; body < body_end; ++body) + mBodyManager.GetBody(*body).GetMotionProperties()->SetIslandIndexInternal(island_idx); + } +} + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wundefined-func-template") // ConstraintManager::sWarmStartVelocityConstraints / ContactConstraintManager::WarmStartVelocityConstraints is instantiated in the cpp file + +void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update velocities and need to read positions to do so + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + float delta_time = ioContext->mStepDeltaTime; + Constraint **active_constraints = ioContext->mActiveConstraints; + + // Only the first step to correct for the delta time difference in the previous update + float warm_start_impulse_ratio = ioStep->mIsFirst? ioContext->mWarmStartImpulseRatio : 1.0f; + + bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter; + for (;;) + { + // First try to get work from large islands + if (check_split_islands) + { + bool first_iteration; + uint split_island_index; + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration)) + { + case LargeIslandSplitter::EStatus::BatchRetrieved: + { + if (first_iteration) + { + // Iteration 0 is used to warm start the batch (we added 1 to the number of iterations in LargeIslandSplitter::SplitIsland) + DummyCalculateSolverSteps dummy; + ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, dummy); + mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, dummy); + } + else + { + // Solve velocity constraints + ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time); + mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end); + } + + // Mark the batch as processed + bool last_iteration, final_batch; + mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch); + + // Save back the lambdas in the contact cache for the warm start of the next physics update + if (last_iteration) + mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end); + + // We processed work, loop again + continue; + } + + case LargeIslandSplitter::EStatus::WaitingForBatch: + break; + + case LargeIslandSplitter::EStatus::AllBatchesDone: + check_split_islands = false; + break; + } + } + + // If that didn't succeed try to process an island + if (check_islands) + { + // Next island + uint32 island_idx = ioStep->mSolveVelocityConstraintsNextIsland++; + if (island_idx >= mIslandBuilder.GetNumIslands()) + { + // We processed all islands, stop checking islands + check_islands = false; + continue; + } + + JPH_PROFILE("Island"); + + // Get iterators for this island + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + bool has_constraints = mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end); + bool has_contacts = mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end); + + // If we don't have any contacts or constraints, we know that none of the following islands have any contacts or constraints + // (because they're sorted by most constraints first). This means we're done. + if (!has_contacts && !has_constraints) + { + #ifdef JPH_ENABLE_ASSERTS + // Validate our assumption that the next islands don't have any constraints or contacts + for (; island_idx < mIslandBuilder.GetNumIslands(); ++island_idx) + { + JPH_ASSERT(!mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end)); + JPH_ASSERT(!mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end)); + } + #endif // JPH_ENABLE_ASSERTS + + check_islands = false; + continue; + } + + // Sorting is costly but needed for a deterministic simulation, allow the user to turn this off + if (mPhysicsSettings.mDeterministicSimulation) + { + // Sort constraints to give a deterministic simulation + ConstraintManager::sSortConstraints(active_constraints, constraints_begin, constraints_end); + + // Sort contacts to give a deterministic simulation + mContactManager.SortContacts(contacts_begin, contacts_end); + } + + // Split up large islands + CalculateSolverSteps steps_calculator(mPhysicsSettings); + if (mPhysicsSettings.mUseLargeIslandSplitter + && mLargeIslandSplitter.SplitIsland(island_idx, mIslandBuilder, mBodyManager, mContactManager, active_constraints, steps_calculator)) + continue; // Loop again to try to fetch the newly split island + + // We didn't create a split, just run the solver now for this entire island. Begin by warm starting. + ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, steps_calculator); + mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, steps_calculator); + steps_calculator.Finalize(); + + // Store the number of position steps for later + mIslandBuilder.SetNumPositionSteps(island_idx, steps_calculator.GetNumPositionSteps()); + + // Solve velocity constraints + for (uint velocity_step = 0; velocity_step < steps_calculator.GetNumVelocitySteps(); ++velocity_step) + { + bool applied_impulse = ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time); + applied_impulse |= mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end); + if (!applied_impulse) + break; + } + + // Save back the lambdas in the contact cache for the warm start of the next physics update + mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end); + + // We processed work, loop again + continue; + } + + if (check_islands) + { + // If there are islands, we don't need to wait and can pick up new work + continue; + } + else if (check_split_islands) + { + // If there are split islands, but we did't do any work, give up a time slice + std::this_thread::yield(); + } + else + { + // No more work + break; + } + } +} + +JPH_SUPPRESS_WARNING_POP + +void PhysicsSystem::JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ + // Reserve enough space for all bodies that may need a cast + TempAllocator *temp_allocator = ioContext->mTempAllocator; + JPH_ASSERT(ioStep->mCCDBodies == nullptr); + ioStep->mCCDBodiesCapacity = mBodyManager.GetNumActiveCCDBodies(); + ioStep->mCCDBodies = (CCDBody *)temp_allocator->Allocate(ioStep->mCCDBodiesCapacity * sizeof(CCDBody)); + + // Initialize the mapping table between active body and CCD body + JPH_ASSERT(ioStep->mActiveBodyToCCDBody == nullptr); + ioStep->mNumActiveBodyToCCDBody = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + ioStep->mActiveBodyToCCDBody = (int *)temp_allocator->Allocate(ioStep->mNumActiveBodyToCCDBody * sizeof(int)); + + // Prepare the split island builder for solving the position constraints + mLargeIslandSplitter.PrepareForSolvePositions(); +} + +void PhysicsSystem::JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update positions and need velocity to do so, we also clamp velocities so need to write to them + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); +#endif + + float delta_time = ioContext->mStepDeltaTime; + const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody); + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx; + + // We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement. + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + + for (;;) + { + // Atomically fetch a batch of bodies + uint32 active_body_idx = ioStep->mIntegrateVelocityReadIdx.fetch_add(cIntegrateVelocityBatchSize); + if (active_body_idx >= num_active_bodies) + break; + + // Calculate the end of the batch + uint32 active_body_idx_end = min(num_active_bodies, active_body_idx + cIntegrateVelocityBatchSize); + + // Process the batch + while (active_body_idx < active_body_idx_end) + { + // Update the positions using an Symplectic Euler step (which integrates using the updated velocity v1' rather + // than the original velocity v1): + // x1' = x1 + h * v1' + // At this point the active bodies list does not change, so it is safe to access the array. + BodyID body_id = active_bodies[active_body_idx]; + Body &body = mBodyManager.GetBody(body_id); + MotionProperties *mp = body.GetMotionProperties(); + + JPH_DET_LOG("JobIntegrateVelocity: id: " << body_id << " v: " << body.GetLinearVelocity() << " w: " << body.GetAngularVelocity()); + + // Clamp velocities (not for kinematic bodies) + if (body.IsDynamic()) + { + mp->ClampLinearVelocity(); + mp->ClampAngularVelocity(); + } + + // Update the rotation of the body according to the angular velocity + // For motion type discrete we need to do this anyway, for motion type linear cast we have multiple choices + // 1. Rotate the body first and then sweep + // 2. First sweep and then rotate the body at the end + // 3. Pick some in between rotation (e.g. half way), then sweep and finally rotate the remainder + // (1) has some clear advantages as when a long thin body hits a surface away from the center of mass, this will result in a large angular velocity and a limited reduction in linear velocity. + // When simulation the rotation first before doing the translation, the body will be able to rotate away from the contact point allowing the center of mass to approach the surface. When using + // approach (2) in this case what will happen is that we will immediately detect the same collision again (the body has not rotated and the body was already colliding at the end of the previous + // time step) resulting in a lot of stolen time and the body appearing to be frozen in an unnatural pose (like it is glued at an angle to the surface). (2) obviously has some negative side effects + // too as simulating the rotation first may cause it to tunnel through a small object that the linear cast might have otherwise detected. In any case a linear cast is not good for detecting + // tunneling due to angular rotation, so we don't care about that too much (you'd need a full cast to take angular effects into account). + body.AddRotationStep(body.GetAngularVelocity() * delta_time); + + // Get delta position + Vec3 delta_pos = body.GetLinearVelocity() * delta_time; + + // If the position should be updated (or if it is delayed because of CCD) + bool update_position = true; + + switch (mp->GetMotionQuality()) + { + case EMotionQuality::Discrete: + // No additional collision checking to be done + break; + + case EMotionQuality::LinearCast: + if (body.IsDynamic() // Kinematic bodies cannot be stopped + && !body.IsSensor()) // We don't support CCD sensors + { + // Determine inner radius (the smallest sphere that fits into the shape) + float inner_radius = body.GetShape()->GetInnerRadius(); + JPH_ASSERT(inner_radius > 0.0f, "The shape has no inner radius, this makes the shape unsuitable for the linear cast motion quality as we cannot move it without risking tunneling."); + + // Measure translation in this step and check if it above the threshold to perform a linear cast + float linear_cast_threshold_sq = Square(mPhysicsSettings.mLinearCastThreshold * inner_radius); + if (delta_pos.LengthSq() > linear_cast_threshold_sq) + { + // This body needs a cast + uint32 ccd_body_idx = ioStep->mNumCCDBodies++; + JPH_ASSERT(active_body_idx < ioStep->mNumActiveBodyToCCDBody); + ioStep->mActiveBodyToCCDBody[active_body_idx] = ccd_body_idx; + new (&ioStep->mCCDBodies[ccd_body_idx]) CCDBody(body_id, delta_pos, linear_cast_threshold_sq, min(mPhysicsSettings.mPenetrationSlop, mPhysicsSettings.mLinearCastMaxPenetration * inner_radius)); + + update_position = false; + } + } + break; + } + + if (update_position) + { + // Move the body now + body.AddPositionStep(delta_pos); + + // If the body was activated due to an earlier CCD step it will have an index in the active + // body list that it higher than the highest one we processed during FindCollisions + // which means it hasn't been assigned an island and will not be updated by an island + // this means that we need to update its bounds manually + if (mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions) + { + body.CalculateWorldSpaceBoundsInternal(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = body.GetID(); + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + } + + // We did not create a CCD body + ioStep->mActiveBodyToCCDBody[active_body_idx] = -1; + } + + active_body_idx++; + } + } + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); +} + +void PhysicsSystem::JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const +{ + // Validate that our reservations were correct + JPH_ASSERT(ioStep->mNumCCDBodies <= mBodyManager.GetNumActiveCCDBodies()); + + if (ioStep->mNumCCDBodies == 0) + { + // No continuous collision detection jobs -> kick the next job ourselves + ioStep->mContactRemovedCallbacks.RemoveDependency(); + } + else + { + // Run the continuous collision detection jobs + int num_continuous_collision_jobs = min(int(ioStep->mNumCCDBodies + cNumCCDBodiesPerJob - 1) / cNumCCDBodiesPerJob, ioContext->GetMaxConcurrency()); + ioStep->mResolveCCDContacts.AddDependency(num_continuous_collision_jobs); + ioStep->mContactRemovedCallbacks.AddDependency(num_continuous_collision_jobs - 1); // Already had 1 dependency + for (int i = 0; i < num_continuous_collision_jobs; ++i) + { + JobHandle job = ioContext->mJobSystem->CreateJob("FindCCDContacts", cColorFindCCDContacts, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobFindCCDContacts(ioContext, ioStep); + + ioStep->mResolveCCDContacts.RemoveDependency(); + ioStep->mContactRemovedCallbacks.RemoveDependency(); + }); + ioContext->mBarrier->AddJob(job); + } + } +} + +// Helper function to calculate the motion of a body during this CCD step +inline static Vec3 sCalculateBodyMotion(const Body &inBody, float inDeltaTime) +{ + // If the body is linear casting, the body has not yet moved so we need to calculate its motion + if (inBody.IsDynamic() && inBody.GetMotionProperties()->GetMotionQuality() == EMotionQuality::LinearCast) + return inDeltaTime * inBody.GetLinearVelocity(); + + // Body has already moved, so we don't need to correct for anything + return Vec3::sZero(); +} + +// Helper function that finds the CCD body corresponding to a body (if it exists) +inline static PhysicsUpdateContext::Step::CCDBody *sGetCCDBody(const Body &inBody, PhysicsUpdateContext::Step *inStep) +{ + // Only rigid bodies can have a CCD body + if (!inBody.IsRigidBody()) + return nullptr; + + // If the body has no motion properties it cannot have a CCD body + const MotionProperties *motion_properties = inBody.GetMotionPropertiesUnchecked(); + if (motion_properties == nullptr) + return nullptr; + + // If it is not active it cannot have a CCD body + uint32 active_index = motion_properties->GetIndexInActiveBodiesInternal(); + if (active_index == Body::cInactiveIndex) + return nullptr; + + // Check if the active body has a corresponding CCD body + JPH_ASSERT(active_index < inStep->mNumActiveBodyToCCDBody); // Ensure that the body has a mapping to CCD body + int ccd_index = inStep->mActiveBodyToCCDBody[active_index]; + if (ccd_index < 0) + return nullptr; + + PhysicsUpdateContext::Step::CCDBody *ccd_body = &inStep->mCCDBodies[ccd_index]; + JPH_ASSERT(ccd_body->mBodyID1 == inBody.GetID(), "We found the wrong CCD body!"); + return ccd_body; +} + +void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions, but the validate callback may read body positions and velocities + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); +#endif + + // Allocation context for allocating new contact points + ContactAllocator contact_allocator(mContactManager.GetContactAllocator()); + + // Settings + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces; + settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + settings.mReturnDeepestPoint = true; + settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll; + + for (;;) + { + // Fetch the next body to cast + uint32 idx = ioStep->mNextCCDBody++; + if (idx >= ioStep->mNumCCDBodies) + break; + CCDBody &ccd_body = ioStep->mCCDBodies[idx]; + const Body &body = mBodyManager.GetBody(ccd_body.mBodyID1); + + // Filter out layers + DefaultBroadPhaseLayerFilter broadphase_layer_filter = GetDefaultBroadPhaseLayerFilter(body.GetObjectLayer()); + DefaultObjectLayerFilter object_layer_filter = GetDefaultLayerFilter(body.GetObjectLayer()); + + #ifdef JPH_DEBUG_RENDERER + // Draw start and end shape of cast + if (sDrawMotionQualityLinearCast) + { + RMat44 com = body.GetCenterOfMassTransform(); + body.GetShape()->Draw(DebugRenderer::sInstance, com, Vec3::sReplicate(1.0f), Color::sGreen, false, true); + DebugRenderer::sInstance->DrawArrow(com.GetTranslation(), com.GetTranslation() + ccd_body.mDeltaPosition, Color::sGreen, 0.1f); + body.GetShape()->Draw(DebugRenderer::sInstance, com.PostTranslated(ccd_body.mDeltaPosition), Vec3::sReplicate(1.0f), Color::sRed, false, true); + } + #endif // JPH_DEBUG_RENDERER + + // Create a collector that will find the maximum distance allowed to travel while not penetrating more than 'max penetration' + class CCDNarrowPhaseCollector : public CastShapeCollector + { + public: + CCDNarrowPhaseCollector(const BodyManager &inBodyManager, ContactConstraintManager &inContactConstraintManager, CCDBody &inCCDBody, ShapeCastResult &inResult, float inDeltaTime) : + mBodyManager(inBodyManager), + mContactConstraintManager(inContactConstraintManager), + mCCDBody(inCCDBody), + mResult(inResult), + mDeltaTime(inDeltaTime) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + JPH_PROFILE_FUNCTION(); + + // Check if this is a possible earlier hit than the one before + float fraction = inResult.mFraction; + if (fraction < mCCDBody.mFractionPlusSlop) + { + // Normalize normal + Vec3 normal = inResult.mPenetrationAxis.Normalized(); + + // Calculate how much we can add to the fraction to penetrate the collision point by mMaxPenetration. + // Note that the normal is pointing towards body 2! + // Let the extra distance that we can travel along delta_pos be 'dist': mMaxPenetration / dist = cos(angle between normal and delta_pos) = normal . delta_pos / |delta_pos| + // <=> dist = mMaxPenetration * |delta_pos| / normal . delta_pos + // Converting to a faction: delta_fraction = dist / |delta_pos| = mLinearCastTreshold / normal . delta_pos + float denominator = normal.Dot(mCCDBody.mDeltaPosition); + if (denominator > mCCDBody.mMaxPenetration) // Avoid dividing by zero, if extra hit fraction > 1 there's also no point in continuing + { + float fraction_plus_slop = fraction + mCCDBody.mMaxPenetration / denominator; + if (fraction_plus_slop < mCCDBody.mFractionPlusSlop) + { + const Body &body2 = mBodyManager.GetBody(inResult.mBodyID2); + + // Check if we've already accepted all hits from this body + if (mValidateBodyPair) + { + // Validate the contact result + const Body &body1 = mBodyManager.GetBody(mCCDBody.mBodyID1); + ValidateResult validate_result = mContactConstraintManager.ValidateContactPoint(body1, body2, body1.GetCenterOfMassPosition(), inResult); // Note that the center of mass of body 1 is the start of the sweep and is used as base offset below + switch (validate_result) + { + case ValidateResult::AcceptContact: + // Just continue + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept this and all following contacts from this body + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Reject this and all following contacts from this body + mRejectAll = true; + ForceEarlyOut(); + return; + } + } + + // This is the earliest hit so far, store it + mCCDBody.mContactNormal = normal; + mCCDBody.mBodyID2 = inResult.mBodyID2; + mCCDBody.mSubShapeID2 = inResult.mSubShapeID2; + mCCDBody.mFraction = fraction; + mCCDBody.mFractionPlusSlop = fraction_plus_slop; + mResult = inResult; + + // Result was assuming body 2 is not moving, but it is, so we need to correct for it + Vec3 movement2 = fraction * sCalculateBodyMotion(body2, mDeltaTime); + if (!movement2.IsNearZero()) + { + mResult.mContactPointOn1 += movement2; + mResult.mContactPointOn2 += movement2; + for (Vec3 &v : mResult.mShape1Face) + v += movement2; + for (Vec3 &v : mResult.mShape2Face) + v += movement2; + } + + // Update early out fraction + UpdateEarlyOutFraction(fraction_plus_slop); + } + } + } + } + + bool mValidateBodyPair; ///< If we still have to call the ValidateContactPoint for this body pair + bool mRejectAll; ///< Reject all further contacts between this body pair + + private: + const BodyManager & mBodyManager; + ContactConstraintManager & mContactConstraintManager; + CCDBody & mCCDBody; + ShapeCastResult & mResult; + float mDeltaTime; + BodyID mAcceptedBodyID; + }; + + // Narrowphase collector + ShapeCastResult cast_shape_result; + CCDNarrowPhaseCollector np_collector(mBodyManager, mContactManager, ccd_body, cast_shape_result, ioContext->mStepDeltaTime); + + // This collector wraps the narrowphase collector and collects the closest hit + class CCDBroadPhaseCollector : public CastShapeBodyCollector + { + public: + CCDBroadPhaseCollector(const CCDBody &inCCDBody, const Body &inBody1, const RShapeCast &inShapeCast, ShapeCastSettings &inShapeCastSettings, SimShapeFilterWrapper &inShapeFilter, CCDNarrowPhaseCollector &ioCollector, const BodyManager &inBodyManager, PhysicsUpdateContext::Step *inStep, float inDeltaTime) : + mCCDBody(inCCDBody), + mBody1(inBody1), + mBody1Extent(inShapeCast.mShapeWorldBounds.GetExtent()), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mShapeFilter(inShapeFilter), + mCollector(ioCollector), + mBodyManager(inBodyManager), + mStep(inStep), + mDeltaTime(inDeltaTime) + { + } + + virtual void AddHit(const BroadPhaseCastResult &inResult) override + { + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inResult.mFraction <= GetEarlyOutFraction(), "This hit should not have been passed on to the collector"); + + // Test if we're colliding with ourselves + if (mBody1.GetID() == inResult.mBodyID) + return; + + // Avoid treating duplicates, if both bodies are doing CCD then only consider collision if body ID < other body ID + const Body &body2 = mBodyManager.GetBody(inResult.mBodyID); + const CCDBody *ccd_body2 = sGetCCDBody(body2, mStep); + if (ccd_body2 != nullptr && mCCDBody.mBodyID1 > ccd_body2->mBodyID1) + return; + + // Test group filter + if (!mBody1.GetCollisionGroup().CanCollide(body2.GetCollisionGroup())) + return; + + // TODO: For now we ignore sensors + if (body2.IsSensor()) + return; + + // Get relative movement of these two bodies + Vec3 direction = mShapeCast.mDirection - sCalculateBodyMotion(body2, mDeltaTime); + + // Test if the remaining movement is less than our movement threshold + if (direction.LengthSq() < mCCDBody.mLinearCastThresholdSq) + return; + + // Get the bounds of 2, widen it by the extent of 1 and test a ray to see if it hits earlier than the current early out fraction + AABox bounds = body2.GetWorldSpaceBounds(); + bounds.mMin -= mBody1Extent; + bounds.mMax += mBody1Extent; + float hit_fraction = RayAABox(Vec3(mShapeCast.mCenterOfMassStart.GetTranslation()), RayInvDirection(direction), bounds.mMin, bounds.mMax); + if (hit_fraction > GetPositiveEarlyOutFraction()) // If early out fraction <= 0, we have the possibility of finding a deeper hit so we need to clamp the early out fraction + return; + + // Reset collector (this is a new body pair) + mCollector.ResetEarlyOutFraction(GetEarlyOutFraction()); + mCollector.mValidateBodyPair = true; + mCollector.mRejectAll = false; + + // Set body ID on shape filter + mShapeFilter.SetBody2(&body2); + + // Provide direction as hint for the active edges algorithm + mShapeCastSettings.mActiveEdgeMovementDirection = direction; + + // Do narrow phase collision check + RShapeCast relative_cast(mShapeCast.mShape, mShapeCast.mScale, mShapeCast.mCenterOfMassStart, direction, mShapeCast.mShapeWorldBounds); + body2.GetTransformedShape().CastShape(relative_cast, mShapeCastSettings, mShapeCast.mCenterOfMassStart.GetTranslation(), mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + if (!mCollector.mRejectAll) + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + const CCDBody & mCCDBody; + const Body & mBody1; + Vec3 mBody1Extent; + RShapeCast mShapeCast; + ShapeCastSettings & mShapeCastSettings; + SimShapeFilterWrapper & mShapeFilter; + CCDNarrowPhaseCollector & mCollector; + const BodyManager & mBodyManager; + PhysicsUpdateContext::Step *mStep; + float mDeltaTime; + }; + + // Create shape filter + SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, &body); + SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper(); + + // Check if we collide with any other body. Note that we use the non-locking interface as we know the broadphase cannot be modified at this point. + RShapeCast shape_cast(body.GetShape(), Vec3::sReplicate(1.0f), body.GetCenterOfMassTransform(), ccd_body.mDeltaPosition); + CCDBroadPhaseCollector bp_collector(ccd_body, body, shape_cast, settings, shape_filter, np_collector, mBodyManager, ioStep, ioContext->mStepDeltaTime); + mBroadPhase->CastAABoxNoLock({ shape_cast.mShapeWorldBounds, shape_cast.mDirection }, bp_collector, broadphase_layer_filter, object_layer_filter); + + // Check if there was a hit + if (ccd_body.mFractionPlusSlop < 1.0f) + { + const Body &body2 = mBodyManager.GetBody(ccd_body.mBodyID2); + + // Determine contact manifold + ContactManifold manifold; + manifold.mBaseOffset = shape_cast.mCenterOfMassStart.GetTranslation(); + ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldToleranceSq, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + manifold.mSubShapeID1 = cast_shape_result.mSubShapeID1; + manifold.mSubShapeID2 = cast_shape_result.mSubShapeID2; + manifold.mPenetrationDepth = cast_shape_result.mPenetrationDepth; + manifold.mWorldSpaceNormal = ccd_body.mContactNormal; + + // Call contact point callbacks + mContactManager.OnCCDContactAdded(contact_allocator, body, body2, manifold, ccd_body.mContactSettings); + + if (ccd_body.mContactSettings.mIsSensor) + { + // If this is a sensor, we don't want to solve the contact + ccd_body.mFractionPlusSlop = 1.0f; + ccd_body.mBodyID2 = BodyID(); + } + else + { + // Calculate the average position from the manifold (this will result in the same impulse applied as when we apply impulses to all contact points) + if (manifold.mRelativeContactPointsOn2.size() > 1) + { + Vec3 average_contact_point = Vec3::sZero(); + for (const Vec3 &v : manifold.mRelativeContactPointsOn2) + average_contact_point += v; + average_contact_point /= (float)manifold.mRelativeContactPointsOn2.size(); + ccd_body.mContactPointOn2 = manifold.mBaseOffset + average_contact_point; + } + else + ccd_body.mContactPointOn2 = manifold.mBaseOffset + cast_shape_result.mContactPointOn2; + } + } + } + + // Collect information from the contact allocator and accumulate it in the step. + sFinalizeContactAllocator(*ioStep, contact_allocator); +} + +void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // Read/write body access + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); + + // We activate bodies that we collide with + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx; + TempAllocator *temp_allocator = ioContext->mTempAllocator; + + // Check if there's anything to do + uint num_ccd_bodies = ioStep->mNumCCDBodies; + if (num_ccd_bodies > 0) + { + // Sort on fraction so that we process earliest collisions first + // This is needed to make the simulation deterministic and also to be able to stop contact processing + // between body pairs if an earlier hit was found involving the body by another CCD body + // (if it's body ID < this CCD body's body ID - see filtering logic in CCDBroadPhaseCollector) + CCDBody **sorted_ccd_bodies = (CCDBody **)temp_allocator->Allocate(num_ccd_bodies * sizeof(CCDBody *)); + JPH_SCOPE_EXIT([temp_allocator, sorted_ccd_bodies, num_ccd_bodies]{ temp_allocator->Free(sorted_ccd_bodies, num_ccd_bodies * sizeof(CCDBody *)); }); + { + JPH_PROFILE("Sort"); + + // We don't want to copy the entire struct (it's quite big), so we create a pointer array first + CCDBody *src_ccd_bodies = ioStep->mCCDBodies; + CCDBody **dst_ccd_bodies = sorted_ccd_bodies; + CCDBody **dst_ccd_bodies_end = dst_ccd_bodies + num_ccd_bodies; + while (dst_ccd_bodies < dst_ccd_bodies_end) + *(dst_ccd_bodies++) = src_ccd_bodies++; + + // Which we then sort + QuickSort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2) + { + if (inBody1->mFractionPlusSlop != inBody2->mFractionPlusSlop) + return inBody1->mFractionPlusSlop < inBody2->mFractionPlusSlop; + + return inBody1->mBodyID1 < inBody2->mBodyID1; + }); + } + + // We can collide with bodies that are not active, we track them here so we can activate them in one go at the end. + // This is also needed because we can't modify the active body array while we iterate it. + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_activate = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_activate = 0; + + // We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement. + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + + for (uint i = 0; i < num_ccd_bodies; ++i) + { + const CCDBody *ccd_body = sorted_ccd_bodies[i]; + Body &body1 = mBodyManager.GetBody(ccd_body->mBodyID1); + MotionProperties *body_mp = body1.GetMotionProperties(); + + // If there was a hit + if (!ccd_body->mBodyID2.IsInvalid()) + { + Body &body2 = mBodyManager.GetBody(ccd_body->mBodyID2); + + // Determine if the other body has a CCD body + CCDBody *ccd_body2 = sGetCCDBody(body2, ioStep); + if (ccd_body2 != nullptr) + { + JPH_ASSERT(ccd_body2->mBodyID2 != ccd_body->mBodyID1, "If we collided with another body, that other body should have ignored collisions with us!"); + + // Check if the other body found a hit that is further away + if (ccd_body2->mFraction > ccd_body->mFraction) + { + // Reset the colliding body of the other CCD body. The other body will shorten its distance traveled and will not do any collision response (we'll do that). + // This means that at this point we have triggered a contact point add/persist for our further hit by accident for the other body. + // We accept this as calling the contact point callbacks here would require persisting the manifolds up to this point and doing the callbacks single threaded. + ccd_body2->mBodyID2 = BodyID(); + ccd_body2->mFractionPlusSlop = ccd_body->mFraction; + } + } + + // If the other body moved less than us before hitting something, we're not colliding with it so we again have triggered contact point add/persist callbacks by accident. + // We'll just move to the collision position anyway (as that's the last position we know is good), but we won't do any collision response. + if (ccd_body2 == nullptr || ccd_body2->mFraction >= ccd_body->mFraction) + { + const ContactSettings &contact_settings = ccd_body->mContactSettings; + + // Calculate contact point velocity for body 1 + Vec3 r1_plus_u = Vec3(ccd_body->mContactPointOn2 - (body1.GetCenterOfMassPosition() + ccd_body->mFraction * ccd_body->mDeltaPosition)); + Vec3 v1 = body1.GetPointVelocityCOM(r1_plus_u); + + // Calculate inverse mass for body 1 + float inv_m1 = contact_settings.mInvMassScale1 * body_mp->GetInverseMass(); + + if (body2.IsRigidBody()) + { + // Calculate contact point velocity for body 2 + Vec3 r2 = Vec3(ccd_body->mContactPointOn2 - body2.GetCenterOfMassPosition()); + Vec3 v2 = body2.GetPointVelocityCOM(r2); + + // Calculate relative contact velocity + Vec3 relative_velocity = v2 - v1; + float normal_velocity = relative_velocity.Dot(ccd_body->mContactNormal); + + // Calculate velocity bias due to restitution + float normal_velocity_bias; + if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution) + normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity; + else + normal_velocity_bias = 0.0f; + + // Get inverse mass of body 2 + float inv_m2 = body2.GetMotionPropertiesUnchecked() != nullptr? contact_settings.mInvMassScale2 * body2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + + // Solve contact constraint + AxisConstraintPart contact_constraint; + contact_constraint.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, ccd_body->mContactNormal, normal_velocity_bias); + contact_constraint.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX); + + // Apply friction + if (contact_settings.mCombinedFriction > 0.0f) + { + // Calculate friction direction by removing normal velocity from the relative velocity + Vec3 friction_direction = relative_velocity - normal_velocity * ccd_body->mContactNormal; + float friction_direction_len_sq = friction_direction.LengthSq(); + if (friction_direction_len_sq > 1.0e-12f) + { + // Normalize friction direction + friction_direction /= sqrt(friction_direction_len_sq); + + // Calculate max friction impulse + float max_lambda_f = contact_settings.mCombinedFriction * contact_constraint.GetTotalLambda(); + + AxisConstraintPart friction; + friction.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, friction_direction); + friction.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, friction_direction, -max_lambda_f, max_lambda_f); + } + } + + // Clamp velocity of body 2 + if (body2.IsDynamic()) + { + MotionProperties *body2_mp = body2.GetMotionProperties(); + body2_mp->ClampLinearVelocity(); + body2_mp->ClampAngularVelocity(); + } + } + else + { + SoftBodyMotionProperties *soft_mp = static_cast(body2.GetMotionProperties()); + const SoftBodyShape *soft_shape = static_cast(body2.GetShape()); + + // Convert the sub shape ID of the soft body to a face + uint32 face_idx = soft_shape->GetFaceIndex(ccd_body->mSubShapeID2); + const SoftBodyMotionProperties::Face &face = soft_mp->GetFace(face_idx); + + // Get vertices of the face + SoftBodyMotionProperties::Vertex &vtx0 = soft_mp->GetVertex(face.mVertex[0]); + SoftBodyMotionProperties::Vertex &vtx1 = soft_mp->GetVertex(face.mVertex[1]); + SoftBodyMotionProperties::Vertex &vtx2 = soft_mp->GetVertex(face.mVertex[2]); + + // Inverse mass of the face + float vtx0_mass = vtx0.mInvMass > 0.0f? 1.0f / vtx0.mInvMass : 1.0e10f; + float vtx1_mass = vtx1.mInvMass > 0.0f? 1.0f / vtx1.mInvMass : 1.0e10f; + float vtx2_mass = vtx2.mInvMass > 0.0f? 1.0f / vtx2.mInvMass : 1.0e10f; + float inv_m2 = 1.0f / (vtx0_mass + vtx1_mass + vtx2_mass); + + // Calculate barycentric coordinates of the contact point on the soft body's face + float u, v, w; + RMat44 inv_body2_transform = body2.GetInverseCenterOfMassTransform(); + Vec3 local_contact = Vec3(inv_body2_transform * ccd_body->mContactPointOn2); + ClosestPoint::GetBaryCentricCoordinates(vtx0.mPosition - local_contact, vtx1.mPosition - local_contact, vtx2.mPosition - local_contact, u, v, w); + + // Calculate contact point velocity for the face + Vec3 v2 = inv_body2_transform.Multiply3x3Transposed(u * vtx0.mVelocity + v * vtx1.mVelocity + w * vtx2.mVelocity); + float normal_velocity = (v2 - v1).Dot(ccd_body->mContactNormal); + + // Calculate velocity bias due to restitution + float normal_velocity_bias; + if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution) + normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity; + else + normal_velocity_bias = 0.0f; + + // Calculate resulting velocity change (the math here is similar to AxisConstraintPart but without an inertia term for body 2 as we treat it as a point mass) + Vec3 r1_plus_u_x_n = r1_plus_u.Cross(ccd_body->mContactNormal); + Vec3 invi1_r1_plus_u_x_n = contact_settings.mInvInertiaScale1 * body1.GetInverseInertia().Multiply3x3(r1_plus_u_x_n); + float jv = r1_plus_u_x_n.Dot(body_mp->GetAngularVelocity()) - normal_velocity - normal_velocity_bias; + float inv_effective_mass = inv_m1 + inv_m2 + invi1_r1_plus_u_x_n.Dot(r1_plus_u_x_n); + float lambda = jv / inv_effective_mass; + body_mp->SubLinearVelocityStep((lambda * inv_m1) * ccd_body->mContactNormal); + body_mp->SubAngularVelocityStep(lambda * invi1_r1_plus_u_x_n); + Vec3 delta_v2 = inv_body2_transform.Multiply3x3(lambda * ccd_body->mContactNormal); + vtx0.mVelocity += delta_v2 * vtx0.mInvMass; + vtx1.mVelocity += delta_v2 * vtx1.mInvMass; + vtx2.mVelocity += delta_v2 * vtx2.mInvMass; + } + + // Clamp velocity of body 1 + body_mp->ClampLinearVelocity(); + body_mp->ClampAngularVelocity(); + + // Activate the 2nd body if it is not already active + if (body2.IsDynamic() && !body2.IsActive()) + { + bodies_to_activate[num_bodies_to_activate++] = ccd_body->mBodyID2; + if (num_bodies_to_activate == cBodiesBatch) + { + // Batch is full, activate now + mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate); + num_bodies_to_activate = 0; + } + } + + #ifdef JPH_DEBUG_RENDERER + if (sDrawMotionQualityLinearCast) + { + // Draw the collision location + RMat44 collision_transform = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFraction * ccd_body->mDeltaPosition); + body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform, Vec3::sReplicate(1.0f), Color::sYellow, false, true); + + // Draw the collision location + slop + RMat44 collision_transform_plus_slop = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFractionPlusSlop * ccd_body->mDeltaPosition); + body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform_plus_slop, Vec3::sReplicate(1.0f), Color::sOrange, false, true); + + // Draw contact normal + DebugRenderer::sInstance->DrawArrow(ccd_body->mContactPointOn2, ccd_body->mContactPointOn2 - ccd_body->mContactNormal, Color::sYellow, 0.1f); + + // Draw post contact velocity + DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetLinearVelocity(), Color::sOrange, 0.1f); + DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetAngularVelocity(), Color::sPurple, 0.1f); + } + #endif // JPH_DEBUG_RENDERER + } + } + + // Update body position + body1.AddPositionStep(ccd_body->mDeltaPosition * ccd_body->mFractionPlusSlop); + + // If the body was activated due to an earlier CCD step it will have an index in the active + // body list that it higher than the highest one we processed during FindCollisions + // which means it hasn't been assigned an island and will not be updated by an island + // this means that we need to update its bounds manually + if (body_mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions) + { + body1.CalculateWorldSpaceBoundsInternal(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = body1.GetID(); + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + } + } + + // Activate the requested bodies + if (num_bodies_to_activate > 0) + mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate); + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + } + + // Ensure we free the CCD bodies array now, will not call the destructor! + temp_allocator->Free(ioStep->mActiveBodyToCCDBody, ioStep->mNumActiveBodyToCCDBody * sizeof(int)); + ioStep->mActiveBodyToCCDBody = nullptr; + ioStep->mNumActiveBodyToCCDBody = 0; + temp_allocator->Free(ioStep->mCCDBodies, ioStep->mCCDBodiesCapacity * sizeof(CCDBody)); + ioStep->mCCDBodies = nullptr; + ioStep->mCCDBodiesCapacity = 0; +} + +void PhysicsSystem::JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We don't touch any bodies + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Reset the Body::EFlags::InvalidateContactCache flag for all bodies + mBodyManager.ValidateContactCacheForAllBodies(); + + // Finalize the contact cache (this swaps the read and write versions of the contact cache) + // Trigger all contact removed callbacks by looking at last step contact points that have not been flagged as reused + mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(ioStep->mNumBodyPairs, ioStep->mNumManifolds); +} + +class PhysicsSystem::BodiesToSleep : public NonCopyable +{ +public: + static constexpr int cBodiesToSleepSize = 512; + static constexpr int cMaxBodiesToPutInBuffer = 128; + + inline BodiesToSleep(BodyManager &inBodyManager, BodyID *inBodiesToSleepBuffer) : mBodyManager(inBodyManager), mBodiesToSleepBuffer(inBodiesToSleepBuffer), mBodiesToSleepCur(inBodiesToSleepBuffer) { } + + inline ~BodiesToSleep() + { + // Flush the bodies to sleep buffer + int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer); + if (num_bodies_in_buffer > 0) + mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer); + } + + inline void PutToSleep(const BodyID *inBegin, const BodyID *inEnd) + { + int num_bodies_to_sleep = int(inEnd - inBegin); + if (num_bodies_to_sleep > cMaxBodiesToPutInBuffer) + { + // Too many bodies, deactivate immediately + mBodyManager.DeactivateBodies(inBegin, num_bodies_to_sleep); + } + else + { + // Check if there's enough space in the bodies to sleep buffer + int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer); + if (num_bodies_in_buffer + num_bodies_to_sleep > cBodiesToSleepSize) + { + // Flush the bodies to sleep buffer + mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer); + mBodiesToSleepCur = mBodiesToSleepBuffer; + } + + // Copy the bodies in the buffer + memcpy(mBodiesToSleepCur, inBegin, num_bodies_to_sleep * sizeof(BodyID)); + mBodiesToSleepCur += num_bodies_to_sleep; + } + } + +private: + BodyManager & mBodyManager; + BodyID * mBodiesToSleepBuffer; + BodyID * mBodiesToSleepCur; +}; + +void PhysicsSystem::CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep) +{ + // Get the bodies that belong to this island + BodyID *bodies_begin, *bodies_end; + mIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_begin, bodies_end); + + // Only check sleeping in the last step + // Also resets force and torque used during the apply gravity phase + if (ioStep->mIsLast) + { + JPH_PROFILE("Check Sleeping"); + + static_assert(int(ECanSleep::CannotSleep) == 0 && int(ECanSleep::CanSleep) == 1, "Loop below makes this assumption"); + int all_can_sleep = mPhysicsSettings.mAllowSleeping? int(ECanSleep::CanSleep) : int(ECanSleep::CannotSleep); + + float time_before_sleep = mPhysicsSettings.mTimeBeforeSleep; + float max_movement = mPhysicsSettings.mPointVelocitySleepThreshold * time_before_sleep; + + for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id) + { + Body &body = mBodyManager.GetBody(*body_id); + + // Update bounding box + body.CalculateWorldSpaceBoundsInternal(); + + // Update sleeping + all_can_sleep &= int(body.UpdateSleepStateInternal(ioContext->mStepDeltaTime, max_movement, time_before_sleep)); + + // Reset force and torque + MotionProperties *mp = body.GetMotionProperties(); + mp->ResetForce(); + mp->ResetTorque(); + } + + // If all bodies indicate they can sleep we can deactivate them + if (all_can_sleep == int(ECanSleep::CanSleep)) + ioBodiesToSleep.PutToSleep(bodies_begin, bodies_end); + } + else + { + JPH_PROFILE("Update Bounds"); + + // Update bounding box only for all other steps + for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id) + { + Body &body = mBodyManager.GetBody(*body_id); + body.CalculateWorldSpaceBoundsInternal(); + } + } + + // Notify broadphase of changed objects (find ccd contacts can do linear casts in the next step, so we need to do this every step) + // Note: Shuffles the BodyID's around!!! + mBroadPhase->NotifyBodiesAABBChanged(bodies_begin, int(bodies_end - bodies_begin), false); +} + +void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We fix up position errors + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::ReadWrite); + + // Can only deactivate bodies + BodyManager::GrantActiveBodiesAccess grant_active(false, true); +#endif + + float delta_time = ioContext->mStepDeltaTime; + float baumgarte = mPhysicsSettings.mBaumgarte; + Constraint **active_constraints = ioContext->mActiveConstraints; + + // Keep a buffer of bodies that need to go to sleep in order to not constantly lock the active bodies mutex and create contention between all solving threads + BodiesToSleep bodies_to_sleep(mBodyManager, (BodyID *)JPH_STACK_ALLOC(BodiesToSleep::cBodiesToSleepSize * sizeof(BodyID))); + + bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter; + for (;;) + { + // First try to get work from large islands + if (check_split_islands) + { + bool first_iteration; + uint split_island_index; + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration)) + { + case LargeIslandSplitter::EStatus::BatchRetrieved: + // Solve the batch + ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte); + mContactManager.SolvePositionConstraints(contacts_begin, contacts_end); + + // Mark the batch as processed + bool last_iteration, final_batch; + mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch); + + // The final batch will update all bounds and check sleeping + if (final_batch) + CheckSleepAndUpdateBounds(mLargeIslandSplitter.GetIslandIndex(split_island_index), ioContext, ioStep, bodies_to_sleep); + + // We processed work, loop again + continue; + + case LargeIslandSplitter::EStatus::WaitingForBatch: + break; + + case LargeIslandSplitter::EStatus::AllBatchesDone: + check_split_islands = false; + break; + } + } + + // If that didn't succeed try to process an island + if (check_islands) + { + // Next island + uint32 island_idx = ioStep->mSolvePositionConstraintsNextIsland++; + if (island_idx >= mIslandBuilder.GetNumIslands()) + { + // We processed all islands, stop checking islands + check_islands = false; + continue; + } + + JPH_PROFILE("Island"); + + // Get iterators for this island + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end); + mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end); + + // If this island is a large island, it will be picked up as a batch and we don't need to do anything here + uint num_items = uint(constraints_end - constraints_begin) + uint(contacts_end - contacts_begin); + if (mPhysicsSettings.mUseLargeIslandSplitter + && num_items >= LargeIslandSplitter::cLargeIslandTreshold) + continue; + + // Check if this island needs solving + if (num_items > 0) + { + // Iterate + uint num_position_steps = mIslandBuilder.GetNumPositionSteps(island_idx); + for (uint position_step = 0; position_step < num_position_steps; ++position_step) + { + bool applied_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte); + applied_impulse |= mContactManager.SolvePositionConstraints(contacts_begin, contacts_end); + if (!applied_impulse) + break; + } + } + + // After solving we will update all bounds and check sleeping + CheckSleepAndUpdateBounds(island_idx, ioContext, ioStep, bodies_to_sleep); + + // We processed work, loop again + continue; + } + + if (check_islands) + { + // If there are islands, we don't need to wait and can pick up new work + continue; + } + else if (check_split_islands) + { + // If there are split islands, but we did't do any work, give up a time slice + std::this_thread::yield(); + } + else + { + // No more work + break; + } + } +} + +void PhysicsSystem::JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ + JPH_PROFILE_FUNCTION(); + + { + #ifdef JPH_ENABLE_ASSERTS + // Reading soft body positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); + #endif + + // Get the active soft bodies + BodyIDVector active_bodies; + mBodyManager.GetActiveBodies(EBodyType::SoftBody, active_bodies); + + // Quit if there are no active soft bodies + if (active_bodies.empty()) + { + // Kick the next step + if (ioStep->mStartNextStep.IsValid()) + ioStep->mStartNextStep.RemoveDependency(); + return; + } + + // Sort to get a deterministic update order + QuickSort(active_bodies.begin(), active_bodies.end()); + + // Allocate soft body contexts + ioContext->mNumSoftBodies = (uint)active_bodies.size(); + ioContext->mSoftBodyUpdateContexts = (SoftBodyUpdateContext *)ioContext->mTempAllocator->Allocate(ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext)); + + // Initialize soft body contexts + for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx) + { + new (sb_ctx) SoftBodyUpdateContext; + Body &body = mBodyManager.GetBody(active_bodies[sb_ctx - ioContext->mSoftBodyUpdateContexts]); + SoftBodyMotionProperties *mp = static_cast(body.GetMotionProperties()); + mp->InitializeUpdateContext(ioContext->mStepDeltaTime, body, *this, *sb_ctx); + } + } + + // We're ready to collide the first soft body + ioContext->mSoftBodyToCollide.store(0, memory_order_release); + + // Determine number of jobs to spawn + int num_soft_body_jobs = ioContext->GetMaxConcurrency(); + + // Create finalize job + ioStep->mSoftBodyFinalize = ioContext->mJobSystem->CreateJob("SoftBodyFinalize", cColorSoftBodyFinalize, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobSoftBodyFinalize(ioContext); + + // Kick the next step + if (ioStep->mStartNextStep.IsValid()) + ioStep->mStartNextStep.RemoveDependency(); + }, num_soft_body_jobs); // depends on: soft body simulate + ioContext->mBarrier->AddJob(ioStep->mSoftBodyFinalize); + + // Create simulate jobs + ioStep->mSoftBodySimulate.resize(num_soft_body_jobs); + for (int i = 0; i < num_soft_body_jobs; ++i) + ioStep->mSoftBodySimulate[i] = ioContext->mJobSystem->CreateJob("SoftBodySimulate", cColorSoftBodySimulate, [ioStep, i]() + { + ioStep->mContext->mPhysicsSystem->JobSoftBodySimulate(ioStep->mContext, i); + + ioStep->mSoftBodyFinalize.RemoveDependency(); + }, num_soft_body_jobs); // depends on: soft body collide + ioContext->mBarrier->AddJobs(ioStep->mSoftBodySimulate.data(), ioStep->mSoftBodySimulate.size()); + + // Create collision jobs + ioStep->mSoftBodyCollide.resize(num_soft_body_jobs); + for (int i = 0; i < num_soft_body_jobs; ++i) + ioStep->mSoftBodyCollide[i] = ioContext->mJobSystem->CreateJob("SoftBodyCollide", cColorSoftBodyCollide, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobSoftBodyCollide(ioContext); + + for (const JobHandle &h : ioStep->mSoftBodySimulate) + h.RemoveDependency(); + }); // depends on: nothing + ioContext->mBarrier->AddJobs(ioStep->mSoftBodyCollide.data(), ioStep->mSoftBodyCollide.size()); +} + +void PhysicsSystem::JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const +{ +#ifdef JPH_ENABLE_ASSERTS + // Reading rigid body positions and velocities + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); +#endif + + for (;;) + { + // Fetch the next soft body + uint sb_idx = ioContext->mSoftBodyToCollide.fetch_add(1, std::memory_order_acquire); + if (sb_idx >= ioContext->mNumSoftBodies) + break; + + // Do a broadphase check + SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[sb_idx]; + sb_ctx.mMotionProperties->DetermineCollidingShapes(sb_ctx, *this, GetBodyLockInterfaceNoLock()); + } +} + +void PhysicsSystem::JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const +{ +#ifdef JPH_ENABLE_ASSERTS + // Updating velocities of soft bodies, allow the contact listener to read the soft body state + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + // Calculate at which body we start to distribute the workload across the threads + uint num_soft_bodies = ioContext->mNumSoftBodies; + uint start_idx = inThreadIndex * num_soft_bodies / ioContext->GetMaxConcurrency(); + + // Keep running partial updates until everything has been updated + uint status; + do + { + // Reset status + status = 0; + + // Update all soft bodies + for (uint i = 0; i < num_soft_bodies; ++i) + { + // Fetch the soft body context + SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[(start_idx + i) % num_soft_bodies]; + + // To avoid trashing the cache too much, we prefer to stick to one soft body until we cannot progress it any further + uint sb_status; + do + { + sb_status = (uint)sb_ctx.mMotionProperties->ParallelUpdate(sb_ctx, mPhysicsSettings); + status |= sb_status; + } while (sb_status == (uint)SoftBodyMotionProperties::EStatus::DidWork); + } + + // If we didn't perform any work, yield the thread so that something else can run + if (!(status & (uint)SoftBodyMotionProperties::EStatus::DidWork)) + std::this_thread::yield(); + } + while (status != (uint)SoftBodyMotionProperties::EStatus::Done); +} + +void PhysicsSystem::JobSoftBodyFinalize(PhysicsUpdateContext *ioContext) +{ +#ifdef JPH_ENABLE_ASSERTS + // Updating rigid body velocities and soft body positions / velocities + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); + + // Can activate and deactivate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, true); +#endif + + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + BodyID *bodies_to_put_to_sleep = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_put_to_sleep = 0; + + for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx) + { + // Apply the rigid body velocity deltas + sb_ctx->mMotionProperties->UpdateRigidBodyVelocities(*sb_ctx, GetBodyInterfaceNoLock()); + + // Update the position + sb_ctx->mBody->SetPositionAndRotationInternal(sb_ctx->mBody->GetPosition() + sb_ctx->mDeltaPosition, sb_ctx->mBody->GetRotation(), false); + + BodyID id = sb_ctx->mBody->GetID(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = id; + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + + if (sb_ctx->mCanSleep == ECanSleep::CanSleep) + { + // This body should go to sleep + bodies_to_put_to_sleep[num_bodies_to_put_to_sleep++] = id; + if (num_bodies_to_put_to_sleep == cBodiesBatch) + { + mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep); + num_bodies_to_put_to_sleep = 0; + } + } + } + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + + // Notify bodies to go to sleep + if (num_bodies_to_put_to_sleep > 0) + mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep); + + // Free soft body contexts + ioContext->mTempAllocator->Free(ioContext->mSoftBodyUpdateContexts, ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext)); +} + +void PhysicsSystem::SaveState(StateRecorder &inStream, EStateRecorderState inState, const StateRecorderFilter *inFilter) const +{ + JPH_PROFILE_FUNCTION(); + + inStream.Write(inState); + + if (uint8(inState) & uint8(EStateRecorderState::Global)) + { + inStream.Write(mPreviousStepDeltaTime); + inStream.Write(mGravity); + } + + if (uint8(inState) & uint8(EStateRecorderState::Bodies)) + mBodyManager.SaveState(inStream, inFilter); + + if (uint8(inState) & uint8(EStateRecorderState::Contacts)) + mContactManager.SaveState(inStream, inFilter); + + if (uint8(inState) & uint8(EStateRecorderState::Constraints)) + mConstraintManager.SaveState(inStream, inFilter); +} + +bool PhysicsSystem::RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter) +{ + JPH_PROFILE_FUNCTION(); + + EStateRecorderState state = EStateRecorderState::All; // Set this value for validation. If a partial state is saved, validation will not work anyway. + inStream.Read(state); + + if (uint8(state) & uint8(EStateRecorderState::Global)) + { + inStream.Read(mPreviousStepDeltaTime); + inStream.Read(mGravity); + } + + if (uint8(state) & uint8(EStateRecorderState::Bodies)) + { + if (!mBodyManager.RestoreState(inStream)) + return false; + + // Update bounding boxes for all bodies in the broadphase + if (inStream.IsLastPart()) + { + Array bodies; + for (const Body *b : mBodyManager.GetBodies()) + if (BodyManager::sIsValidBodyPointer(b) && b->IsInBroadPhase()) + bodies.push_back(b->GetID()); + if (!bodies.empty()) + mBroadPhase->NotifyBodiesAABBChanged(&bodies[0], (int)bodies.size()); + } + } + + if (uint8(state) & uint8(EStateRecorderState::Contacts)) + { + if (!mContactManager.RestoreState(inStream, inFilter)) + return false; + } + + if (uint8(state) & uint8(EStateRecorderState::Constraints)) + { + if (!mConstraintManager.RestoreState(inStream)) + return false; + } + + return true; +} + +void PhysicsSystem::SaveBodyState(const Body &inBody, StateRecorder &inStream) const +{ + mBodyManager.SaveBodyState(inBody, inStream); +} + +void PhysicsSystem::RestoreBodyState(Body &ioBody, StateRecorder &inStream) +{ + mBodyManager.RestoreBodyState(ioBody, inStream); + + BodyID id = ioBody.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h new file mode 100644 index 000000000000..5b1d1acf54ca --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h @@ -0,0 +1,338 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class JobSystem; +class StateRecorder; +class TempAllocator; +class PhysicsStepListener; +class SoftBodyContactListener; +class SimShapeFilter; + +/// The main class for the physics system. It contains all rigid bodies and simulates them. +/// +/// The main simulation is performed by the Update() call on multiple threads (if the JobSystem is configured to use them). Please refer to the general architecture overview in the Docs folder for more information. +class JPH_EXPORT PhysicsSystem : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / Destructor + PhysicsSystem() : mContactManager(mPhysicsSettings) JPH_IF_ENABLE_ASSERTS(, mConstraintManager(&mBodyManager)) { } + ~PhysicsSystem(); + + /// Initialize the system. + /// @param inMaxBodies Maximum number of bodies to support. + /// @param inNumBodyMutexes Number of body mutexes to use. Should be a power of 2 in the range [1, 64], use 0 to auto detect. + /// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching. + /// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world). + /// @param inBroadPhaseLayerInterface Information on the mapping of object layers to broad phase layers. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + /// @param inObjectVsBroadPhaseLayerFilter Filter callback function that is used to determine if an object layer collides with a broad phase layer. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + /// @param inObjectLayerPairFilter Filter callback function that is used to determine if two object layers collide. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + void Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter); + + /// Listener that is notified whenever a body is activated/deactivated + void SetBodyActivationListener(BodyActivationListener *inListener) { mBodyManager.SetBodyActivationListener(inListener); } + BodyActivationListener * GetBodyActivationListener() const { return mBodyManager.GetBodyActivationListener(); } + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed. + /// You can't change contact listener during PhysicsSystem::Update but it can be changed at any other time. + void SetContactListener(ContactListener *inListener) { mContactManager.SetContactListener(inListener); } + ContactListener * GetContactListener() const { return mContactManager.GetContactListener(); } + + /// Listener that is notified whenever a contact point between a soft body and another body + void SetSoftBodyContactListener(SoftBodyContactListener *inListener) { mSoftBodyContactListener = inListener; } + SoftBodyContactListener * GetSoftBodyContactListener() const { return mSoftBodyContactListener; } + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(ContactConstraintManager::CombineFunction inCombineFriction) { mContactManager.SetCombineFriction(inCombineFriction); } + ContactConstraintManager::CombineFunction GetCombineFriction() const { return mContactManager.GetCombineFriction(); } + + /// Set the function that combines the restitution of two bodies and returns it + /// Default method is max(restitution1, restitution1) + void SetCombineRestitution(ContactConstraintManager::CombineFunction inCombineRestition) { mContactManager.SetCombineRestitution(inCombineRestition); } + ContactConstraintManager::CombineFunction GetCombineRestitution() const { return mContactManager.GetCombineRestitution(); } + + /// Set/get the shape filter that will be used during simulation. This can be used to exclude shapes within a body from colliding with each other. + /// E.g. if you have a high detail and a low detail collision model, you can attach them to the same body in a StaticCompoundShape and use the ShapeFilter + /// to exclude the high detail collision model when simulating and exclude the low detail collision model when casting rays. Note that in this case + /// you would need to pass the inverse of inShapeFilter to the CastRay function. Pass a nullptr to disable the shape filter. + /// The PhysicsSystem does not own the ShapeFilter, make sure it stays alive during the lifetime of the PhysicsSystem. + void SetSimShapeFilter(const SimShapeFilter *inShapeFilter) { mSimShapeFilter = inShapeFilter; } + const SimShapeFilter * GetSimShapeFilter() const { return mSimShapeFilter; } + + /// Control the main constants of the physics simulation + void SetPhysicsSettings(const PhysicsSettings &inSettings) { mPhysicsSettings = inSettings; } + const PhysicsSettings & GetPhysicsSettings() const { return mPhysicsSettings; } + + /// Access to the body interface. This interface allows to to create / remove bodies and to change their properties. + const BodyInterface & GetBodyInterface() const { return mBodyInterfaceLocking; } + BodyInterface & GetBodyInterface() { return mBodyInterfaceLocking; } + const BodyInterface & GetBodyInterfaceNoLock() const { return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care! + BodyInterface & GetBodyInterfaceNoLock() { return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care! + + /// Access to the broadphase interface that allows coarse collision queries + const BroadPhaseQuery & GetBroadPhaseQuery() const { return *mBroadPhase; } + + /// Interface that allows fine collision queries against first the broad phase and then the narrow phase. + const NarrowPhaseQuery & GetNarrowPhaseQuery() const { return mNarrowPhaseQueryLocking; } + const NarrowPhaseQuery & GetNarrowPhaseQueryNoLock() const { return mNarrowPhaseQueryNoLock; } ///< Version that does not lock the bodies, use with great care! + + /// Add constraint to the world + void AddConstraint(Constraint *inConstraint) { mConstraintManager.Add(&inConstraint, 1); } + + /// Remove constraint from the world + void RemoveConstraint(Constraint *inConstraint) { mConstraintManager.Remove(&inConstraint, 1); } + + /// Batch add constraints. + void AddConstraints(Constraint **inConstraints, int inNumber) { mConstraintManager.Add(inConstraints, inNumber); } + + /// Batch remove constraints. + void RemoveConstraints(Constraint **inConstraints, int inNumber) { mConstraintManager.Remove(inConstraints, inNumber); } + + /// Get a list of all constraints + Constraints GetConstraints() const { return mConstraintManager.GetConstraints(); } + + /// Optimize the broadphase, needed only if you've added many bodies prior to calling Update() for the first time. + /// Don't call this every frame as PhysicsSystem::Update spreads out the same work over multiple frames. + /// If you add many bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize and if the bodies in a batch are + /// in a roughly unoccupied space (e.g. a new level section) then a call to OptimizeBroadPhase is also not needed + /// as batch adding creates an efficient bounding volume hierarchy. + /// Don't call this function while bodies are being modified from another thread or use the locking BodyInterface to modify bodies. + void OptimizeBroadPhase(); + + /// Adds a new step listener + void AddStepListener(PhysicsStepListener *inListener); + + /// Removes a step listener + void RemoveStepListener(PhysicsStepListener *inListener); + + /// Simulate the system. + /// The world steps for a total of inDeltaTime seconds. This is divided in inCollisionSteps iterations. + /// Each iteration consists of collision detection followed by an integration step. + /// This function internally spawns jobs using inJobSystem and waits for them to complete, so no jobs will be running when this function returns. + EPhysicsUpdateError Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem); + + /// Saving state for replay + void SaveState(StateRecorder &inStream, EStateRecorderState inState = EStateRecorderState::All, const StateRecorderFilter *inFilter = nullptr) const; + + /// Restoring state for replay. Returns false if failed. + bool RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter = nullptr); + + /// Saving state of a single body. + void SaveBodyState(const Body &inBody, StateRecorder &inStream) const; + + /// Restoring state of a single body. + void RestoreBodyState(Body &ioBody, StateRecorder &inStream); + +#ifdef JPH_DEBUG_RENDERER + // Drawing properties + static bool sDrawMotionQualityLinearCast; ///< Draw debug info for objects that perform continuous collision detection through the linear cast motion quality + + /// Draw the state of the bodies (debugging purposes) + void DrawBodies(const BodyManager::DrawSettings &inSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr) { mBodyManager.Draw(inSettings, mPhysicsSettings, inRenderer, inBodyFilter); } + + /// Draw the constraints only (debugging purposes) + void DrawConstraints(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraints(inRenderer); } + + /// Draw the constraint limits only (debugging purposes) + void DrawConstraintLimits(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraintLimits(inRenderer); } + + /// Draw the constraint reference frames only (debugging purposes) + void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraintReferenceFrame(inRenderer); } +#endif // JPH_DEBUG_RENDERER + + /// Set gravity value + void SetGravity(Vec3Arg inGravity) { mGravity = inGravity; } + Vec3 GetGravity() const { return mGravity; } + + /// Returns a locking interface that won't actually lock the body. Use with great care! + inline const BodyLockInterfaceNoLock & GetBodyLockInterfaceNoLock() const { return mBodyLockInterfaceNoLock; } + + /// Returns a locking interface that locks the body so other threads cannot modify it. + inline const BodyLockInterfaceLocking & GetBodyLockInterface() const { return mBodyLockInterfaceLocking; } + + /// Get an broadphase layer filter that uses the default pair filter and a specified object layer to determine if broadphase layers collide + DefaultBroadPhaseLayerFilter GetDefaultBroadPhaseLayerFilter(ObjectLayer inLayer) const { return DefaultBroadPhaseLayerFilter(*mObjectVsBroadPhaseLayerFilter, inLayer); } + + /// Get an object layer filter that uses the default pair filter and a specified layer to determine if layers collide + DefaultObjectLayerFilter GetDefaultLayerFilter(ObjectLayer inLayer) const { return DefaultObjectLayerFilter(*mObjectLayerPairFilter, inLayer); } + + /// Gets the current amount of bodies that are in the body manager + uint GetNumBodies() const { return mBodyManager.GetNumBodies(); } + + /// Gets the current amount of active bodies that are in the body manager + uint32 GetNumActiveBodies(EBodyType inType) const { return mBodyManager.GetNumActiveBodies(inType); } + + /// Get the maximum amount of bodies that this physics system supports + uint GetMaxBodies() const { return mBodyManager.GetMaxBodies(); } + + /// Helper struct that counts the number of bodies of each type + using BodyStats = BodyManager::BodyStats; + + /// Get stats about the bodies in the body manager (slow, iterates through all bodies) + BodyStats GetBodyStats() const { return mBodyManager.GetBodyStats(); } + + /// Get copy of the list of all bodies under protection of a lock. + /// @param outBodyIDs On return, this will contain the list of BodyIDs + void GetBodies(BodyIDVector &outBodyIDs) const { return mBodyManager.GetBodyIDs(outBodyIDs); } + + /// Get copy of the list of active bodies under protection of a lock. + /// @param inType The type of bodies to get + /// @param outBodyIDs On return, this will contain the list of BodyIDs + void GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const { return mBodyManager.GetActiveBodies(inType, outBodyIDs); } + + /// Get the list of active bodies, use GetNumActiveBodies() to find out how long the list is. + /// Note: Not thread safe. The active bodies list can change at any moment when other threads are doing work. Use GetActiveBodies() if you need a thread safe version. + const BodyID * GetActiveBodiesUnsafe(EBodyType inType) const { return mBodyManager.GetActiveBodiesUnsafe(inType); } + + /// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, so at least one of the bodies must be active in order for this function to work. + /// It queries the state at the time of the last PhysicsSystem::Update and will return true if the bodies were in contact, even if one of the bodies was moved / removed afterwards. + /// This function can be called from any thread when the PhysicsSystem::Update is not running. During PhysicsSystem::Update this function is only valid during contact callbacks: + /// - During the ContactListener::OnContactAdded callback this function can be used to determine if a different contact pair between the bodies was active in the previous simulation step (function returns true) or if this is the first step that the bodies are touching (function returns false). + /// - During the ContactListener::OnContactRemoved callback this function can be used to determine if this is the last contact pair between the bodies (function returns false) or if there are other contacts still present (function returns true). + bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const { return mContactManager.WereBodiesInContact(inBody1ID, inBody2ID); } + + /// Get the bounding box of all bodies in the physics system + AABox GetBounds() const { return mBroadPhase->GetBounds(); } + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Trace the accumulated broadphase stats to the TTY + void ReportBroadphaseStats() { mBroadPhase->ReportStats(); } +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + using CCDBody = PhysicsUpdateContext::Step::CCDBody; + + // Various job entry points + void JobStepListeners(PhysicsUpdateContext::Step *ioStep); + void JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const; + void JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const; + void JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex); + void JobFinalizeIslands(PhysicsUpdateContext *ioContext); + void JobBodySetIslandIndex(); + void JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const; + void JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep); + void JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const; + void JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const; + void JobSoftBodyFinalize(PhysicsUpdateContext *ioContext); + + /// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet + void TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const; + + using ContactAllocator = ContactConstraintManager::ContactAllocator; + + /// Process narrow phase for a single body pair + void ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair); + + /// This helper batches up bodies that need to put to sleep to avoid contention on the activation mutex + class BodiesToSleep; + + /// Called at the end of JobSolveVelocityConstraints to check if bodies need to go to sleep and to update their bounding box in the broadphase + void CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep); + + /// Number of constraints to process at once in JobDetermineActiveConstraints + static constexpr int cDetermineActiveConstraintsBatchSize = 64; + + /// Number of constraints to process at once in JobSetupVelocityConstraints, we want a low number of threads working on this so we take fairly large batches + static constexpr int cSetupVelocityConstraintsBatchSize = 256; + + /// Number of bodies to process at once in JobApplyGravity + static constexpr int cApplyGravityBatchSize = 64; + + /// Number of active bodies to test for collisions per batch + static constexpr int cActiveBodiesBatchSize = 16; + + /// Number of active bodies to integrate velocities for + static constexpr int cIntegrateVelocityBatchSize = 64; + + /// Number of contacts that need to be queued before another narrow phase job is started + static constexpr int cNarrowPhaseBatchSize = 16; + + /// Number of continuous collision shape casts that need to be queued before another job is started + static constexpr int cNumCCDBodiesPerJob = 4; + + /// Broadphase layer filter that decides if two objects can collide + const ObjectVsBroadPhaseLayerFilter *mObjectVsBroadPhaseLayerFilter = nullptr; + + /// Object layer filter that decides if two objects can collide + const ObjectLayerPairFilter *mObjectLayerPairFilter = nullptr; + + /// The body manager keeps track which bodies are in the simulation + BodyManager mBodyManager; + + /// Body locking interfaces + BodyLockInterfaceNoLock mBodyLockInterfaceNoLock { mBodyManager }; + BodyLockInterfaceLocking mBodyLockInterfaceLocking { mBodyManager }; + + /// Body interfaces + BodyInterface mBodyInterfaceNoLock; + BodyInterface mBodyInterfaceLocking; + + /// Narrow phase query interface + NarrowPhaseQuery mNarrowPhaseQueryNoLock; + NarrowPhaseQuery mNarrowPhaseQueryLocking; + + /// The broadphase does quick collision detection between body pairs + BroadPhase * mBroadPhase = nullptr; + + /// The soft body contact listener + SoftBodyContactListener * mSoftBodyContactListener = nullptr; + + /// The shape filter that is used to filter out sub shapes during simulation + const SimShapeFilter * mSimShapeFilter = nullptr; + + /// Simulation settings + PhysicsSettings mPhysicsSettings; + + /// The contact manager resolves all contacts during a simulation step + ContactConstraintManager mContactManager; + + /// All non-contact constraints + ConstraintManager mConstraintManager; + + /// Keeps track of connected bodies and builds islands for multithreaded velocity/position update + IslandBuilder mIslandBuilder; + + /// Will split large islands into smaller groups of bodies that can be processed in parallel + LargeIslandSplitter mLargeIslandSplitter; + + /// Mutex protecting mStepListeners + Mutex mStepListenersMutex; + + /// List of physics step listeners + using StepListeners = Array; + StepListeners mStepListeners; + + /// This is the global gravity vector + Vec3 mGravity = Vec3(0, -9.81f, 0); + + /// Previous frame's delta time of one sub step to allow scaling previous frame's constraint impulses + float mPreviousStepDeltaTime = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp new file mode 100644 index 000000000000..7c60ae6be66c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp @@ -0,0 +1,23 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +PhysicsUpdateContext::PhysicsUpdateContext(TempAllocator &inTempAllocator) : + mTempAllocator(&inTempAllocator), + mSteps(inTempAllocator) +{ +} + +PhysicsUpdateContext::~PhysicsUpdateContext() +{ + JPH_ASSERT(mBodyPairs == nullptr); + JPH_ASSERT(mActiveConstraints == nullptr); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h new file mode 100644 index 000000000000..fe99c46cccf6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h @@ -0,0 +1,172 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class IslandBuilder; +class Constraint; +class TempAllocator; +class SoftBodyUpdateContext; + +/// Information used during the Update call +class PhysicsUpdateContext : public NonCopyable +{ +public: + /// Destructor + explicit PhysicsUpdateContext(TempAllocator &inTempAllocator); + ~PhysicsUpdateContext(); + + static constexpr int cMaxConcurrency = 32; ///< Maximum supported amount of concurrent jobs + + using JobHandleArray = StaticArray; + + struct Step; + + struct BodyPairQueue + { + atomic mWriteIdx { 0 }; ///< Next index to write in mBodyPair array (need to add thread index * mMaxBodyPairsPerQueue and modulo mMaxBodyPairsPerQueue) + uint8 mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Moved to own cache line to avoid conflicts with consumer jobs + + atomic mReadIdx { 0 }; ///< Next index to read in mBodyPair array (need to add thread index * mMaxBodyPairsPerQueue and modulo mMaxBodyPairsPerQueue) + uint8 mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Moved to own cache line to avoid conflicts with producer/consumer jobs + }; + + using BodyPairQueues = StaticArray; + + using JobMask = uint32; ///< A mask that has as many bits as we can have concurrent jobs + static_assert(sizeof(JobMask) * 8 >= cMaxConcurrency); + + /// Structure that contains data needed for each collision step. + struct Step + { + Step() = default; + Step(const Step &) { JPH_ASSERT(false); } // vector needs a copy constructor, but we're never going to call it + + PhysicsUpdateContext *mContext; ///< The physics update context + + bool mIsFirst; ///< If this is the first step + bool mIsLast; ///< If this is the last step + + BroadPhase::UpdateState mBroadPhaseUpdateState; ///< Handle returned by Broadphase::UpdatePrepare + + uint32 mNumActiveBodiesAtStepStart; ///< Number of bodies that were active at the start of the physics update step. Only these bodies will receive gravity (they are the first N in the active body list). + + atomic mDetermineActiveConstraintReadIdx { 0 }; ///< Next constraint for determine active constraints + uint8 mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mNumActiveConstraints { 0 }; ///< Number of constraints in the mActiveConstraints array + uint8 mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mSetupVelocityConstraintsReadIdx { 0 }; ///< Next constraint for setting up velocity constraints + uint8 mPadding3[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mStepListenerReadIdx { 0 }; ///< Next step listener to call + uint8 mPadding4[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mApplyGravityReadIdx { 0 }; ///< Next body to apply gravity to + uint8 mPadding5[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mActiveBodyReadIdx { 0 }; ///< Index of fist active body that has not yet been processed by the broadphase + uint8 mPadding6[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + BodyPairQueues mBodyPairQueues; ///< Queues in which to put body pairs that need to be tested by the narrowphase + + uint32 mMaxBodyPairsPerQueue; ///< Amount of body pairs that we can queue per queue + + atomic mActiveFindCollisionJobs; ///< A bitmask that indicates which jobs are still active + + atomic mNumBodyPairs { 0 }; ///< The number of body pairs found in this step (used to size the contact cache in the next step) + atomic mNumManifolds { 0 }; ///< The number of manifolds found in this step (used to size the contact cache in the next step) + + atomic mSolveVelocityConstraintsNextIsland { 0 }; ///< Next island that needs to be processed for the solve velocity constraints step (doesn't need own cache line since position jobs don't run at same time) + atomic mSolvePositionConstraintsNextIsland { 0 }; ///< Next island that needs to be processed for the solve position constraints step (doesn't need own cache line since velocity jobs don't run at same time) + + /// Contains the information needed to cast a body through the scene to do continuous collision detection + struct CCDBody + { + CCDBody(BodyID inBodyID1, Vec3Arg inDeltaPosition, float inLinearCastThresholdSq, float inMaxPenetration) : mDeltaPosition(inDeltaPosition), mBodyID1(inBodyID1), mLinearCastThresholdSq(inLinearCastThresholdSq), mMaxPenetration(inMaxPenetration) { } + + Vec3 mDeltaPosition; ///< Desired rotation step + Vec3 mContactNormal; ///< World space normal of closest hit (only valid if mFractionPlusSlop < 1) + RVec3 mContactPointOn2; ///< World space contact point on body 2 of closest hit (only valid if mFractionPlusSlop < 1) + BodyID mBodyID1; ///< Body 1 (the body that is performing collision detection) + BodyID mBodyID2; ///< Body 2 (the body of the closest hit, only valid if mFractionPlusSlop < 1) + SubShapeID mSubShapeID2; ///< Sub shape of body 2 that was hit (only valid if mFractionPlusSlop < 1) + float mFraction = 1.0f; ///< Fraction at which the hit occurred + float mFractionPlusSlop = 1.0f; ///< Fraction at which the hit occurred + extra delta to allow body to penetrate by mMaxPenetration + float mLinearCastThresholdSq; ///< Maximum allowed squared movement before doing a linear cast (determined by inner radius of shape) + float mMaxPenetration; ///< Maximum allowed penetration (determined by inner radius of shape) + ContactSettings mContactSettings; ///< The contact settings for this contact + }; + atomic mIntegrateVelocityReadIdx { 0 }; ///< Next active body index to take when integrating velocities + CCDBody * mCCDBodies = nullptr; ///< List of bodies that need to do continuous collision detection + uint32 mCCDBodiesCapacity = 0; ///< Capacity of the mCCDBodies list + atomic mNumCCDBodies = 0; ///< Number of CCD bodies in mCCDBodies + atomic mNextCCDBody { 0 }; ///< Next unprocessed body index in mCCDBodies + int * mActiveBodyToCCDBody = nullptr; ///< A mapping between an index in BodyManager::mActiveBodies and the index in mCCDBodies + uint32 mNumActiveBodyToCCDBody = 0; ///< Number of indices in mActiveBodyToCCDBody + + // Jobs in order of execution (some run in parallel) + JobHandle mBroadPhasePrepare; ///< Prepares the new tree in the background + JobHandleArray mStepListeners; ///< Listeners to notify of the beginning of a physics step + JobHandleArray mDetermineActiveConstraints; ///< Determine which constraints will be active during this step + JobHandleArray mApplyGravity; ///< Update velocities of bodies with gravity + JobHandleArray mFindCollisions; ///< Find all collisions between active bodies an the world + JobHandle mUpdateBroadphaseFinalize; ///< Swap the newly built tree with the current tree + JobHandleArray mSetupVelocityConstraints; ///< Calculate properties for all constraints in the constraint manager + JobHandle mBuildIslandsFromConstraints; ///< Go over all constraints and assign the bodies they're attached to to an island + JobHandle mFinalizeIslands; ///< Finalize calculation simulation islands + JobHandle mBodySetIslandIndex; ///< Set the current island index on each body (not used by the simulation, only for drawing purposes) + JobHandleArray mSolveVelocityConstraints; ///< Solve the constraints in the velocity domain + JobHandle mPreIntegrateVelocity; ///< Setup integration of all body positions + JobHandleArray mIntegrateVelocity; ///< Integrate all body positions + JobHandle mPostIntegrateVelocity; ///< Finalize integration of all body positions + JobHandle mResolveCCDContacts; ///< Updates the positions and velocities for all bodies that need continuous collision detection + JobHandleArray mSolvePositionConstraints; ///< Solve all constraints in the position domain + JobHandle mContactRemovedCallbacks; ///< Calls the contact removed callbacks + JobHandle mSoftBodyPrepare; ///< Prepares updating the soft bodies + JobHandleArray mSoftBodyCollide; ///< Finds all colliding shapes for soft bodies + JobHandleArray mSoftBodySimulate; ///< Simulates all particles + JobHandle mSoftBodyFinalize; ///< Finalizes the soft body update + JobHandle mStartNextStep; ///< Job that kicks the next step (empty for the last step) + }; + + using Steps = Array>; + + /// Maximum amount of concurrent jobs on this machine + int GetMaxConcurrency() const { const int max_concurrency = PhysicsUpdateContext::cMaxConcurrency; return min(max_concurrency, mJobSystem->GetMaxConcurrency()); } ///< Need to put max concurrency in temp var as min requires a reference + + PhysicsSystem * mPhysicsSystem; ///< The physics system we belong to + TempAllocator * mTempAllocator; ///< Temporary allocator used during the update + JobSystem * mJobSystem; ///< Job system that processes jobs + JobSystem::Barrier * mBarrier; ///< Barrier used to wait for all physics jobs to complete + + float mStepDeltaTime; ///< Delta time for a simulation step (collision step) + float mWarmStartImpulseRatio; ///< Ratio of this step delta time vs last step + atomic mErrors { 0 }; ///< Errors that occurred during the update, actual type is EPhysicsUpdateError + + Constraint ** mActiveConstraints = nullptr; ///< Constraints that were active at the start of the physics update step (activating bodies can activate constraints and we need a consistent snapshot). Only these constraints will be resolved. + + BodyPair * mBodyPairs = nullptr; ///< A list of body pairs found by the broadphase + + IslandBuilder * mIslandBuilder; ///< Keeps track of connected bodies and builds islands for multithreaded velocity/position update + + Steps mSteps; + + uint mNumSoftBodies; ///< Number of active soft bodies in the simulation + SoftBodyUpdateContext * mSoftBodyUpdateContexts = nullptr; ///< Contexts for updating soft bodies + atomic mSoftBodyToCollide { 0 }; ///< Next soft body to take when running SoftBodyCollide jobs +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp new file mode 100644 index 000000000000..9d7e5b675d42 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp @@ -0,0 +1,705 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::Part) +{ + JPH_ADD_BASE_CLASS(RagdollSettings::Part, BodyCreationSettings) + + JPH_ADD_ATTRIBUTE(RagdollSettings::Part, mToParent) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::AdditionalConstraint) +{ + JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mBodyIdx) + JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mConstraint) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings) +{ + JPH_ADD_ATTRIBUTE(RagdollSettings, mSkeleton) + JPH_ADD_ATTRIBUTE(RagdollSettings, mParts) + JPH_ADD_ATTRIBUTE(RagdollSettings, mAdditionalConstraints) +} + +static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock(); +} + +static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? static_cast(inSystem->GetBodyLockInterface()) : static_cast(inSystem->GetBodyLockInterfaceNoLock()); +} + +bool RagdollSettings::Stabilize() +{ + // Based on: Stop my Constraints from Blowing Up! - Oliver Strunk (Havok) + // Do 2 things: + // 1. Limit the mass ratios between parents and children (slide 16) + // 2. Increase the inertia of parents so that they're bigger or equal to the sum of their children (slide 34) + + // If we don't have any joints there's nothing to stabilize + if (mSkeleton->GetJointCount() == 0) + return true; + + // The skeleton can contain one or more static bodies. We can't modify the mass for those so we start a new stabilization chain for each joint under a static body until we reach the next static body. + // This array keeps track of which joints have been processed. + Array visited; + visited.resize(mSkeleton->GetJointCount()); + for (size_t v = 0; v < visited.size(); ++v) + { + // Mark static bodies as visited so we won't process these + Part &p = mParts[v]; + bool has_mass_properties = p.HasMassProperties(); + visited[v] = !has_mass_properties; + + if (has_mass_properties && p.mOverrideMassProperties != EOverrideMassProperties::MassAndInertiaProvided) + { + // Mass properties not yet calculated, do it now + p.mMassPropertiesOverride = p.GetMassProperties(); + p.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + } + } + + // Find first unvisited part that either has no parent or that has a parent that was visited + for (int first_idx = 0; first_idx < mSkeleton->GetJointCount(); ++first_idx) + { + int first_idx_parent = mSkeleton->GetJoint(first_idx).mParentJointIndex; + if (!visited[first_idx] && (first_idx_parent == -1 || visited[first_idx_parent])) + { + // Find all children of first_idx and their children up to the next static part + int next_to_process = 0; + Array indices; + indices.reserve(mSkeleton->GetJointCount()); + visited[first_idx] = true; + indices.push_back(first_idx); + do + { + int parent_idx = indices[next_to_process++]; + for (int child_idx = 0; child_idx < mSkeleton->GetJointCount(); ++child_idx) + if (!visited[child_idx] && mSkeleton->GetJoint(child_idx).mParentJointIndex == parent_idx) + { + visited[child_idx] = true; + indices.push_back(child_idx); + } + } while (next_to_process < (int)indices.size()); + + // If there's only 1 body, we can't redistribute mass + if (indices.size() == 1) + continue; + + const float cMinMassRatio = 0.8f; + const float cMaxMassRatio = 1.2f; + + // Ensure that the mass ratio from parent to child is within a range + float total_mass_ratio = 1.0f; + Array mass_ratios; + mass_ratios.resize(mSkeleton->GetJointCount()); + mass_ratios[indices[0]] = 1.0f; + for (int i = 1; i < (int)indices.size(); ++i) + { + int child_idx = indices[i]; + int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex; + float ratio = mParts[child_idx].mMassPropertiesOverride.mMass / mParts[parent_idx].mMassPropertiesOverride.mMass; + mass_ratios[child_idx] = mass_ratios[parent_idx] * Clamp(ratio, cMinMassRatio, cMaxMassRatio); + total_mass_ratio += mass_ratios[child_idx]; + } + + // Calculate total mass of this chain + float total_mass = 0.0f; + for (int idx : indices) + total_mass += mParts[idx].mMassPropertiesOverride.mMass; + + // Calculate how much mass belongs to a ratio of 1 + float ratio_to_mass = total_mass / total_mass_ratio; + + // Adjust all masses and inertia tensors for the new mass + for (int i : indices) + { + Part &p = mParts[i]; + float old_mass = p.mMassPropertiesOverride.mMass; + float new_mass = mass_ratios[i] * ratio_to_mass; + p.mMassPropertiesOverride.mMass = new_mass; + p.mMassPropertiesOverride.mInertia *= new_mass / old_mass; + p.mMassPropertiesOverride.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); + } + + const float cMaxInertiaIncrease = 2.0f; + + // Get the principal moments of inertia for all parts + struct Principal + { + Mat44 mRotation; + Vec3 mDiagonal; + float mChildSum = 0.0f; + }; + Array principals; + principals.resize(mParts.size()); + for (int i : indices) + if (!mParts[i].mMassPropertiesOverride.DecomposePrincipalMomentsOfInertia(principals[i].mRotation, principals[i].mDiagonal)) + { + JPH_ASSERT(false, "Failed to decompose the inertia tensor!"); + return false; + } + + // Calculate sum of child inertias + // Walk backwards so we sum the leaves first + for (int i = (int)indices.size() - 1; i > 0; --i) + { + int child_idx = indices[i]; + int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex; + principals[parent_idx].mChildSum += principals[child_idx].mDiagonal[0] + principals[child_idx].mChildSum; + } + + // Adjust inertia tensors for all parts + for (int i : indices) + { + Part &p = mParts[i]; + Principal &principal = principals[i]; + if (principal.mChildSum != 0.0f) + { + // Calculate minimum inertia this object should have based on it children + float minimum = min(cMaxInertiaIncrease * principal.mDiagonal[0], principal.mChildSum); + principal.mDiagonal = Vec3::sMax(principal.mDiagonal, Vec3::sReplicate(minimum)); + + // Recalculate moment of inertia in body space + p.mMassPropertiesOverride.mInertia = principal.mRotation * Mat44::sScale(principal.mDiagonal) * principal.mRotation.Inversed3x3(); + } + } + } + } + + return true; +} + +void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices, float inMinSeparationDistance) +{ + int joint_count = mSkeleton->GetJointCount(); + JPH_ASSERT(joint_count == (int)mParts.size()); + + // Create a group filter table that disables collisions between parent and child + Ref group_filter = new GroupFilterTable(joint_count); + for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx) + { + int parent_joint = mSkeleton->GetJoint(joint_idx).mParentJointIndex; + if (parent_joint >= 0) + group_filter->DisableCollision(joint_idx, parent_joint); + } + + // If joint matrices are provided + if (inJointMatrices != nullptr) + { + // Loop over all joints + for (int j1 = 0; j1 < joint_count; ++j1) + { + // Shape and transform for joint 1 + const Part &part1 = mParts[j1]; + const Shape *shape1 = part1.GetShape(); + Vec3 scale1; + Mat44 com1 = (inJointMatrices[j1].PreTranslated(shape1->GetCenterOfMass())).Decompose(scale1); + + // Loop over all other joints + for (int j2 = j1 + 1; j2 < joint_count; ++j2) + if (group_filter->IsCollisionEnabled(j1, j2)) // Only if collision is still enabled we need to test + { + // Shape and transform for joint 2 + const Part &part2 = mParts[j2]; + const Shape *shape2 = part2.GetShape(); + Vec3 scale2; + Mat44 com2 = (inJointMatrices[j2].PreTranslated(shape2->GetCenterOfMass())).Decompose(scale2); + + // Collision settings + CollideShapeSettings settings; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll; + settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces; + settings.mMaxSeparationDistance = inMinSeparationDistance; + + // Only check if one of the two bodies can become dynamic + if (part1.HasMassProperties() || part2.HasMassProperties()) + { + // If there is a collision, disable the collision between the joints + AnyHitCollisionCollector collector; + if (part1.HasMassProperties()) // Ensure that the first shape is always a dynamic one (we can't check mesh vs convex but we can check convex vs mesh) + CollisionDispatch::sCollideShapeVsShape(shape1, shape2, scale1, scale2, com1, com2, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector); + else + CollisionDispatch::sCollideShapeVsShape(shape2, shape1, scale2, scale1, com2, com1, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector); + if (collector.HadHit()) + group_filter->DisableCollision(j1, j2); + } + } + } + } + + // Loop over the body parts and assign them a sub group ID and the group filter + for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx) + { + Part &part = mParts[joint_idx]; + part.mCollisionGroup.SetSubGroupID(joint_idx); + part.mCollisionGroup.SetGroupFilter(group_filter); + } +} + +void RagdollSettings::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const +{ + BodyCreationSettings::ShapeToIDMap shape_to_id; + BodyCreationSettings::MaterialToIDMap material_to_id; + BodyCreationSettings::GroupFilterToIDMap group_filter_to_id; + + // Save skeleton + mSkeleton->SaveBinaryState(inStream); + + // Save parts + inStream.Write((uint32)mParts.size()); + for (const Part &p : mParts) + { + // Write body creation settings + p.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr); + + // Save constraint + inStream.Write(p.mToParent != nullptr); + if (p.mToParent != nullptr) + p.mToParent->SaveBinaryState(inStream); + } + + // Save additional constraints + inStream.Write((uint32)mAdditionalConstraints.size()); + for (const AdditionalConstraint &c : mAdditionalConstraints) + { + // Save bodies indices + inStream.Write(c.mBodyIdx); + + // Save constraint + c.mConstraint->SaveBinaryState(inStream); + } +} + +RagdollSettings::RagdollResult RagdollSettings::sRestoreFromBinaryState(StreamIn &inStream) +{ + RagdollResult result; + + // Restore skeleton + Skeleton::SkeletonResult skeleton_result = Skeleton::sRestoreFromBinaryState(inStream); + if (skeleton_result.HasError()) + { + result.SetError(skeleton_result.GetError()); + return result; + } + + // Create ragdoll + Ref ragdoll = new RagdollSettings(); + ragdoll->mSkeleton = skeleton_result.Get(); + + BodyCreationSettings::IDToShapeMap id_to_shape; + BodyCreationSettings::IDToMaterialMap id_to_material; + BodyCreationSettings::IDToGroupFilterMap id_to_group_filter; + + // Reserve some memory to avoid frequent reallocations + id_to_shape.reserve(1024); + id_to_material.reserve(128); + id_to_group_filter.reserve(128); + + // Read parts + uint32 len = 0; + inStream.Read(len); + ragdoll->mParts.resize(len); + for (Part &p : ragdoll->mParts) + { + // Read creation settings + BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter); + if (bcs_result.HasError()) + { + result.SetError(bcs_result.GetError()); + return result; + } + static_cast(p) = bcs_result.Get(); + + // Read constraint + bool has_constraint = false; + inStream.Read(has_constraint); + if (has_constraint) + { + ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (constraint_result.HasError()) + { + result.SetError(constraint_result.GetError()); + return result; + } + p.mToParent = DynamicCast(constraint_result.Get()); + } + } + + // Read additional constraints + len = 0; + inStream.Read(len); + ragdoll->mAdditionalConstraints.resize(len); + for (AdditionalConstraint &c : ragdoll->mAdditionalConstraints) + { + // Read body indices + inStream.Read(c.mBodyIdx); + + // Read constraint + ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (constraint_result.HasError()) + { + result.SetError(constraint_result.GetError()); + return result; + } + c.mConstraint = DynamicCast(constraint_result.Get()); + } + + // Create mapping tables + ragdoll->CalculateBodyIndexToConstraintIndex(); + ragdoll->CalculateConstraintIndexToBodyIdxPair(); + + result.Set(ragdoll); + return result; +} + +Ragdoll *RagdollSettings::CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const +{ + Ragdoll *r = new Ragdoll(inSystem); + r->mRagdollSettings = this; + r->mBodyIDs.reserve(mParts.size()); + r->mConstraints.reserve(mParts.size() + mAdditionalConstraints.size()); + + // Create bodies and constraints + BodyInterface &bi = inSystem->GetBodyInterface(); + Body **bodies = (Body **)JPH_STACK_ALLOC(mParts.size() * sizeof(Body *)); + int joint_idx = 0; + for (const Part &p : mParts) + { + Body *body2 = bi.CreateBody(p); + if (body2 == nullptr) + { + // Out of bodies, failed to create ragdoll + delete r; + return nullptr; + } + body2->GetCollisionGroup().SetGroupID(inCollisionGroup); + body2->SetUserData(inUserData); + + // Temporarily store body pointer for hooking up constraints + bodies[joint_idx] = body2; + + // Create constraint + if (p.mToParent != nullptr) + { + Body *body1 = bodies[mSkeleton->GetJoint(joint_idx).mParentJointIndex]; + r->mConstraints.push_back(p.mToParent->Create(*body1, *body2)); + } + + // Store body ID and constraint in parallel arrays + r->mBodyIDs.push_back(body2->GetID()); + + ++joint_idx; + } + + // Add additional constraints + for (const AdditionalConstraint &c : mAdditionalConstraints) + { + Body *body1 = bodies[c.mBodyIdx[0]]; + Body *body2 = bodies[c.mBodyIdx[1]]; + r->mConstraints.push_back(c.mConstraint->Create(*body1, *body2)); + } + + return r; +} + +void RagdollSettings::CalculateBodyIndexToConstraintIndex() +{ + mBodyIndexToConstraintIndex.clear(); + mBodyIndexToConstraintIndex.reserve(mParts.size()); + + int constraint_index = 0; + for (const Part &p : mParts) + { + if (p.mToParent != nullptr) + mBodyIndexToConstraintIndex.push_back(constraint_index++); + else + mBodyIndexToConstraintIndex.push_back(-1); + } +} + +void RagdollSettings::CalculateConstraintIndexToBodyIdxPair() +{ + mConstraintIndexToBodyIdxPair.clear(); + mConstraintIndexToBodyIdxPair.reserve(mParts.size() + mAdditionalConstraints.size()); + + // Add constraints between parts + int joint_idx = 0; + for (const Part &p : mParts) + { + if (p.mToParent != nullptr) + { + int parent_joint_idx = mSkeleton->GetJoint(joint_idx).mParentJointIndex; + mConstraintIndexToBodyIdxPair.emplace_back(parent_joint_idx, joint_idx); + } + + ++joint_idx; + } + + // Add additional constraints + for (const AdditionalConstraint &c : mAdditionalConstraints) + mConstraintIndexToBodyIdxPair.emplace_back(c.mBodyIdx[0], c.mBodyIdx[1]); +} + +Ragdoll::~Ragdoll() +{ + // Destroy all bodies + mSystem->GetBodyInterface().DestroyBodies(mBodyIDs.data(), (int)mBodyIDs.size()); +} + +void Ragdoll::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies) +{ + // Scope for JPH_STACK_ALLOC + { + // Create copy of body ids since they will be shuffled + int num_bodies = (int)mBodyIDs.size(); + BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID)); + memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID)); + + // Insert bodies as a batch + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + BodyInterface::AddState add_state = bi.AddBodiesPrepare(bodies, num_bodies); + bi.AddBodiesFinalize(bodies, num_bodies, add_state, inActivationMode); + } + + // Add all constraints + mSystem->AddConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size()); +} + +void Ragdoll::RemoveFromPhysicsSystem(bool inLockBodies) +{ + // Remove all constraints before removing the bodies + mSystem->RemoveConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size()); + + // Scope for JPH_STACK_ALLOC + { + // Create copy of body ids since they will be shuffled + int num_bodies = (int)mBodyIDs.size(); + BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID)); + memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID)); + + // Remove all bodies as a batch + sGetBodyInterface(mSystem, inLockBodies).RemoveBodies(bodies, num_bodies); + } +} + +void Ragdoll::Activate(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).ActivateBodies(mBodyIDs.data(), (int)mBodyIDs.size()); +} + +bool Ragdoll::IsActive(bool inLockBodies) const +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Test if any body is active + for (int b = 0; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + if (body->IsActive()) + return true; + } + + return false; +} + +void Ragdoll::SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies) +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiWrite lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Update group ID + for (int b = 0; b < body_count; ++b) + { + Body *body = lock.GetBody(b); + body->GetCollisionGroup().SetGroupID(inGroupID); + } +} + +void Ragdoll::SetPose(const SkeletonPose &inPose, bool inLockBodies) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + SetPose(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inLockBodies); +} + +void Ragdoll::SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies) +{ + // Move bodies instantly into the correct position + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (int i = 0; i < (int)mBodyIDs.size(); ++i) + { + const Mat44 &joint = inJointMatrices[i]; + bi.SetPositionAndRotation(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), EActivation::DontActivate); + } +} + +void Ragdoll::GetPose(SkeletonPose &outPose, bool inLockBodies) +{ + JPH_ASSERT(outPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + RVec3 root_offset; + GetPose(root_offset, outPose.GetJointMatrices().data(), inLockBodies); + outPose.SetRootOffset(root_offset); +} + +void Ragdoll::GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies) +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + if (body_count == 0) + return; + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Get root matrix + const Body *root = lock.GetBody(0); + RMat44 root_transform = root->GetWorldTransform(); + outRootOffset = root_transform.GetTranslation(); + outJointMatrices[0] = Mat44(root_transform.GetColumn4(0), root_transform.GetColumn4(1), root_transform.GetColumn4(2), Vec4(0, 0, 0, 1)); + + // Get other matrices + for (int b = 1; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + RMat44 transform = body->GetWorldTransform(); + outJointMatrices[b] = Mat44(transform.GetColumn4(0), transform.GetColumn4(1), transform.GetColumn4(2), Vec4(Vec3(transform.GetTranslation() - outRootOffset), 1)); + } +} + +void Ragdoll::ResetWarmStart() +{ + for (TwoBodyConstraint *c : mConstraints) + c->ResetWarmStart(); +} + +void Ragdoll::DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + DriveToPoseUsingKinematics(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inDeltaTime, inLockBodies); +} + +void Ragdoll::DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies) +{ + // Move bodies into the correct position using kinematics + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (int i = 0; i < (int)mBodyIDs.size(); ++i) + { + const Mat44 &joint = inJointMatrices[i]; + bi.MoveKinematic(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), inDeltaTime); + } +} + +void Ragdoll::DriveToPoseUsingMotors(const SkeletonPose &inPose) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + // Move bodies into the correct position using constraints + for (int i = 0; i < (int)inPose.GetJointMatrices().size(); ++i) + { + int constraint_idx = mRagdollSettings->GetConstraintIndexForBodyIndex(i); + if (constraint_idx >= 0) + { + // Get desired rotation of this body relative to its parent + const SkeletalAnimation::JointState &joint_state = inPose.GetJoint(i); + + // Drive constraint to target + TwoBodyConstraint *constraint = mConstraints[constraint_idx]; + EConstraintSubType sub_type = constraint->GetSubType(); + if (sub_type == EConstraintSubType::SwingTwist) + { + SwingTwistConstraint *st_constraint = static_cast(constraint); + st_constraint->SetSwingMotorState(EMotorState::Position); + st_constraint->SetTwistMotorState(EMotorState::Position); + st_constraint->SetTargetOrientationBS(joint_state.mRotation); + } + else + JPH_ASSERT(false, "Constraint type not implemented!"); + } + } +} + +void Ragdoll::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.SetLinearAndAngularVelocity(body_id, inLinearVelocity, inAngularVelocity); +} + +void Ragdoll::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.SetLinearVelocity(body_id, inLinearVelocity); +} + +void Ragdoll::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.AddLinearVelocity(body_id, inLinearVelocity); +} + +void Ragdoll::AddImpulse(Vec3Arg inImpulse, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.AddImpulse(body_id, inImpulse); +} + +void Ragdoll::GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const +{ + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs[0]); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + outPosition = body.GetPosition(); + outRotation = body.GetRotation(); + } + else + { + outPosition = RVec3::sZero(); + outRotation = Quat::sIdentity(); + } +} + +AABox Ragdoll::GetWorldSpaceBounds(bool inLockBodies) const +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Encapsulate all bodies + AABox bounds; + for (int b = 0; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + if (body != nullptr) + bounds.Encapsulate(body->GetWorldSpaceBounds()); + } + return bounds; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h new file mode 100644 index 000000000000..8fe401080f77 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h @@ -0,0 +1,240 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Ragdoll; +class PhysicsSystem; + +/// Contains the structure of a ragdoll +class JPH_EXPORT RagdollSettings : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, RagdollSettings) + +public: + /// Stabilize the constraints of the ragdoll + /// @return True on success, false on failure. + bool Stabilize(); + + /// After the ragdoll has been fully configured, call this function to automatically create and add a GroupFilterTable collision filter to all bodies + /// and configure them so that parent and children don't collide. + /// + /// This will: + /// - Create a GroupFilterTable and assign it to all of the bodies in a ragdoll. + /// - Each body in your ragdoll will get a SubGroupID that is equal to the joint index in the Skeleton that it is attached to. + /// - Loop over all joints in the Skeleton and call GroupFilterTable::DisableCollision(joint index, parent joint index). + /// - When a pose is provided through inJointMatrices the function will detect collisions between joints + /// (they must be separated by more than inMinSeparationDistance to be treated as not colliding) and automatically disable collisions. + /// + /// When you create an instance using Ragdoll::CreateRagdoll pass in a unique GroupID for each ragdoll (e.g. a simple counter), note that this number + /// should be unique throughout the PhysicsSystem, so if you have different types of ragdolls they should not share the same GroupID. + void DisableParentChildCollisions(const Mat44 *inJointMatrices = nullptr, float inMinSeparationDistance = 0.0f); + + /// Saves the state of this object in binary form to inStream. + /// @param inStream The stream to save the state to + /// @param inSaveShapes If the shapes should be saved as well (these could be shared between ragdolls, in which case the calling application may want to write custom code to restore them) + /// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared) + void SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const; + + using RagdollResult = Result>; + + /// Restore a saved ragdoll from inStream + static RagdollResult sRestoreFromBinaryState(StreamIn &inStream); + + /// Create ragdoll instance from these settings + /// @return Newly created ragdoll or null when out of bodies + Ragdoll * CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const; + + /// Access to the skeleton of this ragdoll + const Skeleton * GetSkeleton() const { return mSkeleton; } + Skeleton * GetSkeleton() { return mSkeleton; } + + /// Calculate the map needed for GetBodyIndexToConstraintIndex() + void CalculateBodyIndexToConstraintIndex(); + + /// Get table that maps a body index to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body. + /// Note that this will only tell you which constraint connects the body to its parent, it will not look in the additional constraint list. + const Array & GetBodyIndexToConstraintIndex() const { return mBodyIndexToConstraintIndex; } + + /// Map a single body index to a constraint index + int GetConstraintIndexForBodyIndex(int inBodyIndex) const { return mBodyIndexToConstraintIndex[inBodyIndex]; } + + /// Calculate the map needed for GetConstraintIndexToBodyIdxPair() + void CalculateConstraintIndexToBodyIdxPair(); + + using BodyIdxPair = std::pair; + + /// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + const Array & GetConstraintIndexToBodyIdxPair() const { return mConstraintIndexToBodyIdxPair; } + + /// Map a single constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + BodyIdxPair GetBodyIndicesForConstraintIndex(int inConstraintIndex) const { return mConstraintIndexToBodyIdxPair[inConstraintIndex]; } + + /// A single rigid body sub part of the ragdoll + class Part : public BodyCreationSettings + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Part) + + public: + Ref mToParent; + }; + + /// List of ragdoll parts + using PartVector = Array; ///< The constraint that connects this part to its parent part (should be null for the root) + + /// A constraint that connects two bodies in a ragdoll (for non parent child related constraints) + class AdditionalConstraint + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AdditionalConstraint) + + public: + /// Constructors + AdditionalConstraint() = default; + AdditionalConstraint(int inBodyIdx1, int inBodyIdx2, TwoBodyConstraintSettings *inConstraint) : mBodyIdx { inBodyIdx1, inBodyIdx2 }, mConstraint(inConstraint) { } + + int mBodyIdx[2]; ///< Indices of the bodies that this constraint connects + Ref mConstraint; ///< The constraint that connects these bodies + }; + + /// List of additional constraints + using AdditionalConstraintVector = Array; + + /// The skeleton for this ragdoll + Ref mSkeleton; + + /// For each of the joints, the body and constraint attaching it to its parent body (1-on-1 with mSkeleton.GetJoints()) + PartVector mParts; + + /// A list of constraints that connects two bodies in a ragdoll (for non parent child related constraints) + AdditionalConstraintVector mAdditionalConstraints; + +private: + /// Table that maps a body index (index in mBodyIDs) to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body. + Array mBodyIndexToConstraintIndex; + + /// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + Array mConstraintIndexToBodyIdxPair; +}; + +/// Runtime ragdoll information +class JPH_EXPORT Ragdoll : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Ragdoll(PhysicsSystem *inSystem) : mSystem(inSystem) { } + + /// Destructor + ~Ragdoll(); + + /// Add bodies and constraints to the system and optionally activate the bodies + void AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies = true); + + /// Remove bodies and constraints from the system + void RemoveFromPhysicsSystem(bool inLockBodies = true); + + /// Wake up all bodies in the ragdoll + void Activate(bool inLockBodies = true); + + /// Check if one or more of the bodies in the ragdoll are active. + /// Note that this involves locking the bodies (if inLockBodies is true) and looping over them. An alternative and possibly faster + /// way could be to install a BodyActivationListener and count the number of active bodies of a ragdoll as they're activated / deactivated + /// (basically check if the body that activates / deactivates is in GetBodyIDs() and increment / decrement a counter). + bool IsActive(bool inLockBodies = true) const; + + /// Set the group ID on all bodies in the ragdoll + void SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies = true); + + /// Set the ragdoll to a pose (calls BodyInterface::SetPositionAndRotation to instantly move the ragdoll) + void SetPose(const SkeletonPose &inPose, bool inLockBodies = true); + + /// Lower level version of SetPose that directly takes the world space joint matrices + void SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies = true); + + /// Get the ragdoll pose (uses the world transform of the bodies to calculate the pose) + void GetPose(SkeletonPose &outPose, bool inLockBodies = true); + + /// Lower level version of GetPose that directly returns the world space joint matrices + void GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies = true); + + /// This function calls ResetWarmStart on all constraints. It can be used after calling SetPose to reset previous frames impulses. See: Constraint::ResetWarmStart. + void ResetWarmStart(); + + /// Drive the ragdoll to a specific pose by setting velocities on each of the bodies so that it will reach inPose in inDeltaTime + void DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies = true); + + /// Lower level version of DriveToPoseUsingKinematics that directly takes the world space joint matrices + void DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies = true); + + /// Drive the ragdoll to a specific pose by activating the motors on each constraint + void DriveToPoseUsingMotors(const SkeletonPose &inPose); + + /// Control the linear and velocity of all bodies in the ragdoll + void SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true); + + /// Set the world space linear velocity of all bodies in the ragdoll. + void SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add a world space velocity (in m/s) to all bodies in the ragdoll. + void AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add impulse to all bodies of the ragdoll (center of mass of each of them) + void AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true); + + /// Get the position and orientation of the root of the ragdoll + void GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const; + + /// Get number of bodies in the ragdoll + size_t GetBodyCount() const { return mBodyIDs.size(); } + + /// Access a body ID + BodyID GetBodyID(int inBodyIndex) const { return mBodyIDs[inBodyIndex]; } + + /// Access to the array of body IDs + const Array & GetBodyIDs() const { return mBodyIDs; } + + /// Get number of constraints in the ragdoll + size_t GetConstraintCount() const { return mConstraints.size(); } + + /// Access a constraint by index + TwoBodyConstraint * GetConstraint(int inConstraintIndex) { return mConstraints[inConstraintIndex]; } + + /// Access a constraint by index + const TwoBodyConstraint * GetConstraint(int inConstraintIndex) const { return mConstraints[inConstraintIndex]; } + + /// Get world space bounding box for all bodies of the ragdoll + AABox GetWorldSpaceBounds(bool inLockBodies = true) const; + + /// Get the settings object that created this ragdoll + const RagdollSettings * GetRagdollSettings() const { return mRagdollSettings; } + +private: + /// For RagdollSettings::CreateRagdoll function + friend class RagdollSettings; + + /// The settings that created this ragdoll + RefConst mRagdollSettings; + + /// The bodies and constraints that this ragdoll consists of (1-on-1 with mRagdollSettings->mParts) + Array mBodyIDs; + + /// Array of constraints that connect the bodies together + Array> mConstraints; + + /// Cached physics system + PhysicsSystem * mSystem; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h new file mode 100644 index 000000000000..27e185375f34 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Body; +class SoftBodyManifold; + +/// Return value for the OnSoftBodyContactValidate callback. Determines if the contact will be processed or not. +enum class SoftBodyValidateResult +{ + AcceptContact, ///< Accept this contact + RejectContact, ///< Reject this contact +}; + +/// Contact settings for a soft body contact. +/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to. +class SoftBodyContactSettings +{ +public: + float mInvMassScale1 = 1.0f; ///< Scale factor for the inverse mass of the soft body (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvMassScale2 = 1.0f; ///< Scale factor for the inverse mass of the other body (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale2 = 1.0f; ///< Scale factor for the inverse inertia of the other body (usually same as mInvMassScale2) + bool mIsSensor; ///< If the contact should be treated as a sensor vs body contact (no collision response) +}; + +/// A listener class that receives collision contact events for soft bodies against rigid bodies. +/// It can be registered with the PhysicsSystem. +class SoftBodyContactListener +{ +public: + /// Ensure virtual destructor + virtual ~SoftBodyContactListener() = default; + + /// Called whenever the soft body's aabox overlaps with another body's aabox (so receiving this callback doesn't tell if any of the vertices will collide). + /// This callback can be used to change the behavior of the collision response for all vertices in the soft body or to completely reject the contact. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// @param inSoftBody The soft body that collided. It is safe to access this as the soft body is only updated on the current thread. + /// @param inOtherBody The other body that collided. Note that accessing the position/orientation/velocity of inOtherBody may result in a race condition as other threads may be modifying the body at the same time. + /// @param ioSettings The settings for all contact points that are generated by this collision. + /// @return Whether the contact should be processed or not. + virtual SoftBodyValidateResult OnSoftBodyContactValidate([[maybe_unused]] const Body &inSoftBody, [[maybe_unused]] const Body &inOtherBody, [[maybe_unused]] SoftBodyContactSettings &ioSettings) { return SoftBodyValidateResult::AcceptContact; } + + /// Called after all contact points for a soft body have been handled. You only receive one callback per body pair per simulation step and can use inManifold to iterate through all contacts. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// You will receive a single callback for a soft body per simulation step for performance reasons, this callback will apply to all vertices in the soft body. + /// @param inSoftBody The soft body that collided. It is safe to access this as the soft body is only updated on the current thread. + /// @param inManifold The manifold that describes the contact surface between the two bodies. Other bodies may be modified by other threads during this callback. + virtual void OnSoftBodyContactAdded([[maybe_unused]] const Body &inSoftBody, const SoftBodyManifold &inManifold) { /* Do nothing */ } +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp new file mode 100644 index 000000000000..3ae026df64a0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodyCreationSettings) +{ + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mSettings) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPosition) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRotation) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUserData) + JPH_ADD_ENUM_ATTRIBUTE(SoftBodyCreationSettings, mObjectLayer) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mCollisionGroup) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mNumIterations) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mLinearDamping) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMaxLinearVelocity) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRestitution) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mFriction) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPressure) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mGravityFactor) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUpdatePosition) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMakeRotationIdentity) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mAllowSleeping) +} + +void SoftBodyCreationSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mUserData); + inStream.Write(mObjectLayer); + mCollisionGroup.SaveBinaryState(inStream); + inStream.Write(mNumIterations); + inStream.Write(mLinearDamping); + inStream.Write(mMaxLinearVelocity); + inStream.Write(mRestitution); + inStream.Write(mFriction); + inStream.Write(mPressure); + inStream.Write(mGravityFactor); + inStream.Write(mUpdatePosition); + inStream.Write(mMakeRotationIdentity); + inStream.Write(mAllowSleeping); +} + +void SoftBodyCreationSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mUserData); + inStream.Read(mObjectLayer); + mCollisionGroup.RestoreBinaryState(inStream); + inStream.Read(mNumIterations); + inStream.Read(mLinearDamping); + inStream.Read(mMaxLinearVelocity); + inStream.Read(mRestitution); + inStream.Read(mFriction); + inStream.Read(mPressure); + inStream.Read(mGravityFactor); + inStream.Read(mUpdatePosition); + inStream.Read(mMakeRotationIdentity); + inStream.Read(mAllowSleeping); +} + +void SoftBodyCreationSettings::SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const +{ + // Save creation settings + SaveBinaryState(inStream); + + // Save shared settings + if (ioSharedSettingsMap != nullptr && ioMaterialMap != nullptr) + mSettings->SaveWithMaterials(inStream, *ioSharedSettingsMap, *ioMaterialMap); + else + inStream.Write(~uint32(0)); + + // Save group filter + StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap); +} + +SoftBodyCreationSettings::SBCSResult SoftBodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToSharedSettingsMap &ioSharedSettingsMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap) +{ + SBCSResult result; + + // Read creation settings + SoftBodyCreationSettings settings; + settings.RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Error reading body creation settings"); + return result; + } + + // Read shared settings + SoftBodySharedSettings::SettingsResult settings_result = SoftBodySharedSettings::sRestoreWithMaterials(inStream, ioSharedSettingsMap, ioMaterialMap); + if (settings_result.HasError()) + { + result.SetError(settings_result.GetError()); + return result; + } + settings.mSettings = settings_result.Get(); + + // Read group filter + Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap); + if (gfresult.HasError()) + { + result.SetError(gfresult.GetError()); + return result; + } + settings.mCollisionGroup.SetGroupFilter(gfresult.Get()); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h new file mode 100644 index 000000000000..b5dc163767ec --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// This class contains the information needed to create a soft body object +/// Note: Soft bodies are still in development and come with several caveats. Read the Architecture and API documentation for more information! +class JPH_EXPORT SoftBodyCreationSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodyCreationSettings) + +public: + /// Constructor + SoftBodyCreationSettings() = default; + SoftBodyCreationSettings(const SoftBodySharedSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, ObjectLayer inObjectLayer) : mSettings(inSettings), mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer) { } + + /// Saves the state of this object in binary form to inStream. Doesn't store the shared settings nor the group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the shared settings nor the group filter. + void RestoreBinaryState(StreamIn &inStream); + + using GroupFilterToIDMap = StreamUtils::ObjectToIDMap; + using IDToGroupFilterMap = StreamUtils::IDToObjectMap; + using SharedSettingsToIDMap = SoftBodySharedSettings::SharedSettingsToIDMap; + using IDToSharedSettingsMap = SoftBodySharedSettings::IDToSharedSettingsMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this body creation settings, its shared settings and group filter. Pass in an empty map in ioSharedSettingsMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + /// Pass nullptr to ioSharedSettingsMap and ioMaterial map to skip saving shared settings and materials + /// Pass nullptr to ioGroupFilterMap to skip saving group filters + void SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const; + + using SBCSResult = Result; + + /// Restore a shape, all its children and materials. Pass in an empty map in ioSharedSettingsMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static SBCSResult sRestoreWithChildren(StreamIn &inStream, IDToSharedSettingsMap &ioSharedSettingsMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap); + + RefConst mSettings; ///< Defines the configuration of this soft body + + RVec3 mPosition { RVec3::sZero() }; ///< Initial position of the soft body + Quat mRotation { Quat::sIdentity() }; ///< Initial rotation of the soft body + + /// User data value (can be used by application) + uint64 mUserData = 0; + + ///@name Collision settings + ObjectLayer mObjectLayer = 0; ///< The collision layer this body belongs to (determines if two objects can collide) + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + uint32 mNumIterations = 5; ///< Number of solver iterations + float mLinearDamping = 0.1f; ///< Linear damping: dv/dt = -mLinearDamping * v + float mMaxLinearVelocity = 500.0f; ///< Maximum linear velocity that a vertex can reach (m/s) + float mRestitution = 0.0f; ///< Restitution when colliding + float mFriction = 0.2f; ///< Friction coefficient when colliding + float mPressure = 0.0f; ///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure + float mGravityFactor = 1.0f; ///< Value to multiply gravity with for this body + bool mUpdatePosition = true; ///< Update the position of the body while simulating (set to false for something that is attached to the static world) + bool mMakeRotationIdentity = true; ///< Bake specified mRotation in the vertices and set the body rotation to identity (simulation is slightly more accurate if the rotation of a soft body is kept to identity) + bool mAllowSleeping = true; ///< If this body can go to sleep or not +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h new file mode 100644 index 000000000000..de21ec50de0d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h @@ -0,0 +1,74 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// An interface to query which vertices of a soft body are colliding with other bodies +class SoftBodyManifold +{ +public: + /// Get the vertices of the soft body for iterating + const Array & GetVertices() const { return mVertices; } + + /// Check if a vertex has collided with something in this update + JPH_INLINE bool HasContact(const SoftBodyVertex &inVertex) const + { + return inVertex.mHasContact; + } + + /// Get the local space contact point (multiply by GetCenterOfMassTransform() of the soft body to get world space) + JPH_INLINE Vec3 GetLocalContactPoint(const SoftBodyVertex &inVertex) const + { + return inVertex.mPosition - inVertex.mCollisionPlane.SignedDistance(inVertex.mPosition) * inVertex.mCollisionPlane.GetNormal(); + } + + /// Get the contact normal for the vertex (assumes there is a contact). + JPH_INLINE Vec3 GetContactNormal(const SoftBodyVertex &inVertex) const + { + return -inVertex.mCollisionPlane.GetNormal(); + } + + /// Get the body with which the vertex has collided in this update + JPH_INLINE BodyID GetContactBodyID(const SoftBodyVertex &inVertex) const + { + return inVertex.mHasContact? mCollidingShapes[inVertex.mCollidingShapeIndex].mBodyID : BodyID(); + } + + /// Get the number of sensors that are in contact with the soft body + JPH_INLINE uint GetNumSensorContacts() const + { + return (uint)mCollidingSensors.size(); + } + + /// Get the i-th sensor that is in contact with the soft body + JPH_INLINE BodyID GetSensorContactBodyID(uint inIndex) const + { + return mCollidingSensors[inIndex].mBodyID; + } + +private: + /// Allow SoftBodyMotionProperties to construct us + friend class SoftBodyMotionProperties; + + /// Constructor + explicit SoftBodyManifold(const SoftBodyMotionProperties *inMotionProperties) : + mVertices(inMotionProperties->mVertices), + mCollidingShapes(inMotionProperties->mCollidingShapes), + mCollidingSensors(inMotionProperties->mCollidingSensors) + { + } + + using CollidingShape = SoftBodyMotionProperties::CollidingShape; + using CollidingSensor = SoftBodyMotionProperties::CollidingSensor; + + const Array & mVertices; + const Array & mCollidingShapes; + const Array & mCollidingSensors; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp new file mode 100644 index 000000000000..0aad94de88c8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp @@ -0,0 +1,1321 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace JPH::literals; + +void SoftBodyMotionProperties::CalculateMassAndInertia() +{ + MassProperties mp; + + for (const Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + Vec3 pos = v.mPosition; + + // Accumulate mass + float mass = 1.0f / v.mInvMass; + mp.mMass += mass; + + // Inertia tensor, diagonal + // See equations https://en.wikipedia.org/wiki/Moment_of_inertia section 'Inertia Tensor' + for (int i = 0; i < 3; ++i) + mp.mInertia(i, i) += mass * (Square(pos[(i + 1) % 3]) + Square(pos[(i + 2) % 3])); + + // Inertia tensor off diagonal + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + if (i != j) + mp.mInertia(i, j) -= mass * pos[i] * pos[j]; + } + else + { + // If one vertex is kinematic, the entire body will have infinite mass and inertia + SetInverseMass(0.0f); + SetInverseInertia(Vec3::sZero(), Quat::sIdentity()); + return; + } + + SetMassProperties(EAllowedDOFs::All, mp); +} + +void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSettings) +{ + // Store settings + mSettings = inSettings.mSettings; + mNumIterations = inSettings.mNumIterations; + mPressure = inSettings.mPressure; + mUpdatePosition = inSettings.mUpdatePosition; + + // Initialize vertices + mVertices.resize(inSettings.mSettings->mVertices.size()); + Mat44 rotation = inSettings.mMakeRotationIdentity? Mat44::sRotation(inSettings.mRotation) : Mat44::sIdentity(); + for (Array::size_type v = 0, s = mVertices.size(); v < s; ++v) + { + const SoftBodySharedSettings::Vertex &in_vertex = inSettings.mSettings->mVertices[v]; + Vertex &out_vertex = mVertices[v]; + out_vertex.mPreviousPosition = out_vertex.mPosition = rotation * Vec3(in_vertex.mPosition); + out_vertex.mVelocity = rotation.Multiply3x3(Vec3(in_vertex.mVelocity)); + out_vertex.ResetCollision(); + out_vertex.mInvMass = in_vertex.mInvMass; + mLocalBounds.Encapsulate(out_vertex.mPosition); + } + + // Allocate space for skinned vertices + if (!inSettings.mSettings->mSkinnedConstraints.empty()) + mSkinState.resize(mVertices.size()); + + // We don't know delta time yet, so we can't predict the bounds and use the local bounds as the predicted bounds + mLocalPredictedBounds = mLocalBounds; + + CalculateMassAndInertia(); +} + +float SoftBodyMotionProperties::GetVolumeTimesSix() const +{ + float six_volume = 0.0f; + for (const Face &f : mSettings->mFaces) + { + Vec3 x1 = mVertices[f.mVertex[0]].mPosition; + Vec3 x2 = mVertices[f.mVertex[1]].mPosition; + Vec3 x3 = mVertices[f.mVertex[2]].mPosition; + six_volume += x1.Cross(x2).Dot(x3); // We pick zero as the origin as this is the center of the bounding box so should give good accuracy + } + return six_volume; +} + +void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface) +{ + JPH_PROFILE_FUNCTION(); + + // Reset flag prior to collision detection + mNeedContactCallback = false; + + struct Collector : public CollideShapeBodyCollector + { + Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, const AABox &inLocalBounds, SimShapeFilterWrapper &inShapeFilter, Array &ioHits, Array &ioSensors) : + mContext(inContext), + mInverseTransform(inContext.mCenterOfMassTransform.InversedRotationTranslation()), + mLocalBounds(inLocalBounds), + mBodyLockInterface(inBodyLockInterface), + mCombineFriction(inSystem.GetCombineFriction()), + mCombineRestitution(inSystem.GetCombineRestitution()), + mShapeFilter(inShapeFilter), + mHits(ioHits), + mSensors(ioSensors) + { + } + + virtual void AddHit(const BodyID &inResult) override + { + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.Succeeded()) + { + const Body &soft_body = *mContext.mBody; + const Body &body = lock.GetBody(); + if (body.IsRigidBody() // TODO: We should support soft body vs soft body + && soft_body.GetCollisionGroup().CanCollide(body.GetCollisionGroup())) + { + SoftBodyContactSettings settings; + settings.mIsSensor = body.IsSensor(); + + if (mContext.mContactListener == nullptr) + { + // If we have no contact listener, we can ignore sensors + if (settings.mIsSensor) + return; + } + else + { + // Call the contact listener to see if we should accept this contact + if (mContext.mContactListener->OnSoftBodyContactValidate(soft_body, body, settings) != SoftBodyValidateResult::AcceptContact) + return; + + // Check if there will be any interaction + if (!settings.mIsSensor + && settings.mInvMassScale1 == 0.0f + && (body.GetMotionType() != EMotionType::Dynamic || settings.mInvMassScale2 == 0.0f)) + return; + } + + // Calculate transform of this body relative to the soft body + Mat44 com = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44(); + + // Collect leaf shapes + mShapeFilter.SetBody2(&body); + struct LeafShapeCollector : public TransformedShapeCollector + { + virtual void AddHit(const TransformedShape &inResult) override + { + mHits.emplace_back(Mat44::sRotationTranslation(inResult.mShapeRotation, Vec3(inResult.mShapePositionCOM)), inResult.GetShapeScale(), inResult.mShape); + } + + Array mHits; + }; + LeafShapeCollector collector; + body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sReplicate(1.0f), SubShapeIDCreator(), collector, mShapeFilter); + if (collector.mHits.empty()) + return; + + if (settings.mIsSensor) + { + CollidingSensor cs; + cs.mCenterOfMassTransform = com; + cs.mShapes = std::move(collector.mHits); + cs.mBodyID = inResult; + mSensors.push_back(cs); + } + else + { + CollidingShape cs; + cs.mCenterOfMassTransform = com; + cs.mShapes = std::move(collector.mHits); + cs.mBodyID = inResult; + cs.mMotionType = body.GetMotionType(); + cs.mUpdateVelocities = false; + cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID()); + cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID()); + cs.mSoftBodyInvMassScale = settings.mInvMassScale1; + if (cs.mMotionType == EMotionType::Dynamic) + { + const MotionProperties *mp = body.GetMotionProperties(); + cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass(); + cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation()); + cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity()); + cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity()); + } + mHits.push_back(cs); + } + } + } + } + + private: + const SoftBodyUpdateContext &mContext; + RMat44 mInverseTransform; + AABox mLocalBounds; + const BodyLockInterface & mBodyLockInterface; + ContactConstraintManager::CombineFunction mCombineFriction; + ContactConstraintManager::CombineFunction mCombineRestitution; + SimShapeFilterWrapper & mShapeFilter; + Array & mHits; + Array & mSensors; + }; + + // Calculate local bounding box + AABox local_bounds = mLocalBounds; + local_bounds.Encapsulate(mLocalPredictedBounds); + local_bounds.ExpandBy(Vec3::sReplicate(mSettings->mVertexRadius)); + + // Calculate world space bounding box + AABox world_bounds = local_bounds.Transformed(inContext.mCenterOfMassTransform); + + // Create shape filter + SimShapeFilterWrapperUnion shape_filter_union(inContext.mSimShapeFilter, inContext.mBody); + SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper(); + + Collector collector(inContext, inSystem, inBodyLockInterface, local_bounds, shape_filter, mCollidingShapes, mCollidingSensors); + ObjectLayer layer = inContext.mBody->GetObjectLayer(); + DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(layer); + DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(layer); + inSystem.GetBroadPhaseQuery().CollideAABox(world_bounds, collector, broadphase_layer_filter, object_layer_filter); +} + +void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices) +{ + JPH_PROFILE_FUNCTION(); + + // Generate collision planes + for (const CollidingShape &cs : mCollidingShapes) + for (const LeafShape &shape : cs.mShapes) + shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, CollideSoftBodyVertexIterator(mVertices.data() + inVertexStart), inNumVertices, int(&cs - mCollidingShapes.data())); +} + +void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSensor) +{ + JPH_PROFILE_FUNCTION(); + + Plane collision_plane; + float largest_penetration = -FLT_MAX; + int colliding_shape_idx = -1; + + // Collide sensor against all vertices + CollideSoftBodyVertexIterator vertex_iterator( + StridedPtr(&mVertices[0].mPosition, sizeof(SoftBodyVertex)), // The position and mass come from the soft body vertex + StridedPtr(&mVertices[0].mInvMass, sizeof(SoftBodyVertex)), + StridedPtr(&collision_plane, 0), // We want all vertices to result in a single collision so we pass stride 0 + StridedPtr(&largest_penetration, 0), + StridedPtr(&colliding_shape_idx, 0)); + for (const LeafShape &shape : ioSensor.mShapes) + shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, vertex_iterator, uint(mVertices.size()), 0); + ioSensor.mHasContact = largest_penetration > 0.0f; + + // We need a contact callback if one of the sensors collided + if (ioSensor.mHasContact) + mNeedContactCallback = true; +} + +void SoftBodyMotionProperties::ApplyPressure(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float pressure_coefficient = mPressure; + if (pressure_coefficient > 0.0f) + { + // Calculate total volume + float six_volume = GetVolumeTimesSix(); + if (six_volume > 0.0f) + { + // Apply pressure + // p = F / A = n R T / V (see https://en.wikipedia.org/wiki/Pressure) + // Our pressure coefficient is n R T so the impulse is: + // P = F dt = pressure_coefficient / V * A * dt + float coefficient = pressure_coefficient * dt / six_volume; // Need to still multiply by 6 for the volume + for (const Face &f : mSettings->mFaces) + { + Vec3 x1 = mVertices[f.mVertex[0]].mPosition; + Vec3 x2 = mVertices[f.mVertex[1]].mPosition; + Vec3 x3 = mVertices[f.mVertex[2]].mPosition; + + Vec3 impulse = coefficient * (x2 - x1).Cross(x3 - x1); // Area is half the cross product so need to still divide by 2 + for (uint32 i : f.mVertex) + { + Vertex &v = mVertices[i]; + v.mVelocity += v.mInvMass * impulse; // Want to divide by 3 because we spread over 3 vertices + } + } + } + } +} + +void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float linear_damping = max(0.0f, 1.0f - GetLinearDamping() * dt); // See: MotionProperties::ApplyForceTorqueAndDragInternal + + // Integrate + Vec3 sub_step_gravity = inContext.mGravity * dt; + Vec3 sub_step_impulse = GetAccumulatedForce() * dt; + for (Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + // Gravity + v.mVelocity += sub_step_gravity + sub_step_impulse * v.mInvMass; + + // Damping + v.mVelocity *= linear_damping; + + // Integrate + v.mPreviousPosition = v.mPosition; + v.mPosition += v.mVelocity * dt; + } + else + { + // Integrate + v.mPreviousPosition = v.mPosition; + v.mPosition += v.mVelocity * dt; + } +} + +void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + for (const DihedralBend *b = mSettings->mDihedralBendConstraints.data() + inStartIndex, *b_end = mSettings->mDihedralBendConstraints.data() + inEndIndex; b < b_end; ++b) + { + Vertex &v0 = mVertices[b->mVertex[0]]; + Vertex &v1 = mVertices[b->mVertex[1]]; + Vertex &v2 = mVertices[b->mVertex[2]]; + Vertex &v3 = mVertices[b->mVertex[3]]; + + // Get positions + Vec3 x0 = v0.mPosition; + Vec3 x1 = v1.mPosition; + Vec3 x2 = v2.mPosition; + Vec3 x3 = v3.mPosition; + + /* + x2 + e1/ \e3 + / \ + x0----x1 + \ e0 / + e2\ /e4 + x3 + */ + + // Calculate the shared edge of the triangles + Vec3 e = x1 - x0; + float e_len = e.Length(); + if (e_len < 1.0e-6f) + continue; + + // Calculate the normals of the triangles + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 n1 = (x2 - x0).Cross(x1x2); + Vec3 n2 = x1x3.Cross(x3 - x0); + float n1_len_sq = n1.LengthSq(); + float n2_len_sq = n2.LengthSq(); + float n1_len_sq_n2_len_sq = n1_len_sq * n2_len_sq; + if (n1_len_sq_n2_len_sq < 1.0e-24f) + continue; + + // Calculate constraint equation + // As per "Strain Based Dynamics" Appendix A we need to negate the gradients when (n1 x n2) . e > 0, instead we make sure that the sign of the constraint equation is correct + float sign = Sign(n2.Cross(n1).Dot(e)); + float d = n1.Dot(n2) / sqrt(n1_len_sq_n2_len_sq); + float c = sign * ACosApproximate(d) - b->mInitialAngle; + + // Ensure the range is -PI to PI + if (c > JPH_PI) + c -= 2.0f * JPH_PI; + else if (c < -JPH_PI) + c += 2.0f * JPH_PI; + + // Calculate gradient of constraint equation + // Taken from "Strain Based Dynamics" - Matthias Muller et al. (Appendix A) + // with p1 = x2, p2 = x3, p3 = x0 and p4 = x1 + // which in turn is based on "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al. (Section 4) + n1 /= n1_len_sq; + n2 /= n2_len_sq; + Vec3 d0c = (x1x2.Dot(e) * n1 + x1x3.Dot(e) * n2) / e_len; + Vec3 d2c = e_len * n1; + Vec3 d3c = e_len * n2; + + // The sum of the gradients must be zero (see "Strain Based Dynamics" section 4) + Vec3 d1c = -d0c - d2c - d3c; + + // Get masses + float w0 = v0.mInvMass; + float w1 = v1.mInvMass; + float w2 = v2.mInvMass; + float w3 = v3.mInvMass; + + // Calculate -lambda + float denom = w0 * d0c.LengthSq() + w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + b->mCompliance * inv_dt_sq; + if (denom < 1.0e-12f) + continue; + float minus_lambda = c / denom; + + // Apply correction + v0.mPosition = x0 - minus_lambda * w0 * d0c; + v1.mPosition = x1 - minus_lambda * w1 * d1c; + v2.mPosition = x2 - minus_lambda * w2 * d2c; + v3.mPosition = x3 - minus_lambda * w3 * d3c; + } +} + +void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + // Satisfy volume constraints + for (const Volume *v = mSettings->mVolumeConstraints.data() + inStartIndex, *v_end = mSettings->mVolumeConstraints.data() + inEndIndex; v < v_end; ++v) + { + Vertex &v1 = mVertices[v->mVertex[0]]; + Vertex &v2 = mVertices[v->mVertex[1]]; + Vertex &v3 = mVertices[v->mVertex[2]]; + Vertex &v4 = mVertices[v->mVertex[3]]; + + Vec3 x1 = v1.mPosition; + Vec3 x2 = v2.mPosition; + Vec3 x3 = v3.mPosition; + Vec3 x4 = v4.mPosition; + + // Calculate constraint equation + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 x1x4 = x4 - x1; + float c = abs(x1x2.Cross(x1x3).Dot(x1x4)) - v->mSixRestVolume; + + // Calculate gradient of constraint equation + Vec3 d1c = (x4 - x2).Cross(x3 - x2); + Vec3 d2c = x1x3.Cross(x1x4); + Vec3 d3c = x1x4.Cross(x1x2); + Vec3 d4c = x1x2.Cross(x1x3); + + // Get masses + float w1 = v1.mInvMass; + float w2 = v2.mInvMass; + float w3 = v3.mInvMass; + float w4 = v4.mInvMass; + + // Calculate -lambda + float denom = w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v->mCompliance * inv_dt_sq; + if (denom < 1.0e-12f) + continue; + float minus_lambda = c / denom; + + // Apply correction + v1.mPosition = x1 - minus_lambda * w1 * d1c; + v2.mPosition = x2 - minus_lambda * w2 * d2c; + v3.mPosition = x3 - minus_lambda * w3 * d3c; + v4.mPosition = x4 - minus_lambda * w4 * d4c; + } +} + +void SoftBodyMotionProperties::ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + // Early out if nothing to do + if (mSettings->mSkinnedConstraints.empty() || !mEnableSkinConstraints) + return; + + JPH_PROFILE_FUNCTION(); + + // We're going to iterate multiple times over the skin constraints, update the skinned position accordingly. + // If we don't do this, the simulation will see a big jump and the first iteration will cause a big velocity change in the system. + float factor = mSkinStatePreviousPositionValid? inContext.mNextIteration.load(std::memory_order_relaxed) / float(mNumIterations) : 1.0f; + float prev_factor = 1.0f - factor; + + // Apply the constraints + Vertex *vertices = mVertices.data(); + const SkinState *skin_states = mSkinState.data(); + for (const Skinned *s = mSettings->mSkinnedConstraints.data() + inStartIndex, *s_end = mSettings->mSkinnedConstraints.data() + inEndIndex; s < s_end; ++s) + { + Vertex &vertex = vertices[s->mVertex]; + const SkinState &skin_state = skin_states[s->mVertex]; + float max_distance = s->mMaxDistance * mSkinnedMaxDistanceMultiplier; + + // Calculate the skinned position by interpolating from previous to current position + Vec3 skin_pos = prev_factor * skin_state.mPreviousPosition + factor * skin_state.mPosition; + + if (max_distance > 0.0f) + { + // Move vertex if it violated the back stop + if (s->mBackStopDistance < max_distance) + { + // Center of the back stop sphere + Vec3 center = skin_pos - skin_state.mNormal * (s->mBackStopDistance + s->mBackStopRadius); + + // Check if we're inside the back stop sphere + Vec3 delta = vertex.mPosition - center; + float delta_len_sq = delta.LengthSq(); + if (delta_len_sq < Square(s->mBackStopRadius)) + { + // Push the vertex to the surface of the back stop sphere + float delta_len = sqrt(delta_len_sq); + vertex.mPosition = delta_len > 0.0f? + center + delta * (s->mBackStopRadius / delta_len) + : center + skin_state.mNormal * s->mBackStopRadius; + } + } + + // Clamp vertex distance to max distance from skinned position + if (max_distance < FLT_MAX) + { + Vec3 delta = vertex.mPosition - skin_pos; + float delta_len_sq = delta.LengthSq(); + float max_distance_sq = Square(max_distance); + if (delta_len_sq > max_distance_sq) + vertex.mPosition = skin_pos + delta * sqrt(max_distance_sq / delta_len_sq); + } + } + else + { + // Kinematic: Just update the vertex position + vertex.mPosition = skin_pos; + } + } +} + +void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + // Satisfy edge constraints + for (const Edge *e = mSettings->mEdgeConstraints.data() + inStartIndex, *e_end = mSettings->mEdgeConstraints.data() + inEndIndex; e < e_end; ++e) + { + Vertex &v0 = mVertices[e->mVertex[0]]; + Vertex &v1 = mVertices[e->mVertex[1]]; + + // Get positions + Vec3 x0 = v0.mPosition; + Vec3 x1 = v1.mPosition; + + // Calculate current length + Vec3 delta = x1 - x0; + float length = delta.Length(); + + // Apply correction + float denom = length * (v0.mInvMass + v1.mInvMass + e->mCompliance * inv_dt_sq); + if (denom < 1.0e-12f) + continue; + Vec3 correction = delta * (length - e->mRestLength) / denom; + v0.mPosition = x0 + v0.mInvMass * correction; + v1.mPosition = x1 - v1.mInvMass * correction; + } +} + +void SoftBodyMotionProperties::ApplyLRAConstraints(uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + // Satisfy LRA constraints + Vertex *vertices = mVertices.data(); + for (const LRA *lra = mSettings->mLRAConstraints.data() + inStartIndex, *lra_end = mSettings->mLRAConstraints.data() + inEndIndex; lra < lra_end; ++lra) + { + JPH_ASSERT(lra->mVertex[0] < mVertices.size()); + JPH_ASSERT(lra->mVertex[1] < mVertices.size()); + const Vertex &vertex0 = vertices[lra->mVertex[0]]; + Vertex &vertex1 = vertices[lra->mVertex[1]]; + + Vec3 x0 = vertex0.mPosition; + Vec3 delta = vertex1.mPosition - x0; + float delta_len_sq = delta.LengthSq(); + if (delta_len_sq > Square(lra->mMaxDistance)) + vertex1.mPosition = x0 + delta * lra->mMaxDistance / sqrt(delta_len_sq); + } +} + +void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float restitution_treshold = -2.0f * inContext.mGravity.Length() * dt; + float vertex_radius = mSettings->mVertexRadius; + for (Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + // Remember previous velocity for restitution calculations + Vec3 prev_v = v.mVelocity; + + // XPBD velocity update + v.mVelocity = (v.mPosition - v.mPreviousPosition) / dt; + + // Satisfy collision constraint + if (v.mCollidingShapeIndex >= 0) + { + // Check if there is a collision + float projected_distance = -v.mCollisionPlane.SignedDistance(v.mPosition) + vertex_radius; + if (projected_distance > 0.0f) + { + // Remember that there was a collision + v.mHasContact = true; + + // We need a contact callback if one of the vertices collided + mNeedContactCallback = true; + + // Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position) + CollidingShape &cs = mCollidingShapes[v.mCollidingShapeIndex]; + Vec3 contact_normal = v.mCollisionPlane.GetNormal(); + v.mPosition += contact_normal * projected_distance; + + // Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al. + // See section 3.6: + // Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object + // r2 are the contact point relative to the center of mass of body 2 + // Lagrange multiplier for contact: lambda = -c / (w1 + w2) + // Where c is the constraint equation (the distance to the plane, negative because penetrating) + // Contact normal force: fn = lambda / dt^2 + // Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1) + // Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29 + // Relative velocity: vr = v1 - v2 - omega2 x r2 + // Normal velocity: vn = vr . contact_normal + // Tangential velocity: vt = vr - contact_normal * vn + // Impulse: p = dv / (w1 + w2) + // Changes in particle velocities: + // v1 = v1 + p / m1 + // v2 = v2 - p / m2 (no change when colliding with a static body) + // w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body) + if (cs.mMotionType == EMotionType::Dynamic) + { + // Calculate normal and tangential velocity (equation 30) + Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation(); + Vec3 v2 = cs.GetPointVelocity(r2); + Vec3 relative_velocity = v.mVelocity - v2; + Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity); + Vec3 v_tangential = relative_velocity - v_normal; + float v_tangential_length = v_tangential.Length(); + + // Calculate resulting inverse mass of vertex + float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass; + + // Calculate inverse effective mass + Vec3 r2_cross_n = r2.Cross(contact_normal); + float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n); + float w1_plus_w2 = vertex_inv_mass + w2; + if (w1_plus_w2 > 0.0f) + { + // Calculate delta relative velocity due to friction (modified equation 31) + Vec3 dv; + if (v_tangential_length > 0.0f) + dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + else + dv = Vec3::sZero(); + + // Calculate delta relative velocity due to restitution (equation 35) + dv += v_normal; + float prev_v_normal = (prev_v - v2).Dot(contact_normal); + if (prev_v_normal < restitution_treshold) + dv += cs.mRestitution * prev_v_normal * contact_normal; + + // Calculate impulse + Vec3 p = dv / w1_plus_w2; + + // Apply impulse to particle + v.mVelocity -= p * vertex_inv_mass; + + // Apply impulse to rigid body + cs.mLinearVelocity += p * cs.mInvMass; + cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p); + + // Mark that the velocities of the body we hit need to be updated + cs.mUpdateVelocities = true; + } + } + else if (cs.mSoftBodyInvMassScale > 0.0f) + { + // Body is not movable, equations are simpler + + // Calculate normal and tangential velocity (equation 30) + Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity); + Vec3 v_tangential = v.mVelocity - v_normal; + float v_tangential_length = v_tangential.Length(); + + // Apply friction (modified equation 31) + if (v_tangential_length > 0.0f) + v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + + // Apply restitution (equation 35) + v.mVelocity -= v_normal; + float prev_v_normal = prev_v.Dot(contact_normal); + if (prev_v_normal < restitution_treshold) + v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal; + } + } + } + } +} + +void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + JPH_PROFILE_FUNCTION(); + + // Contact callback + if (mNeedContactCallback && ioContext.mContactListener != nullptr) + { + // Remove non-colliding sensors from the list + for (int i = int(mCollidingSensors.size()) - 1; i >= 0; --i) + if (!mCollidingSensors[i].mHasContact) + { + mCollidingSensors[i] = std::move(mCollidingSensors.back()); + mCollidingSensors.pop_back(); + } + + ioContext.mContactListener->OnSoftBodyContactAdded(*ioContext.mBody, SoftBodyManifold(this)); + } + + // Loop through vertices once more to update the global state + float dt = ioContext.mDeltaTime; + float max_linear_velocity_sq = Square(GetMaxLinearVelocity()); + float max_v_sq = 0.0f; + Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero(); + mLocalPredictedBounds = mLocalBounds = { }; + for (Vertex &v : mVertices) + { + // Calculate max square velocity + float v_sq = v.mVelocity.LengthSq(); + max_v_sq = max(max_v_sq, v_sq); + + // Clamp if velocity is too high + if (v_sq > max_linear_velocity_sq) + v.mVelocity *= sqrt(max_linear_velocity_sq / v_sq); + + // Calculate local linear/angular velocity + linear_velocity += v.mVelocity; + angular_velocity += v.mPosition.Cross(v.mVelocity); + + // Update local bounding box + mLocalBounds.Encapsulate(v.mPosition); + + // Create predicted position for the next frame in order to detect collisions before they happen + mLocalPredictedBounds.Encapsulate(v.mPosition + v.mVelocity * dt + ioContext.mDisplacementDueToGravity); + + // Reset collision data for the next iteration + v.ResetCollision(); + } + + // Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space + float num_vertices_divider = float(max(int(mVertices.size()), 1)); + SetLinearVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(linear_velocity / num_vertices_divider)); + SetAngularVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(angular_velocity / num_vertices_divider)); + + if (mUpdatePosition) + { + // Shift the body so that the position is the center of the local bounds + Vec3 delta = mLocalBounds.GetCenter(); + ioContext.mDeltaPosition = ioContext.mCenterOfMassTransform.Multiply3x3(delta); + for (Vertex &v : mVertices) + v.mPosition -= delta; + + // Update the skin state too since we will use this position as the previous position in the next update + for (SkinState &s : mSkinState) + s.mPosition -= delta; + JPH_IF_DEBUG_RENDERER(mSkinStateTransform.SetTranslation(mSkinStateTransform.GetTranslation() + ioContext.mDeltaPosition);) + + // Offset bounds to match new position + mLocalBounds.Translate(-delta); + mLocalPredictedBounds.Translate(-delta); + } + else + ioContext.mDeltaPosition = Vec3::sZero(); + + // Test if we should go to sleep + if (GetAllowSleeping()) + { + if (max_v_sq > inPhysicsSettings.mPointVelocitySleepThreshold) + { + ResetSleepTestTimer(); + ioContext.mCanSleep = ECanSleep::CannotSleep; + } + else + ioContext.mCanSleep = AccumulateSleepTime(dt, inPhysicsSettings.mTimeBeforeSleep); + } + else + ioContext.mCanSleep = ECanSleep::CannotSleep; + + // If SkinVertices is not called after this then don't use the previous position as the skin is static + mSkinStatePreviousPositionValid = false; + + // Reset force accumulator + ResetForce(); +} + +void SoftBodyMotionProperties::UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface) +{ + JPH_PROFILE_FUNCTION(); + + // Write back velocity deltas + for (const CollidingShape &cs : mCollidingShapes) + if (cs.mUpdateVelocities) + inBodyInterface.AddLinearAndAngularVelocity(cs.mBodyID, inContext.mCenterOfMassTransform.Multiply3x3(cs.mLinearVelocity - cs.mOriginalLinearVelocity), inContext.mCenterOfMassTransform.Multiply3x3(cs.mAngularVelocity - cs.mOriginalAngularVelocity)); + + // Clear colliding shapes/sensors to avoid hanging on to references to shapes + mCollidingShapes.clear(); + mCollidingSensors.clear(); +} + +void SoftBodyMotionProperties::InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext) +{ + JPH_PROFILE_FUNCTION(); + + // Store body + ioContext.mBody = &inSoftBody; + ioContext.mMotionProperties = this; + ioContext.mContactListener = inSystem.GetSoftBodyContactListener(); + ioContext.mSimShapeFilter = inSystem.GetSimShapeFilter(); + + // Convert gravity to local space + ioContext.mCenterOfMassTransform = inSoftBody.GetCenterOfMassTransform(); + ioContext.mGravity = ioContext.mCenterOfMassTransform.Multiply3x3Transposed(GetGravityFactor() * inSystem.GetGravity()); + + // Calculate delta time for sub step + ioContext.mDeltaTime = inDeltaTime; + ioContext.mSubStepDeltaTime = inDeltaTime / mNumIterations; + + // Calculate total displacement we'll have due to gravity over all sub steps + // The total displacement as produced by our integrator can be written as: Sum(i * g * dt^2, i = 0..mNumIterations). + // This is bigger than 0.5 * g * dt^2 because we first increment the velocity and then update the position + // Using Sum(i, i = 0..n) = n * (n + 1) / 2 we can write this as: + ioContext.mDisplacementDueToGravity = (0.5f * mNumIterations * (mNumIterations + 1) * Square(ioContext.mSubStepDeltaTime)) * ioContext.mGravity; +} + +void SoftBodyMotionProperties::StartNextIteration(const SoftBodyUpdateContext &ioContext) +{ + ApplyPressure(ioContext); + + IntegratePositions(ioContext); +} + +void SoftBodyMotionProperties::StartFirstIteration(SoftBodyUpdateContext &ioContext) +{ + // Start the first iteration + JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); + JPH_ASSERT(iteration == 0); + StartNextIteration(ioContext); + ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release); +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext) +{ + // Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it) + uint num_vertices = (uint)mVertices.size(); + if (ioContext.mNextCollisionVertex.load(memory_order_relaxed) < num_vertices) + { + // Fetch next batch of vertices to process + uint next_vertex = ioContext.mNextCollisionVertex.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_acquire); + if (next_vertex < num_vertices) + { + // Process collision planes + uint num_vertices_to_process = min(SoftBodyUpdateContext::cVertexCollisionBatch, num_vertices - next_vertex); + DetermineCollisionPlanes(next_vertex, num_vertices_to_process); + uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_release) + num_vertices_to_process; + if (vertices_processed >= num_vertices) + { + // Determine next state + if (mCollidingSensors.empty()) + StartFirstIteration(ioContext); + else + ioContext.mState.store(SoftBodyUpdateContext::EState::DetermineSensorCollisions, memory_order_release); + } + return EStatus::DidWork; + } + } + + return EStatus::NoWork; +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext) +{ + // Do a relaxed read to see if there are more sensors to process + uint num_sensors = (uint)mCollidingSensors.size(); + if (ioContext.mNextSensorIndex.load(memory_order_relaxed) < num_sensors) + { + // Fetch next sensor to process + uint sensor_index = ioContext.mNextSensorIndex.fetch_add(1, memory_order_acquire); + if (sensor_index < num_sensors) + { + // Process this sensor + DetermineSensorCollisions(mCollidingSensors[sensor_index]); + + // Determine next state + uint sensors_processed = ioContext.mNumSensorsProcessed.fetch_add(1, memory_order_release) + 1; + if (sensors_processed >= num_sensors) + StartFirstIteration(ioContext); + return EStatus::DidWork; + } + } + + return EStatus::NoWork; +} + +void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex) +{ + // Determine start and end + SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0 }; + const SoftBodySharedSettings::UpdateGroup &prev = inGroupIndex > 0? mSettings->mUpdateGroups[inGroupIndex - 1] : start; + const SoftBodySharedSettings::UpdateGroup ¤t = mSettings->mUpdateGroups[inGroupIndex]; + + // Process volume constraints + ApplyVolumeConstraints(ioContext, prev.mVolumeEndIndex, current.mVolumeEndIndex); + + // Process bend constraints + ApplyDihedralBendConstraints(ioContext, prev.mDihedralBendEndIndex, current.mDihedralBendEndIndex); + + // Process skinned constraints + ApplySkinConstraints(ioContext, prev.mSkinnedEndIndex, current.mSkinnedEndIndex); + + // Process edges + ApplyEdgeConstraints(ioContext, prev.mEdgeEndIndex, current.mEdgeEndIndex); + + // Process LRA constraints + ApplyLRAConstraints(prev.mLRAEndIndex, current.mLRAEndIndex); +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + uint num_groups = (uint)mSettings->mUpdateGroups.size(); + JPH_ASSERT(num_groups > 0, "SoftBodySharedSettings::Optimize should have been called!"); + --num_groups; // Last group is the non-parallel group, we don't want to execute it in parallel + + // Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it) + uint next_group = ioContext.mNextConstraintGroup.load(memory_order_relaxed); + if (next_group < num_groups || (num_groups == 0 && next_group == 0)) + { + // Fetch the next group process + next_group = ioContext.mNextConstraintGroup.fetch_add(1, memory_order_acquire); + if (next_group < num_groups || (num_groups == 0 && next_group == 0)) + { + uint num_groups_processed = 0; + if (num_groups > 0) + { + // Process this group + ProcessGroup(ioContext, next_group); + + // Increment total number of groups processed + num_groups_processed = ioContext.mNumConstraintGroupsProcessed.fetch_add(1, memory_order_relaxed) + 1; + } + + if (num_groups_processed >= num_groups) + { + // Finish the iteration + JPH_PROFILE("FinishIteration"); + + // Process non-parallel group + ProcessGroup(ioContext, num_groups); + + ApplyCollisionConstraintsAndUpdateVelocities(ioContext); + + uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); + if (iteration < mNumIterations) + { + // Start a new iteration + StartNextIteration(ioContext); + + // Reset group logic + ioContext.mNumConstraintGroupsProcessed.store(0, memory_order_relaxed); + ioContext.mNextConstraintGroup.store(0, memory_order_release); + } + else + { + // On final iteration we update the state + UpdateSoftBodyState(ioContext, inPhysicsSettings); + + ioContext.mState.store(SoftBodyUpdateContext::EState::Done, memory_order_release); + return EStatus::Done; + } + } + + return EStatus::DidWork; + } + } + return EStatus::NoWork; +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + switch (ioContext.mState.load(memory_order_relaxed)) + { + case SoftBodyUpdateContext::EState::DetermineCollisionPlanes: + return ParallelDetermineCollisionPlanes(ioContext); + + case SoftBodyUpdateContext::EState::DetermineSensorCollisions: + return ParallelDetermineSensorCollisions(ioContext); + + case SoftBodyUpdateContext::EState::ApplyConstraints: + return ParallelApplyConstraints(ioContext, inPhysicsSettings); + + case SoftBodyUpdateContext::EState::Done: + return EStatus::Done; + + default: + JPH_ASSERT(false); + return EStatus::NoWork; + } +} + +void SoftBodyMotionProperties::SkinVertices([[maybe_unused]] RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, [[maybe_unused]] uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator) +{ + // Calculate the skin matrices + uint num_skin_matrices = uint(mSettings->mInvBindMatrices.size()); + uint skin_matrices_size = num_skin_matrices * sizeof(Mat44); + Mat44 *skin_matrices = (Mat44 *)ioTempAllocator.Allocate(skin_matrices_size); + JPH_SCOPE_EXIT([&ioTempAllocator, skin_matrices, skin_matrices_size]{ ioTempAllocator.Free(skin_matrices, skin_matrices_size); }); + const Mat44 *skin_matrices_end = skin_matrices + num_skin_matrices; + const InvBind *inv_bind_matrix = mSettings->mInvBindMatrices.data(); + for (Mat44 *s = skin_matrices; s < skin_matrices_end; ++s, ++inv_bind_matrix) + *s = inJointMatrices[inv_bind_matrix->mJointIndex] * inv_bind_matrix->mInvBind; + + // Skin the vertices + JPH_IF_DEBUG_RENDERER(mSkinStateTransform = inCenterOfMassTransform;) + JPH_IF_ENABLE_ASSERTS(uint num_vertices = uint(mSettings->mVertices.size());) + JPH_ASSERT(mSkinState.size() == num_vertices); + const SoftBodySharedSettings::Vertex *in_vertices = mSettings->mVertices.data(); + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + // Get bind pose + JPH_ASSERT(s.mVertex < num_vertices); + Vec3 bind_pos = Vec3::sLoadFloat3Unsafe(in_vertices[s.mVertex].mPosition); + + // Skin vertex + Vec3 pos = Vec3::sZero(); + for (const SkinWeight &w : s.mWeights) + { + // We assume that the first zero weight is the end of the list + if (w.mWeight == 0.0f) + break; + + JPH_ASSERT(w.mInvBindIndex < num_skin_matrices); + pos += w.mWeight * (skin_matrices[w.mInvBindIndex] * bind_pos); + } + SkinState &skin_state = mSkinState[s.mVertex]; + skin_state.mPreviousPosition = skin_state.mPosition; + skin_state.mPosition = pos; + } + + // Calculate the normals + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + Vec3 normal = Vec3::sZero(); + uint32 num_faces = s.mNormalInfo >> 24; + if (num_faces > 0) + { + // Calculate normal + const uint32 *f = &mSettings->mSkinnedConstraintNormals[s.mNormalInfo & 0xffffff]; + const uint32 *f_end = f + num_faces; + while (f < f_end) + { + const Face &face = mSettings->mFaces[*f]; + Vec3 v0 = mSkinState[face.mVertex[0]].mPosition; + Vec3 v1 = mSkinState[face.mVertex[1]].mPosition; + Vec3 v2 = mSkinState[face.mVertex[2]].mPosition; + normal += (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sZero()); + ++f; + } + normal = normal.NormalizedOr(Vec3::sZero()); + } + mSkinState[s.mVertex].mNormal = normal; + } + + if (inHardSkinAll) + { + // Hard skin all vertices and reset their velocities + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + Vertex &vertex = mVertices[s.mVertex]; + SkinState &skin_state = mSkinState[s.mVertex]; + skin_state.mPreviousPosition = skin_state.mPosition; + vertex.mPosition = skin_state.mPosition; + vertex.mVelocity = Vec3::sZero(); + } + } + else if (!mEnableSkinConstraints) + { + // Hard skin only the kinematic vertices as we will not solve the skin constraints later + for (const Skinned &s : mSettings->mSkinnedConstraints) + if (s.mMaxDistance == 0.0f) + { + Vertex &vertex = mVertices[s.mVertex]; + vertex.mPosition = mSkinState[s.mVertex].mPosition; + } + } + + // Indicate that the previous positions are valid for the coming update + mSkinStatePreviousPositionValid = true; +} + +void SoftBodyMotionProperties::CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem) +{ + JPH_PROFILE_FUNCTION(); + + // Create update context + SoftBodyUpdateContext context; + InitializeUpdateContext(inDeltaTime, ioSoftBody, inSystem, context); + + // Determine bodies we're colliding with + DetermineCollidingShapes(context, inSystem, inSystem.GetBodyLockInterface()); + + // Call the internal update until it finishes + EStatus status; + const PhysicsSettings &settings = inSystem.GetPhysicsSettings(); + while ((status = ParallelUpdate(context, settings)) == EStatus::DidWork) + continue; + JPH_ASSERT(status == EStatus::Done); + + // Update the state of the bodies we've collided with + UpdateRigidBodyVelocities(context, inSystem.GetBodyInterface()); + + // Update position of the soft body + if (mUpdatePosition) + inSystem.GetBodyInterface().SetPosition(ioSoftBody.GetID(), ioSoftBody.GetPosition() + context.mDeltaPosition, EActivation::DontActivate); +} + +#ifdef JPH_DEBUG_RENDERER + +void SoftBodyMotionProperties::DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + for (const Vertex &v : mVertices) + inRenderer->DrawMarker(inCenterOfMassTransform * v.mPosition, v.mInvMass > 0.0f? Color::sGreen : Color::sRed, 0.05f); +} + +void SoftBodyMotionProperties::DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + for (const Vertex &v : mVertices) + inRenderer->DrawArrow(inCenterOfMassTransform * v.mPosition, inCenterOfMassTransform * (v.mPosition + v.mVelocity), Color::sYellow, 0.01f); +} + +template +inline void SoftBodyMotionProperties::DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const +{ + uint start = 0; + for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i) + { + uint end = inGetEndIndex(mSettings->mUpdateGroups[i]); + + Color base_color; + if (inConstraintColor != ESoftBodyConstraintColor::ConstraintType) + base_color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group + else + base_color = inBaseColor; + + for (uint idx = start; idx < end; ++idx) + { + Color color = inConstraintColor == ESoftBodyConstraintColor::ConstraintOrder? base_color * (float(idx - start) / (end - start)) : base_color; + inDrawConstraint(idx, color); + } + + start = end; + } +} + +void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mEdgeEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Edge &e = mSettings->mEdgeConstraints[inIndex]; + inRenderer->DrawLine(inCenterOfMassTransform * mVertices[e.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[e.mVertex[1]].mPosition, inColor); + }, + Color::sWhite); +} + +void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mDihedralBendEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const DihedralBend &b = mSettings->mDihedralBendConstraints[inIndex]; + + RVec3 x0 = inCenterOfMassTransform * mVertices[b.mVertex[0]].mPosition; + RVec3 x1 = inCenterOfMassTransform * mVertices[b.mVertex[1]].mPosition; + RVec3 x2 = inCenterOfMassTransform * mVertices[b.mVertex[2]].mPosition; + RVec3 x3 = inCenterOfMassTransform * mVertices[b.mVertex[3]].mPosition; + RVec3 c_edge = 0.5_r * (x0 + x1); + RVec3 c0 = (x0 + x1 + x2) / 3.0_r; + RVec3 c1 = (x0 + x1 + x3) / 3.0_r; + + inRenderer->DrawArrow(0.9_r * x0 + 0.1_r * x1, 0.1_r * x0 + 0.9_r * x1, inColor, 0.01f); + inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c0, inColor); + inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c1, inColor); + }, + Color::sGreen); +} + +void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mVolumeEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Volume &v = mSettings->mVolumeConstraints[inIndex]; + + RVec3 x1 = inCenterOfMassTransform * mVertices[v.mVertex[0]].mPosition; + RVec3 x2 = inCenterOfMassTransform * mVertices[v.mVertex[1]].mPosition; + RVec3 x3 = inCenterOfMassTransform * mVertices[v.mVertex[2]].mPosition; + RVec3 x4 = inCenterOfMassTransform * mVertices[v.mVertex[3]].mPosition; + + inRenderer->DrawTriangle(x1, x3, x2, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x2, x3, x4, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x1, x4, x3, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x1, x2, x4, inColor, DebugRenderer::ECastShadow::On); + }, + Color::sYellow); +} + +void SoftBodyMotionProperties::DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mSkinnedEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Skinned &s = mSettings->mSkinnedConstraints[inIndex]; + const SkinState &skin_state = mSkinState[s.mVertex]; + inRenderer->DrawArrow(mSkinStateTransform * skin_state.mPosition, mSkinStateTransform * (skin_state.mPosition + 0.1f * skin_state.mNormal), inColor, 0.01f); + inRenderer->DrawLine(mSkinStateTransform * skin_state.mPosition, inCenterOfMassTransform * mVertices[s.mVertex].mPosition, Color::sBlue); + }, + Color::sOrange); +} + +void SoftBodyMotionProperties::DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mLRAEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const LRA &l = mSettings->mLRAConstraints[inIndex]; + inRenderer->DrawLine(inCenterOfMassTransform * mVertices[l.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[l.mVertex[1]].mPosition, inColor); + }, + Color::sGrey); +} + +void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + inRenderer->DrawWireBox(inCenterOfMassTransform, mLocalPredictedBounds, Color::sRed); +} + +#endif // JPH_DEBUG_RENDERER + +void SoftBodyMotionProperties::SaveState(StateRecorder &inStream) const +{ + MotionProperties::SaveState(inStream); + + for (const Vertex &v : mVertices) + { + inStream.Write(v.mPreviousPosition); + inStream.Write(v.mPosition); + inStream.Write(v.mVelocity); + } + + for (const SkinState &s : mSkinState) + { + inStream.Write(s.mPreviousPosition); + inStream.Write(s.mPosition); + inStream.Write(s.mNormal); + } + + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mLocalPredictedBounds.mMin); + inStream.Write(mLocalPredictedBounds.mMax); +} + +void SoftBodyMotionProperties::RestoreState(StateRecorder &inStream) +{ + MotionProperties::RestoreState(inStream); + + for (Vertex &v : mVertices) + { + inStream.Read(v.mPreviousPosition); + inStream.Read(v.mPosition); + inStream.Read(v.mVelocity); + } + + for (SkinState &s : mSkinState) + { + inStream.Read(s.mPreviousPosition); + inStream.Read(s.mPosition); + inStream.Read(s.mNormal); + } + + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mLocalPredictedBounds.mMin); + inStream.Read(mLocalPredictedBounds.mMax); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h new file mode 100644 index 000000000000..af66b7a2e4a9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h @@ -0,0 +1,297 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class BodyInterface; +class BodyLockInterface; +struct PhysicsSettings; +class Body; +class Shape; +class SoftBodyCreationSettings; +class TempAllocator; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +enum class ESoftBodyConstraintColor; +#endif // JPH_DEBUG_RENDERER + +/// This class contains the runtime information of a soft body. +// +// Based on: XPBD, Extended Position Based Dynamics, Matthias Muller, Ten Minute Physics +// See: https://matthias-research.github.io/pages/tenMinutePhysics/09-xpbd.pdf +class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties +{ +public: + using Vertex = SoftBodyVertex; + using Edge = SoftBodySharedSettings::Edge; + using Face = SoftBodySharedSettings::Face; + using DihedralBend = SoftBodySharedSettings::DihedralBend; + using Volume = SoftBodySharedSettings::Volume; + using InvBind = SoftBodySharedSettings::InvBind; + using SkinWeight = SoftBodySharedSettings::SkinWeight; + using Skinned = SoftBodySharedSettings::Skinned; + using LRA = SoftBodySharedSettings::LRA; + + /// Initialize the soft body motion properties + void Initialize(const SoftBodyCreationSettings &inSettings); + + /// Get the shared settings of the soft body + const SoftBodySharedSettings * GetSettings() const { return mSettings; } + + /// Get the vertices of the soft body + const Array & GetVertices() const { return mVertices; } + Array & GetVertices() { return mVertices; } + + /// Access an individual vertex + const Vertex & GetVertex(uint inIndex) const { return mVertices[inIndex]; } + Vertex & GetVertex(uint inIndex) { return mVertices[inIndex]; } + + /// Get the materials of the soft body + const PhysicsMaterialList & GetMaterials() const { return mSettings->mMaterials; } + + /// Get the faces of the soft body + const Array & GetFaces() const { return mSettings->mFaces; } + + /// Access to an individual face + const Face & GetFace(uint inIndex) const { return mSettings->mFaces[inIndex]; } + + /// Get the number of solver iterations + uint32 GetNumIterations() const { return mNumIterations; } + void SetNumIterations(uint32 inNumIterations) { mNumIterations = inNumIterations; } + + /// Get the pressure of the soft body + float GetPressure() const { return mPressure; } + void SetPressure(float inPressure) { mPressure = inPressure; } + + /// Update the position of the body while simulating (set to false for something that is attached to the static world) + bool GetUpdatePosition() const { return mUpdatePosition; } + void SetUpdatePosition(bool inUpdatePosition) { mUpdatePosition = inUpdatePosition; } + + /// Global setting to turn on/off skin constraints + bool GetEnableSkinConstraints() const { return mEnableSkinConstraints; } + void SetEnableSkinConstraints(bool inEnableSkinConstraints) { mEnableSkinConstraints = inEnableSkinConstraints; } + + /// Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints. 0 to hard skin all vertices. + float GetSkinnedMaxDistanceMultiplier() const { return mSkinnedMaxDistanceMultiplier; } + void SetSkinnedMaxDistanceMultiplier(float inSkinnedMaxDistanceMultiplier) { mSkinnedMaxDistanceMultiplier = inSkinnedMaxDistanceMultiplier; } + + /// Get local bounding box + const AABox & GetLocalBounds() const { return mLocalBounds; } + + /// Get the volume of the soft body. Note can become negative if the shape is inside out! + float GetVolume() const { return GetVolumeTimesSix() / 6.0f; } + + /// Calculate the total mass and inertia of this body based on the current state of the vertices + void CalculateMassAndInertia(); + +#ifdef JPH_DEBUG_RENDERER + /// Draw the state of a soft body + void DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; + void DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; + void DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + /// Skin vertices to supplied joints, information is used by the skinned constraints. + /// @param inCenterOfMassTransform Value of Body::GetCenterOfMassTransform(). + /// @param inJointMatrices The joint matrices must be expressed relative to inCenterOfMassTransform. + /// @param inNumJoints Indicates how large the inJointMatrices array is (used only for validating out of bounds). + /// @param inHardSkinAll Can be used to position all vertices on the skinned vertices and can be used to hard reset the soft body. + /// @param ioTempAllocator Allocator. + void SkinVertices(RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator); + + /// This function allows you to update the soft body immediately without going through the PhysicsSystem. + /// This is useful if the soft body is teleported and needs to 'settle' or it can be used if a the soft body + /// is not added to the PhysicsSystem and needs to be updated manually. One reason for not adding it to the + /// PhyicsSystem is that you might want to update a soft body immediately after updating an animated object + /// that has the soft body attached to it. If the soft body is added to the PhysicsSystem it will be updated + /// by it, so calling this function will effectively update it twice. Note that when you use this function, + /// only the current thread will be used, whereas if you update through the PhysicsSystem, multiple threads may + /// be used. + /// Note that this will bypass any sleep checks. Since the dynamic objects that the soft body touches + /// will not move during this call, there can be simulation artifacts if you call this function multiple times + /// without running the physics simulation step. + void CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem); + + //////////////////////////////////////////////////////////// + // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY + //////////////////////////////////////////////////////////// + + /// Initialize the update context. Not part of the public API. + void InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext); + + /// Do a broad phase check and collect all bodies that can possibly collide with this soft body. Not part of the public API. + void DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface); + + /// Return code for ParallelUpdate + enum class EStatus + { + NoWork = 1 << 0, ///< No work was done because other threads were still working on a batch that cannot run concurrently + DidWork = 1 << 1, ///< Work was done to progress the update + Done = 1 << 2, ///< All work is done + }; + + /// Update the soft body, will process a batch of work. Not part of the public API. + EStatus ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Update the velocities of all rigid bodies that we collided with. Not part of the public API. + void UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface); + +private: + // SoftBodyManifold needs to have access to CollidingShape + friend class SoftBodyManifold; + + // Information about a leaf shape that we're colliding with + struct LeafShape + { + LeafShape() = default; + LeafShape(Mat44Arg inTransform, Vec3Arg inScale, const Shape *inShape) : mTransform(inTransform), mScale(inScale), mShape(inShape) { } + + Mat44 mTransform; ///< Transform of the shape relative to the soft body + Vec3 mScale; ///< Scale of the shape + RefConst mShape; ///< Shape + }; + + // Collect information about the colliding bodies + struct CollidingShape + { + /// Get the velocity of a point on this body + Vec3 GetPointVelocity(Vec3Arg inPointRelativeToCOM) const + { + return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); + } + + Mat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + Array mShapes; ///< Leaf shapes of the body we hit + BodyID mBodyID; ///< Body ID of the body we hit + EMotionType mMotionType; ///< Motion type of the body we hit + float mInvMass; ///< Inverse mass of the body we hit + float mFriction; ///< Combined friction of the two bodies + float mRestitution; ///< Combined restitution of the two bodies + float mSoftBodyInvMassScale; ///< Scale factor for the inverse mass of the soft body vertices + bool mUpdateVelocities; ///< If the linear/angular velocity changed and the body needs to be updated + Mat44 mInvInertia; ///< Inverse inertia in local space to the soft body + Vec3 mLinearVelocity; ///< Linear velocity of the body in local space to the soft body + Vec3 mAngularVelocity; ///< Angular velocity of the body in local space to the soft body + Vec3 mOriginalLinearVelocity; ///< Linear velocity of the body in local space to the soft body at start + Vec3 mOriginalAngularVelocity; ///< Angular velocity of the body in local space to the soft body at start + }; + + // Collect information about the colliding sensors + struct CollidingSensor + { + Mat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + Array mShapes; ///< Leaf shapes of the body we hit + BodyID mBodyID; ///< Body ID of the body we hit + bool mHasContact; ///< If the sensor collided with the soft body + }; + + // Information about the state of all skinned vertices + struct SkinState + { + Vec3 mPreviousPosition = Vec3::sZero(); ///< Previous position of the skinned vertex, used to interpolate between the previous and current position + Vec3 mPosition = Vec3::sNaN(); ///< Current position of the skinned vertex + Vec3 mNormal = Vec3::sNaN(); ///< Normal of the skinned vertex + }; + + /// Do a narrow phase check and determine the closest feature that we can collide with + void DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices); + + /// Do a narrow phase check between a single sensor and the soft body + void DetermineSensorCollisions(CollidingSensor &ioSensor); + + /// Apply pressure force and update the vertex velocities + void ApplyPressure(const SoftBodyUpdateContext &inContext); + + /// Integrate the positions of all vertices by 1 sub step + void IntegratePositions(const SoftBodyUpdateContext &inContext); + + /// Enforce all bend constraints + void ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all volume constraints + void ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all skin constraints + void ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all edge constraints + void ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all LRA constraints + void ApplyLRAConstraints(uint inStartIndex, uint inEndIndex); + + /// Enforce all collision constraints & update all velocities according the XPBD algorithm + void ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext); + + /// Update the state of the soft body (position, velocity, bounds) + void UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Start the first solver iteration + void StartFirstIteration(SoftBodyUpdateContext &ioContext); + + /// Executes tasks that need to run on the start of an iteration (i.e. the stuff that can't run in parallel) + void StartNextIteration(const SoftBodyUpdateContext &ioContext); + + /// Helper function for ParallelUpdate that works on batches of collision planes + EStatus ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext); + + /// Helper function for ParallelUpdate that works on sensor collisions + EStatus ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext); + + /// Helper function for ParallelUpdate that works on batches of constraints + EStatus ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Helper function to update a single group of constraints + void ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex); + + /// Returns 6 times the volume of the soft body + float GetVolumeTimesSix() const; + +#ifdef JPH_DEBUG_RENDERER + /// Helper function to draw constraints + template + inline void DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const; + + RMat44 mSkinStateTransform = RMat44::sIdentity(); ///< The matrix that transforms mSkinState to world space +#endif // JPH_DEBUG_RENDERER + + RefConst mSettings; ///< Configuration of the particles and constraints + Array mVertices; ///< Current state of all vertices in the simulation + Array mCollidingShapes; ///< List of colliding shapes retrieved during the last update + Array mCollidingSensors; ///< List of colliding sensors retrieved during the last update + Array mSkinState; ///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in) + AABox mLocalBounds; ///< Bounding box of all vertices + AABox mLocalPredictedBounds; ///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time + uint32 mNumIterations; ///< Number of solver iterations + float mPressure; ///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure + float mSkinnedMaxDistanceMultiplier = 1.0f; ///< Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints + bool mUpdatePosition; ///< Update the position of the body while simulating (set to false for something that is attached to the static world) + bool mNeedContactCallback = false; ///< True if the soft body has collided with anything in the last update + bool mEnableSkinConstraints = true; ///< If skin constraints are enabled + bool mSkinStatePreviousPositionValid = false; ///< True if the skinning was updated in the last update so that the previous position of the skin state is valid +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp new file mode 100644 index 000000000000..6692b223eb48 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp @@ -0,0 +1,338 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +uint SoftBodyShape::GetSubShapeIDBits() const +{ + // Ensure we have enough bits to encode our shape [0, n - 1] + uint32 n = (uint32)mSoftBodyMotionProperties->GetFaces().size() - 1; + return 32 - CountLeadingZeros(n); +} + +uint32 SoftBodyShape::GetFaceIndex(const SubShapeID &inSubShapeID) const +{ + SubShapeID remainder; + uint32 face_index = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + return face_index; +} + +AABox SoftBodyShape::GetLocalBounds() const +{ + return mSoftBodyMotionProperties->GetLocalBounds(); +} + +bool SoftBodyShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + uint num_triangle_bits = GetSubShapeIDBits(); + uint triangle_idx = uint(-1); + + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3); + if (fraction < ioHit.mFraction) + { + // Store fraction + ioHit.mFraction = fraction; + + // Store triangle index + triangle_idx = uint(&f - mSoftBodyMotionProperties->GetFaces().data()); + } + } + + if (triangle_idx == uint(-1)) + return false; + + ioHit.mSubShapeID2 = inSubShapeIDCreator.PushID(triangle_idx, num_triangle_bits).GetID(); + return true; +} + +void SoftBodyShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + uint num_triangle_bits = GetSubShapeIDBits(); + + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + // Back facing check + if (inRayCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && (x2 - x1).Cross(x3 - x1).Dot(inRay.mDirection) > 0.0f) + continue; + + // Test ray against triangle + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3); + if (fraction < ioCollector.GetEarlyOutFraction()) + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeIDCreator.PushID(uint(&f - mSoftBodyMotionProperties->GetFaces().data()), num_triangle_bits).GetID(); + ioCollector.AddHit(hit); + } + } +} + +void SoftBodyShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const +{ + /* Not implemented */ +} + +const PhysicsMaterial *SoftBodyShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + return mSoftBodyMotionProperties->GetMaterials()[f.mMaterialIndex]; +} + +Vec3 SoftBodyShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + return (x2 - x1).Cross(x3 - x1).NormalizedOr(Vec3::sAxisY()); +} + +void SoftBodyShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + + for (uint32 i : f.mVertex) + outVertices.push_back(inCenterOfMassTransform * (inScale * vertices[i].mPosition)); +} + +void SoftBodyShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + outSubmergedVolume = 0.0f; + outTotalVolume = mSoftBodyMotionProperties->GetVolume(); + outCenterOfBuoyancy = Vec3::sZero(); +} + +#ifdef JPH_DEBUG_RENDERER + +void SoftBodyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + RVec3 x1 = inCenterOfMassTransform * vertices[f.mVertex[0]].mPosition; + RVec3 x2 = inCenterOfMassTransform * vertices[f.mVertex[1]].mPosition; + RVec3 x3 = inCenterOfMassTransform * vertices[f.mVertex[2]].mPosition; + + inRenderer->DrawTriangle(x1, x2, x3, inColor, DebugRenderer::ECastShadow::On); + } +} + +#endif // JPH_DEBUG_RENDERER + +struct SoftBodyShape::SBSGetTrianglesContext +{ + Mat44 mCenterOfMassTransform; + int mTriangleIndex; +}; + +void SoftBodyShape::GetTrianglesStart(GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + SBSGetTrianglesContext &context = reinterpret_cast(ioContext); + context.mCenterOfMassTransform = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale); + context.mTriangleIndex = 0; +} + +int SoftBodyShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + SBSGetTrianglesContext &context = reinterpret_cast(ioContext); + + const Array &faces = mSoftBodyMotionProperties->GetFaces(); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + const PhysicsMaterialList &materials = mSoftBodyMotionProperties->GetMaterials(); + + int num_triangles = min(inMaxTrianglesRequested, (int)faces.size() - context.mTriangleIndex); + for (int i = 0; i < num_triangles; ++i) + { + const SoftBodyMotionProperties::Face &f = faces[context.mTriangleIndex + i]; + + Vec3 x1 = context.mCenterOfMassTransform * vertices[f.mVertex[0]].mPosition; + Vec3 x2 = context.mCenterOfMassTransform * vertices[f.mVertex[1]].mPosition; + Vec3 x3 = context.mCenterOfMassTransform * vertices[f.mVertex[2]].mPosition; + + x1.StoreFloat3(outTriangleVertices++); + x2.StoreFloat3(outTriangleVertices++); + x3.StoreFloat3(outTriangleVertices++); + + if (outMaterials != nullptr) + *outMaterials++ = materials[f.mMaterialIndex]; + } + + context.mTriangleIndex += num_triangles; + return num_triangles; +} + +Shape::Stats SoftBodyShape::GetStats() const +{ + return Stats(sizeof(*this), (uint)mSoftBodyMotionProperties->GetFaces().size()); +} + +float SoftBodyShape::GetVolume() const +{ + return mSoftBodyMotionProperties->GetVolume(); +} + +void SoftBodyShape::sCollideConvexVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape2 = static_cast(inShape2); + + const Array &vertices = shape2->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape2->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape2->GetSubShapeIDBits(); + + CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCollideSphereVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + const SphereShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape2 = static_cast(inShape2); + + const Array &vertices = shape2->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape2->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape2->GetSubShapeIDBits(); + + CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape = static_cast(inShape); + + const Array &vertices = shape->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape->GetSubShapeIDBits(); + + CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape = static_cast(inShape); + + const Array &vertices = shape->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape->GetSubShapeIDBits(); + + CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::SoftBody); + f.mConstruct = nullptr; // Not supposed to be constructed by users! + f.mColor = Color::sDarkGreen; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::SoftBody, sCollideConvexVsSoftBody); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::SoftBody, sCastConvexVsSoftBody); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCollideSphereVsSoftBody); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCastSphereVsSoftBody); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h new file mode 100644 index 000000000000..70605dae409c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class SoftBodyMotionProperties; +class CollideShapeSettings; + +/// Shape used exclusively for soft bodies. Adds the ability to perform collision checks against soft bodies. +class JPH_EXPORT SoftBodyShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SoftBodyShape() : Shape(EShapeType::SoftBody, EShapeSubType::SoftBody) { } + + /// Determine amount of bits needed to encode sub shape id + uint GetSubShapeIDBits() const; + + /// Convert a sub shape ID back to a face index + uint32 GetFaceIndex(const SubShapeID &inSubShapeID) const; + + // See Shape + virtual bool MustBeStatic() const override { return false; } + virtual Vec3 GetCenterOfMass() const override { return Vec3::sZero(); } + virtual AABox GetLocalBounds() const override; + virtual uint GetSubShapeIDBitsRecursive() const override { return GetSubShapeIDBits(); } + virtual float GetInnerRadius() const override { return 0.0f; } + virtual MassProperties GetMassProperties() const override { return MassProperties(); } + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3Arg inBaseOffset +#endif + ) const override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + virtual Stats GetStats() const override; + virtual float GetVolume() const override; + + // Register shape functions with the registry + static void sRegister(); + +private: + // Helper functions called by CollisionDispatch + static void sCollideConvexVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideSphereVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + struct SBSGetTrianglesContext; + + friend class BodyManager; + + const SoftBodyMotionProperties *mSoftBodyMotionProperties; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp new file mode 100644 index 000000000000..7eb8dd555c78 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp @@ -0,0 +1,1032 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Vertex) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mPosition) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mVelocity) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mInvMass) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Face) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mMaterialIndex) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Edge) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mRestLength) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mCompliance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::DihedralBend) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mCompliance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mInitialAngle) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Volume) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mSixRestVolume) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mCompliance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::InvBind) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mJointIndex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mInvBind) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::SkinWeight) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mInvBindIndex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mWeight) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Skinned) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mWeights) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mMaxDistance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopDistance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopRadius) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::LRA) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mMaxDistance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertices) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mFaces) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mDihedralBendConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mLRAConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius) +} + +void SoftBodySharedSettings::CalculateClosestKinematic() +{ + // Check if we already calculated this + if (!mClosestKinematic.empty()) + return; + + // Reserve output size + mClosestKinematic.resize(mVertices.size()); + + // Create a list of connected vertices + Array> connectivity; + connectivity.resize(mVertices.size()); + for (const Edge &e : mEdgeConstraints) + { + connectivity[e.mVertex[0]].push_back(e.mVertex[1]); + connectivity[e.mVertex[1]].push_back(e.mVertex[0]); + } + + // Use Dijkstra's algorithm to find the closest kinematic vertex for each vertex + // See: https://en.wikipedia.org/wiki/Dijkstra's_algorithm + // + // An element in the open list + struct Open + { + // Order so that we get the shortest distance first + bool operator < (const Open &inRHS) const + { + return mDistance > inRHS.mDistance; + } + + uint32 mVertex; + float mDistance; + }; + + // Start with all kinematic elements + Array to_visit; + for (uint32 v = 0; v < mVertices.size(); ++v) + if (mVertices[v].mInvMass == 0.0f) + { + mClosestKinematic[v].mVertex = v; + mClosestKinematic[v].mDistance = 0.0f; + to_visit.push_back({ v, 0.0f }); + BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less { }); + } + + // Visit all vertices remembering the closest kinematic vertex and its distance + JPH_IF_ENABLE_ASSERTS(float last_closest = 0.0f;) + while (!to_visit.empty()) + { + // Pop element from the open list + BinaryHeapPop(to_visit.begin(), to_visit.end(), std::less { }); + Open current = to_visit.back(); + to_visit.pop_back(); + JPH_ASSERT(current.mDistance >= last_closest); + JPH_IF_ENABLE_ASSERTS(last_closest = current.mDistance;) + + // Loop through all of its connected vertices + for (uint32 v : connectivity[current.mVertex]) + { + // Calculate distance from the current vertex to this target vertex and check if it is smaller + float new_distance = current.mDistance + (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current.mVertex].mPosition)).Length(); + if (new_distance < mClosestKinematic[v].mDistance) + { + // Remember new closest vertex + mClosestKinematic[v].mVertex = mClosestKinematic[current.mVertex].mVertex; + mClosestKinematic[v].mDistance = new_distance; + to_visit.push_back({ v, new_distance }); + BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less { }); + } + } + } +} + +void SoftBodySharedSettings::CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType, float inAngleTolerance) +{ + struct EdgeHelper + { + uint32 mVertex[2]; + uint32 mEdgeIdx; + }; + + // Create list of all edges + Array edges; + edges.reserve(mFaces.size() * 3); + for (const Face &f : mFaces) + for (int i = 0; i < 3; ++i) + { + uint32 v0 = f.mVertex[i]; + uint32 v1 = f.mVertex[(i + 1) % 3]; + + EdgeHelper e; + e.mVertex[0] = min(v0, v1); + e.mVertex[1] = max(v0, v1); + e.mEdgeIdx = uint32(&f - mFaces.data()) * 3 + i; + edges.push_back(e); + } + + // Sort the edges + QuickSort(edges.begin(), edges.end(), [](const EdgeHelper &inLHS, const EdgeHelper &inRHS) { return inLHS.mVertex[0] < inRHS.mVertex[0] || (inLHS.mVertex[0] == inRHS.mVertex[0] && inLHS.mVertex[1] < inRHS.mVertex[1]); }); + + // Only add edges if one of the vertices is movable + auto add_edge = [this](uint32 inVtx1, uint32 inVtx2, float inCompliance1, float inCompliance2) { + if ((mVertices[inVtx1].mInvMass > 0.0f || mVertices[inVtx2].mInvMass > 0.0f) + && inCompliance1 < FLT_MAX && inCompliance2 < FLT_MAX) + { + Edge temp_edge; + temp_edge.mVertex[0] = inVtx1; + temp_edge.mVertex[1] = inVtx2; + temp_edge.mCompliance = 0.5f * (inCompliance1 + inCompliance2); + temp_edge.mRestLength = (Vec3(mVertices[inVtx2].mPosition) - Vec3(mVertices[inVtx1].mPosition)).Length(); + JPH_ASSERT(temp_edge.mRestLength > 0.0f); + mEdgeConstraints.push_back(temp_edge); + } + }; + + // Helper function to get the attributes of a vertex + auto attr = [inVertexAttributes, inVertexAttributesLength](uint32 inVertex) { + return inVertexAttributes[min(inVertex, inVertexAttributesLength - 1)]; + }; + + // Create the constraints + float sq_sin_tolerance = Square(Sin(inAngleTolerance)); + float sq_cos_tolerance = Square(Cos(inAngleTolerance)); + mEdgeConstraints.clear(); + mEdgeConstraints.reserve(edges.size()); + for (Array::size_type i = 0; i < edges.size(); ++i) + { + const EdgeHelper &e0 = edges[i]; + + // Get attributes for the vertices of the edge + const VertexAttributes &a0 = attr(e0.mVertex[0]); + const VertexAttributes &a1 = attr(e0.mVertex[1]); + + // Flag that indicates if this edge is a shear edge (if 2 triangles form a quad-like shape and this edge is on the diagonal) + bool is_shear = false; + + // Test if there are any shared edges + for (Array::size_type j = i + 1; j < edges.size(); ++j) + { + const EdgeHelper &e1 = edges[j]; + if (e0.mVertex[0] == e1.mVertex[0] && e0.mVertex[1] == e1.mVertex[1]) + { + // Get opposing vertices + const Face &f0 = mFaces[e0.mEdgeIdx / 3]; + const Face &f1 = mFaces[e1.mEdgeIdx / 3]; + uint32 vopposite0 = f0.mVertex[(e0.mEdgeIdx + 2) % 3]; + uint32 vopposite1 = f1.mVertex[(e1.mEdgeIdx + 2) % 3]; + const VertexAttributes &a_opposite0 = attr(vopposite0); + const VertexAttributes &a_opposite1 = attr(vopposite1); + + // Faces should be roughly in a plane + Vec3 n0 = (Vec3(mVertices[f0.mVertex[2]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f0.mVertex[1]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition)); + Vec3 n1 = (Vec3(mVertices[f1.mVertex[2]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f1.mVertex[1]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition)); + if (Square(n0.Dot(n1)) > sq_cos_tolerance * n0.LengthSq() * n1.LengthSq()) + { + // Faces should approximately form a quad + Vec3 e0_dir = Vec3(mVertices[vopposite0].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition); + Vec3 e1_dir = Vec3(mVertices[vopposite1].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition); + if (Square(e0_dir.Dot(e1_dir)) < sq_sin_tolerance * e0_dir.LengthSq() * e1_dir.LengthSq()) + { + // Shear constraint + add_edge(vopposite0, vopposite1, a_opposite0.mShearCompliance, a_opposite1.mShearCompliance); + is_shear = true; + } + } + + // Bend constraint + switch (inBendType) + { + case EBendType::None: + // Do nothing + break; + + case EBendType::Distance: + // Create an edge constraint to represent the bend constraint + // Use the bend compliance of the shared edge + if (!is_shear) + add_edge(vopposite0, vopposite1, a0.mBendCompliance, a1.mBendCompliance); + break; + + case EBendType::Dihedral: + // Test if both opposite vertices are free to move + if ((mVertices[vopposite0].mInvMass > 0.0f || mVertices[vopposite1].mInvMass > 0.0f) + && a0.mBendCompliance < FLT_MAX && a1.mBendCompliance < FLT_MAX) + { + // Create a bend constraint + // Use the bend compliance of the shared edge + mDihedralBendConstraints.emplace_back(e0.mVertex[0], e0.mVertex[1], vopposite0, vopposite1, 0.5f * (a0.mBendCompliance + a1.mBendCompliance)); + } + break; + } + } + else + { + // Start iterating from the first non-shared edge + i = j - 1; + break; + } + } + + // Create a edge constraint for the current edge + add_edge(e0.mVertex[0], e0.mVertex[1], is_shear? a0.mShearCompliance : a0.mCompliance, is_shear? a1.mShearCompliance : a1.mCompliance); + } + mEdgeConstraints.shrink_to_fit(); + + // Calculate the initial angle for all bend constraints + CalculateBendConstraintConstants(); + + // Check if any vertices have LRA constraints + bool has_lra_constraints = false; + for (const VertexAttributes *va = inVertexAttributes; va < inVertexAttributes + inVertexAttributesLength; ++va) + if (va->mLRAType != ELRAType::None) + { + has_lra_constraints = true; + break; + } + if (has_lra_constraints) + { + // Ensure we have calculated the closest kinematic vertex for each vertex + CalculateClosestKinematic(); + + // Find non-kinematic vertices + for (uint32 v = 0; v < (uint32)mVertices.size(); ++v) + if (mVertices[v].mInvMass > 0.0f) + { + // Check if a closest vertex was found + uint32 closest = mClosestKinematic[v].mVertex; + if (closest != 0xffffffff) + { + // Check which LRA constraint to create + const VertexAttributes &va = attr(v); + switch (va.mLRAType) + { + case ELRAType::None: + break; + + case ELRAType::EuclideanDistance: + mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * (Vec3(mVertices[closest].mPosition) - Vec3(mVertices[v].mPosition)).Length()); + break; + + case ELRAType::GeodesicDistance: + mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * mClosestKinematic[v].mDistance); + break; + } + } + } + } +} + +void SoftBodySharedSettings::CalculateEdgeLengths() +{ + for (Edge &e : mEdgeConstraints) + { + e.mRestLength = (Vec3(mVertices[e.mVertex[1]].mPosition) - Vec3(mVertices[e.mVertex[0]].mPosition)).Length(); + JPH_ASSERT(e.mRestLength > 0.0f); + } +} + +void SoftBodySharedSettings::CalculateLRALengths(float inMaxDistanceMultiplier) +{ + for (LRA &l : mLRAConstraints) + { + l.mMaxDistance = inMaxDistanceMultiplier * (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length(); + JPH_ASSERT(l.mMaxDistance > 0.0f); + } +} + +void SoftBodySharedSettings::CalculateBendConstraintConstants() +{ + for (DihedralBend &b : mDihedralBendConstraints) + { + // Get positions + Vec3 x0 = Vec3(mVertices[b.mVertex[0]].mPosition); + Vec3 x1 = Vec3(mVertices[b.mVertex[1]].mPosition); + Vec3 x2 = Vec3(mVertices[b.mVertex[2]].mPosition); + Vec3 x3 = Vec3(mVertices[b.mVertex[3]].mPosition); + + /* + x2 + e1/ \e3 + / \ + x0----x1 + \ e0 / + e2\ /e4 + x3 + */ + + // Calculate edges + Vec3 e0 = x1 - x0; + Vec3 e1 = x2 - x0; + Vec3 e2 = x3 - x0; + + // Normals of both triangles + Vec3 n1 = e0.Cross(e1); + Vec3 n2 = e2.Cross(e0); + float denom = sqrt(n1.LengthSq() * n2.LengthSq()); + if (denom < 1.0e-12f) + b.mInitialAngle = 0.0f; + else + { + float sign = Sign(n2.Cross(n1).Dot(e0)); + b.mInitialAngle = sign * ACosApproximate(n1.Dot(n2) / denom); // Runtime uses the approximation too + } + } +} + +void SoftBodySharedSettings::CalculateVolumeConstraintVolumes() +{ + for (Volume &v : mVolumeConstraints) + { + Vec3 x1(mVertices[v.mVertex[0]].mPosition); + Vec3 x2(mVertices[v.mVertex[1]].mPosition); + Vec3 x3(mVertices[v.mVertex[2]].mPosition); + Vec3 x4(mVertices[v.mVertex[3]].mPosition); + + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 x1x4 = x4 - x1; + + v.mSixRestVolume = abs(x1x2.Cross(x1x3).Dot(x1x4)); + } +} + +void SoftBodySharedSettings::CalculateSkinnedConstraintNormals() +{ + // Clear any previous results + mSkinnedConstraintNormals.clear(); + + // If there are no skinned constraints, we're done + if (mSkinnedConstraints.empty()) + return; + + // First collect all vertices that are skinned + using VertexIndexSet = UnorderedSet; + VertexIndexSet skinned_vertices; + skinned_vertices.reserve(VertexIndexSet::size_type(mSkinnedConstraints.size())); + for (const Skinned &s : mSkinnedConstraints) + skinned_vertices.insert(s.mVertex); + + // Now collect all faces that connect only to skinned vertices + using ConnectedFacesMap = UnorderedMap; + ConnectedFacesMap connected_faces; + connected_faces.reserve(ConnectedFacesMap::size_type(mVertices.size())); + for (const Face &f : mFaces) + { + // Must connect to only skinned vertices + bool valid = true; + for (uint32 v : f.mVertex) + valid &= skinned_vertices.find(v) != skinned_vertices.end(); + if (!valid) + continue; + + // Store faces that connect to vertices + for (uint32 v : f.mVertex) + connected_faces[v].insert(uint32(&f - mFaces.data())); + } + + // Populate the list of connecting faces per skinned vertex + mSkinnedConstraintNormals.reserve(mFaces.size()); + for (Skinned &s : mSkinnedConstraints) + { + uint32 start = uint32(mSkinnedConstraintNormals.size()); + JPH_ASSERT((start >> 24) == 0); + ConnectedFacesMap::const_iterator connected_faces_it = connected_faces.find(s.mVertex); + if (connected_faces_it != connected_faces.cend()) + { + const VertexIndexSet &faces = connected_faces_it->second; + uint32 num = uint32(faces.size()); + JPH_ASSERT(num < 256); + mSkinnedConstraintNormals.insert(mSkinnedConstraintNormals.end(), faces.begin(), faces.end()); + QuickSort(mSkinnedConstraintNormals.begin() + start, mSkinnedConstraintNormals.begin() + start + num); + s.mNormalInfo = start + (num << 24); + } + else + s.mNormalInfo = 0; + } + mSkinnedConstraintNormals.shrink_to_fit(); +} + +void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) +{ + // Clear any previous results + mUpdateGroups.clear(); + + // Create a list of connected vertices + struct Connection + { + uint32 mVertex; + uint32 mCount; + }; + Array> connectivity; + connectivity.resize(mVertices.size()); + auto add_connection = [&connectivity](uint inV1, uint inV2) { + for (int i = 0; i < 2; ++i) + { + bool found = false; + for (Connection &c : connectivity[inV1]) + if (c.mVertex == inV2) + { + c.mCount++; + found = true; + break; + } + if (!found) + connectivity[inV1].push_back({ inV2, 1 }); + + std::swap(inV1, inV2); + } + }; + for (const Edge &c : mEdgeConstraints) + add_connection(c.mVertex[0], c.mVertex[1]); + for (const LRA &c : mLRAConstraints) + add_connection(c.mVertex[0], c.mVertex[1]); + for (const DihedralBend &c : mDihedralBendConstraints) + { + add_connection(c.mVertex[0], c.mVertex[1]); + add_connection(c.mVertex[0], c.mVertex[2]); + add_connection(c.mVertex[0], c.mVertex[3]); + add_connection(c.mVertex[1], c.mVertex[2]); + add_connection(c.mVertex[1], c.mVertex[3]); + add_connection(c.mVertex[2], c.mVertex[3]); + } + for (const Volume &c : mVolumeConstraints) + { + add_connection(c.mVertex[0], c.mVertex[1]); + add_connection(c.mVertex[0], c.mVertex[2]); + add_connection(c.mVertex[0], c.mVertex[3]); + add_connection(c.mVertex[1], c.mVertex[2]); + add_connection(c.mVertex[1], c.mVertex[3]); + add_connection(c.mVertex[2], c.mVertex[3]); + } + // Skinned constraints only update 1 vertex, so we don't need special logic here + + // Maps each of the vertices to a group index + Array group_idx; + group_idx.resize(mVertices.size(), -1); + + // Which group we are currently filling and its vertices + int current_group_idx = 0; + Array current_group; + + // Start greedy algorithm to group vertices + for (;;) + { + // Find the bounding box of the ungrouped vertices + AABox bounds; + for (uint i = 0; i < (uint)mVertices.size(); ++i) + if (group_idx[i] == -1) + bounds.Encapsulate(Vec3(mVertices[i].mPosition)); + + // Determine longest and shortest axis + Vec3 bounds_size = bounds.GetSize(); + uint max_axis = bounds_size.GetHighestComponentIndex(); + uint min_axis = bounds_size.GetLowestComponentIndex(); + if (min_axis == max_axis) + min_axis = (min_axis + 1) % 3; + uint mid_axis = 3 - min_axis - max_axis; + + // Find the vertex that has the lowest value on the axis with the largest extent + uint current_vertex = UINT_MAX; + Float3 current_vertex_position { FLT_MAX, FLT_MAX, FLT_MAX }; + for (uint i = 0; i < (uint)mVertices.size(); ++i) + if (group_idx[i] == -1) + { + const Float3 &vertex_position = mVertices[i].mPosition; + float max_axis_value = vertex_position[max_axis]; + float mid_axis_value = vertex_position[mid_axis]; + float min_axis_value = vertex_position[min_axis]; + + if (max_axis_value < current_vertex_position[max_axis] + || (max_axis_value == current_vertex_position[max_axis] + && (mid_axis_value < current_vertex_position[mid_axis] + || (mid_axis_value == current_vertex_position[mid_axis] + && min_axis_value < current_vertex_position[min_axis])))) + { + current_vertex_position = mVertices[i].mPosition; + current_vertex = i; + } + } + if (current_vertex == UINT_MAX) + break; + + // Initialize the current group with 1 vertex + current_group.push_back(current_vertex); + group_idx[current_vertex] = current_group_idx; + + // Fill up the group + for (;;) + { + // Find the vertex that is most connected to the current group + uint best_vertex = UINT_MAX; + uint best_num_connections = 0; + float best_dist_sq = FLT_MAX; + for (uint i = 0; i < (uint)current_group.size(); ++i) // For all vertices in the current group + for (const Connection &c : connectivity[current_group[i]]) // For all connections to other vertices + { + uint v = c.mVertex; + if (group_idx[v] == -1) // Ungrouped vertices only + { + // Count the number of connections to this group + uint num_connections = 0; + for (const Connection &v2 : connectivity[v]) + if (group_idx[v2.mVertex] == current_group_idx) + num_connections += v2.mCount; + + // Calculate distance to group centroid + float dist_sq = (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current_group.front()].mPosition)).LengthSq(); + + if (best_vertex == UINT_MAX + || num_connections > best_num_connections + || (num_connections == best_num_connections && dist_sq < best_dist_sq)) + { + best_vertex = v; + best_num_connections = num_connections; + best_dist_sq = dist_sq; + } + } + } + + // Add the best vertex to the current group + if (best_vertex != UINT_MAX) + { + current_group.push_back(best_vertex); + group_idx[best_vertex] = current_group_idx; + } + + // Create a new group? + if (current_group.size() >= SoftBodyUpdateContext::cVertexConstraintBatch // If full, yes + || (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2 && best_vertex == UINT_MAX)) // If half full and we found no connected vertex, yes + { + current_group.clear(); + current_group_idx++; + break; + } + + // If we didn't find a connected vertex, we need to find a new starting vertex + if (best_vertex == UINT_MAX) + break; + } + } + + // If the last group is more than half full, we'll keep it as a separate group, otherwise we merge it with the 'non parallel' group + if (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2) + ++current_group_idx; + + // We no longer need the current group array, free the memory + current_group.clear(); + current_group.shrink_to_fit(); + + // We're done with the connectivity list, free the memory + connectivity.clear(); + connectivity.shrink_to_fit(); + + // Assign the constraints to their groups + struct Group + { + uint GetSize() const + { + return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size(); + } + + Array mEdgeConstraints; + Array mLRAConstraints; + Array mDihedralBendConstraints; + Array mVolumeConstraints; + Array mSkinnedConstraints; + }; + Array groups; + groups.resize(current_group_idx + 1); // + non parallel group + for (const Edge &e : mEdgeConstraints) + { + int g1 = group_idx[e.mVertex[0]]; + int g2 = group_idx[e.mVertex[1]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0); + if (g1 == g2) // In the same group + groups[g1].mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data())); + else // In different groups -> parallel group + groups.back().mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data())); + } + for (const LRA &l : mLRAConstraints) + { + int g1 = group_idx[l.mVertex[0]]; + int g2 = group_idx[l.mVertex[1]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0); + if (g1 == g2) // In the same group + groups[g1].mLRAConstraints.push_back(uint(&l - mLRAConstraints.data())); + else // In different groups -> parallel group + groups.back().mLRAConstraints.push_back(uint(&l - mLRAConstraints.data())); + } + for (const DihedralBend &d : mDihedralBendConstraints) + { + int g1 = group_idx[d.mVertex[0]]; + int g2 = group_idx[d.mVertex[1]]; + int g3 = group_idx[d.mVertex[2]]; + int g4 = group_idx[d.mVertex[3]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0); + if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group + groups[g1].mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data())); + else // In different groups -> parallel group + groups.back().mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data())); + } + for (const Volume &v : mVolumeConstraints) + { + int g1 = group_idx[v.mVertex[0]]; + int g2 = group_idx[v.mVertex[1]]; + int g3 = group_idx[v.mVertex[2]]; + int g4 = group_idx[v.mVertex[3]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0); + if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group + groups[g1].mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data())); + else // In different groups -> parallel group + groups.back().mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data())); + } + for (const Skinned &s : mSkinnedConstraints) + { + int g1 = group_idx[s.mVertex]; + JPH_ASSERT(g1 >= 0); + groups[g1].mSkinnedConstraints.push_back(uint(&s - mSkinnedConstraints.data())); + } + + // Sort the parallel groups from big to small (this means the big groups will be scheduled first and have more time to complete) + QuickSort(groups.begin(), groups.end() - 1, [](const Group &inLHS, const Group &inRHS) { return inLHS.GetSize() > inRHS.GetSize(); }); + + // Make sure we know the closest kinematic vertex so we can sort + CalculateClosestKinematic(); + + // Sort within each group + for (Group &group : groups) + { + // Sort the edge constraints + QuickSort(group.mEdgeConstraints.begin(), group.mEdgeConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Edge &e1 = mEdgeConstraints[inLHS]; + const Edge &e2 = mEdgeConstraints[inRHS]; + + // First sort so that the edge with the smallest distance to a kinematic vertex comes first + float d1 = min(mClosestKinematic[e1.mVertex[0]].mDistance, mClosestKinematic[e1.mVertex[1]].mDistance); + float d2 = min(mClosestKinematic[e2.mVertex[0]].mDistance, mClosestKinematic[e2.mVertex[1]].mDistance); + if (d1 != d2) + return d1 < d2; + + // Order the edges so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges). + // Note we could also re-order the vertices but that would be much more of a burden to the end user + uint32 m1 = e1.GetMinVertexIndex(); + uint32 m2 = e2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the LRA constraints + QuickSort(group.mLRAConstraints.begin(), group.mLRAConstraints.end(), [this](uint inLHS, uint inRHS) + { + const LRA &l1 = mLRAConstraints[inLHS]; + const LRA &l2 = mLRAConstraints[inRHS]; + + // First sort so that the longest constraint comes first (meaning the shortest constraint has the most influence on the end result) + // Most of the time there will be a single LRA constraint per vertex and since the LRA constraint only modifies a single vertex, + // updating one constraint will not violate another constraint. + if (l1.mMaxDistance != l2.mMaxDistance) + return l1.mMaxDistance > l2.mMaxDistance; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = l1.GetMinVertexIndex(); + uint32 m2 = l2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the dihedral bend constraints + QuickSort(group.mDihedralBendConstraints.begin(), group.mDihedralBendConstraints.end(), [this](uint inLHS, uint inRHS) + { + const DihedralBend &b1 = mDihedralBendConstraints[inLHS]; + const DihedralBend &b2 = mDihedralBendConstraints[inRHS]; + + // First sort so that the constraint with the smallest distance to a kinematic vertex comes first + float d1 = min( + min(mClosestKinematic[b1.mVertex[0]].mDistance, mClosestKinematic[b1.mVertex[1]].mDistance), + min(mClosestKinematic[b1.mVertex[2]].mDistance, mClosestKinematic[b1.mVertex[3]].mDistance)); + float d2 = min( + min(mClosestKinematic[b2.mVertex[0]].mDistance, mClosestKinematic[b2.mVertex[1]].mDistance), + min(mClosestKinematic[b2.mVertex[2]].mDistance, mClosestKinematic[b2.mVertex[3]].mDistance)); + if (d1 != d2) + return d1 < d2; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = b1.GetMinVertexIndex(); + uint32 m2 = b2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the volume constraints + QuickSort(group.mVolumeConstraints.begin(), group.mVolumeConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Volume &v1 = mVolumeConstraints[inLHS]; + const Volume &v2 = mVolumeConstraints[inRHS]; + + // First sort so that the constraint with the smallest distance to a kinematic vertex comes first + float d1 = min( + min(mClosestKinematic[v1.mVertex[0]].mDistance, mClosestKinematic[v1.mVertex[1]].mDistance), + min(mClosestKinematic[v1.mVertex[2]].mDistance, mClosestKinematic[v1.mVertex[3]].mDistance)); + float d2 = min( + min(mClosestKinematic[v2.mVertex[0]].mDistance, mClosestKinematic[v2.mVertex[1]].mDistance), + min(mClosestKinematic[v2.mVertex[2]].mDistance, mClosestKinematic[v2.mVertex[3]].mDistance)); + if (d1 != d2) + return d1 < d2; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = v1.GetMinVertexIndex(); + uint32 m2 = v2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the skinned constraints + QuickSort(group.mSkinnedConstraints.begin(), group.mSkinnedConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Skinned &s1 = mSkinnedConstraints[inLHS]; + const Skinned &s2 = mSkinnedConstraints[inRHS]; + + // Order the skinned constraints so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges). + if (s1.mVertex != s2.mVertex) + return s1.mVertex < s2.mVertex; + + return inLHS < inRHS; + }); + } + + // Temporary store constraints as we reorder them + Array temp_edges; + temp_edges.swap(mEdgeConstraints); + mEdgeConstraints.reserve(temp_edges.size()); + outResults.mEdgeRemap.reserve(temp_edges.size()); + + Array temp_lra; + temp_lra.swap(mLRAConstraints); + mLRAConstraints.reserve(temp_lra.size()); + outResults.mLRARemap.reserve(temp_lra.size()); + + Array temp_dihedral_bend; + temp_dihedral_bend.swap(mDihedralBendConstraints); + mDihedralBendConstraints.reserve(temp_dihedral_bend.size()); + outResults.mDihedralBendRemap.reserve(temp_dihedral_bend.size()); + + Array temp_volume; + temp_volume.swap(mVolumeConstraints); + mVolumeConstraints.reserve(temp_volume.size()); + outResults.mVolumeRemap.reserve(temp_volume.size()); + + Array temp_skinned; + temp_skinned.swap(mSkinnedConstraints); + mSkinnedConstraints.reserve(temp_skinned.size()); + outResults.mSkinnedRemap.reserve(temp_skinned.size()); + + // Finalize update groups + for (const Group &group : groups) + { + // Reorder edge constraints for this group + for (uint idx : group.mEdgeConstraints) + { + mEdgeConstraints.push_back(temp_edges[idx]); + outResults.mEdgeRemap.push_back(idx); + } + + // Reorder LRA constraints for this group + for (uint idx : group.mLRAConstraints) + { + mLRAConstraints.push_back(temp_lra[idx]); + outResults.mLRARemap.push_back(idx); + } + + // Reorder dihedral bend constraints for this group + for (uint idx : group.mDihedralBendConstraints) + { + mDihedralBendConstraints.push_back(temp_dihedral_bend[idx]); + outResults.mDihedralBendRemap.push_back(idx); + } + + // Reorder volume constraints for this group + for (uint idx : group.mVolumeConstraints) + { + mVolumeConstraints.push_back(temp_volume[idx]); + outResults.mVolumeRemap.push_back(idx); + } + + // Reorder skinned constraints for this group + for (uint idx : group.mSkinnedConstraints) + { + mSkinnedConstraints.push_back(temp_skinned[idx]); + outResults.mSkinnedRemap.push_back(idx); + } + + // Store end indices + mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() }); + } + + // Free closest kinematic buffer + mClosestKinematic.clear(); + mClosestKinematic.shrink_to_fit(); +} + +Ref SoftBodySharedSettings::Clone() const +{ + Ref clone = new SoftBodySharedSettings; + clone->mVertices = mVertices; + clone->mFaces = mFaces; + clone->mEdgeConstraints = mEdgeConstraints; + clone->mDihedralBendConstraints = mDihedralBendConstraints; + clone->mVolumeConstraints = mVolumeConstraints; + clone->mSkinnedConstraints = mSkinnedConstraints; + clone->mSkinnedConstraintNormals = mSkinnedConstraintNormals; + clone->mInvBindMatrices = mInvBindMatrices; + clone->mLRAConstraints = mLRAConstraints; + clone->mMaterials = mMaterials; + clone->mVertexRadius = mVertexRadius; + clone->mUpdateGroups = mUpdateGroups; + return clone; +} + +void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mVertices); + inStream.Write(mFaces); + inStream.Write(mEdgeConstraints); + inStream.Write(mDihedralBendConstraints); + inStream.Write(mVolumeConstraints); + inStream.Write(mSkinnedConstraints); + inStream.Write(mSkinnedConstraintNormals); + inStream.Write(mLRAConstraints); + inStream.Write(mVertexRadius); + inStream.Write(mUpdateGroups); + + // Can't write mInvBindMatrices directly because the class contains padding + inStream.Write(mInvBindMatrices, [](const InvBind &inElement, StreamOut &inS) { + inS.Write(inElement.mJointIndex); + inS.Write(inElement.mInvBind); + }); +} + +void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mVertices); + inStream.Read(mFaces); + inStream.Read(mEdgeConstraints); + inStream.Read(mDihedralBendConstraints); + inStream.Read(mVolumeConstraints); + inStream.Read(mSkinnedConstraints); + inStream.Read(mSkinnedConstraintNormals); + inStream.Read(mLRAConstraints); + inStream.Read(mVertexRadius); + inStream.Read(mUpdateGroups); + + inStream.Read(mInvBindMatrices, [](StreamIn &inS, InvBind &outElement) { + inS.Read(outElement.mJointIndex); + inS.Read(outElement.mInvBind); + }); +} + +void SoftBodySharedSettings::SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const +{ + SharedSettingsToIDMap::const_iterator settings_iter = ioSettingsMap.find(this); + if (settings_iter == ioSettingsMap.end()) + { + // Write settings ID + uint32 settings_id = ioSettingsMap.size(); + ioSettingsMap[this] = settings_id; + inStream.Write(settings_id); + + // Write the settings + SaveBinaryState(inStream); + + // Write materials + StreamUtils::SaveObjectArray(inStream, mMaterials, &ioMaterialMap); + } + else + { + // Known settings, just write the ID + inStream.Write(settings_iter->second); + } +} + +SoftBodySharedSettings::SettingsResult SoftBodySharedSettings::sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap) +{ + SettingsResult result; + + // Read settings id + uint32 settings_id; + inStream.Read(settings_id); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read settings id"); + return result; + } + + // Check nullptr settings + if (settings_id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if we already read this settings + if (settings_id < ioSettingsMap.size()) + { + result.Set(ioSettingsMap[settings_id]); + return result; + } + + // Create new object + Ref settings = new SoftBodySharedSettings; + + // Read state + settings->RestoreBinaryState(inStream); + + // Read materials + Result mlresult = StreamUtils::RestoreObjectArray(inStream, ioMaterialMap); + if (mlresult.HasError()) + { + result.SetError(mlresult.GetError()); + return result; + } + settings->mMaterials = mlresult.Get(); + + // Add the settings to the map + ioSettingsMap.push_back(settings); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h new file mode 100644 index 000000000000..d4b05a6ccdae --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h @@ -0,0 +1,335 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// This class defines the setup of all particles and their constraints. +/// It is used during the simulation and can be shared between multiple soft bodies. +class JPH_EXPORT SoftBodySharedSettings : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodySharedSettings) + +public: + /// Which type of bend constraint should be created + enum class EBendType + { + None, ///< No bend constraints will be created + Distance, ///< A simple distance constraint + Dihedral, ///< A dihedral bend constraint (most expensive, but also supports triangles that are initially not in the same plane) + }; + + /// The type of long range attachment constraint to create + enum class ELRAType + { + None, ///< Don't create a LRA constraint + EuclideanDistance, ///< Create a LRA constraint based on Euclidean distance between the closest kinematic vertex and this vertex + GeodesicDistance, ///< Create a LRA constraint based on the geodesic distance between the closest kinematic vertex and this vertex (follows the edge constraints) + }; + + /// Per vertex attributes used during the CreateConstraints function. + /// For an edge or shear constraint, the compliance is averaged between the two attached vertices. + /// For a bend constraint, the compliance is averaged between the two vertices on the shared edge. + struct JPH_EXPORT VertexAttributes + { + /// Constructor + VertexAttributes() = default; + VertexAttributes(float inCompliance, float inShearCompliance, float inBendCompliance, ELRAType inLRAType = ELRAType::None, float inLRAMaxDistanceMultiplier = 1.0f) : mCompliance(inCompliance), mShearCompliance(inShearCompliance), mBendCompliance(inBendCompliance), mLRAType(inLRAType), mLRAMaxDistanceMultiplier(inLRAMaxDistanceMultiplier) { } + + float mCompliance = 0.0f; ///< The compliance of the normal edges. Set to FLT_MAX to disable regular edges for any edge involving this vertex. + float mShearCompliance = 0.0f; ///< The compliance of the shear edges. Set to FLT_MAX to disable shear edges for any edge involving this vertex. + float mBendCompliance = FLT_MAX; ///< The compliance of the bend edges. Set to FLT_MAX to disable bend edges for any bend constraint involving this vertex. + ELRAType mLRAType = ELRAType::None; ///< The type of long range attachment constraint to create. + float mLRAMaxDistanceMultiplier = 1.0f; ///< Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose. + }; + + /// Automatically create constraints based on the faces of the soft body + /// @param inVertexAttributes A list of attributes for each vertex (1-on-1 with mVertices, note that if the list is smaller than mVertices the last element will be repeated). This defines the properties of the constraints that are created. + /// @param inVertexAttributesLength The length of inVertexAttributes + /// @param inBendType The type of bend constraint to create + /// @param inAngleTolerance Shear edges are created when two connected triangles form a quad (are roughly in the same plane and form a square with roughly 90 degree angles). This defines the tolerance (in radians). + void CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType = EBendType::Distance, float inAngleTolerance = DegreesToRadians(8.0f)); + + /// Calculate the initial lengths of all springs of the edges of this soft body (if you use CreateConstraint, this is already done) + void CalculateEdgeLengths(); + + /// Calculate the max lengths for the long range attachment constraints based on Euclidean distance (if you use CreateConstraints, this is already done) + /// @param inMaxDistanceMultiplier Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose. + void CalculateLRALengths(float inMaxDistanceMultiplier = 1.0f); + + /// Calculate the constants for the bend constraints (if you use CreateConstraints, this is already done) + void CalculateBendConstraintConstants(); + + /// Calculates the initial volume of all tetrahedra of this soft body + void CalculateVolumeConstraintVolumes(); + + /// Calculate information needed to be able to calculate the skinned constraint normals at run-time + void CalculateSkinnedConstraintNormals(); + + /// Information about the optimization of the soft body, the indices of certain elements may have changed. + class OptimizationResults + { + public: + Array mEdgeRemap; ///< Maps old edge index to new edge index + Array mLRARemap; ///< Maps old LRA index to new LRA index + Array mDihedralBendRemap; ///< Maps old dihedral bend index to new dihedral bend index + Array mVolumeRemap; ///< Maps old volume constraint index to new volume constraint index + Array mSkinnedRemap; ///< Maps old skinned constraint index to new skinned constraint index + }; + + /// Optimize the soft body settings for simulation. This will reorder constraints so they can be executed in parallel. + void Optimize(OptimizationResults &outResults); + + /// Optimize the soft body settings without results + void Optimize() { OptimizationResults results; Optimize(results); } + + /// Clone this object + Ref Clone() const; + + /// Saves the state of this object in binary form to inStream. Doesn't store the material list. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the material list. + void RestoreBinaryState(StreamIn &inStream); + + using SharedSettingsToIDMap = StreamUtils::ObjectToIDMap; + using IDToSharedSettingsMap = StreamUtils::IDToObjectMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this shared settings and its materials. Pass in an empty map ioSettingsMap / ioMaterialMap or reuse the same map while saving multiple settings objects to the same stream in order to avoid writing duplicates. + void SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const; + + using SettingsResult = Result>; + + /// Restore a shape and materials. Pass in an empty map in ioSettingsMap / ioMaterialMap or reuse the same map while reading multiple settings objects from the same stream in order to restore duplicates. + static SettingsResult sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap); + + /// A vertex is a particle, the data in this structure is only used during creation of the soft body and not during simulation + struct JPH_EXPORT Vertex + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Vertex) + + /// Constructor + Vertex() = default; + Vertex(const Float3 &inPosition, const Float3 &inVelocity = Float3(0, 0, 0), float inInvMass = 1.0f) : mPosition(inPosition), mVelocity(inVelocity), mInvMass(inInvMass) { } + + Float3 mPosition { 0, 0, 0 }; ///< Initial position of the vertex + Float3 mVelocity { 0, 0, 0 }; ///< Initial velocity of the vertex + float mInvMass = 1.0f; ///< Initial inverse of the mass of the vertex + }; + + /// A face defines the surface of the body + struct JPH_EXPORT Face + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Face) + + /// Constructor + Face() = default; + Face(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inMaterialIndex = 0) : mVertex { inVertex1, inVertex2, inVertex3 }, mMaterialIndex(inMaterialIndex) { } + + /// Check if this is a degenerate face (a face which points to the same vertex twice) + bool IsDegenerate() const { return mVertex[0] == mVertex[1] || mVertex[0] == mVertex[2] || mVertex[1] == mVertex[2]; } + + uint32 mVertex[3]; ///< Indices of the vertices that form the face + uint32 mMaterialIndex = 0; ///< Index of the material of the face in SoftBodySharedSettings::mMaterials + }; + + /// An edge keeps two vertices at a constant distance using a spring: |x1 - x2| = rest length + struct JPH_EXPORT Edge + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Edge) + + /// Constructor + Edge() = default; + Edge(uint32 inVertex1, uint32 inVertex2, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(mVertex[0], mVertex[1]); } + + uint32 mVertex[2]; ///< Indices of the vertices that form the edge + float mRestLength = 1.0f; ///< Rest length of the spring + float mCompliance = 0.0f; ///< Inverse of the stiffness of the spring + }; + + /** + * A dihedral bend constraint keeps the angle between two triangles constant along their shared edge. + * + * x2 + * / \ + * / t0 \ + * x0----x1 + * \ t1 / + * \ / + * x3 + * + * x0..x3 are the vertices, t0 and t1 are the triangles that share the edge x0..x1 + * + * Based on: + * - "Position Based Dynamics" - Matthias Muller et al. + * - "Strain Based Dynamics" - Matthias Muller et al. + * - "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al. + */ + struct JPH_EXPORT DihedralBend + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, DihedralBend) + + /// Constructor + DihedralBend() = default; + DihedralBend(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inVertex4, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2, inVertex3, inVertex4 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); } + + uint32 mVertex[4]; ///< Indices of the vertices of the 2 triangles that share an edge (the first 2 vertices are the shared edge) + float mCompliance = 0.0f; ///< Inverse of the stiffness of the constraint + float mInitialAngle = 0.0f; ///< Initial angle between the normals of the triangles (pi - dihedral angle). + }; + + /// Volume constraint, keeps the volume of a tetrahedron constant + struct JPH_EXPORT Volume + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Volume) + + /// Constructor + Volume() = default; + Volume(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inVertex4, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2, inVertex3, inVertex4 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); } + + uint32 mVertex[4]; ///< Indices of the vertices that form the tetrhedron + float mSixRestVolume = 1.0f; ///< 6 times the rest volume of the tetrahedron (calculated by CalculateVolumeConstraintVolumes()) + float mCompliance = 0.0f; ///< Inverse of the stiffness of the constraint + }; + + /// An inverse bind matrix take a skinned vertex from its bind pose into joint local space + class JPH_EXPORT InvBind + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, InvBind) + + public: + /// Constructor + InvBind() = default; + InvBind(uint32 inJointIndex, Mat44Arg inInvBind) : mJointIndex(inJointIndex), mInvBind(inInvBind) { } + + uint32 mJointIndex = 0; ///< Joint index to which this is attached + Mat44 mInvBind = Mat44::sIdentity(); ///< The inverse bind matrix, this takes a vertex in its bind pose (Vertex::mPosition) to joint local space + }; + + /// A joint and its skin weight + class JPH_EXPORT SkinWeight + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkinWeight) + + public: + /// Constructor + SkinWeight() = default; + SkinWeight(uint32 inInvBindIndex, float inWeight) : mInvBindIndex(inInvBindIndex), mWeight(inWeight) { } + + uint32 mInvBindIndex = 0; ///< Index in mInvBindMatrices + float mWeight = 0.0f; ///< Weight with which it is skinned + }; + + /// A constraint that skins a vertex to joints and limits the distance that the simulated vertex can travel from this vertex + class JPH_EXPORT Skinned + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skinned) + + public: + /// Constructor + Skinned() = default; + Skinned(uint32 inVertex, float inMaxDistance, float inBackStopDistance, float inBackStopRadius) : mVertex(inVertex), mMaxDistance(inMaxDistance), mBackStopDistance(inBackStopDistance), mBackStopRadius(inBackStopRadius) { } + + /// Normalize the weights so that they add up to 1 + void NormalizeWeights() + { + // Get the total weight + float total = 0.0f; + for (const SkinWeight &w : mWeights) + total += w.mWeight; + + // Normalize + if (total > 0.0f) + for (SkinWeight &w : mWeights) + w.mWeight /= total; + } + + /// Maximum number of skin weights + static constexpr uint cMaxSkinWeights = 4; + + uint32 mVertex = 0; ///< Index in mVertices which indicates which vertex is being skinned + SkinWeight mWeights[cMaxSkinWeights]; ///< Skin weights, the bind pose of the vertex is assumed to be stored in Vertex::mPosition. The first weight that is zero indicates the end of the list. Weights should add up to 1. + float mMaxDistance = FLT_MAX; ///< Maximum distance that this vertex can reach from the skinned vertex, disabled when FLT_MAX. 0 when you want to hard skin the vertex to the skinned vertex. + float mBackStopDistance = FLT_MAX; ///< Disabled if mBackStopDistance >= mMaxDistance. The faces surrounding mVertex determine an average normal. mBackStopDistance behind the vertex in the opposite direction of this normal, the back stop sphere starts. The simulated vertex will be pushed out of this sphere and it can be used to approximate the volume of the skinned mesh behind the skinned vertex. + float mBackStopRadius = 40.0f; ///< Radius of the backstop sphere. By default this is a fairly large radius so the sphere approximates a plane. + uint32 mNormalInfo = 0; ///< Information needed to calculate the normal of this vertex, lowest 24 bit is start index in mSkinnedConstraintNormals, highest 8 bit is number of faces (generated by CalculateSkinnedConstraintNormals()) + }; + + /// A long range attachment constraint, this is a constraint that sets a max distance between a kinematic vertex and a dynamic vertex + /// See: "Long Range Attachments - A Method to Simulate Inextensible Clothing in Computer Games", Tae-Yong Kim, Nuttapong Chentanez and Matthias Mueller-Fischer + class JPH_EXPORT LRA + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, LRA) + + public: + /// Constructor + LRA() = default; + LRA(uint32 inVertex1, uint32 inVertex2, float inMaxDistance) : mVertex { inVertex1, inVertex2 }, mMaxDistance(inMaxDistance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(mVertex[0], mVertex[1]); } + + uint32 mVertex[2]; ///< The vertices that are connected. The first vertex should be kinematic, the 2nd dynamic. + float mMaxDistance = 0.0f; ///< The maximum distance between the vertices + }; + + /// Add a face to this soft body + void AddFace(const Face &inFace) { JPH_ASSERT(!inFace.IsDegenerate()); mFaces.push_back(inFace); } + + Array mVertices; ///< The list of vertices or particles of the body + Array mFaces; ///< The list of faces of the body + Array mEdgeConstraints; ///< The list of edges or springs of the body + Array mDihedralBendConstraints; ///< The list of dihedral bend constraints of the body + Array mVolumeConstraints; ///< The list of volume constraints of the body that keep the volume of tetrahedra in the soft body constant + Array mSkinnedConstraints; ///< The list of vertices that are constrained to a skinned vertex + Array mInvBindMatrices; ///< The list of inverse bind matrices for skinning vertices + Array mLRAConstraints; ///< The list of long range attachment constraints + PhysicsMaterialList mMaterials { PhysicsMaterial::sDefault }; ///< The materials of the faces of the body, referenced by Face::mMaterialIndex + float mVertexRadius = 0.0f; ///< How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting + +private: + friend class SoftBodyMotionProperties; + + /// Calculate the closest kinematic vertex array + void CalculateClosestKinematic(); + + /// Tracks the closest kinematic vertex + struct ClosestKinematic + { + uint32 mVertex = 0xffffffff; ///< Vertex index of closest kinematic vertex + float mDistance = FLT_MAX; ///< Distance to the closest kinematic vertex + }; + + /// Tracks the end indices of the various constraint groups + struct UpdateGroup + { + uint mEdgeEndIndex; ///< The end index of the edge constraints in this group + uint mLRAEndIndex; ///< The end index of the LRA constraints in this group + uint mDihedralBendEndIndex; ///< The end index of the dihedral bend constraints in this group + uint mVolumeEndIndex; ///< The end index of the volume constraints in this group + uint mSkinnedEndIndex; ///< The end index of the skinned constraints in this group + }; + + Array mClosestKinematic; ///< The closest kinematic vertex to each vertex in mVertices + Array mUpdateGroups; ///< The end indices for each group of constraints that can be updated in parallel + Array mSkinnedConstraintNormals; ///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals() +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h new file mode 100644 index 000000000000..2f565f5693c1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h @@ -0,0 +1,59 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class SoftBodyMotionProperties; +class SoftBodyContactListener; +class SimShapeFilter; + +/// Temporary data used by the update of a soft body +class SoftBodyUpdateContext : public NonCopyable +{ +public: + static constexpr uint cVertexCollisionBatch = 64; ///< Number of vertices to process in a batch in DetermineCollisionPlanes + static constexpr uint cVertexConstraintBatch = 256; ///< Number of vertices to group for processing batches of constraints in ApplyEdgeConstraints + + // Input + Body * mBody; ///< Body that is being updated + SoftBodyMotionProperties * mMotionProperties; ///< Motion properties of that body + SoftBodyContactListener * mContactListener; ///< Contact listener to fire callbacks to + const SimShapeFilter * mSimShapeFilter; ///< Shape filter to use for collision detection + RMat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + Vec3 mGravity; ///< Gravity vector in local space of the soft body + Vec3 mDisplacementDueToGravity; ///< Displacement of the center of mass due to gravity in the current time step + float mDeltaTime; ///< Delta time for the current time step + float mSubStepDeltaTime; ///< Delta time for each sub step + + /// Describes progress in the current update + enum class EState + { + DetermineCollisionPlanes, ///< Determine collision planes for vertices in parallel + DetermineSensorCollisions, ///< Determine collisions with sensors in parallel + ApplyConstraints, ///< Apply constraints in parallel + Done ///< Update is finished + }; + + // State of the update + atomic mState { EState::DetermineCollisionPlanes };///< Current state of the update + atomic mNextCollisionVertex { 0 }; ///< Next vertex to process for DetermineCollisionPlanes + atomic mNumCollisionVerticesProcessed { 0 }; ///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can go to the next step + atomic mNextSensorIndex { 0 }; ///< Next sensor to process for DetermineCollisionPlanes + atomic mNumSensorsProcessed { 0 }; ///< Number of sensors processed by DetermineSensorCollisions, used to determine if we can go to the next step + atomic mNextIteration { 0 }; ///< Next simulation iteration to process + atomic mNextConstraintGroup { 0 }; ///< Next constraint group to process + atomic mNumConstraintGroupsProcessed { 0 }; ///< Number of groups processed, used to determine if we can go to the next iteration + + // Output + Vec3 mDeltaPosition; ///< Delta position of the body in the current time step, should be applied after the update + ECanSleep mCanSleep; ///< Can the body sleep? Should be applied after the update +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h new file mode 100644 index 000000000000..72d4291e216c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Run time information for a single particle of a soft body +/// Note that at run-time you should only modify the inverse mass and/or velocity of a vertex to control the soft body. +/// Modifying the position can lead to missed collisions. +/// The other members are used internally by the soft body solver. +class SoftBodyVertex +{ +public: + /// Reset collision information to prepare for a new collision check + inline void ResetCollision() + { + mLargestPenetration = -FLT_MAX; + mCollidingShapeIndex = -1; + mHasContact = false; + } + + Vec3 mPreviousPosition; ///< Internal use only. Position at the previous time step + Vec3 mPosition; ///< Position, relative to the center of mass of the soft body + Vec3 mVelocity; ///< Velocity, relative to the center of mass of the soft body + Plane mCollisionPlane; ///< Internal use only. Nearest collision plane, relative to the center of mass of the soft body + int mCollidingShapeIndex; ///< Internal use only. Index in the colliding shapes list of the body we may collide with + bool mHasContact; ///< True if the vertex has collided with anything in the last update + float mLargestPenetration; ///< Internal use only. Used while finding the collision plane, stores the largest penetration found so far + float mInvMass; ///< Inverse mass (1 / mass) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h b/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h new file mode 100644 index 000000000000..c3bd477107f1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h @@ -0,0 +1,136 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class Constraint; +class BodyID; + +JPH_SUPPRESS_WARNING_PUSH +JPH_GCC_SUPPRESS_WARNING("-Wshadow") // GCC complains about the 'Constraints' value conflicting with the 'Constraints' typedef + +/// A bit field that determines which aspects of the simulation to save +enum class EStateRecorderState : uint8 +{ + None = 0, ///< Save nothing + Global = 1, ///< Save global physics system state (delta time, gravity, etc.) + Bodies = 2, ///< Save the state of bodies + Contacts = 4, ///< Save the state of contacts + Constraints = 8, ///< Save the state of constraints + All = Global | Bodies | Contacts | Constraints ///< Save all state +}; + +JPH_SUPPRESS_WARNING_POP + +/// Bitwise OR operator for EStateRecorderState +constexpr EStateRecorderState operator | (EStateRecorderState inLHS, EStateRecorderState inRHS) +{ + return EStateRecorderState(uint8(inLHS) | uint8(inRHS)); +} + +/// Bitwise AND operator for EStateRecorderState +constexpr EStateRecorderState operator & (EStateRecorderState inLHS, EStateRecorderState inRHS) +{ + return EStateRecorderState(uint8(inLHS) & uint8(inRHS)); +} + +/// Bitwise XOR operator for EStateRecorderState +constexpr EStateRecorderState operator ^ (EStateRecorderState inLHS, EStateRecorderState inRHS) +{ + return EStateRecorderState(uint8(inLHS) ^ uint8(inRHS)); +} + +/// Bitwise NOT operator for EStateRecorderState +constexpr EStateRecorderState operator ~ (EStateRecorderState inAllowedDOFs) +{ + return EStateRecorderState(~uint8(inAllowedDOFs)); +} + +/// Bitwise OR assignment operator for EStateRecorderState +constexpr EStateRecorderState & operator |= (EStateRecorderState &ioLHS, EStateRecorderState inRHS) +{ + ioLHS = ioLHS | inRHS; + return ioLHS; +} + +/// Bitwise AND assignment operator for EStateRecorderState +constexpr EStateRecorderState & operator &= (EStateRecorderState &ioLHS, EStateRecorderState inRHS) +{ + ioLHS = ioLHS & inRHS; + return ioLHS; +} + +/// Bitwise XOR assignment operator for EStateRecorderState +constexpr EStateRecorderState & operator ^= (EStateRecorderState &ioLHS, EStateRecorderState inRHS) +{ + ioLHS = ioLHS ^ inRHS; + return ioLHS; +} + +/// User callbacks that allow determining which parts of the simulation should be saved by a StateRecorder +class JPH_EXPORT StateRecorderFilter +{ +public: + /// Destructor + virtual ~StateRecorderFilter() = default; + + ///@name Functions called during SaveState + ///@{ + + /// If the state of a specific body should be saved + virtual bool ShouldSaveBody([[maybe_unused]] const Body &inBody) const { return true; } + + /// If the state of a specific constraint should be saved + virtual bool ShouldSaveConstraint([[maybe_unused]] const Constraint &inConstraint) const { return true; } + + /// If the state of a specific contact should be saved + virtual bool ShouldSaveContact([[maybe_unused]] const BodyID &inBody1, [[maybe_unused]] const BodyID &inBody2) const { return true; } + + ///@} + ///@name Functions called during RestoreState + ///@{ + + /// If the state of a specific contact should be restored + virtual bool ShouldRestoreContact([[maybe_unused]] const BodyID &inBody1, [[maybe_unused]] const BodyID &inBody2) const { return true; } + + ///@} +}; + +/// Class that records the state of a physics system. Can be used to check if the simulation is deterministic by putting the recorder in validation mode. +/// Can be used to restore the state to an earlier point in time. Note that only the state that is modified by the simulation is saved, configuration settings +/// like body friction or restitution, motion quality etc. are not saved and need to be saved by the user if desired. +class JPH_EXPORT StateRecorder : public StreamIn, public StreamOut +{ +public: + /// Constructor + StateRecorder() = default; + StateRecorder(const StateRecorder &inRHS) : mIsValidating(inRHS.mIsValidating) { } + + /// Sets the stream in validation mode. In this case the physics system ensures that before it calls ReadBytes that it will + /// ensure that those bytes contain the current state. This makes it possible to step and save the state, restore to the previous + /// step and step again and when the recorded state is not the same it can restore the expected state and any byte that changes + /// due to a ReadBytes function can be caught to find out which part of the simulation is not deterministic. + /// Note that validation only works when saving the full state of the simulation (EStateRecorderState::All, StateRecorderFilter == nullptr). + void SetValidating(bool inValidating) { mIsValidating = inValidating; } + bool IsValidating() const { return mIsValidating; } + + /// This allows splitting the state in multiple parts. While restoring, only the last part should have this flag set to true. + /// Note that you should ensure that the different parts contain information for disjoint sets of bodies, constraints and contacts. + /// E.g. if you restore the same contact twice, you get undefined behavior. In order to create disjoint sets you can use the StateRecorderFilter. + /// Note that validation is not compatible with restoring a simulation state in multiple parts. + void SetIsLastPart(bool inIsLastPart) { mIsLastPart = inIsLastPart; } + bool IsLastPart() const { return mIsLastPart; } + +private: + bool mIsValidating = false; + bool mIsLastPart = true; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp new file mode 100644 index 000000000000..7624135896e6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp @@ -0,0 +1,90 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void StateRecorderImpl::WriteBytes(const void *inData, size_t inNumBytes) +{ + mStream.write((const char *)inData, inNumBytes); +} + +void StateRecorderImpl::Rewind() +{ + mStream.seekg(0, std::stringstream::beg); +} + +void StateRecorderImpl::Clear() +{ + mStream.clear(); + mStream.str({}); +} + +void StateRecorderImpl::ReadBytes(void *outData, size_t inNumBytes) +{ + if (IsValidating()) + { + // Read data in temporary buffer to compare with current value + void *data = JPH_STACK_ALLOC(inNumBytes); + mStream.read((char *)data, inNumBytes); + if (memcmp(data, outData, inNumBytes) != 0) + { + // Mismatch, print error + Trace("Mismatch reading %u bytes", (uint)inNumBytes); + for (size_t i = 0; i < inNumBytes; ++i) + { + int b1 = reinterpret_cast(outData)[i]; + int b2 = reinterpret_cast(data)[i]; + if (b1 != b2) + Trace("Offset %d: %02X -> %02X", i, b1, b2); + } + JPH_BREAKPOINT; + } + + // Copy the temporary data to the final destination + memcpy(outData, data, inNumBytes); + return; + } + + mStream.read((char *)outData, inNumBytes); +} + +bool StateRecorderImpl::IsEqual(StateRecorderImpl &inReference) +{ + // Get length of new state + mStream.seekg(0, std::stringstream::end); + std::streamoff this_len = mStream.tellg(); + mStream.seekg(0, std::stringstream::beg); + + // Get length of old state + inReference.mStream.seekg(0, std::stringstream::end); + std::streamoff reference_len = inReference.mStream.tellg(); + inReference.mStream.seekg(0, std::stringstream::beg); + + // Compare size + bool fail = reference_len != this_len; + if (fail) + { + Trace("Failed to properly recover state, different stream length!"); + return false; + } + + // Compare byte by byte + for (std::streamoff i = 0, l = this_len; !fail && i < l; ++i) + { + fail = inReference.mStream.get() != mStream.get(); + if (fail) + { + Trace("Failed to properly recover state, different at offset %d!", (int)i); + return false; + } + } + + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h new file mode 100644 index 000000000000..c994ece3e4c0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h @@ -0,0 +1,50 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of the StateRecorder class that uses a stringstream as underlying store and that implements checking if the state doesn't change upon reading +class JPH_EXPORT StateRecorderImpl final : public StateRecorder +{ +public: + /// Constructor + StateRecorderImpl() = default; + StateRecorderImpl(StateRecorderImpl &&inRHS) : StateRecorder(inRHS), mStream(std::move(inRHS.mStream)) { } + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) override; + + /// Rewind the stream for reading + void Rewind(); + + /// Clear the stream for reuse + void Clear(); + + /// Read a string of bytes from the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) override; + + // See StreamIn + virtual bool IsEOF() const override { return mStream.eof(); } + + // See StreamIn / StreamOut + virtual bool IsFailed() const override { return mStream.fail(); } + + /// Compare this state with a reference state and ensure they are the same + bool IsEqual(StateRecorderImpl &inReference); + + /// Convert the binary data to a string + std::string GetData() const { return mStream.str(); } + + /// Get size of the binary data in bytes + size_t GetDataSize() { return size_t(mStream.tellp()); } + +private: + std::stringstream mStream; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp new file mode 100644 index 000000000000..1bfa564f76fb --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp @@ -0,0 +1,293 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MotorcycleControllerSettings) +{ + JPH_ADD_BASE_CLASS(MotorcycleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mMaxLeanAngle) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringConstant) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringDamping) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficient) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficientDecay) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSmoothingFactor) +} + +VehicleController *MotorcycleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new MotorcycleController(*this, inConstraint); +} + +void MotorcycleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + WheeledVehicleControllerSettings::SaveBinaryState(inStream); + + inStream.Write(mMaxLeanAngle); + inStream.Write(mLeanSpringConstant); + inStream.Write(mLeanSpringDamping); + inStream.Write(mLeanSpringIntegrationCoefficient); + inStream.Write(mLeanSpringIntegrationCoefficientDecay); + inStream.Write(mLeanSmoothingFactor); +} + +void MotorcycleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + WheeledVehicleControllerSettings::RestoreBinaryState(inStream); + + inStream.Read(mMaxLeanAngle); + inStream.Read(mLeanSpringConstant); + inStream.Read(mLeanSpringDamping); + inStream.Read(mLeanSpringIntegrationCoefficient); + inStream.Read(mLeanSpringIntegrationCoefficientDecay); + inStream.Read(mLeanSmoothingFactor); +} + +MotorcycleController::MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + WheeledVehicleController(inSettings, inConstraint), + mMaxLeanAngle(inSettings.mMaxLeanAngle), + mLeanSpringConstant(inSettings.mLeanSpringConstant), + mLeanSpringDamping(inSettings.mLeanSpringDamping), + mLeanSpringIntegrationCoefficient(inSettings.mLeanSpringIntegrationCoefficient), + mLeanSpringIntegrationCoefficientDecay(inSettings.mLeanSpringIntegrationCoefficientDecay), + mLeanSmoothingFactor(inSettings.mLeanSmoothingFactor) +{ +} + +float MotorcycleController::GetWheelBase() const +{ + float low = FLT_MAX, high = -FLT_MAX; + + for (const Wheel *w : mConstraint.GetWheels()) + { + const WheelSettings *s = w->GetSettings(); + + // Measure distance along the forward axis by looking at the fully extended suspension. + // If the suspension force point is active, use that instead. + Vec3 force_point = s->mEnableSuspensionForcePoint? s->mSuspensionForcePoint : s->mPosition + s->mSuspensionDirection * s->mSuspensionMaxLength; + float value = force_point.Dot(mConstraint.GetLocalForward()); + + // Update min and max + low = min(low, value); + high = max(high, value); + } + + return high - low; +} + +void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + WheeledVehicleController::PreCollide(inDeltaTime, inPhysicsSystem); + + const Body *body = mConstraint.GetVehicleBody(); + Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward(); + float wheel_base = GetWheelBase(); + Vec3 world_up = mConstraint.GetWorldUp(); + + if (mEnableLeanController) + { + // Calculate the target lean vector, this is in the direction of the total applied impulse by the ground on the wheels + Vec3 target_lean = Vec3::sZero(); + for (const Wheel *w : mConstraint.GetWheels()) + if (w->HasContact()) + target_lean += w->GetContactNormal() * w->GetSuspensionLambda() + w->GetContactLateral() * w->GetLateralLambda(); + + // Normalize the impulse + target_lean = target_lean.NormalizedOr(world_up); + + // Smooth the impulse to avoid jittery behavior + mTargetLean = mLeanSmoothingFactor * mTargetLean + (1.0f - mLeanSmoothingFactor) * target_lean; + + // Remove forward component, we can only lean sideways + mTargetLean -= forward * mTargetLean.Dot(forward); + mTargetLean = mTargetLean.NormalizedOr(world_up); + + // Clamp the target lean against the max lean angle + Vec3 adjusted_world_up = world_up - forward * world_up.Dot(forward); + adjusted_world_up = adjusted_world_up.NormalizedOr(world_up); + float w_angle = -Sign(mTargetLean.Cross(adjusted_world_up).Dot(forward)) * ACos(mTargetLean.Dot(adjusted_world_up)); + if (abs(w_angle) > mMaxLeanAngle) + mTargetLean = Quat::sRotation(forward, Sign(w_angle) * mMaxLeanAngle) * adjusted_world_up; + + // Integrate the delta angle + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up)); + mLeanSpringIntegratedDeltaAngle += d_angle * inDeltaTime; + } + else + { + // Controller not enabled, reset target lean + mTargetLean = world_up; + + // Reset integrated delta angle + mLeanSpringIntegratedDeltaAngle = 0; + } + + JPH_DET_LOG("WheeledVehicleController::PreCollide: mTargetLean: " << mTargetLean); + + // Calculate max steering angle based on the max lean angle we're willing to take + // See: https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Leaning + // LeanAngle = Atan(Velocity^2 / (Gravity * TurnRadius)) + // And: https://en.wikipedia.org/wiki/Turning_radius (we're ignoring the tire width) + // The CasterAngle is the added according to https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Turning (this is the same formula but without small angle approximation) + // TurnRadius = WheelBase / (Sin(SteerAngle) * Cos(CasterAngle)) + // => SteerAngle = ASin(WheelBase * Tan(LeanAngle) * Gravity / (Velocity^2 * Cos(CasterAngle)) + // The caster angle is different for each wheel so we can only calculate part of the equation here + float max_steer_angle_factor = wheel_base * Tan(mMaxLeanAngle) * (mConstraint.IsGravityOverridden()? mConstraint.GetGravityOverride() : inPhysicsSystem.GetGravity()).Length(); + + // Calculate forward velocity + float velocity = body->GetLinearVelocity().Dot(forward); + float velocity_sq = Square(velocity); + + // Decompose steering into sign and direction + float steer_strength = abs(mRightInput); + float steer_sign = -Sign(mRightInput); + + for (Wheel *w_base : mConstraint.GetWheels()) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *s = w->GetSettings(); + + // Check if this wheel can steer + if (s->mMaxSteerAngle != 0.0f) + { + // Calculate cos(caster angle), the angle between the steering axis and the up vector + float cos_caster_angle = s->mSteeringAxis.Dot(mConstraint.GetLocalUp()); + + // Calculate steer angle + float steer_angle = steer_strength * w->GetSettings()->mMaxSteerAngle; + + // Clamp to max steering angle + if (mEnableLeanSteeringLimit + && velocity_sq > 1.0e-6f && cos_caster_angle > 1.0e-6f) + { + float max_steer_angle = ASin(max_steer_angle_factor / (velocity_sq * cos_caster_angle)); + steer_angle = min(steer_angle, max_steer_angle); + } + + // Set steering angle + w->SetSteerAngle(steer_sign * steer_angle); + } + } + + // Reset applied impulse + mAppliedImpulse = 0; +} + +bool MotorcycleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = WheeledVehicleController::SolveLongitudinalAndLateralConstraints(inDeltaTime); + + if (mEnableLeanController) + { + // Only apply a lean impulse if all wheels are in contact, otherwise we can easily spin out + bool all_in_contact = true; + for (const Wheel *w : mConstraint.GetWheels()) + if (!w->HasContact() || w->GetSuspensionLambda() <= 0.0f) + { + all_in_contact = false; + break; + } + + if (all_in_contact) + { + Body *body = mConstraint.GetVehicleBody(); + const MotionProperties *mp = body->GetMotionProperties(); + + Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward(); + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + + // Calculate delta to target angle and derivative + float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up)); + float ddt_angle = body->GetAngularVelocity().Dot(forward); + + // Calculate impulse to apply to get to target lean angle + float total_impulse = (mLeanSpringConstant * d_angle - mLeanSpringDamping * ddt_angle + mLeanSpringIntegrationCoefficient * mLeanSpringIntegratedDeltaAngle) * inDeltaTime; + + // Remember angular velocity pre angular impulse + Vec3 old_w = mp->GetAngularVelocity(); + + // Apply impulse taking into account the impulse we've applied earlier + float delta_impulse = total_impulse - mAppliedImpulse; + body->AddAngularImpulse(delta_impulse * forward); + mAppliedImpulse = total_impulse; + + // Calculate delta angular velocity due to angular impulse + Vec3 dw = mp->GetAngularVelocity() - old_w; + Vec3 linear_acceleration = Vec3::sZero(); + float total_lambda = 0.0f; + for (Wheel *w_base : mConstraint.GetWheels()) + { + const WheelWV *w = static_cast(w_base); + + // We weigh the importance of each contact point according to the contact force + float lambda = w->GetSuspensionLambda(); + total_lambda += lambda; + + // Linear acceleration of contact point is dw x com_to_contact + Vec3 r = Vec3(w->GetContactPosition() - body->GetCenterOfMassPosition()); + linear_acceleration += lambda * dw.Cross(r); + } + + // Apply linear impulse to COM to cancel the average velocity change on the wheels due to the angular impulse + Vec3 linear_impulse = -linear_acceleration / (total_lambda * mp->GetInverseMass()); + body->AddImpulse(linear_impulse); + + // Return true if we applied an impulse + impulse |= delta_impulse != 0.0f; + } + else + { + // Decay the integrated angle because we won't be applying a torque this frame + // Uses 1st order Taylor approximation of e^(-decay * dt) = 1 - decay * dt + mLeanSpringIntegratedDeltaAngle *= max(0.0f, 1.0f - mLeanSpringIntegrationCoefficientDecay * inDeltaTime); + } + } + + return impulse; +} + +void MotorcycleController::SaveState(StateRecorder &inStream) const +{ + WheeledVehicleController::SaveState(inStream); + + inStream.Write(mTargetLean); +} + +void MotorcycleController::RestoreState(StateRecorder &inStream) +{ + WheeledVehicleController::RestoreState(inStream); + + inStream.Read(mTargetLean); +} + +#ifdef JPH_DEBUG_RENDERER + +void MotorcycleController::Draw(DebugRenderer *inRenderer) const +{ + WheeledVehicleController::Draw(inRenderer); + + // Draw current and desired lean angle + Body *body = mConstraint.GetVehicleBody(); + RVec3 center_of_mass = body->GetCenterOfMassPosition(); + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + inRenderer->DrawArrow(center_of_mass, center_of_mass + up, Color::sYellow, 0.1f); + inRenderer->DrawArrow(center_of_mass, center_of_mass + mTargetLean, Color::sRed, 0.1f); +} + +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h new file mode 100644 index 000000000000..bf8ebfa43f4b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h @@ -0,0 +1,116 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Settings of a two wheeled motorcycle (adds a spring to balance the motorcycle) +/// Note: The motor cycle controller is still in development and may need a lot of tweaks/hacks to work properly! +class JPH_EXPORT MotorcycleControllerSettings : public WheeledVehicleControllerSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MotorcycleControllerSettings) + +public: + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + /// How far we're willing to make the bike lean over in turns (in radians) + float mMaxLeanAngle = DegreesToRadians(45.0f); + + /// Spring constant for the lean spring + float mLeanSpringConstant = 5000.0f; + + /// Spring damping constant for the lean spring + float mLeanSpringDamping = 1000.0f; + + /// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller + float mLeanSpringIntegrationCoefficient = 0.0f; + + /// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value + float mLeanSpringIntegrationCoefficientDecay = 4.0f; + + /// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes) + /// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current + float mLeanSmoothingFactor = 0.8f; +}; + +/// Runtime controller class +class JPH_EXPORT MotorcycleController : public WheeledVehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Get the distance between the front and back wheels + float GetWheelBase() const; + + /// Enable or disable the lean spring. This allows you to temporarily disable the lean spring to allow the motorcycle to fall over. + void EnableLeanController(bool inEnable) { mEnableLeanController = inEnable; } + + /// Check if the lean spring is enabled. + bool IsLeanControllerEnabled() const { return mEnableLeanController; } + + /// Enable or disable the lean steering limit. When enabled (default) the steering angle is limited based on the vehicle speed to prevent steering that would cause an inertial force that causes the motorcycle to topple over. + void EnableLeanSteeringLimit(bool inEnable) { mEnableLeanSteeringLimit = inEnable; } + bool IsLeanSteeringLimitEnabled() const { return mEnableLeanSteeringLimit; } + + /// Spring constant for the lean spring + void SetLeanSpringConstant(float inConstant) { mLeanSpringConstant = inConstant; } + float GetLeanSpringConstant() const { return mLeanSpringConstant; } + + /// Spring damping constant for the lean spring + void SetLeanSpringDamping(float inDamping) { mLeanSpringDamping = inDamping; } + float GetLeanSpringDamping() const { return mLeanSpringDamping; } + + /// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller + void SetLeanSpringIntegrationCoefficient(float inCoefficient) { mLeanSpringIntegrationCoefficient = inCoefficient; } + float GetLeanSpringIntegrationCoefficient() const { return mLeanSpringIntegrationCoefficient; } + + /// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value + void SetLeanSpringIntegrationCoefficientDecay(float inDecay) { mLeanSpringIntegrationCoefficientDecay = inDecay; } + float GetLeanSpringIntegrationCoefficientDecay() const { return mLeanSpringIntegrationCoefficientDecay; } + + /// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes) + /// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current + void SetLeanSmoothingFactor(float inFactor) { mLeanSmoothingFactor = inFactor; } + float GetLeanSmoothingFactor() const { return mLeanSmoothingFactor; } + +protected: + // See: VehicleController + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Configuration properties + bool mEnableLeanController = true; + bool mEnableLeanSteeringLimit = true; + float mMaxLeanAngle; + float mLeanSpringConstant; + float mLeanSpringDamping; + float mLeanSpringIntegrationCoefficient; + float mLeanSpringIntegrationCoefficientDecay; + float mLeanSmoothingFactor; + + // Run-time calculated target lean vector + Vec3 mTargetLean = Vec3::sZero(); + + // Integrated error for the lean spring + float mLeanSpringIntegratedDeltaAngle = 0.0f; + + // Run-time total angular impulse applied to turn the cycle towards the target lean angle + float mAppliedImpulse = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp new file mode 100644 index 000000000000..d3cfcffc54e0 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp @@ -0,0 +1,531 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TrackedVehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(TrackedVehicleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mEngine) + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTransmission) + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTracks) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsTV) +{ + JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLongitudinalFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLateralFriction) +} + +void WheelSettingsTV::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLongitudinalFriction); + inStream.Write(mLateralFriction); +} + +void WheelSettingsTV::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLongitudinalFriction); + inStream.Read(mLateralFriction); +} + +WheelTV::WheelTV(const WheelSettingsTV &inSettings) : + Wheel(inSettings) +{ +} + +void WheelTV::CalculateAngularVelocity(const VehicleConstraint &inConstraint) +{ + const WheelSettingsTV *settings = GetSettings(); + const Wheels &wheels = inConstraint.GetWheels(); + const VehicleTrack &track = static_cast(inConstraint.GetController())->GetTracks()[mTrackIndex]; + + // Calculate angular velocity of this wheel + mAngularVelocity = track.mAngularVelocity * wheels[track.mDrivenWheel]->GetSettings()->mRadius / settings->mRadius; +} + +void WheelTV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint) +{ + CalculateAngularVelocity(inConstraint); + + // Update rotation of wheel + mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI); + + // Reset brake impulse, will be set during post collision again + mBrakeImpulse = 0.0f; + + if (mContactBody != nullptr) + { + // Friction at the point of this wheel between track and floor + const WheelSettingsTV *settings = GetSettings(); + VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction(); + mCombinedLongitudinalFriction = settings->mLongitudinalFriction; + mCombinedLateralFriction = settings->mLateralFriction; + combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID); + } + else + { + // No collision + mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f; + } +} + +VehicleController *TrackedVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new TrackedVehicleController(*this, inConstraint); +} + +TrackedVehicleControllerSettings::TrackedVehicleControllerSettings() +{ + // Numbers guestimated from: https://en.wikipedia.org/wiki/M1_Abrams + mEngine.mMinRPM = 500.0f; + mEngine.mMaxRPM = 4000.0f; + mEngine.mMaxTorque = 500.0f; // Note actual torque for M1 is around 5000 but we need a reduced mass in order to keep the simulation sane + + mTransmission.mShiftDownRPM = 1000.0f; + mTransmission.mShiftUpRPM = 3500.0f; + mTransmission.mGearRatios = { 4.0f, 3.0f, 2.0f, 1.0f }; + mTransmission.mReverseGearRatios = { -4.0f, -3.0f }; +} + +void TrackedVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + mEngine.SaveBinaryState(inStream); + + mTransmission.SaveBinaryState(inStream); + + for (const VehicleTrackSettings &t : mTracks) + t.SaveBinaryState(inStream); +} + +void TrackedVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + mEngine.RestoreBinaryState(inStream); + + mTransmission.RestoreBinaryState(inStream); + + for (VehicleTrackSettings &t : mTracks) + t.RestoreBinaryState(inStream); +} + +TrackedVehicleController::TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + VehicleController(inConstraint) +{ + // Copy engine settings + static_cast(mEngine) = inSettings.mEngine; + JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f); + JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM); + mEngine.SetCurrentRPM(mEngine.mMinRPM); + + // Copy transmission settings + static_cast(mTransmission) = inSettings.mTransmission; +#ifdef JPH_ENABLE_ASSERTS + for (float r : inSettings.mTransmission.mGearRatios) + JPH_ASSERT(r > 0.0f); + for (float r : inSettings.mTransmission.mReverseGearRatios) + JPH_ASSERT(r < 0.0f); +#endif // JPH_ENABLE_ASSERTS + JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f); + JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f); + JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM); + JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM); + + // Copy track settings + for (uint i = 0; i < std::size(mTracks); ++i) + { + const VehicleTrackSettings &d = inSettings.mTracks[i]; + static_cast(mTracks[i]) = d; + JPH_ASSERT(d.mInertia >= 0.0f); + JPH_ASSERT(d.mAngularDamping >= 0.0f); + JPH_ASSERT(d.mMaxBrakeTorque >= 0.0f); + JPH_ASSERT(d.mDifferentialRatio > 0.0f); + } +} + +bool TrackedVehicleController::AllowSleep() const +{ + return mForwardInput == 0.0f // No user input + && mTransmission.AllowSleep() // Transmission is not shifting + && mEngine.AllowSleep(); // Engine is idling +} + +void TrackedVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + Wheels &wheels = mConstraint.GetWheels(); + + // Fill in track index + for (size_t t = 0; t < std::size(mTracks); ++t) + for (uint w : mTracks[t].mWheels) + static_cast(wheels[w])->mTrackIndex = (uint)t; + + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + for (VehicleTrack &t : mTracks) + t.mAngularVelocity *= max(0.0f, 1.0f - t.mAngularDamping * inDeltaTime); +} + +void TrackedVehicleController::SyncLeftRightTracks() +{ + // Apply left to right ratio according to track inertias + VehicleTrack &tl = mTracks[(int)ETrackSide::Left]; + VehicleTrack &tr = mTracks[(int)ETrackSide::Right]; + + if (mLeftRatio * mRightRatio > 0.0f) + { + // Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = -dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks + float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mLeftRatio * tr.mInertia + mRightRatio * tl.mInertia); + tl.mAngularVelocity += impulse * tl.mInertia; + tr.mAngularVelocity -= impulse * tr.mInertia; + } + else + { + // Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks + float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mRightRatio * tl.mInertia - mLeftRatio * tr.mInertia); + tl.mAngularVelocity += impulse * tl.mInertia; + tr.mAngularVelocity += impulse * tr.mInertia; + } +} + +void TrackedVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + + Wheels &wheels = mConstraint.GetWheels(); + + // Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again) + for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index) + { + WheelTV *w = static_cast(wheels[wheel_index]); + w->Update(wheel_index, inDeltaTime, mConstraint); + } + + // First calculate engine speed based on speed of all wheels + bool can_engine_apply_torque = false; + if (mTransmission.GetCurrentGear() != 0 && mTransmission.GetClutchFriction() > 1.0e-3f) + { + float transmission_ratio = mTransmission.GetCurrentRatio(); + bool forward = transmission_ratio >= 0.0f; + float fastest_wheel_speed = forward? -FLT_MAX : FLT_MAX; + for (const VehicleTrack &t : mTracks) + { + if (forward) + fastest_wheel_speed = max(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio); + else + fastest_wheel_speed = min(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio); + for (uint w : t.mWheels) + if (wheels[w]->HasContact()) + { + can_engine_apply_torque = true; + break; + } + } + + // Update RPM only if the tracks are connected to the engine + if (fastest_wheel_speed > -FLT_MAX && fastest_wheel_speed < FLT_MAX) + mEngine.SetCurrentRPM(fastest_wheel_speed * mTransmission.GetCurrentRatio() * VehicleEngine::cAngularVelocityToRPM); + } + else + { + // Update engine with damping + mEngine.ApplyDamping(inDeltaTime); + + // In auto transmission mode, don't accelerate the engine when switching gears + float forward_input = mTransmission.mMode == ETransmissionMode::Manual? abs(mForwardInput) : 0.0f; + + // Engine not connected to wheels, update RPM based on engine inertia alone + mEngine.ApplyTorque(mEngine.GetTorque(forward_input), inDeltaTime); + } + + // Update transmission + // Note: only allow switching gears up when the tracks are rolling in the same direction + mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, mLeftRatio * mRightRatio > 0.0f && can_engine_apply_torque); + + // Calculate the amount of torque the transmission gives to the differentials + float transmission_ratio = mTransmission.GetCurrentRatio(); + float transmission_torque = mTransmission.GetClutchFriction() * transmission_ratio * mEngine.GetTorque(abs(mForwardInput)); + if (transmission_torque != 0.0f) + { + // Apply the transmission torque to the wheels + for (uint i = 0; i < std::size(mTracks); ++i) + { + VehicleTrack &t = mTracks[i]; + + // Get wheel rotation ratio for this track + float ratio = i == 0? mLeftRatio : mRightRatio; + + // Calculate the max angular velocity of the driven wheel of the track given current engine RPM + // Note this adds 0.1% slop to avoid numerical accuracy issues + float track_max_angular_velocity = mEngine.GetCurrentRPM() / (transmission_ratio * t.mDifferentialRatio * ratio * VehicleEngine::cAngularVelocityToRPM) * 1.001f; + + // Calculate torque on the driven wheel + float differential_torque = t.mDifferentialRatio * ratio * transmission_torque; + + // Apply torque to driven wheel + if (t.mAngularVelocity * track_max_angular_velocity < 0.0f || abs(t.mAngularVelocity) < abs(track_max_angular_velocity)) + t.mAngularVelocity += differential_torque * inDeltaTime / t.mInertia; + } + } + + // Ensure that we have the correct ratio between the two tracks + SyncLeftRightTracks(); + + // Braking + for (VehicleTrack &t : mTracks) + { + // Calculate brake torque + float brake_torque = mBrakeInput * t.mMaxBrakeTorque; + if (brake_torque > 0.0f) + { + // Calculate how much torque is needed to stop the track from rotating in this time step + float brake_torque_to_lock_track = abs(t.mAngularVelocity) * t.mInertia / inDeltaTime; + if (brake_torque > brake_torque_to_lock_track) + { + // Wheels are locked + t.mAngularVelocity = 0.0f; + brake_torque -= brake_torque_to_lock_track; + } + else + { + // Slow down the track + t.mAngularVelocity -= Sign(t.mAngularVelocity) * brake_torque * inDeltaTime / t.mInertia; + } + } + + if (brake_torque > 0.0f) + { + // Sum the radius of all wheels touching the floor + float total_radius = 0.0f; + for (uint wheel_index : t.mWheels) + { + const WheelTV *w = static_cast(wheels[wheel_index]); + + if (w->HasContact()) + total_radius += w->GetSettings()->mRadius; + } + + if (total_radius > 0.0f) + { + brake_torque /= total_radius; + for (uint wheel_index : t.mWheels) + { + WheelTV *w = static_cast(wheels[wheel_index]); + if (w->HasContact()) + { + // Impulse: p = F * dt = Torque / Wheel_Radius * dt, Torque = Total_Torque * Wheel_Radius / Summed_Radius => p = Total_Torque * dt / Summed_Radius + w->mBrakeImpulse = brake_torque * inDeltaTime; + } + } + } + } + } + + // Update wheel angular velocity based on that of the track + for (Wheel *w_base : wheels) + { + WheelTV *w = static_cast(w_base); + w->CalculateAngularVelocity(mConstraint); + } +} + +bool TrackedVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = false; + + for (Wheel *w_base : mConstraint.GetWheels()) + if (w_base->HasContact()) + { + WheelTV *w = static_cast(w_base); + const WheelSettingsTV *settings = w->GetSettings(); + VehicleTrack &track = mTracks[w->mTrackIndex]; + + // Calculate max impulse that we can apply on the ground + float max_longitudinal_friction_impulse = w->mCombinedLongitudinalFriction * w->GetSuspensionLambda(); + + // Calculate relative velocity between wheel contact point and floor in longitudinal direction + Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity(); + float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal()); + + // Calculate brake force to apply + float min_longitudinal_impulse, max_longitudinal_impulse; + if (w->mBrakeImpulse != 0.0f) + { + // Limit brake force by max tire friction + float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse); + + // Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle) + if (relative_longitudinal_velocity >= 0.0f) + { + min_longitudinal_impulse = -brake_impulse; + max_longitudinal_impulse = 0.0f; + } + else + { + min_longitudinal_impulse = 0.0f; + max_longitudinal_impulse = brake_impulse; + } + + // Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + } + else + { + // Assume we want to apply an angular impulse that makes the delta velocity between track and ground zero in one time step, calculate the amount of linear impulse needed to do that + float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius; + float linear_impulse = (track.mAngularVelocity - desired_angular_velocity) * track.mInertia / settings->mRadius; + + // Limit the impulse by max track friction + float prev_lambda = w->GetLongitudinalLambda(); + min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse); + + // Longitudinal impulse + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + + // Update the angular velocity of the track according to the lambda that was applied + track.mAngularVelocity -= (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / track.mInertia; + SyncLeftRightTracks(); + } + } + + for (Wheel *w_base : mConstraint.GetWheels()) + if (w_base->HasContact()) + { + WheelTV *w = static_cast(w_base); + + // Update angular velocity of wheel for the next iteration + w->CalculateAngularVelocity(mConstraint); + + // Lateral friction + float max_lateral_friction_impulse = w->mCombinedLateralFriction * w->GetSuspensionLambda(); + impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_friction_impulse, max_lateral_friction_impulse); + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void TrackedVehicleController::Draw(DebugRenderer *inRenderer) const +{ + float constraint_size = mConstraint.GetDrawConstraintSize(); + + // Draw RPM + Body *body = mConstraint.GetVehicleBody(); + Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp(); + RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition; + Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward(); + mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM); + + // Draw current vehicle state + String status = StringFormat("Forward: %.1f, LRatio: %.1f, RRatio: %.1f, Brake: %.1f\n" + "Gear: %d, Clutch: %.1f, EngineRPM: %.0f, V: %.1f km/h", + (double)mForwardInput, (double)mLeftRatio, (double)mRightRatio, (double)mBrakeInput, + mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6); + inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size); + + for (const VehicleTrack &t : mTracks) + { + const WheelTV *w = static_cast(mConstraint.GetWheels()[t.mDrivenWheel]); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body->GetCenterOfMassPosition() + body->GetRotation() * (settings->mPosition - body->GetShape()->GetCenterOfMass()); + + DebugRenderer::sInstance->DrawText3D(ws_position, StringFormat("W: %.1f", (double)t.mAngularVelocity), Color::sWhite, constraint_size); + } + + RMat44 body_transform = body->GetWorldTransform(); + + for (const Wheel *w_base : mConstraint.GetWheels()) + { + const WheelTV *w = static_cast(w_base); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Draw suspension + RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength; + RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength; + inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed); + inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen); + + // Draw current length + RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength(); + inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size); + + // Draw wheel basis + Vec3 wheel_forward, wheel_up, wheel_right; + mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right); + wheel_forward = body_transform.Multiply3x3(wheel_forward); + wheel_up = body_transform.Multiply3x3(wheel_up); + wheel_right = body_transform.Multiply3x3(wheel_right); + Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue); + inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow); + + // Draw wheel + RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos); + wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle())); + inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe); + + if (w->HasContact()) + { + // Draw contact + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue); + + DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("S: %.2f", (double)w->GetSuspensionLength()), Color::sWhite, constraint_size); + } + } +} + +#endif // JPH_DEBUG_RENDERER + +void TrackedVehicleController::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mForwardInput); + inStream.Write(mLeftRatio); + inStream.Write(mRightRatio); + inStream.Write(mBrakeInput); + + mEngine.SaveState(inStream); + mTransmission.SaveState(inStream); + + for (const VehicleTrack &t : mTracks) + t.SaveState(inStream); +} + +void TrackedVehicleController::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mForwardInput); + inStream.Read(mLeftRatio); + inStream.Read(mRightRatio); + inStream.Read(mBrakeInput); + + mEngine.RestoreState(inStream); + mTransmission.RestoreState(inStream); + + for (VehicleTrack &t : mTracks) + t.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h new file mode 100644 index 000000000000..8567c97073db --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h @@ -0,0 +1,166 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// WheelSettings object specifically for TrackedVehicleController +class JPH_EXPORT WheelSettingsTV : public WheelSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsTV) + +public: + // See: WheelSettings + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + float mLongitudinalFriction = 4.0f; ///< Friction in forward direction of tire + float mLateralFriction = 2.0f; ///< Friction in sideway direction of tire +}; + +/// Wheel object specifically for TrackedVehicleController +class JPH_EXPORT WheelTV : public Wheel +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit WheelTV(const WheelSettingsTV &inWheel); + + /// Override GetSettings and cast to the correct class + const WheelSettingsTV * GetSettings() const { return StaticCast(mSettings); } + + /// Update the angular velocity of the wheel based on the angular velocity of the track + void CalculateAngularVelocity(const VehicleConstraint &inConstraint); + + /// Update the wheel rotation based on the current angular velocity + void Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint); + + int mTrackIndex = -1; ///< Index in mTracks to which this wheel is attached (calculated on initialization) + float mCombinedLongitudinalFriction = 0.0f; ///< Combined friction coefficient in longitudinal direction (combines terrain and track) + float mCombinedLateralFriction = 0.0f; ///< Combined friction coefficient in lateral direction (combines terrain and track) + float mBrakeImpulse = 0.0f; ///< Amount of impulse that the brakes can apply to the floor (excluding friction), spread out from brake impulse applied on track +}; + +/// Settings of a vehicle with tank tracks +/// +/// Default settings are based around what I could find about the M1 Abrams tank. +/// Note to avoid issues with very heavy objects vs very light objects the mass of the tank should be a lot lower (say 10x) than that of a real tank. That means that the engine/brake torque is also 10x less. +class JPH_EXPORT TrackedVehicleControllerSettings : public VehicleControllerSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TrackedVehicleControllerSettings) + +public: + // Constructor + TrackedVehicleControllerSettings(); + + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + VehicleEngineSettings mEngine; ///< The properties of the engine + VehicleTransmissionSettings mTransmission; ///< The properties of the transmission (aka gear box) + VehicleTrackSettings mTracks[(int)ETrackSide::Num]; ///< List of tracks and their properties +}; + +/// Runtime controller class for vehicle with tank tracks +class JPH_EXPORT TrackedVehicleController : public VehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Set input from driver + /// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + /// @param inLeftRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + /// @param inRightRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + /// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetDriverInput(float inForward, float inLeftRatio, float inRightRatio, float inBrake) { JPH_ASSERT(inLeftRatio != 0.0f && inRightRatio != 0.0f); mForwardInput = inForward; mLeftRatio = inLeftRatio; mRightRatio = inRightRatio; mBrakeInput = inBrake; } + + /// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + void SetForwardInput(float inForward) { mForwardInput = inForward; } + float GetForwardInput() const { return mForwardInput; } + + /// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + void SetLeftRatio(float inLeftRatio) { JPH_ASSERT(inLeftRatio != 0.0f); mLeftRatio = inLeftRatio; } + float GetLeftRatio() const { return mLeftRatio; } + + /// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + void SetRightRatio(float inRightRatio) { JPH_ASSERT(inRightRatio != 0.0f); mRightRatio = inRightRatio; } + float GetRightRatio() const { return mRightRatio; } + + /// Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetBrakeInput(float inBrake) { mBrakeInput = inBrake; } + float GetBrakeInput() const { return mBrakeInput; } + + /// Get current engine state + const VehicleEngine & GetEngine() const { return mEngine; } + + /// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleEngine & GetEngine() { return mEngine; } + + /// Get current transmission state + const VehicleTransmission & GetTransmission() const { return mTransmission; } + + /// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTransmission & GetTransmission() { return mTransmission; } + + /// Get the tracks this vehicle has + const VehicleTracks & GetTracks() const { return mTracks; } + + /// Get the tracks this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTracks & GetTracks() { return mTracks; } + +#ifdef JPH_DEBUG_RENDERER + /// Debug drawing of RPM meter + void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; } +#endif // JPH_DEBUG_RENDERER + +protected: + /// Synchronize angular velocities of left and right tracks according to their ratios + void SyncLeftRightTracks(); + + // See: VehicleController + virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsTV))); return new WheelTV(static_cast(inWheel)); } + virtual bool AllowSleep() const override; + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Control information + float mForwardInput = 0.0f; ///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + float mLeftRatio = 1.0f; ///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + float mRightRatio = 1.0f; ///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + float mBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the brake pedal is pressed + + // Simulation information + VehicleEngine mEngine; ///< Engine state of the vehicle + VehicleTransmission mTransmission; ///< Transmission state of the vehicle + VehicleTracks mTracks; ///< Tracks of the vehicle + +#ifdef JPH_DEBUG_RENDERER + // Debug settings + Vec3 mRPMMeterPosition { 0, 1, 0 }; ///< Position (in local space of the body) of the RPM meter when drawing the constraint + float mRPMMeterSize = 0.5f; ///< Size of the RPM meter when drawing the constraint +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp new file mode 100644 index 000000000000..859bcafa175f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleAntiRollBar) +{ + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mLeftWheel) + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mRightWheel) + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mStiffness) +} + +void VehicleAntiRollBar::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLeftWheel); + inStream.Write(mRightWheel); + inStream.Write(mStiffness); +} + +void VehicleAntiRollBar::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLeftWheel); + inStream.Read(mRightWheel); + inStream.Read(mStiffness); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h new file mode 100644 index 000000000000..4a9c6707d4f2 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// An anti rollbar is a stiff spring that connects two wheels to reduce the amount of roll the vehicle makes in sharp corners +/// See: https://en.wikipedia.org/wiki/Anti-roll_bar +class JPH_EXPORT VehicleAntiRollBar +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleAntiRollBar) + +public: + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + int mLeftWheel = 0; ///< Index (in mWheels) that represents the left wheel of this anti-rollbar + int mRightWheel = 1; ///< Index (in mWheels) that represents the right wheel of this anti-rollbar + float mStiffness = 1000.0f; ///< Stiffness (spring constant in N/m) of anti rollbar, can be 0 to disable the anti-rollbar +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp new file mode 100644 index 000000000000..56246835794f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp @@ -0,0 +1,376 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool VehicleCollisionTesterRay::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float wheel_radius = wheel_settings->mRadius; + float ray_length = wheel_settings->mSuspensionMaxLength + wheel_radius; + RRayCast ray { inOrigin, ray_length * inDirection }; + + class MyCollector : public CastRayCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RRayCast &inRay, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) : + mPhysicsSystem(inPhysicsSystem), + mRay(inRay), + mUpDirection(inUpDirection), + mCosMaxSlopeAngle(inCosMaxSlopeAngle) + { + } + + virtual void AddHit(const RayCastResult &inResult) override + { + // Test if this collision is closer than the previous one + if (inResult.mFraction < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Test that we're not hitting a vertical wall + RVec3 contact_pos = mRay.GetPointOnRay(inResult.mFraction); + Vec3 normal = body->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, contact_pos); + if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle) + { + // Update early out fraction to this hit + UpdateEarlyOutFraction(inResult.mFraction); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = contact_pos; + mContactNormal = normal; + } + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + RRayCast mRay; + Vec3 mUpDirection; + float mCosMaxSlopeAngle; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + }; + + RayCastSettings settings; + + MyCollector collector(inPhysicsSystem, ray, mUp, mCosMaxSlopeAngle); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastRay(ray, settings, collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max(0.0f, ray_length * collector.GetEarlyOutFraction() - wheel_radius); + + return true; +} + +void VehicleCollisionTesterRay::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Reproject the contact position using the suspension ray and the plane formed by the contact position and normal + ioContactPosition = inOrigin + Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal) / d_dot_n * inDirection; + + // The suspension length is simply the distance between the contact position and the suspension origin excluding the wheel radius + ioSuspensionLength = Clamp(Vec3(ioContactPosition - inOrigin).Dot(inDirection) - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + SphereShape sphere(mRadius); + sphere.SetEmbedded(); + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float wheel_radius = wheel_settings->mRadius; + float shape_cast_length = wheel_settings->mSuspensionMaxLength + wheel_radius - mRadius; + RShapeCast shape_cast(&sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length); + + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = true; + + class MyCollector : public CastShapeCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) : + mPhysicsSystem(inPhysicsSystem), + mShapeCast(inShapeCast), + mUpDirection(inUpDirection), + mCosMaxSlopeAngle(inCosMaxSlopeAngle) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Test if this collision is closer/deeper than the previous one + float early_out = inResult.GetEarlyOutFraction(); + if (early_out < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Test that we're not hitting a vertical wall + Vec3 normal = -inResult.mPenetrationAxis.Normalized(); + if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle) + { + // Update early out fraction to this hit + UpdateEarlyOutFraction(early_out); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2; + mContactNormal = normal; + mFraction = inResult.mFraction; + } + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + const RShapeCast & mShapeCast; + Vec3 mUpDirection; + float mCosMaxSlopeAngle; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + float mFraction; + }; + + MyCollector collector(inPhysicsSystem, shape_cast, mUp, mCosMaxSlopeAngle); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max(0.0f, shape_cast_length * collector.mFraction + mRadius - wheel_radius); + + return true; +} + +void VehicleCollisionTesterCastSphere::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal + // This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = mRadius for fraction + float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal); + float fraction = (mRadius + oc_dot_n) / d_dot_n; + ioContactPosition = inOrigin + fraction * inDirection - mRadius * ioContactNormal; + + // Calculate the new suspension length in the same way as the cast sphere normally does + ioSuspensionLength = Clamp(fraction + mRadius - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float max_suspension_length = wheel_settings->mSuspensionMaxLength; + + // Get the wheel transform given that the cylinder rotates around the Y axis + RMat44 shape_cast_start = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX()); + shape_cast_start.SetTranslation(inOrigin); + + // Construct a cylinder with the dimensions of the wheel + float wheel_half_width = 0.5f * wheel_settings->mWidth; + CylinderShape cylinder(wheel_half_width, wheel_settings->mRadius, min(wheel_half_width, wheel_settings->mRadius) * mConvexRadiusFraction); + cylinder.SetEmbedded(); + + RShapeCast shape_cast(&cylinder, Vec3::sReplicate(1.0f), shape_cast_start, inDirection * max_suspension_length); + + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = true; + + class MyCollector : public CastShapeCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast) : + mPhysicsSystem(inPhysicsSystem), + mShapeCast(inShapeCast) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Test if this collision is closer/deeper than the previous one + float early_out = inResult.GetEarlyOutFraction(); + if (early_out < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Update early out fraction to this hit + UpdateEarlyOutFraction(early_out); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2; + mContactNormal = -inResult.mPenetrationAxis.Normalized(); + mFraction = inResult.mFraction; + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + const RShapeCast & mShapeCast; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + float mFraction; + }; + + MyCollector collector(inPhysicsSystem, shape_cast); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max_suspension_length * collector.mFraction; + + return true; +} + +void VehicleCollisionTesterCastCylinder::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Wheel size + float half_width = 0.5f * wheel_settings->mWidth; + float radius = wheel_settings->mRadius; + + // Get the inverse local space contact normal for a cylinder pointing along Y + RMat44 wheel_transform = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX()); + Vec3 inverse_local_normal = -wheel_transform.Multiply3x3Transposed(ioContactNormal); + + // Get the support point of this normal in local space of the cylinder + // See CylinderShape::Cylinder::GetSupport + float x = inverse_local_normal.GetX(), y = inverse_local_normal.GetY(), z = inverse_local_normal.GetZ(); + float o = sqrt(Square(x) + Square(z)); + Vec3 support_point; + if (o > 0.0f) + support_point = Vec3((radius * x) / o, Sign(y) * half_width, (radius * z) / o); + else + support_point = Vec3(0, Sign(y) * half_width, 0); + + // Rotate back to world space + support_point = wheel_transform.Multiply3x3(support_point); + + // Now we can use inOrigin + support_point as the start of a ray of our suspension to the contact plane + // as know that it is the first point on the wheel that will hit the plane + RVec3 origin = inOrigin + support_point; + + // Calculate contact position and suspension length, the is the same as VehicleCollisionTesterRay + // but we don't need to take the radius into account anymore + Vec3 oc(ioContactPosition - origin); + ioContactPosition = origin + oc.Dot(ioContactNormal) / d_dot_n * inDirection; + ioSuspensionLength = Clamp(oc.Dot(inDirection), 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h new file mode 100644 index 000000000000..7c222756c8b6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h @@ -0,0 +1,146 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class VehicleConstraint; +class BroadPhaseLayerFilter; +class ObjectLayerFilter; +class BodyFilter; + +/// Class that does collision detection between wheels and ground +class JPH_EXPORT VehicleCollisionTester : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructors + VehicleCollisionTester() = default; + explicit VehicleCollisionTester(ObjectLayer inObjectLayer) : mObjectLayer(inObjectLayer) { } + + /// Virtual destructor + virtual ~VehicleCollisionTester() = default; + + /// Object layer to use for collision detection, this is used when the filters are not overridden + ObjectLayer GetObjectLayer() const { return mObjectLayer; } + void SetObjectLayer(ObjectLayer inObjectLayer) { mObjectLayer = inObjectLayer; } + + /// Access to the broad phase layer filter, when set this overrides the object layer supplied in the constructor + void SetBroadPhaseLayerFilter(const BroadPhaseLayerFilter *inFilter) { mBroadPhaseLayerFilter = inFilter; } + const BroadPhaseLayerFilter * GetBroadPhaseLayerFilter() const { return mBroadPhaseLayerFilter; } + + /// Access to the object layer filter, when set this overrides the object layer supplied in the constructor + void SetObjectLayerFilter(const ObjectLayerFilter *inFilter) { mObjectLayerFilter = inFilter; } + const ObjectLayerFilter * GetObjectLayerFilter() const { return mObjectLayerFilter; } + + /// Access to the body filter, when set this overrides the default filter that filters out the vehicle body + void SetBodyFilter(const BodyFilter *inFilter) { mBodyFilter = inFilter; } + const BodyFilter * GetBodyFilter() const { return mBodyFilter; } + + /// Do a collision test with the world + /// @param inPhysicsSystem The physics system that should be tested against + /// @param inVehicleConstraint The vehicle constraint + /// @param inWheelIndex Index of the wheel that we're testing collision for + /// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point + /// @param inDirection Direction for the test (unit vector, world space) + /// @param inVehicleBodyID This body should be filtered out during collision detection to avoid self collisions + /// @param outBody Body that the wheel collided with + /// @param outSubShapeID Sub shape ID that the wheel collided with + /// @param outContactPosition Contact point between wheel and floor, in world space + /// @param outContactNormal Contact normal between wheel and floor, pointing away from the floor + /// @param outSuspensionLength New length of the suspension [0, inSuspensionMaxLength] + /// @return True when collision found, false if not + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const = 0; + + /// Do a cheap contact properties prediction based on the contact properties from the last collision test (provided as input parameters) + /// @param inPhysicsSystem The physics system that should be tested against + /// @param inVehicleConstraint The vehicle constraint + /// @param inWheelIndex Index of the wheel that we're testing collision for + /// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point + /// @param inDirection Direction for the test (unit vector, world space) + /// @param inVehicleBodyID The body ID for the vehicle itself + /// @param ioBody Body that the wheel previously collided with + /// @param ioSubShapeID Sub shape ID that the wheel collided with during the last check + /// @param ioContactPosition Contact point between wheel and floor during the last check, in world space + /// @param ioContactNormal Contact normal between wheel and floor during the last check, pointing away from the floor + /// @param ioSuspensionLength New length of the suspension [0, inSuspensionMaxLength] + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const = 0; + +protected: + const BroadPhaseLayerFilter * mBroadPhaseLayerFilter = nullptr; + const ObjectLayerFilter * mObjectLayerFilter = nullptr; + const BodyFilter * mBodyFilter = nullptr; + ObjectLayer mObjectLayer = cObjectLayerInvalid; +}; + +/// Collision tester that tests collision using a raycast +class JPH_EXPORT VehicleCollisionTesterRay : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inUp World space up vector, used to avoid colliding with vertical walls. + /// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls. + VehicleCollisionTesterRay(ObjectLayer inObjectLayer, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + Vec3 mUp; + float mCosMaxSlopeAngle; +}; + +/// Collision tester that tests collision using a sphere cast +class JPH_EXPORT VehicleCollisionTesterCastSphere : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inUp World space up vector, used to avoid colliding with vertical walls. + /// @param inRadius Radius of sphere + /// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls. + VehicleCollisionTesterCastSphere(ObjectLayer inObjectLayer, float inRadius, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mRadius(inRadius), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + float mRadius; + Vec3 mUp; + float mCosMaxSlopeAngle; +}; + +/// Collision tester that tests collision using a cylinder shape +class JPH_EXPORT VehicleCollisionTesterCastCylinder : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inConvexRadiusFraction Fraction of half the wheel width (or wheel radius if it is smaller) that is used as the convex radius + VehicleCollisionTesterCastCylinder(ObjectLayer inObjectLayer, float inConvexRadiusFraction = 0.1f) : VehicleCollisionTester(inObjectLayer), mConvexRadiusFraction(inConvexRadiusFraction) { JPH_ASSERT(mConvexRadiusFraction >= 0.0f && mConvexRadiusFraction <= 1.0f); } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + float mConvexRadiusFraction; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp new file mode 100644 index 000000000000..fb76aa674079 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp @@ -0,0 +1,697 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(VehicleConstraintSettings) +{ + JPH_ADD_BASE_CLASS(VehicleConstraintSettings, ConstraintSettings) + + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mUp) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mForward) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mMaxPitchRollAngle) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mWheels) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mAntiRollBars) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mController) +} + +void VehicleConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mUp); + inStream.Write(mForward); + inStream.Write(mMaxPitchRollAngle); + + uint32 num_anti_rollbars = (uint32)mAntiRollBars.size(); + inStream.Write(num_anti_rollbars); + for (const VehicleAntiRollBar &r : mAntiRollBars) + r.SaveBinaryState(inStream); + + uint32 num_wheels = (uint32)mWheels.size(); + inStream.Write(num_wheels); + for (const WheelSettings *w : mWheels) + w->SaveBinaryState(inStream); + + inStream.Write(mController->GetRTTI()->GetHash()); + mController->SaveBinaryState(inStream); +} + +void VehicleConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mUp); + inStream.Read(mForward); + inStream.Read(mMaxPitchRollAngle); + + uint32 num_anti_rollbars = 0; + inStream.Read(num_anti_rollbars); + mAntiRollBars.resize(num_anti_rollbars); + for (VehicleAntiRollBar &r : mAntiRollBars) + r.RestoreBinaryState(inStream); + + uint32 num_wheels = 0; + inStream.Read(num_wheels); + mWheels.resize(num_wheels); + for (WheelSettings *w : mWheels) + w->RestoreBinaryState(inStream); + + uint32 hash = 0; + inStream.Read(hash); + const RTTI *rtti = Factory::sInstance->Find(hash); + mController = reinterpret_cast(rtti->CreateObject()); + mController->RestoreBinaryState(inStream); +} + +VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings) : + Constraint(inSettings), + mBody(&inVehicleBody), + mForward(inSettings.mForward), + mUp(inSettings.mUp), + mWorldUp(inSettings.mUp) +{ + // Check sanity of incoming settings + JPH_ASSERT(inSettings.mUp.IsNormalized()); + JPH_ASSERT(inSettings.mForward.IsNormalized()); + JPH_ASSERT(!inSettings.mWheels.empty()); + + // Store max pitch/roll angle + SetMaxPitchRollAngle(inSettings.mMaxPitchRollAngle); + + // Copy anti-rollbar settings + mAntiRollBars.resize(inSettings.mAntiRollBars.size()); + for (uint i = 0; i < mAntiRollBars.size(); ++i) + { + const VehicleAntiRollBar &r = inSettings.mAntiRollBars[i]; + mAntiRollBars[i] = r; + JPH_ASSERT(r.mStiffness >= 0.0f); + } + + // Construct our controller class + mController = inSettings.mController->ConstructController(*this); + + // Create wheels + mWheels.resize(inSettings.mWheels.size()); + for (uint i = 0; i < mWheels.size(); ++i) + mWheels[i] = mController->ConstructWheel(*inSettings.mWheels[i]); + + // Use the body ID as a seed for the step counter so that not all vehicles will update at the same time + mCurrentStep = uint32(Hash64(inVehicleBody.GetID().GetIndex())); +} + +VehicleConstraint::~VehicleConstraint() +{ + // Destroy controller + delete mController; + + // Destroy our wheels + for (Wheel *w : mWheels) + delete w; +} + +void VehicleConstraint::GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const +{ + const WheelSettings *settings = inWheel->mSettings; + + Quat steer_rotation = Quat::sRotation(settings->mSteeringAxis, inWheel->mSteerAngle); + outUp = steer_rotation * settings->mWheelUp; + outForward = steer_rotation * settings->mWheelForward; + outRight = outForward.Cross(outUp).Normalized(); + outForward = outUp.Cross(outRight).Normalized(); +} + +Mat44 VehicleConstraint::GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const +{ + JPH_ASSERT(inWheelIndex < mWheels.size()); + + const Wheel *wheel = mWheels[inWheelIndex]; + const WheelSettings *settings = wheel->mSettings; + + // Use the two vectors provided to calculate a matrix that takes us from wheel model space to X = right, Y = up, Z = forward (the space where we will rotate the wheel) + Mat44 wheel_to_rotational = Mat44(Vec4(inWheelRight, 0), Vec4(inWheelUp, 0), Vec4(inWheelUp.Cross(inWheelRight), 0), Vec4(0, 0, 0, 1)).Transposed(); + + // Calculate the matrix that takes us from the rotational space to vehicle local space + Vec3 local_forward, local_up, local_right; + GetWheelLocalBasis(wheel, local_forward, local_up, local_right); + Vec3 local_wheel_pos = settings->mPosition + settings->mSuspensionDirection * wheel->mSuspensionLength; + Mat44 rotational_to_local(Vec4(local_right, 0), Vec4(local_up, 0), Vec4(local_forward, 0), Vec4(local_wheel_pos, 1)); + + // Calculate transform of rotated wheel + return rotational_to_local * Mat44::sRotationX(wheel->mAngle) * wheel_to_rotational; +} + +RMat44 VehicleConstraint::GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const +{ + return mBody->GetWorldTransform() * GetWheelLocalTransform(inWheelIndex, inWheelRight, inWheelUp); +} + +void VehicleConstraint::OnStep(const PhysicsStepListenerContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + // Callback to higher-level systems. We do it before PreCollide, in case steering changes. + if (mPreStepCallback != nullptr) + mPreStepCallback(*this, inContext); + + if (mIsGravityOverridden) + { + // If gravity is overridden, we replace the normal gravity calculations + if (mBody->IsActive()) + { + MotionProperties *mp = mBody->GetMotionProperties(); + mp->SetGravityFactor(0.0f); + mBody->AddForce(mGravityOverride / mp->GetInverseMass()); + } + + // And we calculate the world up using the custom gravity + mWorldUp = (-mGravityOverride).NormalizedOr(mWorldUp); + } + else + { + // Calculate new world up vector by inverting gravity + mWorldUp = (-inContext.mPhysicsSystem->GetGravity()).NormalizedOr(mWorldUp); + } + + // Callback on our controller + mController->PreCollide(inContext.mDeltaTime, *inContext.mPhysicsSystem); + + // Calculate if this constraint is active by checking if our main vehicle body is active or any of the bodies we touch are active + mIsActive = mBody->IsActive(); + + // Test how often we need to update the wheels + uint num_steps_between_collisions = mIsActive? mNumStepsBetweenCollisionTestActive : mNumStepsBetweenCollisionTestInactive; + + RMat44 body_transform = mBody->GetWorldTransform(); + + // Test collision for wheels + for (uint wheel_index = 0; wheel_index < mWheels.size(); ++wheel_index) + { + Wheel *w = mWheels[wheel_index]; + const WheelSettings *settings = w->mSettings; + + // Calculate suspension origin and direction + RVec3 ws_origin = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Test if we need to update this wheel + if (num_steps_between_collisions == 0 + || (mCurrentStep + wheel_index) % num_steps_between_collisions != 0) + { + // Simplified wheel contact test + if (!w->mContactBodyID.IsInvalid()) + { + // Test if the body is still valid + w->mContactBody = inContext.mPhysicsSystem->GetBodyLockInterfaceNoLock().TryGetBody(w->mContactBodyID); + if (w->mContactBody == nullptr) + { + // It's not, forget the contact + w->mContactBodyID = BodyID(); + w->mContactSubShapeID = SubShapeID(); + w->mSuspensionLength = settings->mSuspensionMaxLength; + } + else + { + // Extrapolate the wheel contact properties + mVehicleCollisionTester->PredictContactProperties(*inContext.mPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength); + } + } + } + else + { + // Full wheel contact test, start by resetting the contact data + w->mContactBodyID = BodyID(); + w->mContactBody = nullptr; + w->mContactSubShapeID = SubShapeID(); + w->mSuspensionLength = settings->mSuspensionMaxLength; + + // Test collision to find the floor + if (mVehicleCollisionTester->Collide(*inContext.mPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength)) + { + // Store ID (pointer is not valid outside of the simulation step) + w->mContactBodyID = w->mContactBody->GetID(); + } + } + + if (w->mContactBody != nullptr) + { + // Store contact velocity, cache this as the contact body may be removed + w->mContactPointVelocity = w->mContactBody->GetPointVelocity(w->mContactPosition); + + // Determine plane constant for axle contact plane + w->mAxlePlaneConstant = RVec3(w->mContactNormal).Dot(ws_origin + w->mSuspensionLength * ws_direction); + + // Check if body is active, if so the entire vehicle should be active + mIsActive |= w->mContactBody->IsActive(); + + // Determine world space forward using steering angle and body rotation + Vec3 forward, up, right; + GetWheelLocalBasis(w, forward, up, right); + forward = body_transform.Multiply3x3(forward); + right = body_transform.Multiply3x3(right); + + // The longitudinal axis is in the up/forward plane + w->mContactLongitudinal = w->mContactNormal.Cross(right); + + // Make sure that the longitudinal axis is aligned with the forward axis + if (w->mContactLongitudinal.Dot(forward) < 0.0f) + w->mContactLongitudinal = -w->mContactLongitudinal; + + // Normalize it + w->mContactLongitudinal = w->mContactLongitudinal.NormalizedOr(w->mContactNormal.GetNormalizedPerpendicular()); + + // The lateral axis is perpendicular to contact normal and longitudinal axis + w->mContactLateral = w->mContactLongitudinal.Cross(w->mContactNormal).Normalized(); + } + } + + // Callback to higher-level systems. We do it immediately after wheel collision. + if (mPostCollideCallback != nullptr) + mPostCollideCallback(*this, inContext); + + // Calculate anti-rollbar impulses + for (const VehicleAntiRollBar &r : mAntiRollBars) + { + Wheel *lw = mWheels[r.mLeftWheel]; + Wheel *rw = mWheels[r.mRightWheel]; + + if (lw->mContactBody != nullptr && rw->mContactBody != nullptr) + { + // Calculate the impulse to apply based on the difference in suspension length + float difference = rw->mSuspensionLength - lw->mSuspensionLength; + float impulse = difference * r.mStiffness * inContext.mDeltaTime; + lw->mAntiRollBarImpulse = -impulse; + rw->mAntiRollBarImpulse = impulse; + } + else + { + // When one of the wheels is not on the ground we don't apply any impulses + lw->mAntiRollBarImpulse = rw->mAntiRollBarImpulse = 0.0f; + } + } + + // Callback on our controller + mController->PostCollide(inContext.mDeltaTime, *inContext.mPhysicsSystem); + + // Callback to higher-level systems. We do it before the sleep section, in case velocities change. + if (mPostStepCallback != nullptr) + mPostStepCallback(*this, inContext); + + // If the wheels are rotating, we don't want to go to sleep yet + bool allow_sleep = mController->AllowSleep(); + if (allow_sleep) + for (const Wheel *w : mWheels) + if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f)) + { + allow_sleep = false; + break; + } + if (mBody->GetAllowSleeping() != allow_sleep) + mBody->SetAllowSleeping(allow_sleep); + + // Increment step counter + ++mCurrentStep; +} + +void VehicleConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + // Find dynamic bodies that our wheels are touching + BodyID *body_ids = (BodyID *)JPH_STACK_ALLOC((mWheels.size() + 1) * sizeof(BodyID)); + int num_bodies = 0; + bool needs_to_activate = false; + for (const Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + // Avoid adding duplicates + bool duplicate = false; + BodyID id = w->mContactBody->GetID(); + for (int i = 0; i < num_bodies; ++i) + if (body_ids[i] == id) + { + duplicate = true; + break; + } + if (duplicate) + continue; + + if (w->mContactBody->IsDynamic()) + { + body_ids[num_bodies++] = id; + needs_to_activate |= !w->mContactBody->IsActive(); + } + } + + // Activate bodies, note that if we get here we have already told the system that we're active so that means our main body needs to be active too + if (!mBody->IsActive()) + { + // Our main body is not active, activate it too + body_ids[num_bodies] = mBody->GetID(); + inBodyManager.ActivateBodies(body_ids, num_bodies + 1); + } + else if (needs_to_activate) + { + // Only activate bodies the wheels are touching + inBodyManager.ActivateBodies(body_ids, num_bodies); + } + + // Link the bodies into the same island + uint32 min_active_index = Body::cInactiveIndex; + for (int i = 0; i < num_bodies; ++i) + { + const Body &body = inBodyManager.GetBody(body_ids[i]); + min_active_index = min(min_active_index, body.GetIndexInActiveBodiesInternal()); + ioBuilder.LinkBodies(mBody->GetIndexInActiveBodiesInternal(), body.GetIndexInActiveBodiesInternal()); + } + + // Link the constraint in the island + ioBuilder.LinkConstraint(inConstraintIndex, mBody->GetIndexInActiveBodiesInternal(), min_active_index); +} + +uint VehicleConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const +{ + return ioSplitter.AssignToNonParallelSplit(mBody); +} + +void VehicleConstraint::CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const +{ + // Determine point to apply force to + RVec3 force_point; + if (inWheel.mSettings->mEnableSuspensionForcePoint) + force_point = mBody->GetWorldTransform() * inWheel.mSettings->mSuspensionForcePoint; + else + force_point = inWheel.mContactPosition; + + // Calculate r1 + u and r2 + outR1PlusU = Vec3(force_point - mBody->GetCenterOfMassPosition()); + outR2 = Vec3(force_point - inWheel.mContactBody->GetCenterOfMassPosition()); +} + +void VehicleConstraint::CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform) +{ + // Check if a limit was specified + if (mCosMaxPitchRollAngle > -1.0f) + { + // Calculate cos of angle between world up vector and vehicle up vector + Vec3 vehicle_up = inBodyTransform.Multiply3x3(mUp); + mCosPitchRollAngle = mWorldUp.Dot(vehicle_up); + if (mCosPitchRollAngle < mCosMaxPitchRollAngle) + { + // Calculate rotation axis to rotate vehicle towards up + Vec3 rotation_axis = mWorldUp.Cross(vehicle_up); + float len = rotation_axis.Length(); + if (len > 0.0f) + mPitchRollRotationAxis = rotation_axis / len; + + mPitchRollPart.CalculateConstraintProperties(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis); + } + else + mPitchRollPart.Deactivate(); + } + else + mPitchRollPart.Deactivate(); +} + +void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + RMat44 body_transform = mBody->GetWorldTransform(); + + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + const WheelSettings *settings = w->mSettings; + + Vec3 neg_contact_normal = -w->mContactNormal; + + Vec3 r1_plus_u, r2; + CalculateSuspensionForcePoint(*w, r1_plus_u, r2); + + // Suspension spring + if (settings->mSuspensionMaxLength > settings->mSuspensionMinLength) + { + float stiffness, damping; + if (settings->mSuspensionSpring.mMode == ESpringMode::FrequencyAndDamping) + { + // Calculate effective mass based on vehicle configuration (the stiffness of the spring should not be affected by the dynamics of the vehicle): K = 1 / (J M^-1 J^T) + // Note that if no suspension force point is supplied we don't know where the force is applied so we assume it is applied at average suspension length + Vec3 force_point = settings->mEnableSuspensionForcePoint? settings->mSuspensionForcePoint : settings->mPosition + 0.5f * (settings->mSuspensionMinLength + settings->mSuspensionMaxLength) * settings->mSuspensionDirection; + Vec3 force_point_x_neg_up = force_point.Cross(-mUp); + const MotionProperties *mp = mBody->GetMotionProperties(); + float effective_mass = 1.0f / (mp->GetInverseMass() + force_point_x_neg_up.Dot(mp->GetLocalSpaceInverseInertia().Multiply3x3(force_point_x_neg_up))); + + // Convert frequency and damping to stiffness and damping + float omega = 2.0f * JPH_PI * settings->mSuspensionSpring.mFrequency; + stiffness = effective_mass * Square(omega); + damping = 2.0f * effective_mass * settings->mSuspensionSpring.mDamping * omega; + } + else + { + // In this case we can simply copy the properties + stiffness = settings->mSuspensionSpring.mStiffness; + damping = settings->mSuspensionSpring.mDamping; + } + + // Calculate the damping and frequency of the suspension spring given the angle between the suspension direction and the contact normal + // If the angle between the suspension direction and the inverse of the contact normal is alpha then the force on the spring relates to the force along the contact normal as: + // + // Fspring = Fnormal * cos(alpha) + // + // The spring force is: + // + // Fspring = -k * x + // + // where k is the spring constant and x is the displacement of the spring. So we have: + // + // Fnormal * cos(alpha) = -k * x <=> Fnormal = -k / cos(alpha) * x + // + // So we can see this as a spring with spring constant: + // + // k' = k / cos(alpha) + // + // In the same way the velocity relates like: + // + // Vspring = Vnormal * cos(alpha) + // + // Which results in the modified damping constant c: + // + // c' = c / cos(alpha) + // + // Note that we clamp 1 / cos(alpha) to the range [0.1, 1] in order not to increase the stiffness / damping by too much. + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + float cos_angle = max(0.1f, ws_direction.Dot(neg_contact_normal)); + stiffness /= cos_angle; + damping /= cos_angle; + + // Get the value of the constraint equation + float c = w->mSuspensionLength - settings->mSuspensionMaxLength - settings->mSuspensionPreloadLength; + + w->mSuspensionPart.CalculateConstraintPropertiesWithStiffnessAndDamping(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, stiffness, damping); + } + else + w->mSuspensionPart.Deactivate(); + + // Check if we reached the 'max up' position and if so add a hard velocity constraint that stops any further movement in the normal direction + if (w->mSuspensionLength < settings->mSuspensionMinLength) + w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal); + else + w->mSuspensionMaxUpPart.Deactivate(); + + // Friction and propulsion + w->mLongitudinalPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLongitudinal); + w->mLateralPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLateral); + } + else + { + // No contact -> disable everything + w->mSuspensionPart.Deactivate(); + w->mSuspensionMaxUpPart.Deactivate(); + w->mLongitudinalPart.Deactivate(); + w->mLateralPart.Deactivate(); + } + + CalculatePitchRollConstraintProperties(body_transform); +} + +void VehicleConstraint::ResetWarmStart() +{ + for (Wheel *w : mWheels) + { + w->mSuspensionPart.Deactivate(); + w->mSuspensionMaxUpPart.Deactivate(); + w->mLongitudinalPart.Deactivate(); + w->mLateralPart.Deactivate(); + } + + mPitchRollPart.Deactivate(); +} + +void VehicleConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + w->mSuspensionPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio); + w->mSuspensionMaxUpPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio); + w->mLongitudinalPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLongitudinal, 0.0f); // Don't warm start the longitudinal part (the engine/brake force, we don't want to preserve anything from the last frame) + w->mLateralPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLateral, inWarmStartImpulseRatio); + } + + mPitchRollPart.WarmStart(*mBody, Body::sFixedToWorld, inWarmStartImpulseRatio); +} + +bool VehicleConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve suspension + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + // Suspension spring, note that it can only push and not pull + if (w->mSuspensionPart.IsActive()) + impulse |= w->mSuspensionPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX); + + // When reaching the minimal suspension length only allow forces pushing the bodies away + if (w->mSuspensionMaxUpPart.IsActive()) + impulse |= w->mSuspensionMaxUpPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX); + } + + // Solve the horizontal movement of the vehicle + impulse |= mController->SolveLongitudinalAndLateralConstraints(inDeltaTime); + + // Apply the pitch / roll constraint to avoid the vehicle from toppling over + if (mPitchRollPart.IsActive()) + impulse |= mPitchRollPart.SolveVelocityConstraint(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis, 0, FLT_MAX); + + return impulse; +} + +bool VehicleConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + RMat44 body_transform = mBody->GetWorldTransform(); + + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + const WheelSettings *settings = w->mSettings; + + // Check if we reached the 'max up' position now that the body has possibly moved + // We do this by calculating the axle position at minimum suspension length and making sure it does not go through the + // plane defined by the contact normal and the axle position when the contact happened + // TODO: This assumes that only the vehicle moved and not the ground as we kept the axle contact plane in world space + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + RVec3 ws_position = body_transform * settings->mPosition; + RVec3 min_suspension_pos = ws_position + settings->mSuspensionMinLength * ws_direction; + float max_up_error = float(RVec3(w->mContactNormal).Dot(min_suspension_pos) - w->mAxlePlaneConstant); + if (max_up_error < 0.0f) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + // Recalculate constraint properties since the body may have moved + Vec3 r1_plus_u, r2; + CalculateSuspensionForcePoint(*w, r1_plus_u, r2); + w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal); + + impulse |= w->mSuspensionMaxUpPart.SolvePositionConstraint(*mBody, *w->mContactBody, neg_contact_normal, max_up_error, inBaumgarte); + } + } + + // Apply the pitch / roll constraint to avoid the vehicle from toppling over + CalculatePitchRollConstraintProperties(body_transform); + if (mPitchRollPart.IsActive()) + impulse |= mPitchRollPart.SolvePositionConstraint(*mBody, Body::sFixedToWorld, mCosPitchRollAngle - mCosMaxPitchRollAngle, inBaumgarte); + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void VehicleConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + mController->Draw(inRenderer); +} + +void VehicleConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ +} + +#endif // JPH_DEBUG_RENDERER + +void VehicleConstraint::SaveState(StateRecorder &inStream) const +{ + Constraint::SaveState(inStream); + + mController->SaveState(inStream); + + for (const Wheel *w : mWheels) + { + inStream.Write(w->mAngularVelocity); + inStream.Write(w->mAngle); + inStream.Write(w->mContactBodyID); // Used by MotorcycleController::PreCollide + inStream.Write(w->mContactPosition); // Used by VehicleCollisionTester::PredictContactProperties + inStream.Write(w->mContactNormal); // Used by MotorcycleController::PreCollide + inStream.Write(w->mContactLateral); // Used by MotorcycleController::PreCollide + inStream.Write(w->mSuspensionLength); // Used by VehicleCollisionTester::PredictContactProperties + + w->mSuspensionPart.SaveState(inStream); + w->mSuspensionMaxUpPart.SaveState(inStream); + w->mLongitudinalPart.SaveState(inStream); + w->mLateralPart.SaveState(inStream); + } + + inStream.Write(mPitchRollRotationAxis); // When rotation is too small we use last frame so we need to store it + mPitchRollPart.SaveState(inStream); + inStream.Write(mCurrentStep); +} + +void VehicleConstraint::RestoreState(StateRecorder &inStream) +{ + Constraint::RestoreState(inStream); + + mController->RestoreState(inStream); + + for (Wheel *w : mWheels) + { + inStream.Read(w->mAngularVelocity); + inStream.Read(w->mAngle); + inStream.Read(w->mContactBodyID); + inStream.Read(w->mContactPosition); + inStream.Read(w->mContactNormal); + inStream.Read(w->mContactLateral); + inStream.Read(w->mSuspensionLength); + w->mContactBody = nullptr; // No longer valid + + w->mSuspensionPart.RestoreState(inStream); + w->mSuspensionMaxUpPart.RestoreState(inStream); + w->mLongitudinalPart.RestoreState(inStream); + w->mLateralPart.RestoreState(inStream); + } + + inStream.Read(mPitchRollRotationAxis); + mPitchRollPart.RestoreState(inStream); + inStream.Read(mCurrentStep); +} + +Ref VehicleConstraint::GetConstraintSettings() const +{ + JPH_ASSERT(false); // Not implemented yet + return nullptr; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h new file mode 100644 index 000000000000..9459f8e43737 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// Configuration for constraint that simulates a wheeled vehicle. +/// +/// The properties in this constraint are largely based on "Car Physics for Games" by Marco Monster. +/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html +class JPH_EXPORT VehicleConstraintSettings : public ConstraintSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, VehicleConstraintSettings) + +public: + /// Saves the contents of the constraint settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const override; + + Vec3 mUp { 0, 1, 0 }; ///< Vector indicating the up direction of the vehicle (in local space to the body) + Vec3 mForward { 0, 0, 1 }; ///< Vector indicating forward direction of the vehicle (in local space to the body) + float mMaxPitchRollAngle = JPH_PI; ///< Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off. + Array> mWheels; ///< List of wheels and their properties + Array mAntiRollBars; ///< List of anti rollbars and their properties + Ref mController; ///< Defines how the vehicle can accelerate / decelerate + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// Constraint that simulates a vehicle +/// Note: Don't forget to register the constraint as a StepListener with the PhysicsSystem! +/// +/// When the vehicle drives over very light objects (rubble) you may see the car body dip down. This is a known issue and is an artifact of the iterative solver that Jolt is using. +/// Basically if a light object is sandwiched between two heavy objects (the static floor and the car body), the light object is not able to transfer enough force from the ground to +/// the car body to keep the car body up. You can see this effect in the HeavyOnLightTest sample, the boxes on the right have a lot of penetration because they're on top of light objects. +/// +/// There are a couple of ways to improve this: +/// +/// 1. You can increase the number of velocity steps (global settings PhysicsSettings::mNumVelocitySteps or if you only want to increase it on +/// the vehicle you can use VehicleConstraintSettings::mNumVelocityStepsOverride). E.g. going from 10 to 30 steps in the HeavyOnLightTest sample makes the penetration a lot less. +/// The number of position steps can also be increased (the first prevents the body from going down, the second corrects it if the problem did +/// occur which inevitably happens due to numerical drift). This solution costs CPU cycles. +/// +/// 2. You can reduce the mass difference between the vehicle body and the rubble on the floor (by making the rubble heavier or the car lighter). +/// +/// 3. You could filter out collisions between the vehicle collision test and the rubble completely. This would make the wheels ignore the rubble but would cause the vehicle to drive +/// through it as if nothing happened. You could create fake wheels (keyframed bodies) that move along with the vehicle and that only collide with rubble (and not the vehicle or the ground). +/// This would cause the vehicle to push away the rubble without the rubble being able to affect the vehicle (unless it hits the main body of course). +/// +/// Note that when driving over rubble, you may see the wheel jump up and down quite quickly because one frame a collision is found and the next frame not. +/// To alleviate this, it may be needed to smooth the motion of the visual mesh for the wheel. +class JPH_EXPORT VehicleConstraint : public Constraint, public PhysicsStepListener +{ +public: + /// Constructor / destructor + VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings); + virtual ~VehicleConstraint() override; + + /// Get the type of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Vehicle; } + + /// Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off. + void SetMaxPitchRollAngle(float inMaxPitchRollAngle) { mCosMaxPitchRollAngle = Cos(inMaxPitchRollAngle); } + + /// Set the interface that tests collision between wheel and ground + void SetVehicleCollisionTester(const VehicleCollisionTester *inTester) { mVehicleCollisionTester = inTester; } + + /// Callback function to combine the friction of a tire with the friction of the body it is colliding with. + /// On input ioLongitudinalFriction and ioLateralFriction contain the friction of the tire, on output they should contain the combined friction with inBody2. + using CombineFunction = function; + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(const CombineFunction &inCombineFriction) { mCombineFriction = inCombineFriction; } + const CombineFunction & GetCombineFriction() const { return mCombineFriction; } + + /// Callback function to notify of current stage in PhysicsStepListener::OnStep. + using StepCallback = function; + + /// Callback function to notify that PhysicsStepListener::OnStep has started for this vehicle. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. control steering. This is the last moment that the position/orientation of the vehicle can be changed. + /// Wheel collision checks have not been performed yet. + const StepCallback & GetPreStepCallback() const { return mPreStepCallback; } + void SetPreStepCallback(const StepCallback &inPreStepCallback) { mPreStepCallback = inPreStepCallback; } + + /// Callback function to notify that PhysicsStepListener::OnStep has just completed wheel collision checks. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. detect tire contact or to modify the velocity of the vehicle based on the wheel contacts. + /// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed. + const StepCallback & GetPostCollideCallback() const { return mPostCollideCallback; } + void SetPostCollideCallback(const StepCallback &inPostCollideCallback) { mPostCollideCallback = inPostCollideCallback; } + + /// Callback function to notify that PhysicsStepListener::OnStep has completed for this vehicle. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. control the vehicle in the air. + /// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed. + const StepCallback & GetPostStepCallback() const { return mPostStepCallback; } + void SetPostStepCallback(const StepCallback &inPostStepCallback) { mPostStepCallback = inPostStepCallback; } + + /// Override gravity for this vehicle. Note that overriding gravity will set the gravity factor of the vehicle body to 0 and apply gravity in the PhysicsStepListener instead. + void OverrideGravity(Vec3Arg inGravity) { mGravityOverride = inGravity; mIsGravityOverridden = true; } + bool IsGravityOverridden() const { return mIsGravityOverridden; } + Vec3 GetGravityOverride() const { return mGravityOverride; } + void ResetGravityOverride() { mIsGravityOverridden = false; mBody->GetMotionProperties()->SetGravityFactor(1.0f); } ///< Note that resetting the gravity override will restore the gravity factor of the vehicle body to 1. + + /// Get the local space forward vector of the vehicle + Vec3 GetLocalForward() const { return mForward; } + + /// Get the local space up vector of the vehicle + Vec3 GetLocalUp() const { return mUp; } + + /// Vector indicating the world space up direction (used to limit vehicle pitch/roll), calculated every frame by inverting gravity + Vec3 GetWorldUp() const { return mWorldUp; } + + /// Access to the vehicle body + Body * GetVehicleBody() const { return mBody; } + + /// Access to the vehicle controller interface (determines acceleration / deceleration) + const VehicleController * GetController() const { return mController; } + + /// Access to the vehicle controller interface (determines acceleration / deceleration) + VehicleController * GetController() { return mController; } + + /// Get the state of the wheels + const Wheels & GetWheels() const { return mWheels; } + + /// Get the state of a wheels (writable interface, allows you to make changes to the configuration which will take effect the next time step) + Wheels & GetWheels() { return mWheels; } + + /// Get the state of a wheel + Wheel * GetWheel(uint inIdx) { return mWheels[inIdx]; } + const Wheel * GetWheel(uint inIdx) const { return mWheels[inIdx]; } + + /// Get the basis vectors for the wheel in local space to the vehicle body (note: basis does not rotate when the wheel rotates around its axis) + /// @param inWheel Wheel to fetch basis for + /// @param outForward Forward vector for the wheel + /// @param outUp Up vector for the wheel + /// @param outRight Right vector for the wheel + void GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const; + + /// Get the transform of a wheel in local space to the vehicle body, returns a matrix that transforms a cylinder aligned with the Y axis in body space (not COM space) + /// @param inWheelIndex Index of the wheel to fetch + /// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels) + /// @param inWheelUp Unit vector that indicates up in model space of the wheel + Mat44 GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const; + + /// Get the transform of a wheel in world space, returns a matrix that transforms a cylinder aligned with the Y axis in world space + /// @param inWheelIndex Index of the wheel to fetch + /// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels) + /// @param inWheelUp Unit vector that indicates up in model space of the wheel + RMat44 GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const; + + /// Number of simulation steps between wheel collision tests when the vehicle is active. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc. + /// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion. + /// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle. + /// If you set this to test less than every step, you may see simulation artifacts. This setting can be used to reduce the cost of simulating vehicles in the distance. + void SetNumStepsBetweenCollisionTestActive(uint inSteps) { mNumStepsBetweenCollisionTestActive = inSteps; } + uint GetNumStepsBetweenCollisionTestActive() const { return mNumStepsBetweenCollisionTestActive; } + + /// Number of simulation steps between wheel collision tests when the vehicle is inactive. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc. + /// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion. + /// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle. + /// This number can be lower than the number of steps when the vehicle is active as the only purpose of this test is + /// to allow the vehicle to wake up in response to bodies moving into the wheels but not touching the body of the vehicle. + void SetNumStepsBetweenCollisionTestInactive(uint inSteps) { mNumStepsBetweenCollisionTestInactive = inSteps; } + uint GetNumStepsBetweenCollisionTestInactive() const { return mNumStepsBetweenCollisionTestInactive; } + + // Generic interface of a constraint + virtual bool IsActive() const override { return mIsActive && Constraint::IsActive(); } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override; + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + +private: + // See: PhysicsStepListener + virtual void OnStep(const PhysicsStepListenerContext &inContext) override; + + // Calculate the position where the suspension and traction forces should be applied in world space, relative to the center of mass of both bodies + void CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const; + + // Calculate the constraint properties for mPitchRollPart + void CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform); + + // Gravity override + bool mIsGravityOverridden = false; ///< If the gravity is currently overridden + Vec3 mGravityOverride = Vec3::sZero(); ///< Gravity override value, replaces PhysicsSystem::GetGravity() when mIsGravityOverridden is true + + // Simulation information + Body * mBody; ///< Body of the vehicle + Vec3 mForward; ///< Local space forward vector for the vehicle + Vec3 mUp; ///< Local space up vector for the vehicle + Vec3 mWorldUp; ///< Vector indicating the world space up direction (used to limit vehicle pitch/roll) + Wheels mWheels; ///< Wheel states of the vehicle + Array mAntiRollBars; ///< Anti rollbars of the vehicle + VehicleController * mController; ///< Controls the acceleration / deceleration of the vehicle + bool mIsActive = false; ///< If this constraint is active + uint mNumStepsBetweenCollisionTestActive = 1; ///< Number of simulation steps between wheel collision tests when the vehicle is active + uint mNumStepsBetweenCollisionTestInactive = 1; ///< Number of simulation steps between wheel collision tests when the vehicle is inactive + uint mCurrentStep = 0; ///< Current step number, used to determine when to test a wheel + + // Prevent vehicle from toppling over + float mCosMaxPitchRollAngle; ///< Cos of the max pitch/roll angle + float mCosPitchRollAngle; ///< Cos of the current pitch/roll angle + Vec3 mPitchRollRotationAxis { 0, 1, 0 }; ///< Current axis along which to apply torque to prevent the car from toppling over + AngleConstraintPart mPitchRollPart; ///< Constraint part that prevents the car from toppling over + + // Interfaces + RefConst mVehicleCollisionTester; ///< Class that performs testing of collision for the wheels + CombineFunction mCombineFriction = [](uint, float &ioLongitudinalFriction, float &ioLateralFriction, const Body &inBody2, const SubShapeID &) + { + float body_friction = inBody2.GetFriction(); + + ioLongitudinalFriction = sqrt(ioLongitudinalFriction * body_friction); + ioLateralFriction = sqrt(ioLateralFriction * body_friction); + }; + + // Callbacks + StepCallback mPreStepCallback; + StepCallback mPostCollideCallback; + StepCallback mPostStepCallback; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp new file mode 100644 index 000000000000..ffa8ff1a64b1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(VehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(VehicleControllerSettings, SerializableObject) +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h new file mode 100644 index 000000000000..0d9095b189a6 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h @@ -0,0 +1,84 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class VehicleController; +class VehicleConstraint; +class WheelSettings; +class Wheel; +class StateRecorder; + +/// Basic settings object for interface that controls acceleration / deceleration of the vehicle +class JPH_EXPORT VehicleControllerSettings : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, VehicleControllerSettings) + +public: + /// Saves the contents of the controller settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const = 0; + + /// Restore the contents of the controller settings in binary form from inStream. + virtual void RestoreBinaryState(StreamIn &inStream) = 0; + + /// Create an instance of the vehicle controller class + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const = 0; +}; + +/// Runtime data for interface that controls acceleration / deceleration of the vehicle +class JPH_EXPORT VehicleController : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / destructor + explicit VehicleController(VehicleConstraint &inConstraint) : mConstraint(inConstraint) { } + virtual ~VehicleController() = default; + + /// Access the vehicle constraint that this controller is part of + VehicleConstraint & GetConstraint() { return mConstraint; } + const VehicleConstraint & GetConstraint() const { return mConstraint; } + +protected: + // The functions below are only for the VehicleConstraint + friend class VehicleConstraint; + + // Create a new instance of wheel + virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const = 0; + + // If the vehicle is allowed to go to sleep + virtual bool AllowSleep() const = 0; + + // Called before the wheel probes have been done + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0; + + // Called after the wheel probes have been done + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0; + + // Solve longitudinal and lateral constraint parts for all of the wheels + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) = 0; + + // Saving state for replay + virtual void SaveState(StateRecorder &inStream) const = 0; + virtual void RestoreState(StateRecorder &inStream) = 0; + +#ifdef JPH_DEBUG_RENDERER + // Drawing interface + virtual void Draw(DebugRenderer *inRenderer) const = 0; +#endif // JPH_DEBUG_RENDERER + + VehicleConstraint & mConstraint; ///< The vehicle constraint we belong to +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp new file mode 100644 index 000000000000..ef7cf4cb914c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp @@ -0,0 +1,81 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleDifferentialSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftWheel) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mRightWheel) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mDifferentialRatio) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftRightSplit) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLimitedSlipRatio) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mEngineTorqueRatio) +} + +void VehicleDifferentialSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLeftWheel); + inStream.Write(mRightWheel); + inStream.Write(mDifferentialRatio); + inStream.Write(mLeftRightSplit); + inStream.Write(mLimitedSlipRatio); + inStream.Write(mEngineTorqueRatio); +} + +void VehicleDifferentialSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLeftWheel); + inStream.Read(mRightWheel); + inStream.Read(mDifferentialRatio); + inStream.Read(mLeftRightSplit); + inStream.Read(mLimitedSlipRatio); + inStream.Read(mEngineTorqueRatio); +} + +void VehicleDifferentialSettings::CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const +{ + // Start with the default torque ratio + outLeftTorqueFraction = 1.0f - mLeftRightSplit; + outRightTorqueFraction = mLeftRightSplit; + + if (mLimitedSlipRatio < FLT_MAX) + { + JPH_ASSERT(mLimitedSlipRatio > 1.0f); + + // This is a limited slip differential, adjust torque ratios according to wheel speeds + float omega_l = max(1.0e-3f, abs(inLeftAngularVelocity)); // prevent div by zero by setting a minimum velocity and ignoring that the wheels may be rotating in different directions + float omega_r = max(1.0e-3f, abs(inRightAngularVelocity)); + float omega_min = min(omega_l, omega_r); + float omega_max = max(omega_l, omega_r); + + // Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mLimitedSlipRotationRatio + float alpha = min((omega_max / omega_min - 1.0f) / (mLimitedSlipRatio - 1.0f), 1.0f); + JPH_ASSERT(alpha >= 0.0f); + float one_min_alpha = 1.0f - alpha; + + if (omega_l < omega_r) + { + // Redirect more power to the left wheel + outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha + alpha; + outRightTorqueFraction = outRightTorqueFraction * one_min_alpha; + } + else + { + // Redirect more power to the right wheel + outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha; + outRightTorqueFraction = outRightTorqueFraction * one_min_alpha + alpha; + } + } + + // Assert the values add up to 1 + JPH_ASSERT(abs(outLeftTorqueFraction + outRightTorqueFraction - 1.0f) < 1.0e-6f); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h new file mode 100644 index 000000000000..304337171700 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h @@ -0,0 +1,39 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class JPH_EXPORT VehicleDifferentialSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleDifferentialSettings) + +public: + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Calculate the torque ratio between left and right wheel + /// @param inLeftAngularVelocity Angular velocity of left wheel (rad / s) + /// @param inRightAngularVelocity Angular velocity of right wheel (rad / s) + /// @param outLeftTorqueFraction Fraction of torque that should go to the left wheel + /// @param outRightTorqueFraction Fraction of torque that should go to the right wheel + void CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const; + + int mLeftWheel = -1; ///< Index (in mWheels) that represents the left wheel of this differential (can be -1 to indicate no wheel) + int mRightWheel = -1; ///< Index (in mWheels) that represents the right wheel of this differential (can be -1 to indicate no wheel) + float mDifferentialRatio = 3.42f; ///< Ratio between rotation speed of gear box and wheels + float mLeftRightSplit = 0.5f; ///< Defines how the engine torque is split across the left and right wheel (0 = left, 0.5 = center, 1 = right) + float mLimitedSlipRatio = 1.4f; ///< Ratio max / min wheel speed. When this ratio is exceeded, all torque gets distributed to the slowest moving wheel. This allows implementing a limited slip differential. Set to FLT_MAX for an open differential. Value should be > 1. + float mEngineTorqueRatio = 1.0f; ///< How much of the engines torque is applied to this differential (0 = none, 1 = full), make sure the sum of all differentials is 1. +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp new file mode 100644 index 000000000000..1352cc0d51ff --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleEngineSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxTorque) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMinRPM) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxRPM) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mNormalizedTorque) +} + +VehicleEngineSettings::VehicleEngineSettings() +{ + mNormalizedTorque.Reserve(3); + mNormalizedTorque.AddPoint(0.0f, 0.8f); + mNormalizedTorque.AddPoint(0.66f, 1.0f); + mNormalizedTorque.AddPoint(1.0f, 0.8f); +} + +void VehicleEngineSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMaxTorque); + inStream.Write(mMinRPM); + inStream.Write(mMaxRPM); + mNormalizedTorque.SaveBinaryState(inStream); +} + +void VehicleEngineSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMaxTorque); + inStream.Read(mMinRPM); + inStream.Read(mMaxRPM); + mNormalizedTorque.RestoreBinaryState(inStream); +} + +void VehicleEngine::ApplyTorque(float inTorque, float inDeltaTime) +{ + // Accelerate engine using torque + mCurrentRPM += cAngularVelocityToRPM * inTorque * inDeltaTime / mInertia; + ClampRPM(); +} + +void VehicleEngine::ApplyDamping(float inDeltaTime) +{ + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mCurrentRPM *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime); + ClampRPM(); +} + +#ifdef JPH_DEBUG_RENDERER + +void VehicleEngine::DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const +{ + // Function to draw part of a pie + auto draw_pie = [this, inRenderer, inSize, inPosition, inForward, inUp](float inMinRPM, float inMaxRPM, Color inColor) { + inRenderer->DrawPie(inPosition, inSize, inForward, inUp, ConvertRPMToAngle(inMinRPM), ConvertRPMToAngle(inMaxRPM), inColor, DebugRenderer::ECastShadow::Off); + }; + + // Draw segment under min RPM + draw_pie(0, mMinRPM, Color::sGrey); + + // Draw segment until inShiftDownRPM + if (mCurrentRPM < inShiftDownRPM) + { + draw_pie(mMinRPM, mCurrentRPM, Color::sRed); + draw_pie(mCurrentRPM, inShiftDownRPM, Color::sDarkRed); + } + else + { + draw_pie(mMinRPM, inShiftDownRPM, Color::sRed); + } + + // Draw segment between inShiftDownRPM and inShiftUpRPM + if (mCurrentRPM > inShiftDownRPM && mCurrentRPM < inShiftUpRPM) + { + draw_pie(inShiftDownRPM, mCurrentRPM, Color::sOrange); + draw_pie(mCurrentRPM, inShiftUpRPM, Color::sDarkOrange); + } + else + { + draw_pie(inShiftDownRPM, inShiftUpRPM, mCurrentRPM <= inShiftDownRPM? Color::sDarkOrange : Color::sOrange); + } + + // Draw segment above inShiftUpRPM + if (mCurrentRPM > inShiftUpRPM) + { + draw_pie(inShiftUpRPM, mCurrentRPM, Color::sGreen); + draw_pie(mCurrentRPM, mMaxRPM, Color::sDarkGreen); + } + else + { + draw_pie(inShiftUpRPM, mMaxRPM, Color::sDarkGreen); + } +} + +#endif // JPH_DEBUG_RENDERER + +void VehicleEngine::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mCurrentRPM); +} + +void VehicleEngine::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mCurrentRPM); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h new file mode 100644 index 000000000000..29212ffdab67 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h @@ -0,0 +1,93 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER + class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Generic properties for a vehicle engine +class JPH_EXPORT VehicleEngineSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleEngineSettings) + +public: + /// Constructor + VehicleEngineSettings(); + + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + float mMaxTorque = 500.0f; ///< Max amount of torque (Nm) that the engine can deliver + float mMinRPM = 1000.0f; ///< Min amount of revolutions per minute (rpm) the engine can produce without stalling + float mMaxRPM = 6000.0f; ///< Max amount of revolutions per minute (rpm) the engine can generate + LinearCurve mNormalizedTorque; ///< Y-axis: Curve that describes a ratio of the max torque the engine can produce (0 = 0, 1 = mMaxTorque). X-axis: the fraction of the RPM of the engine (0 = mMinRPM, 1 = mMaxRPM) + float mInertia = 0.5f; ///< Moment of inertia (kg m^2) of the engine + float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w +}; + +/// Runtime data for engine +class JPH_EXPORT VehicleEngine : public VehicleEngineSettings +{ +public: + /// Multiply an angular velocity (rad/s) with this value to get rounds per minute (RPM) + static constexpr float cAngularVelocityToRPM = 60.0f / (2.0f * JPH_PI); + + /// Clamp the RPM between min and max RPM + inline void ClampRPM() { mCurrentRPM = Clamp(mCurrentRPM, mMinRPM, mMaxRPM); } + + /// Current rotation speed of engine in rounds per minute + float GetCurrentRPM() const { return mCurrentRPM; } + + /// Update rotation speed of engine in rounds per minute + void SetCurrentRPM(float inRPM) { mCurrentRPM = inRPM; ClampRPM(); } + + /// Get current angular velocity of the engine in radians / second + inline float GetAngularVelocity() const { return mCurrentRPM / cAngularVelocityToRPM; } + + /// Get the amount of torque (N m) that the engine can supply + /// @param inAcceleration How much the gas pedal is pressed [0, 1] + float GetTorque(float inAcceleration) const { return inAcceleration * mMaxTorque * mNormalizedTorque.GetValue(mCurrentRPM / mMaxRPM); } + + /// Apply a torque to the engine rotation speed + /// @param inTorque Torque in N m + /// @param inDeltaTime Delta time in seconds + void ApplyTorque(float inTorque, float inDeltaTime); + + /// Update the engine RPM for damping + /// @param inDeltaTime Delta time in seconds + void ApplyDamping(float inDeltaTime); + +#ifdef JPH_DEBUG_RENDERER + // Function that converts RPM to an angle in radians for debugging purposes + float ConvertRPMToAngle(float inRPM) const { return (-0.75f + 1.5f * inRPM / mMaxRPM) * JPH_PI; } + + /// Debug draw a RPM meter + void DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const; +#endif // JPH_DEBUG_RENDERER + + /// If the engine is idle we allow the vehicle to sleep + bool AllowSleep() const { return mCurrentRPM <= 1.01f * mMinRPM; } + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + +private: + float mCurrentRPM = mMinRPM; ///< Current rotation speed of engine in rounds per minute +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp new file mode 100644 index 000000000000..3bef2e804933 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTrackSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDrivenWheel) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mWheels) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mInertia) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mAngularDamping) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mMaxBrakeTorque) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDifferentialRatio) +} + +void VehicleTrackSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mDrivenWheel); + inStream.Write(mWheels); + inStream.Write(mInertia); + inStream.Write(mAngularDamping); + inStream.Write(mMaxBrakeTorque); + inStream.Write(mDifferentialRatio); +} + +void VehicleTrackSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mDrivenWheel); + inStream.Read(mWheels); + inStream.Read(mInertia); + inStream.Read(mAngularDamping); + inStream.Read(mMaxBrakeTorque); + inStream.Read(mDifferentialRatio); +} + +void VehicleTrack::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mAngularVelocity); +} + +void VehicleTrack::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mAngularVelocity); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h new file mode 100644 index 000000000000..dae4f9842e52 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// On which side of the vehicle the track is located (for steering) +enum class ETrackSide : uint +{ + Left = 0, + Right = 1, + Num = 2 +}; + +/// Generic properties for tank tracks +class JPH_EXPORT VehicleTrackSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTrackSettings) + +public: + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + uint mDrivenWheel; ///< Which wheel on the track is connected to the engine + Array mWheels; ///< Indices of wheels that are inside this track, should include the driven wheel too + float mInertia = 10.0f; ///< Moment of inertia (kg m^2) of the track and its wheels as seen on the driven wheel + float mAngularDamping = 0.5f; ///< Damping factor of track and its wheels: dw/dt = -c * w as seen on the driven wheel + float mMaxBrakeTorque = 15000.0f; ///< How much torque (Nm) the brakes can apply on the driven wheel + float mDifferentialRatio = 6.0f; ///< Ratio between rotation speed of gear box and driven wheel of track +}; + +/// Runtime data for tank tracks +class JPH_EXPORT VehicleTrack : public VehicleTrackSettings +{ +public: + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + float mAngularVelocity = 0.0f; ///< Angular velocity of the driven wheel, will determine the speed of the entire track +}; + +using VehicleTracks = VehicleTrack[(int)ETrackSide::Num]; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp new file mode 100644 index 000000000000..43336a537f03 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp @@ -0,0 +1,159 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTransmissionSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE(VehicleTransmissionSettings, mMode) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mGearRatios) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mReverseGearRatios) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchTime) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchReleaseTime) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchLatency) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftUpRPM) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftDownRPM) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchStrength) +} + +void VehicleTransmissionSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMode); + inStream.Write(mGearRatios); + inStream.Write(mReverseGearRatios); + inStream.Write(mSwitchTime); + inStream.Write(mClutchReleaseTime); + inStream.Write(mSwitchLatency); + inStream.Write(mShiftUpRPM); + inStream.Write(mShiftDownRPM); + inStream.Write(mClutchStrength); +} + +void VehicleTransmissionSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMode); + inStream.Read(mGearRatios); + inStream.Read(mReverseGearRatios); + inStream.Read(mSwitchTime); + inStream.Read(mClutchReleaseTime); + inStream.Read(mSwitchLatency); + inStream.Read(mShiftUpRPM); + inStream.Read(mShiftDownRPM); + inStream.Read(mClutchStrength); +} + +void VehicleTransmission::Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp) +{ + // Update current gear and calculate clutch friction + if (mMode == ETransmissionMode::Auto) + { + // Switch gears based on rpm + int old_gear = mCurrentGear; + if (mCurrentGear == 0 // In neutral + || inForwardInput * float(mCurrentGear) < 0.0f) // Changing between forward / reverse + { + // Switch to first gear or reverse depending on input + mCurrentGear = inForwardInput > 0.0f? 1 : (inForwardInput < 0.0f? -1 : 0); + } + else if (mGearSwitchLatencyTimeLeft == 0.0f) // If not in the timout after switching gears + { + if (inCanShiftUp && inCurrentRPM > mShiftUpRPM) + { + if (mCurrentGear < 0) + { + // Shift up, reverse + if (mCurrentGear > -(int)mReverseGearRatios.size()) + mCurrentGear--; + } + else + { + // Shift up, forward + if (mCurrentGear < (int)mGearRatios.size()) + mCurrentGear++; + } + } + else if (inCurrentRPM < mShiftDownRPM) + { + if (mCurrentGear < 0) + { + // Shift down, reverse + int max_gear = inForwardInput != 0.0f? -1 : 0; + if (mCurrentGear < max_gear) + mCurrentGear++; + } + else + { + // Shift down, forward + int min_gear = inForwardInput != 0.0f? 1 : 0; + if (mCurrentGear > min_gear) + mCurrentGear--; + } + } + } + + if (old_gear != mCurrentGear) + { + // We've shifted gear, start switch countdown + mGearSwitchTimeLeft = old_gear != 0? mSwitchTime : 0.0f; + mClutchReleaseTimeLeft = mClutchReleaseTime; + mGearSwitchLatencyTimeLeft = mSwitchLatency; + mClutchFriction = 0.0f; + } + else if (mGearSwitchTimeLeft > 0.0f) + { + // If still switching gears, count down + mGearSwitchTimeLeft = max(0.0f, mGearSwitchTimeLeft - inDeltaTime); + mClutchFriction = 0.0f; + } + else if (mClutchReleaseTimeLeft > 0.0f) + { + // After switching the gears we slowly release the clutch + mClutchReleaseTimeLeft = max(0.0f, mClutchReleaseTimeLeft - inDeltaTime); + mClutchFriction = 1.0f - mClutchReleaseTimeLeft / mClutchReleaseTime; + } + else + { + // Clutch has full friction + mClutchFriction = 1.0f; + + // Count down switch latency + mGearSwitchLatencyTimeLeft = max(0.0f, mGearSwitchLatencyTimeLeft - inDeltaTime); + } + } +} + +float VehicleTransmission::GetCurrentRatio() const +{ + if (mCurrentGear < 0) + return mReverseGearRatios[-mCurrentGear - 1]; + else if (mCurrentGear == 0) + return 0.0f; + else + return mGearRatios[mCurrentGear - 1]; +} + +void VehicleTransmission::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mCurrentGear); + inStream.Write(mClutchFriction); + inStream.Write(mGearSwitchTimeLeft); + inStream.Write(mClutchReleaseTimeLeft); + inStream.Write(mGearSwitchLatencyTimeLeft); +} + +void VehicleTransmission::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mCurrentGear); + inStream.Read(mClutchFriction); + inStream.Read(mGearSwitchTimeLeft); + inStream.Read(mClutchReleaseTimeLeft); + inStream.Read(mGearSwitchLatencyTimeLeft); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h new file mode 100644 index 000000000000..3360217abb1f --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// How gears are shifted +enum class ETransmissionMode : uint8 +{ + Auto, ///< Automatically shift gear up and down + Manual, ///< Manual gear shift (call SetTransmissionInput) +}; + +/// Configuration for the transmission of a vehicle (gear box) +class JPH_EXPORT VehicleTransmissionSettings +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTransmissionSettings) + +public: + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + ETransmissionMode mMode = ETransmissionMode::Auto; ///< How to switch gears + Array mGearRatios { 2.66f, 1.78f, 1.3f, 1.0f, 0.74f }; ///< Ratio in rotation rate between engine and gear box, first element is 1st gear, 2nd element 2nd gear etc. + Array mReverseGearRatios { -2.90f }; ///< Ratio in rotation rate between engine and gear box when driving in reverse + float mSwitchTime = 0.5f; ///< How long it takes to switch gears (s), only used in auto mode + float mClutchReleaseTime = 0.3f; ///< How long it takes to release the clutch (go to full friction), only used in auto mode + float mSwitchLatency = 0.5f; ///< How long to wait after releasing the clutch before another switch is attempted (s), only used in auto mode + float mShiftUpRPM = 4000.0f; ///< If RPM of engine is bigger then this we will shift a gear up, only used in auto mode + float mShiftDownRPM = 2000.0f; ///< If RPM of engine is smaller then this we will shift a gear down, only used in auto mode + float mClutchStrength = 10.0f; ///< Strength of the clutch when fully engaged. Total torque a clutch applies is Torque = ClutchStrength * (Velocity Engine - Avg Velocity Wheels At Clutch) (units: k m^2 s^-1) +}; + +/// Runtime data for transmission +class JPH_EXPORT VehicleTransmission : public VehicleTransmissionSettings +{ +public: + /// Set input from driver regarding the transmission (only relevant when transmission is set to manual mode) + /// @param inCurrentGear Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + /// @param inClutchFriction Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + void Set(int inCurrentGear, float inClutchFriction) { mCurrentGear = inCurrentGear; mClutchFriction = inClutchFriction; } + + /// Update the current gear and clutch friction if the transmission is in auto mode + /// @param inDeltaTime Time step delta time in s + /// @param inCurrentRPM Current RPM for engine + /// @param inForwardInput Hint if the user wants to drive forward (> 0) or backwards (< 0) + /// @param inCanShiftUp Indicates if we want to allow the transmission to shift up (e.g. pass false if wheels are slipping) + void Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp); + + /// Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + int GetCurrentGear() const { return mCurrentGear; } + + /// Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + float GetClutchFriction() const { return mClutchFriction; } + + /// If the auto box is currently switching gears + bool IsSwitchingGear() const { return mGearSwitchTimeLeft > 0.0f; } + + /// Return the transmission ratio based on the current gear (ratio between engine and differential) + float GetCurrentRatio() const; + + /// Only allow sleeping when the transmission is idle + bool AllowSleep() const { return mGearSwitchTimeLeft <= 0.0f && mClutchReleaseTimeLeft <= 0.0f && mGearSwitchLatencyTimeLeft <= 0.0f; } + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + +private: + int mCurrentGear = 0; ///< Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + float mClutchFriction = 1.0f; ///< Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + float mGearSwitchTimeLeft = 0.0f; ///< When switching gears this will be > 0 and will cause the engine to not provide any torque to the wheels for a short time (used for automatic gear switching only) + float mClutchReleaseTimeLeft = 0.0f; ///< After switching gears this will be > 0 and will cause the clutch friction to go from 0 to 1 (used for automatic gear switching only) + float mGearSwitchLatencyTimeLeft = 0.0f; ///< After releasing the clutch this will be > 0 and will prevent another gear switch (used for automatic gear switching only) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp new file mode 100644 index 000000000000..e43ffb8c9a42 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp @@ -0,0 +1,93 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettings) +{ + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionForcePoint) + JPH_ADD_ATTRIBUTE(WheelSettings, mPosition) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionDirection) + JPH_ADD_ATTRIBUTE(WheelSettings, mSteeringAxis) + JPH_ADD_ATTRIBUTE(WheelSettings, mWheelForward) + JPH_ADD_ATTRIBUTE(WheelSettings, mWheelUp) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMinLength) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMaxLength) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionPreloadLength) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mMode, "mSuspensionSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mFrequency, "mSuspensionFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mDamping, "mSuspensionDamping") + JPH_ADD_ATTRIBUTE(WheelSettings, mRadius) + JPH_ADD_ATTRIBUTE(WheelSettings, mWidth) + JPH_ADD_ATTRIBUTE(WheelSettings, mEnableSuspensionForcePoint) +} + +void WheelSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mSuspensionForcePoint); + inStream.Write(mPosition); + inStream.Write(mSuspensionDirection); + inStream.Write(mSteeringAxis); + inStream.Write(mWheelForward); + inStream.Write(mWheelUp); + inStream.Write(mSuspensionMinLength); + inStream.Write(mSuspensionMaxLength); + inStream.Write(mSuspensionPreloadLength); + mSuspensionSpring.SaveBinaryState(inStream); + inStream.Write(mRadius); + inStream.Write(mWidth); + inStream.Write(mEnableSuspensionForcePoint); +} + +void WheelSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mSuspensionForcePoint); + inStream.Read(mPosition); + inStream.Read(mSuspensionDirection); + inStream.Read(mSteeringAxis); + inStream.Read(mWheelForward); + inStream.Read(mWheelUp); + inStream.Read(mSuspensionMinLength); + inStream.Read(mSuspensionMaxLength); + inStream.Read(mSuspensionPreloadLength); + mSuspensionSpring.RestoreBinaryState(inStream); + inStream.Read(mRadius); + inStream.Read(mWidth); + inStream.Read(mEnableSuspensionForcePoint); +} + +Wheel::Wheel(const WheelSettings &inSettings) : + mSettings(&inSettings), + mSuspensionLength(inSettings.mSuspensionMaxLength) +{ + JPH_ASSERT(inSettings.mSuspensionDirection.IsNormalized()); + JPH_ASSERT(inSettings.mSteeringAxis.IsNormalized()); + JPH_ASSERT(inSettings.mWheelForward.IsNormalized()); + JPH_ASSERT(inSettings.mWheelUp.IsNormalized()); + JPH_ASSERT(inSettings.mSuspensionMinLength >= 0.0f); + JPH_ASSERT(inSettings.mSuspensionMaxLength >= inSettings.mSuspensionMinLength); + JPH_ASSERT(inSettings.mSuspensionPreloadLength >= 0.0f); + JPH_ASSERT(inSettings.mSuspensionSpring.mFrequency > 0.0f); + JPH_ASSERT(inSettings.mSuspensionSpring.mDamping >= 0.0f); + JPH_ASSERT(inSettings.mRadius > 0.0f); + JPH_ASSERT(inSettings.mWidth >= 0.0f); +} + +bool Wheel::SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse) +{ + return mLongitudinalPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLongitudinal, inMinImpulse, inMaxImpulse); +} + +bool Wheel::SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse) +{ + return mLateralPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLateral, inMinImpulse, inMaxImpulse); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h new file mode 100644 index 000000000000..0e92caa7a40d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class VehicleConstraint; + +/// Base class for wheel settings, each VehicleController can implement a derived class of this +class JPH_EXPORT WheelSettings : public SerializableObject, public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettings) + +public: + /// Saves the contents in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + virtual void RestoreBinaryState(StreamIn &inStream); + + Vec3 mPosition { 0, 0, 0 }; ///< Attachment point of wheel suspension in local space of the body + Vec3 mSuspensionForcePoint { 0, 0, 0 }; ///< Where tire forces (suspension and traction) are applied, in local space of the body. A good default is the center of the wheel in its neutral pose. See mEnableSuspensionForcePoint. + Vec3 mSuspensionDirection { 0, -1, 0 }; ///< Direction of the suspension in local space of the body, should point down + Vec3 mSteeringAxis { 0, 1, 0 }; ///< Direction of the steering axis in local space of the body, should point up (e.g. for a bike would be -mSuspensionDirection) + Vec3 mWheelUp { 0, 1, 0 }; ///< Up direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mUp but can be used to give the wheel camber or for a bike would be -mSuspensionDirection) + Vec3 mWheelForward { 0, 0, 1 }; ///< Forward direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mForward but can be used to give the wheel toe, does not need to be perpendicular to mWheelUp) + float mSuspensionMinLength = 0.3f; ///< How long the suspension is in max raised position relative to the attachment point (m) + float mSuspensionMaxLength = 0.5f; ///< How long the suspension is in max droop position relative to the attachment point (m) + float mSuspensionPreloadLength = 0.0f; ///< The natural length (m) of the suspension spring is defined as mSuspensionMaxLength + mSuspensionPreloadLength. Can be used to preload the suspension as the spring is compressed by mSuspensionPreloadLength when the suspension is in max droop position. Note that this means when the vehicle touches the ground there is a discontinuity so it will also make the vehicle more bouncy as we're updating with discrete time steps. + SpringSettings mSuspensionSpring { ESpringMode::FrequencyAndDamping, 1.5f, 0.5f }; ///< Settings for the suspension spring + float mRadius = 0.3f; ///< Radius of the wheel (m) + float mWidth = 0.1f; ///< Width of the wheel (m) + bool mEnableSuspensionForcePoint = false; ///< Enables mSuspensionForcePoint, if disabled, the forces are applied at the collision contact point. This leads to a more accurate simulation when interacting with dynamic objects but makes the vehicle less stable. When setting this to true, all forces will be applied to a fixed point on the vehicle body. +}; + +/// Base class for runtime data for a wheel, each VehicleController can implement a derived class of this +class JPH_EXPORT Wheel : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / destructor + explicit Wheel(const WheelSettings &inSettings); + virtual ~Wheel() = default; + + /// Get settings for the wheel + const WheelSettings * GetSettings() const { return mSettings; } + + /// Get the angular velocity (rad/s) for this wheel, note that positive means the wheel is rotating such that the car moves forward + float GetAngularVelocity() const { return mAngularVelocity; } + + /// Update the angular velocity (rad/s) + void SetAngularVelocity(float inVel) { mAngularVelocity = inVel; } + + /// Get the current rotation angle of the wheel in radians [0, 2 pi] + float GetRotationAngle() const { return mAngle; } + + /// Set the current rotation angle of the wheel in radians [0, 2 pi] + void SetRotationAngle(float inAngle) { mAngle = inAngle; } + + /// Get the current steer angle of the wheel in radians [-pi, pi], positive is to the left + float GetSteerAngle() const { return mSteerAngle; } + + /// Set the current steer angle of the wheel in radians [-pi, pi] + void SetSteerAngle(float inAngle) { mSteerAngle = inAngle; } + + /// Returns true if the wheel is touching an object + inline bool HasContact() const { return !mContactBodyID.IsInvalid(); } + + /// Returns the body ID of the body that this wheel is touching + BodyID GetContactBodyID() const { return mContactBodyID; } + + /// Returns the sub shape ID where we're contacting the body + SubShapeID GetContactSubShapeID() const { return mContactSubShapeID; } + + /// Returns the current contact position in world space (note by the time you call this the vehicle has moved) + RVec3 GetContactPosition() const { JPH_ASSERT(HasContact()); return mContactPosition; } + + /// Velocity of the contact point (m / s, not relative to the wheel but in world space) + Vec3 GetContactPointVelocity() const { JPH_ASSERT(HasContact()); return mContactPointVelocity; } + + /// Returns the current contact normal in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactNormal() const { JPH_ASSERT(HasContact()); return mContactNormal; } + + /// Returns longitudinal direction (direction along the wheel relative to floor) in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactLongitudinal() const { JPH_ASSERT(HasContact()); return mContactLongitudinal; } + + /// Returns lateral direction (sideways direction) in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactLateral() const { JPH_ASSERT(HasContact()); return mContactLateral; } + + /// Get the length of the suspension for a wheel (m) relative to the suspension attachment point (hard point) + float GetSuspensionLength() const { return mSuspensionLength; } + + /// Check if the suspension hit its upper limit + bool HasHitHardPoint() const { return mSuspensionMaxUpPart.IsActive(); } + + /// Get the total impulse (N s) that was applied by the suspension + float GetSuspensionLambda() const { return mSuspensionPart.GetTotalLambda() + mSuspensionMaxUpPart.GetTotalLambda(); } + + /// Get total impulse (N s) applied along the forward direction of the wheel + float GetLongitudinalLambda() const { return mLongitudinalPart.GetTotalLambda(); } + + /// Get total impulse (N s) applied along the sideways direction of the wheel + float GetLateralLambda() const { return mLateralPart.GetTotalLambda(); } + + /// Internal function that should only be called by the controller. Used to apply impulses in the forward direction of the vehicle. + bool SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse); + + /// Internal function that should only be called by the controller. Used to apply impulses in the sideways direction of the vehicle. + bool SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse); + +protected: + friend class VehicleConstraint; + + RefConst mSettings; ///< Configuration settings for this wheel + BodyID mContactBodyID; ///< ID of body for ground + SubShapeID mContactSubShapeID; ///< Sub shape ID for ground + Body * mContactBody = nullptr; ///< Body for ground + float mSuspensionLength; ///< Current length of the suspension + RVec3 mContactPosition; ///< Position of the contact point between wheel and ground + Vec3 mContactPointVelocity; ///< Velocity of the contact point (m / s, not relative to the wheel but in world space) + Vec3 mContactNormal; ///< Normal of the contact point between wheel and ground + Vec3 mContactLongitudinal; ///< Vector perpendicular to normal in the forward direction + Vec3 mContactLateral; ///< Vector perpendicular to normal and longitudinal direction in the right direction + Real mAxlePlaneConstant; ///< Constant for the contact plane of the axle, defined as ContactNormal . (WorldSpaceSuspensionPoint + SuspensionLength * WorldSpaceSuspensionDirection) + float mAntiRollBarImpulse = 0.0f; ///< Amount of impulse applied to the suspension from the anti-rollbars + + float mSteerAngle = 0.0f; ///< Rotation around the suspension direction, positive is to the left + float mAngularVelocity = 0.0f; ///< Rotation speed of wheel, positive when the wheels cause the vehicle to move forwards (rad/s) + float mAngle = 0.0f; ///< Current rotation of the wheel (rad, [0, 2 pi]) + + AxisConstraintPart mSuspensionPart; ///< Controls movement up/down along the contact normal + AxisConstraintPart mSuspensionMaxUpPart; ///< Adds a hard limit when reaching the minimal suspension length + AxisConstraintPart mLongitudinalPart; ///< Controls movement forward/backward + AxisConstraintPart mLateralPart; ///< Controls movement sideways (slip) +}; + +using Wheels = Array; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp new file mode 100644 index 000000000000..8e4a71a4fb85 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp @@ -0,0 +1,845 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +//#define JPH_TRACE_VEHICLE_STATS + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheeledVehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(WheeledVehicleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mEngine) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mTransmission) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentials) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentialLimitedSlipRatio) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsWV) +{ + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mInertia) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mAngularDamping) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxSteerAngle) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLongitudinalFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLateralFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxBrakeTorque) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxHandBrakeTorque) +} + +WheelSettingsWV::WheelSettingsWV() +{ + mLongitudinalFriction.Reserve(3); + mLongitudinalFriction.AddPoint(0.0f, 0.0f); + mLongitudinalFriction.AddPoint(0.06f, 1.2f); + mLongitudinalFriction.AddPoint(0.2f, 1.0f); + + mLateralFriction.Reserve(3); + mLateralFriction.AddPoint(0.0f, 0.0f); + mLateralFriction.AddPoint(3.0f, 1.2f); + mLateralFriction.AddPoint(20.0f, 1.0f); +} + +void WheelSettingsWV::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mInertia); + inStream.Write(mAngularDamping); + inStream.Write(mMaxSteerAngle); + mLongitudinalFriction.SaveBinaryState(inStream); + mLateralFriction.SaveBinaryState(inStream); + inStream.Write(mMaxBrakeTorque); + inStream.Write(mMaxHandBrakeTorque); +} + +void WheelSettingsWV::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mInertia); + inStream.Read(mAngularDamping); + inStream.Read(mMaxSteerAngle); + mLongitudinalFriction.RestoreBinaryState(inStream); + mLateralFriction.RestoreBinaryState(inStream); + inStream.Read(mMaxBrakeTorque); + inStream.Read(mMaxHandBrakeTorque); +} + +WheelWV::WheelWV(const WheelSettingsWV &inSettings) : + Wheel(inSettings) +{ + JPH_ASSERT(inSettings.mInertia >= 0.0f); + JPH_ASSERT(inSettings.mAngularDamping >= 0.0f); + JPH_ASSERT(abs(inSettings.mMaxSteerAngle) <= 0.5f * JPH_PI); + JPH_ASSERT(inSettings.mMaxBrakeTorque >= 0.0f); + JPH_ASSERT(inSettings.mMaxHandBrakeTorque >= 0.0f); +} + +void WheelWV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint) +{ + const WheelSettingsWV *settings = GetSettings(); + + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mAngularVelocity *= max(0.0f, 1.0f - settings->mAngularDamping * inDeltaTime); + + // Update rotation of wheel + mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI); + + if (mContactBody != nullptr) + { + const Body *body = inConstraint.GetVehicleBody(); + + // Calculate relative velocity between wheel contact point and floor + Vec3 relative_velocity = body->GetPointVelocity(mContactPosition) - mContactPointVelocity; + + // Cancel relative velocity in the normal plane + relative_velocity -= mContactNormal.Dot(relative_velocity) * mContactNormal; + float relative_longitudinal_velocity = relative_velocity.Dot(mContactLongitudinal); + + // Calculate longitudinal friction based on difference between velocity of rolling wheel and drive surface + float relative_longitudinal_velocity_denom = Sign(relative_longitudinal_velocity) * max(1.0e-3f, abs(relative_longitudinal_velocity)); // Ensure we don't divide by zero + mLongitudinalSlip = abs((mAngularVelocity * settings->mRadius - relative_longitudinal_velocity) / relative_longitudinal_velocity_denom); + float longitudinal_slip_friction = settings->mLongitudinalFriction.GetValue(mLongitudinalSlip); + + // Calculate lateral friction based on slip angle + float relative_velocity_len = relative_velocity.Length(); + mLateralSlip = relative_velocity_len < 1.0e-3f ? 0.0f : ACos(abs(relative_longitudinal_velocity) / relative_velocity_len); + float lateral_slip_angle = RadiansToDegrees(mLateralSlip); + float lateral_slip_friction = settings->mLateralFriction.GetValue(lateral_slip_angle); + + // Tire friction + VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction(); + mCombinedLongitudinalFriction = longitudinal_slip_friction; + mCombinedLateralFriction = lateral_slip_friction; + combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID); + } + else + { + // No collision + mLongitudinalSlip = 0.0f; + mLateralSlip = 0.0f; + mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f; + } +} + +VehicleController *WheeledVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new WheeledVehicleController(*this, inConstraint); +} + +void WheeledVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + mEngine.SaveBinaryState(inStream); + + mTransmission.SaveBinaryState(inStream); + + uint32 num_differentials = (uint32)mDifferentials.size(); + inStream.Write(num_differentials); + for (const VehicleDifferentialSettings &d : mDifferentials) + d.SaveBinaryState(inStream); + + inStream.Write(mDifferentialLimitedSlipRatio); +} + +void WheeledVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + mEngine.RestoreBinaryState(inStream); + + mTransmission.RestoreBinaryState(inStream); + + uint32 num_differentials = 0; + inStream.Read(num_differentials); + mDifferentials.resize(num_differentials); + for (VehicleDifferentialSettings &d : mDifferentials) + d.RestoreBinaryState(inStream); + + inStream.Read(mDifferentialLimitedSlipRatio); +} + +WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + VehicleController(inConstraint) +{ + // Copy engine settings + static_cast(mEngine) = inSettings.mEngine; + JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f); + JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM); + mEngine.SetCurrentRPM(mEngine.mMinRPM); + + // Copy transmission settings + static_cast(mTransmission) = inSettings.mTransmission; +#ifdef JPH_ENABLE_ASSERTS + for (float r : inSettings.mTransmission.mGearRatios) + JPH_ASSERT(r > 0.0f); + for (float r : inSettings.mTransmission.mReverseGearRatios) + JPH_ASSERT(r < 0.0f); +#endif // JPH_ENABLE_ASSERTS + JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f); + JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f); + JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM); + JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM); + JPH_ASSERT(inSettings.mTransmission.mClutchStrength > 0.0f); + + // Copy differential settings + mDifferentials.resize(inSettings.mDifferentials.size()); + for (uint i = 0; i < mDifferentials.size(); ++i) + { + const VehicleDifferentialSettings &d = inSettings.mDifferentials[i]; + mDifferentials[i] = d; + JPH_ASSERT(d.mDifferentialRatio > 0.0f); + JPH_ASSERT(d.mLeftRightSplit >= 0.0f && d.mLeftRightSplit <= 1.0f); + JPH_ASSERT(d.mEngineTorqueRatio >= 0.0f); + JPH_ASSERT(d.mLimitedSlipRatio > 1.0f); + } + + mDifferentialLimitedSlipRatio = inSettings.mDifferentialLimitedSlipRatio; + JPH_ASSERT(mDifferentialLimitedSlipRatio > 1.0f); +} + +float WheeledVehicleController::GetWheelSpeedAtClutch() const +{ + float wheel_speed_at_clutch = 0.0f; + int num_driven_wheels = 0; + for (const VehicleDifferentialSettings &d : mDifferentials) + { + int wheels[] = { d.mLeftWheel, d.mRightWheel }; + for (int w : wheels) + if (w >= 0) + { + wheel_speed_at_clutch += mConstraint.GetWheel(w)->GetAngularVelocity() * d.mDifferentialRatio; + num_driven_wheels++; + } + } + return wheel_speed_at_clutch / float(num_driven_wheels) * VehicleEngine::cAngularVelocityToRPM * mTransmission.GetCurrentRatio(); +} + +bool WheeledVehicleController::AllowSleep() const +{ + return mForwardInput == 0.0f // No user input + && mTransmission.AllowSleep() // Transmission is not shifting + && mEngine.AllowSleep(); // Engine is idling +} + +void WheeledVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + +#ifdef JPH_TRACE_VEHICLE_STATS + static bool sTracedHeader = false; + if (!sTracedHeader) + { + Trace("Time, ForwardInput, Gear, ClutchFriction, EngineRPM, WheelRPM, Velocity (km/h)"); + sTracedHeader = true; + } + static float sTime = 0.0f; + sTime += inDeltaTime; + Trace("%.3f, %.1f, %d, %.1f, %.1f, %.1f, %.1f", sTime, mForwardInput, mTransmission.GetCurrentGear(), mTransmission.GetClutchFriction(), mEngine.GetCurrentRPM(), GetWheelSpeedAtClutch(), mConstraint.GetVehicleBody()->GetLinearVelocity().Length() * 3.6f); +#endif // JPH_TRACE_VEHICLE_STATS + + for (Wheel *w_base : mConstraint.GetWheels()) + { + WheelWV *w = static_cast(w_base); + + // Set steering angle + w->SetSteerAngle(-mRightInput * w->GetSettings()->mMaxSteerAngle); + } +} + +void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + + // Remember old RPM so we can detect if we're increasing or decreasing + float old_engine_rpm = mEngine.GetCurrentRPM(); + + Wheels &wheels = mConstraint.GetWheels(); + + // Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again) + for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index) + { + WheelWV *w = static_cast(wheels[wheel_index]); + w->Update(wheel_index, inDeltaTime, mConstraint); + } + + // In auto transmission mode, don't accelerate the engine when switching gears + float forward_input = abs(mForwardInput); + if (mTransmission.mMode == ETransmissionMode::Auto) + forward_input *= mTransmission.GetClutchFriction(); + + // Apply engine damping + mEngine.ApplyDamping(inDeltaTime); + + // Calculate engine torque + float engine_torque = mEngine.GetTorque(forward_input); + + // Define a struct that contains information about driven differentials (i.e. that have wheels connected) + struct DrivenDifferential + { + const VehicleDifferentialSettings * mDifferential; + float mAngularVelocity; + float mClutchToDifferentialTorqueRatio; + float mTempTorqueFactor; + }; + + // Collect driven differentials and their speeds + Array driven_differentials; + driven_differentials.reserve(mDifferentials.size()); + float differential_omega_min = FLT_MAX, differential_omega_max = 0.0f; + for (const VehicleDifferentialSettings &d : mDifferentials) + { + float avg_omega = 0.0f; + int avg_omega_denom = 0; + int indices[] = { d.mLeftWheel, d.mRightWheel }; + for (int idx : indices) + if (idx != -1) + { + avg_omega += wheels[idx]->GetAngularVelocity(); + avg_omega_denom++; + } + + if (avg_omega_denom > 0) + { + avg_omega = abs(avg_omega * d.mDifferentialRatio / float(avg_omega_denom)); // ignoring that the differentials may be rotating in different directions + driven_differentials.push_back({ &d, avg_omega, d.mEngineTorqueRatio, 0 }); + + // Remember min and max velocity + differential_omega_min = min(differential_omega_min, avg_omega); + differential_omega_max = max(differential_omega_max, avg_omega); + } + } + + if (mDifferentialLimitedSlipRatio < FLT_MAX // Limited slip differential needs to be turned on + && differential_omega_max > differential_omega_min) // There needs to be a velocity difference + { + // Calculate factor based on relative speed of a differential + float sum_factor = 0.0f; + for (DrivenDifferential &d : driven_differentials) + { + // Differential with max velocity gets factor 0, differential with min velocity 1 + d.mTempTorqueFactor = (differential_omega_max - d.mAngularVelocity) / (differential_omega_max - differential_omega_min); + sum_factor += d.mTempTorqueFactor; + } + + // Normalize the result + for (DrivenDifferential &d : driven_differentials) + d.mTempTorqueFactor /= sum_factor; + + // Prevent div by zero + differential_omega_min = max(1.0e-3f, differential_omega_min); + differential_omega_max = max(1.0e-3f, differential_omega_max); + + // Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mDifferentialLimitedSlipRatio + float alpha = min((differential_omega_max / differential_omega_min - 1.0f) / (mDifferentialLimitedSlipRatio - 1.0f), 1.0f); + JPH_ASSERT(alpha >= 0.0f); + float one_min_alpha = 1.0f - alpha; + + // Update torque ratio for all differentials + for (DrivenDifferential &d : driven_differentials) + d.mClutchToDifferentialTorqueRatio = one_min_alpha * d.mClutchToDifferentialTorqueRatio + alpha * d.mTempTorqueFactor; + } + +#ifdef JPH_ENABLE_ASSERTS + // Assert the values add up to 1 + float sum_torque_factors = 0.0f; + for (DrivenDifferential &d : driven_differentials) + sum_torque_factors += d.mClutchToDifferentialTorqueRatio; + JPH_ASSERT(abs(sum_torque_factors - 1.0f) < 1.0e-6f); +#endif // JPH_ENABLE_ASSERTS + + // Define a struct that collects information about the wheels that connect to the engine + struct DrivenWheel + { + WheelWV * mWheel; + float mClutchToWheelRatio; + float mClutchToWheelTorqueRatio; + float mEstimatedAngularImpulse; + }; + Array driven_wheels; + driven_wheels.reserve(wheels.size()); + + // Collect driven wheels + float transmission_ratio = mTransmission.GetCurrentRatio(); + for (const DrivenDifferential &dd : driven_differentials) + { + VehicleDifferentialSettings d = *dd.mDifferential; + + WheelWV *wl = d.mLeftWheel != -1? static_cast(wheels[d.mLeftWheel]) : nullptr; + WheelWV *wr = d.mRightWheel != -1? static_cast(wheels[d.mRightWheel]) : nullptr; + + float clutch_to_wheel_ratio = transmission_ratio * d.mDifferentialRatio; + + if (wl != nullptr && wr != nullptr) + { + // Calculate torque ratio + float ratio_l, ratio_r; + d.CalculateTorqueRatio(wl->GetAngularVelocity(), wr->GetAngularVelocity(), ratio_l, ratio_r); + + // Add both wheels + driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_l, 0.0f }); + driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_r, 0.0f }); + } + else if (wl != nullptr) + { + // Only left wheel, all power to left + driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f }); + } + else if (wr != nullptr) + { + // Only right wheel, all power to right + driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f }); + } + } + + bool solved = false; + if (!driven_wheels.empty()) + { + // Define the torque at the clutch at time t as: + // + // tc(t):=S*(we(t)-sum(R(j)*ww(j,t),j,1,N)/N) + // + // Where: + // S is the total strength of clutch (= friction * strength) + // we(t) is the engine angular velocity at time t + // R(j) is the total gear ratio of clutch to wheel for wheel j + // ww(j,t) is the angular velocity of wheel j at time t + // N is the amount of wheels + // + // The torque that increases the engine angular velocity at time t is: + // + // te(t):=TE-tc(t) + // + // Where: + // TE is the torque delivered by the engine + // + // The torque that increases the wheel angular velocity for wheel i at time t is: + // + // tw(i,t):=TW(i)+R(i)*F(i)*tc(t) + // + // Where: + // TW(i) is the torque applied to the wheel outside of the engine (brake + torque due to friction with the ground) + // F(i) is the fraction of the engine torque applied from engine to wheel i + // + // Because the angular acceleration and torque are connected through: Torque = I * dw/dt + // + // We have the angular acceleration of the engine at time t: + // + // ddt_we(t):=te(t)/Ie + // + // Where: + // Ie is the inertia of the engine + // + // We have the angular acceleration of wheel i at time t: + // + // ddt_ww(i,t):=tw(i,t)/Iw(i) + // + // Where: + // Iw(i) is the inertia of wheel i + // + // We could take a simple Euler step to calculate the resulting accelerations but because the system is very stiff this turns out to be unstable, so we need to use implicit Euler instead: + // + // we(t+dt)=we(t)+dt*ddt_we(t+dt) + // + // and: + // + // ww(i,t+dt)=ww(i,t)+dt*ddt_ww(i,t+dt) + // + // Expanding both equations (the equations above are in wxMaxima format and this can easily be done by expand(%)): + // + // For wheel: + // + // ww(i,t+dt) + (S*dt*F(i)*R(i)*sum(R(j)*ww(j,t+dt),j,1,N))/(N*Iw(i)) - (S*dt*F(i)*R(i)*we(t+dt))/Iw(i) = ww(i,t)+(dt*TW(i))/Iw(i) + // + // For engine: + // + // we(t+dt) + (S*dt*we(t+dt))/Ie - (S*dt*sum(R(j)*ww(j,t+dt),j,1,N))/(Ie*N) = we(t)+(TE*dt)/Ie + // + // Defining a vector w(t) = (ww(1, t), ww(2, t), ..., ww(N, t), we(t)) we can write both equations as a matrix multiplication: + // + // a * w(t + dt) = b + // + // We then invert the matrix to get the new angular velocities. + + // Dimension of matrix is N + 1 + int n = (int)driven_wheels.size() + 1; + + // Last column of w is for the engine angular velocity + int engine = n - 1; + + // Define a and b + DynMatrix a(n, n); + DynMatrix b(n, 1); + + // Get number of driven wheels as a float + float num_driven_wheels_float = float(driven_wheels.size()); + + // Angular velocity of engine + float w_engine = mEngine.GetAngularVelocity(); + + // Calculate the total strength of the clutch + float clutch_strength = transmission_ratio != 0.0f? mTransmission.GetClutchFriction() * mTransmission.mClutchStrength : 0.0f; + + // dt / Ie + float dt_div_ie = inDeltaTime / mEngine.mInertia; + + // Calculate scale factor for impulses based on previous delta time + float impulse_scale = mPreviousDeltaTime > 0.0f? inDeltaTime / mPreviousDeltaTime : 0.0f; + + // Iterate the rows for the wheels + for (int i = 0; i < (int)driven_wheels.size(); ++i) + { + DrivenWheel &w_i = driven_wheels[i]; + const WheelSettingsWV *settings = w_i.mWheel->GetSettings(); + + // Get wheel inertia + float inertia = settings->mInertia; + + // S * R(i) + float s_r = clutch_strength * w_i.mClutchToWheelRatio; + + // dt * S * R(i) * F(i) / Iw + float dt_s_r_f_div_iw = inDeltaTime * s_r * w_i.mClutchToWheelTorqueRatio / inertia; + + // Fill in the columns of a for wheel j + for (int j = 0; j < (int)driven_wheels.size(); ++j) + { + const DrivenWheel &w_j = driven_wheels[j]; + a(i, j) = dt_s_r_f_div_iw * w_j.mClutchToWheelRatio / num_driven_wheels_float; + } + + // Add ww(i, t+dt) + a(i, i) += 1.0f; + + // Add the column for the engine + a(i, engine) = -dt_s_r_f_div_iw; + + // Calculate external angular impulse operating on the wheel: TW(i) * dt + float dt_tw = 0.0f; + + // Combine brake with hand brake torque + float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque; + if (brake_torque > 0.0f) + { + // We're braking + // Calculate brake angular impulse + float sign; + if (w_i.mWheel->GetAngularVelocity() != 0.0f) + sign = Sign(w_i.mWheel->GetAngularVelocity()); + else + sign = Sign(mTransmission.GetCurrentRatio()); // When wheels have locked up use the transmission ratio to determine the sign + dt_tw = sign * inDeltaTime * brake_torque; + } + + if (w_i.mWheel->HasContact()) + { + // We have wheel contact with the floor + // Note that we don't know the torque due to the ground contact yet, so we use the impulse applied from the last frame to estimate it + // Wheel torque TW = force * radius = lambda / dt * radius + dt_tw += impulse_scale * w_i.mWheel->GetLongitudinalLambda() * settings->mRadius; + } + + w_i.mEstimatedAngularImpulse = dt_tw; + + // Fill in the constant b = ww(i,t)+(dt*TW(i))/Iw(i) + b(i, 0) = w_i.mWheel->GetAngularVelocity() - dt_tw / inertia; + + // To avoid looping over the wheels again, we also fill in the wheel columns of the engine row here + a(engine, i) = -dt_div_ie * s_r / num_driven_wheels_float; + } + + // Finalize the engine row + a(engine, engine) = (1.0f + dt_div_ie * clutch_strength); + b(engine, 0) = w_engine + dt_div_ie * engine_torque; + + // Solve the linear equation + if (GaussianElimination(a, b)) + { + // Update the angular velocities for the wheels + for (int i = 0; i < (int)driven_wheels.size(); ++i) + { + DrivenWheel &w_i = driven_wheels[i]; + const WheelSettingsWV *settings = w_i.mWheel->GetSettings(); + + // Get solved wheel angular velocity + float angular_velocity = b(i, 0); + + // We estimated TW and applied it in the equation above, but we haven't actually applied this torque yet so we undo it here. + // It will be applied when we solve the actual braking / the constraints with the floor. + angular_velocity += w_i.mEstimatedAngularImpulse / settings->mInertia; + + // Update angular velocity + w_i.mWheel->SetAngularVelocity(angular_velocity); + } + + // Update the engine RPM + mEngine.SetCurrentRPM(b(engine, 0) * VehicleEngine::cAngularVelocityToRPM); + + // The speeds have been solved + solved = true; + } + else + { + JPH_ASSERT(false, "New engine/wheel speeds could not be calculated!"); + } + } + + if (!solved) + { + // Engine not connected to wheels, apply all torque to engine rotation + mEngine.ApplyTorque(engine_torque, inDeltaTime); + } + + // Calculate if any of the wheels are slipping, this is used to prevent gear switching + bool wheels_slipping = false; + for (const DrivenWheel &w : driven_wheels) + wheels_slipping |= w.mClutchToWheelTorqueRatio > 0.0f && (!w.mWheel->HasContact() || w.mWheel->mLongitudinalSlip > 0.1f); + + // Only allow shifting up when we're not slipping and we're increasing our RPM. + // After a jump, we have a very high engine RPM but once we hit the ground the RPM should be decreasing and we don't want to shift up + // during that time. + bool can_shift_up = !wheels_slipping && mEngine.GetCurrentRPM() >= old_engine_rpm; + + // Update transmission + mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, can_shift_up); + + // Braking + for (Wheel *w_base : wheels) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *settings = w->GetSettings(); + + // Combine brake with hand brake torque + float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque; + if (brake_torque > 0.0f) + { + // Calculate how much torque is needed to stop the wheels from rotating in this time step + float brake_torque_to_lock_wheels = abs(w->GetAngularVelocity()) * settings->mInertia / inDeltaTime; + if (brake_torque > brake_torque_to_lock_wheels) + { + // Wheels are locked + w->SetAngularVelocity(0.0f); + w->mBrakeImpulse = (brake_torque - brake_torque_to_lock_wheels) * inDeltaTime / settings->mRadius; + } + else + { + // Slow down the wheels + w->ApplyTorque(-Sign(w->GetAngularVelocity()) * brake_torque, inDeltaTime); + w->mBrakeImpulse = 0.0f; + } + } + else + { + // Not braking + w->mBrakeImpulse = 0.0f; + } + } + + // Remember previous delta time so we can scale the impulses correctly + mPreviousDeltaTime = inDeltaTime; +} + +bool WheeledVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = false; + + float *max_lateral_friction_impulse = (float *)JPH_STACK_ALLOC(mConstraint.GetWheels().size() * sizeof(float)); + + uint wheel_index = 0; + for (Wheel *w_base : mConstraint.GetWheels()) + { + if (w_base->HasContact()) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *settings = w->GetSettings(); + + // Calculate max impulse that we can apply on the ground + float max_longitudinal_friction_impulse; + mTireMaxImpulseCallback(wheel_index, + max_longitudinal_friction_impulse, max_lateral_friction_impulse[wheel_index], w->GetSuspensionLambda(), + w->mCombinedLongitudinalFriction, w->mCombinedLateralFriction, w->mLongitudinalSlip, w->mLateralSlip, inDeltaTime); + + // Calculate relative velocity between wheel contact point and floor in longitudinal direction + Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity(); + float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal()); + + // Calculate brake force to apply + float min_longitudinal_impulse, max_longitudinal_impulse; + if (w->mBrakeImpulse != 0.0f) + { + // Limit brake force by max tire friction + float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse); + + // Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle) + if (relative_longitudinal_velocity >= 0.0f) + { + min_longitudinal_impulse = -brake_impulse; + max_longitudinal_impulse = 0.0f; + } + else + { + min_longitudinal_impulse = 0.0f; + max_longitudinal_impulse = brake_impulse; + } + + // Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + } + else + { + // Assume we want to apply an angular impulse that makes the delta velocity between wheel and ground zero in one time step, calculate the amount of linear impulse needed to do that + float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius; + float linear_impulse = (w->GetAngularVelocity() - desired_angular_velocity) * settings->mInertia / settings->mRadius; + + // Limit the impulse by max tire friction + float prev_lambda = w->GetLongitudinalLambda(); + min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse); + + // Longitudinal impulse + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + + // Update the angular velocity of the wheels according to the lambda that was applied + w->SetAngularVelocity(w->GetAngularVelocity() - (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / settings->mInertia); + } + } + ++wheel_index; + } + + wheel_index = 0; + for (Wheel *w_base : mConstraint.GetWheels()) + { + if (w_base->HasContact()) + { + WheelWV *w = static_cast(w_base); + + // Lateral friction + float max_lateral_impulse = max_lateral_friction_impulse[wheel_index]; + impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_impulse, max_lateral_impulse); + } + ++wheel_index; + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const +{ + float constraint_size = mConstraint.GetDrawConstraintSize(); + + // Draw RPM + Body *body = mConstraint.GetVehicleBody(); + Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp(); + RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition; + Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward(); + mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM); + + if (mTransmission.GetCurrentRatio() != 0.0f) + { + // Calculate average wheel speed at clutch + float wheel_speed_at_clutch = GetWheelSpeedAtClutch(); + + // Draw the average wheel speed measured at clutch to compare engine RPM with wheel RPM + inRenderer->DrawLine(rpm_meter_pos, rpm_meter_pos + Quat::sRotation(rpm_meter_fwd, mEngine.ConvertRPMToAngle(wheel_speed_at_clutch)) * (rpm_meter_up * 1.1f * mRPMMeterSize), Color::sYellow); + } + + // Draw current vehicle state + String status = StringFormat("Forward: %.1f, Right: %.1f\nBrake: %.1f, HandBrake: %.1f\n" + "Gear: %d, Clutch: %.1f\nEngineRPM: %.0f, V: %.1f km/h", + (double)mForwardInput, (double)mRightInput, (double)mBrakeInput, (double)mHandBrakeInput, + mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6); + inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size); + + RMat44 body_transform = body->GetWorldTransform(); + + for (const Wheel *w_base : mConstraint.GetWheels()) + { + const WheelWV *w = static_cast(w_base); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Draw suspension + RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength; + RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength; + inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed); + inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen); + + // Draw current length + RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength(); + inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size); + + // Draw wheel basis + Vec3 wheel_forward, wheel_up, wheel_right; + mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right); + wheel_forward = body_transform.Multiply3x3(wheel_forward); + wheel_up = body_transform.Multiply3x3(wheel_up); + wheel_right = body_transform.Multiply3x3(wheel_right); + Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue); + inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow); + + // Draw wheel + RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos); + wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle())); + inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe); + + if (w->HasContact()) + { + // Draw contact + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue); + + DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f, S: %.2f\nSlipLateral: %.1f, SlipLong: %.2f\nFrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)RadiansToDegrees(w->mLateralSlip), (double)w->mLongitudinalSlip, (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, constraint_size); + } + else + { + // Draw 'no hit' + DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f", (double)w->GetAngularVelocity()), Color::sRed, constraint_size); + } + } +} + +#endif // JPH_DEBUG_RENDERER + +void WheeledVehicleController::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mForwardInput); + inStream.Write(mRightInput); + inStream.Write(mBrakeInput); + inStream.Write(mHandBrakeInput); + inStream.Write(mPreviousDeltaTime); + + mEngine.SaveState(inStream); + mTransmission.SaveState(inStream); +} + +void WheeledVehicleController::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mForwardInput); + inStream.Read(mRightInput); + inStream.Read(mBrakeInput); + inStream.Read(mHandBrakeInput); + inStream.Read(mPreviousDeltaTime); + + mEngine.RestoreState(inStream); + mTransmission.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h new file mode 100644 index 000000000000..0bfd66fd8b4c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h @@ -0,0 +1,199 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// WheelSettings object specifically for WheeledVehicleController +class JPH_EXPORT WheelSettingsWV : public WheelSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsWV) + +public: + /// Constructor + WheelSettingsWV(); + + // See: WheelSettings + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + float mInertia = 0.9f; ///< Moment of inertia (kg m^2), for a cylinder this would be 0.5 * M * R^2 which is 0.9 for a wheel with a mass of 20 kg and radius 0.3 m + float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w + float mMaxSteerAngle = DegreesToRadians(70.0f); ///< How much this wheel can steer (radians) + LinearCurve mLongitudinalFriction; ///< On the Y-axis: friction in the forward direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip ratio (fraction) defined as (omega_wheel * r_wheel - v_longitudinal) / |v_longitudinal|. You can see slip ratio as the amount the wheel is spinning relative to the floor: 0 means the wheel has full traction and is rolling perfectly in sync with the ground, 1 is for example when the wheel is locked and sliding over the ground. + LinearCurve mLateralFriction; ///< On the Y-axis: friction in the sideways direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip angle (degrees) defined as angle between relative contact velocity and tire direction. + float mMaxBrakeTorque = 1500.0f; ///< How much torque (Nm) the brakes can apply to this wheel + float mMaxHandBrakeTorque = 4000.0f; ///< How much torque (Nm) the hand brake can apply to this wheel (usually only applied to the rear wheels) +}; + +/// Wheel object specifically for WheeledVehicleController +class JPH_EXPORT WheelWV : public Wheel +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit WheelWV(const WheelSettingsWV &inWheel); + + /// Override GetSettings and cast to the correct class + const WheelSettingsWV * GetSettings() const { return StaticCast(mSettings); } + + /// Apply a torque (N m) to the wheel for a particular delta time + void ApplyTorque(float inTorque, float inDeltaTime) + { + mAngularVelocity += inTorque * inDeltaTime / GetSettings()->mInertia; + } + + /// Update the wheel rotation based on the current angular velocity + void Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint); + + float mLongitudinalSlip = 0.0f; ///< Velocity difference between ground and wheel relative to ground velocity + float mLateralSlip = 0.0f; ///< Angular difference (in radians) between ground and wheel relative to ground velocity + float mCombinedLongitudinalFriction = 0.0f; ///< Combined friction coefficient in longitudinal direction (combines terrain and tires) + float mCombinedLateralFriction = 0.0f; ///< Combined friction coefficient in lateral direction (combines terrain and tires) + float mBrakeImpulse = 0.0f; ///< Amount of impulse that the brakes can apply to the floor (excluding friction) +}; + +/// Settings of a vehicle with regular wheels +/// +/// The properties in this controller are largely based on "Car Physics for Games" by Marco Monster. +/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html +class JPH_EXPORT WheeledVehicleControllerSettings : public VehicleControllerSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheeledVehicleControllerSettings) + +public: + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + VehicleEngineSettings mEngine; ///< The properties of the engine + VehicleTransmissionSettings mTransmission; ///< The properties of the transmission (aka gear box) + Array mDifferentials; ///< List of differentials and their properties + float mDifferentialLimitedSlipRatio = 1.4f; ///< Ratio max / min average wheel speed of each differential (measured at the clutch). When the ratio is exceeded all torque gets distributed to the differential with the minimal average velocity. This allows implementing a limited slip differential between differentials. Set to FLT_MAX for an open differential. Value should be > 1. +}; + +/// Runtime controller class +class JPH_EXPORT WheeledVehicleController : public VehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Typedefs + using Differentials = Array; + + /// Set input from driver + /// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + /// @param inRight Value between -1 and 1 indicating desired steering angle (1 = right) + /// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed + /// @param inHandBrake Value between 0 and 1 indicating how strong the hand brake is pulled + void SetDriverInput(float inForward, float inRight, float inBrake, float inHandBrake) { mForwardInput = inForward; mRightInput = inRight; mBrakeInput = inBrake; mHandBrakeInput = inHandBrake; } + + /// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + void SetForwardInput(float inForward) { mForwardInput = inForward; } + float GetForwardInput() const { return mForwardInput; } + + /// Value between -1 and 1 indicating desired steering angle (1 = right) + void SetRightInput(float inRight) { mRightInput = inRight; } + float GetRightInput() const { return mRightInput; } + + /// Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetBrakeInput(float inBrake) { mBrakeInput = inBrake; } + float GetBrakeInput() const { return mBrakeInput; } + + /// Value between 0 and 1 indicating how strong the hand brake is pulled + void SetHandBrakeInput(float inHandBrake) { mHandBrakeInput = inHandBrake; } + float GetHandBrakeInput() const { return mHandBrakeInput; } + + /// Get current engine state + const VehicleEngine & GetEngine() const { return mEngine; } + + /// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleEngine & GetEngine() { return mEngine; } + + /// Get current transmission state + const VehicleTransmission & GetTransmission() const { return mTransmission; } + + /// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTransmission & GetTransmission() { return mTransmission; } + + /// Get the differentials this vehicle has + const Differentials & GetDifferentials() const { return mDifferentials; } + + /// Get the differentials this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step) + Differentials & GetDifferentials() { return mDifferentials; } + + /// Ratio max / min average wheel speed of each differential (measured at the clutch). + float GetDifferentialLimitedSlipRatio() const { return mDifferentialLimitedSlipRatio; } + void SetDifferentialLimitedSlipRatio(float inV) { mDifferentialLimitedSlipRatio = inV; } + + /// Get the average wheel speed of all driven wheels (measured at the clutch) + float GetWheelSpeedAtClutch() const; + + /// Calculate max tire impulses by combining friction, slip, and suspension impulse. Note that the actual applied impulse may be lower (e.g. when the vehicle is stationary on a horizontal surface the actual impulse applied will be 0). + using TireMaxImpulseCallback = function; + const TireMaxImpulseCallback&GetTireMaxImpulseCallback() const { return mTireMaxImpulseCallback; } + void SetTireMaxImpulseCallback(const TireMaxImpulseCallback &inTireMaxImpulseCallback) { mTireMaxImpulseCallback = inTireMaxImpulseCallback; } + +#ifdef JPH_DEBUG_RENDERER + /// Debug drawing of RPM meter + void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; } +#endif // JPH_DEBUG_RENDERER + +protected: + // See: VehicleController + virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsWV))); return new WheelWV(static_cast(inWheel)); } + virtual bool AllowSleep() const override; + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Control information + float mForwardInput = 0.0f; ///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + float mRightInput = 0.0f; ///< Value between -1 and 1 indicating desired steering angle + float mBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the brake pedal is pressed + float mHandBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the hand brake is pulled + + // Simulation information + VehicleEngine mEngine; ///< Engine state of the vehicle + VehicleTransmission mTransmission; ///< Transmission state of the vehicle + Differentials mDifferentials; ///< Differential states of the vehicle + float mDifferentialLimitedSlipRatio; ///< Ratio max / min average wheel speed of each differential (measured at the clutch). + float mPreviousDeltaTime = 0.0f; ///< Delta time of the last step + + // Callback that calculates the max impulse that the tire can apply to the ground + TireMaxImpulseCallback mTireMaxImpulseCallback = + [](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float) + { + outLongitudinalImpulse = inLongitudinalFriction * inSuspensionImpulse; + outLateralImpulse = inLateralFriction * inSuspensionImpulse; + }; + +#ifdef JPH_DEBUG_RENDERER + // Debug settings + Vec3 mRPMMeterPosition { 0, 1, 0 }; ///< Position (in local space of the body) of the RPM meter when drawing the constraint + float mRPMMeterSize = 0.5f; ///< Size of the RPM meter when drawing the constraint +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp b/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp new file mode 100644 index 000000000000..5b61a9160a27 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp @@ -0,0 +1,192 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, Skeleton) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SkeletalAnimation) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RagdollSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PointConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SixDOFConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SliderConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SwingTwistConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, DistanceConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, HingeConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, FixedConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, ConeConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPath) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPathHermite) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, VehicleConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, WheeledVehicleControllerSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RackAndPinionConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GearConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PulleyConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, MotorSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsScene) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsMaterial) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilter) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilterTable) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, BodyCreationSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SoftBodyCreationSettings) + +JPH_NAMESPACE_BEGIN + +bool VerifyJoltVersionIDInternal(uint64 inVersionID) +{ + return inVersionID == JPH_VERSION_ID; +} + +void RegisterTypesInternal(uint64 inVersionID) +{ + // Version check + if (!VerifyJoltVersionIDInternal(inVersionID)) + { + Trace("Version mismatch, make sure you compile the client code with the same Jolt version and compiler definitions!"); + uint64 mismatch = JPH_VERSION_ID ^ inVersionID; + auto check_bit = [mismatch](int inBit, const char *inLabel) { if (mismatch & (uint64(1) << (inBit + 23))) Trace("Mismatching define %s.", inLabel); }; + check_bit(1, "JPH_DOUBLE_PRECISION"); + check_bit(2, "JPH_CROSS_PLATFORM_DETERMINISTIC"); + check_bit(3, "JPH_FLOATING_POINT_EXCEPTIONS_ENABLED"); + check_bit(4, "JPH_PROFILE_ENABLED"); + check_bit(5, "JPH_EXTERNAL_PROFILE"); + check_bit(6, "JPH_DEBUG_RENDERER"); + check_bit(7, "JPH_DISABLE_TEMP_ALLOCATOR"); + check_bit(8, "JPH_DISABLE_CUSTOM_ALLOCATOR"); + check_bit(9, "JPH_OBJECT_LAYER_BITS"); + check_bit(10, "JPH_ENABLE_ASSERTS"); + check_bit(11, "JPH_OBJECT_STREAM"); + std::abort(); + } + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + JPH_ASSERT(Allocate != nullptr && Reallocate != nullptr && Free != nullptr && AlignedAllocate != nullptr && AlignedFree != nullptr, "Need to supply an allocator first or call RegisterDefaultAllocator()"); +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + + JPH_ASSERT(Factory::sInstance != nullptr, "Need to create a factory first!"); + + // Initialize dispatcher + CollisionDispatch::sInit(); + + // Register base classes first so that we can specialize them later + CompoundShape::sRegister(); + ConvexShape::sRegister(); + + // Register compounds before others so that we can specialize them later (register them in reverse order of collision complexity) + MutableCompoundShape::sRegister(); + StaticCompoundShape::sRegister(); + + // Leaf classes + TriangleShape::sRegister(); + PlaneShape::sRegister(); + SphereShape::sRegister(); + BoxShape::sRegister(); + CapsuleShape::sRegister(); + TaperedCapsuleShape::sRegister(); + CylinderShape::sRegister(); + TaperedCylinderShape::sRegister(); + MeshShape::sRegister(); + ConvexHullShape::sRegister(); + HeightFieldShape::sRegister(); + SoftBodyShape::sRegister(); + + // Register these last because their collision functions are simple so we want to execute them first (register them in reverse order of collision complexity) + RotatedTranslatedShape::sRegister(); + OffsetCenterOfMassShape::sRegister(); + ScaledShape::sRegister(); + EmptyShape::sRegister(); + + // Create list of all types + const RTTI *types[] = { + JPH_RTTI(SkeletalAnimation), + JPH_RTTI(Skeleton), + JPH_RTTI(CompoundShapeSettings), + JPH_RTTI(StaticCompoundShapeSettings), + JPH_RTTI(MutableCompoundShapeSettings), + JPH_RTTI(TriangleShapeSettings), + JPH_RTTI(PlaneShapeSettings), + JPH_RTTI(SphereShapeSettings), + JPH_RTTI(BoxShapeSettings), + JPH_RTTI(CapsuleShapeSettings), + JPH_RTTI(TaperedCapsuleShapeSettings), + JPH_RTTI(CylinderShapeSettings), + JPH_RTTI(TaperedCylinderShapeSettings), + JPH_RTTI(ScaledShapeSettings), + JPH_RTTI(MeshShapeSettings), + JPH_RTTI(ConvexHullShapeSettings), + JPH_RTTI(HeightFieldShapeSettings), + JPH_RTTI(RotatedTranslatedShapeSettings), + JPH_RTTI(OffsetCenterOfMassShapeSettings), + JPH_RTTI(EmptyShapeSettings), + JPH_RTTI(RagdollSettings), + JPH_RTTI(PointConstraintSettings), + JPH_RTTI(SixDOFConstraintSettings), + JPH_RTTI(SliderConstraintSettings), + JPH_RTTI(SwingTwistConstraintSettings), + JPH_RTTI(DistanceConstraintSettings), + JPH_RTTI(HingeConstraintSettings), + JPH_RTTI(FixedConstraintSettings), + JPH_RTTI(ConeConstraintSettings), + JPH_RTTI(PathConstraintSettings), + JPH_RTTI(VehicleConstraintSettings), + JPH_RTTI(WheeledVehicleControllerSettings), + JPH_RTTI(PathConstraintPath), + JPH_RTTI(PathConstraintPathHermite), + JPH_RTTI(RackAndPinionConstraintSettings), + JPH_RTTI(GearConstraintSettings), + JPH_RTTI(PulleyConstraintSettings), + JPH_RTTI(MotorSettings), + JPH_RTTI(PhysicsScene), + JPH_RTTI(PhysicsMaterial), + JPH_RTTI(PhysicsMaterialSimple), + JPH_RTTI(GroupFilter), + JPH_RTTI(GroupFilterTable), + JPH_RTTI(BodyCreationSettings), + JPH_RTTI(SoftBodyCreationSettings) + }; + + // Register them all + Factory::sInstance->Register(types, (uint)std::size(types)); + + // Initialize default physics material + if (PhysicsMaterial::sDefault == nullptr) + PhysicsMaterial::sDefault = new PhysicsMaterialSimple("Default", Color::sGrey); +} + +void UnregisterTypes() +{ + // Unregister all types + if (Factory::sInstance != nullptr) + Factory::sInstance->Clear(); + + // Delete default physics material + PhysicsMaterial::sDefault = nullptr; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/RegisterTypes.h b/thirdparty/jolt_physics/Jolt/RegisterTypes.h new file mode 100644 index 000000000000..372ef7f4f40e --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/RegisterTypes.h @@ -0,0 +1,29 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Internal helper function +JPH_EXPORT extern bool VerifyJoltVersionIDInternal(uint64 inVersionID); + +/// This function can be used to verify the library ABI is compatible with your +/// application. +/// Use it in this way: `assert(VerifyJoltVersionID());`. +/// Returns `false` if the library used is not compatible with your app. +JPH_INLINE bool VerifyJoltVersionID() { return VerifyJoltVersionIDInternal(JPH_VERSION_ID); } + +/// Internal helper function +JPH_EXPORT extern void RegisterTypesInternal(uint64 inVersionID); + +/// Register all physics types with the factory and install their collision handlers with the CollisionDispatch class. +/// If you have your own custom shape types you probably need to register their handlers with the CollisionDispatch before calling this function. +/// If you implement your own default material (PhysicsMaterial::sDefault) make sure to initialize it before this function or else this function will create one for you. +JPH_INLINE void RegisterTypes() { RegisterTypesInternal(JPH_VERSION_ID); } + +/// Unregisters all types with the factory and cleans up the default material +JPH_EXPORT extern void UnregisterTypes(); + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp new file mode 100644 index 000000000000..636c1e2f7a6d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp @@ -0,0 +1,1107 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +DebugRenderer *DebugRenderer::sInstance = nullptr; + +// Number of LOD levels to create +static const int sMaxLevel = 4; + +// Distance for each LOD level, these are tweaked for an object of approx. size 1. Use the lod scale to scale these distances. +static const float sLODDistanceForLevel[] = { 5.0f, 10.0f, 40.0f, FLT_MAX }; + +DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor) +{ + // Set position + inV1.StoreFloat3(&mV[0].mPosition); + inV2.StoreFloat3(&mV[1].mPosition); + inV3.StoreFloat3(&mV[2].mPosition); + + // Set color + mV[0].mColor = mV[1].mColor = mV[2].mColor = inColor; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1); + float normal_len = normal.Length(); + if (normal_len > 0.0f) + normal /= normal_len; + Float3 normal3; + normal.StoreFloat3(&normal3); + mV[0].mNormal = mV[1].mNormal = mV[2].mNormal = normal3; + + // Reset UV's + mV[0].mUV = mV[1].mUV = mV[2].mUV = { 0, 0 }; +} + +DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor, Vec3Arg inUVOrigin, Vec3Arg inUVDirection) +{ + // Set position + inV1.StoreFloat3(&mV[0].mPosition); + inV2.StoreFloat3(&mV[1].mPosition); + inV3.StoreFloat3(&mV[2].mPosition); + + // Set color + mV[0].mColor = mV[1].mColor = mV[2].mColor = inColor; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1).Normalized(); + Float3 normal3; + normal.StoreFloat3(&normal3); + mV[0].mNormal = mV[1].mNormal = mV[2].mNormal = normal3; + + // Set UV's + Vec3 uv1 = inV1 - inUVOrigin; + Vec3 uv2 = inV2 - inUVOrigin; + Vec3 uv3 = inV3 - inUVOrigin; + Vec3 axis2 = normal.Cross(inUVDirection); + mV[0].mUV = { inUVDirection.Dot(uv1), axis2.Dot(uv1) }; + mV[1].mUV = { inUVDirection.Dot(uv2), axis2.Dot(uv2) }; + mV[2].mUV = { inUVDirection.Dot(uv3), axis2.Dot(uv3) }; +} + +DebugRenderer::DebugRenderer() +{ + // Store singleton + JPH_ASSERT(sInstance == nullptr); + sInstance = this; +} + +DebugRenderer::~DebugRenderer() +{ + JPH_ASSERT(sInstance == this); + sInstance = nullptr; +} + +void DebugRenderer::DrawWireBox(const AABox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1(Real(inBox.mMin.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v2(Real(inBox.mMin.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v3(Real(inBox.mMin.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v4(Real(inBox.mMin.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v5(Real(inBox.mMax.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v6(Real(inBox.mMax.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v7(Real(inBox.mMax.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v8(Real(inBox.mMax.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMax.GetZ())); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawWireBox(const OrientedBox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v2(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v3(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v4(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v5(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v6(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v7(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v8(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawWireBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMin.GetY(), inBox.mMin.GetZ()); + RVec3 v2 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMin.GetY(), inBox.mMax.GetZ()); + RVec3 v3 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMax.GetY(), inBox.mMin.GetZ()); + RVec3 v4 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMax.GetY(), inBox.mMax.GetZ()); + RVec3 v5 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMin.GetY(), inBox.mMin.GetZ()); + RVec3 v6 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMin.GetY(), inBox.mMax.GetZ()); + RVec3 v7 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMax.GetY(), inBox.mMin.GetZ()); + RVec3 v8 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMax.GetY(), inBox.mMax.GetZ()); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + Vec3 dx(inSize, 0, 0); + Vec3 dy(0, inSize, 0); + Vec3 dz(0, 0, inSize); + DrawLine(inPosition - dy, inPosition + dy, inColor); + DrawLine(inPosition - dx, inPosition + dx, inColor); + DrawLine(inPosition - dz, inPosition + dz, inColor); +} + +void DebugRenderer::DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + // Draw base line + DrawLine(inFrom, inTo, inColor); + + if (inSize > 0.0f) + { + // Draw arrow head + Vec3 dir = Vec3(inTo - inFrom); + float len = dir.Length(); + if (len != 0.0f) + dir = dir * (inSize / len); + else + dir = Vec3(inSize, 0, 0); + Vec3 perp = inSize * dir.GetNormalizedPerpendicular(); + DrawLine(inTo - dir + perp, inTo, inColor); + DrawLine(inTo - dir - perp, inTo, inColor); + } +} + +void DebugRenderer::DrawCoordinateSystem(RMat44Arg inTransform, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(inSize, 0, 0), Color::sRed, 0.1f * inSize); + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(0, inSize, 0), Color::sGreen, 0.1f * inSize); + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(0, 0, inSize), Color::sBlue, 0.1f * inSize); +} + +void DebugRenderer::DrawPlane(RVec3Arg inPoint, Vec3Arg inNormal, ColorArg inColor, float inSize) +{ + // Create orthogonal basis + Vec3 perp1 = inNormal.Cross(Vec3::sAxisY()).NormalizedOr(Vec3::sAxisX()); + Vec3 perp2 = perp1.Cross(inNormal).Normalized(); + perp1 = inNormal.Cross(perp2); + + // Calculate corners + RVec3 corner1 = inPoint + inSize * (perp1 + perp2); + RVec3 corner2 = inPoint + inSize * (perp1 - perp2); + RVec3 corner3 = inPoint + inSize * (-perp1 - perp2); + RVec3 corner4 = inPoint + inSize * (-perp1 + perp2); + + // Draw cross + DrawLine(corner1, corner3, inColor); + DrawLine(corner2, corner4, inColor); + + // Draw square + DrawLine(corner1, corner2, inColor); + DrawLine(corner2, corner3, inColor); + DrawLine(corner3, corner4, inColor); + DrawLine(corner4, corner1, inColor); + + // Draw normal + DrawArrow(inPoint, inPoint + inSize * inNormal, inColor, 0.1f * inSize); +} + +void DebugRenderer::DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + DrawLine(inV1, inV2, inColor); + DrawLine(inV2, inV3, inColor); + DrawLine(inV3, inV1, inColor); +} + +void DebugRenderer::DrawWireSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, int inLevel) +{ + RMat44 matrix = RMat44::sTranslation(inCenter) * Mat44::sScale(inRadius); + + DrawWireUnitSphere(matrix, inColor, inLevel); +} + +void DebugRenderer::DrawWireUnitSphere(RMat44Arg inMatrix, ColorArg inColor, int inLevel) +{ + JPH_PROFILE_FUNCTION(); + + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); +} + +void DebugRenderer::DrawWireUnitSphereRecursive(RMat44Arg inMatrix, ColorArg inColor, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel) +{ + if (inLevel == 0) + { + RVec3 d1 = inMatrix * inDir1; + RVec3 d2 = inMatrix * inDir2; + RVec3 d3 = inMatrix * inDir3; + + DrawLine(d1, d2, inColor); + DrawLine(d2, d3, inColor); + DrawLine(d3, d1, inColor); + } + else + { + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + DrawWireUnitSphereRecursive(inMatrix, inColor, inDir1, center1, center3, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center1, center2, center3, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center1, inDir2, center2, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center3, center2, inDir3, inLevel - 1); + } +} + +void DebugRenderer::Create8thSphereRecursive(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel) +{ + if (inLevel == 0) + { + if (ioIdx1 == 0xffffffff) + { + ioIdx1 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir1).StoreFloat3(&position); + inDir1.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + if (ioIdx2 == 0xffffffff) + { + ioIdx2 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir2).StoreFloat3(&position); + inDir2.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + if (ioIdx3 == 0xffffffff) + { + ioIdx3 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir3).StoreFloat3(&position); + inDir3.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + ioIndices.push_back(ioIdx1); + ioIndices.push_back(ioIdx2); + ioIndices.push_back(ioIdx3); + } + else + { + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + uint32 idx1 = 0xffffffff; + uint32 idx2 = 0xffffffff; + uint32 idx3 = 0xffffffff; + + Create8thSphereRecursive(ioIndices, ioVertices, inDir1, ioIdx1, center1, idx1, center3, idx3, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center1, idx1, center2, idx2, center3, idx3, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center1, idx1, inDir2, ioIdx2, center2, idx2, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center3, idx3, center2, idx2, inDir3, ioIdx3, inUV, inGetSupport, inLevel - 1); + } +} + +void DebugRenderer::Create8thSphere(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel) +{ + uint32 idx1 = 0xffffffff; + uint32 idx2 = 0xffffffff; + uint32 idx3 = 0xffffffff; + + Create8thSphereRecursive(ioIndices, ioVertices, inDir1, idx1, inDir2, idx2, inDir3, idx3, inUV, inGetSupport, inLevel); +} + +DebugRenderer::Batch DebugRenderer::CreateCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, int inLevel) +{ + Array cylinder_vertices; + Array cylinder_indices; + + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.75f) : Float2(0.25f, 0.25f); + + uint32 center_start_idx = (uint32)cylinder_vertices.size(); + + Float3 nt(0.0f, 1.0f, 0.0f); + Float3 nb(0.0f, -1.0f, 0.0f); + cylinder_vertices.push_back({ Float3(0.0f, inTop, 0.0f), nt, uv, Color::sWhite }); + cylinder_vertices.push_back({ Float3(0.0f, inBottom, 0.0f), nb, uv, Color::sWhite }); + + uint32 vtx_start_idx = (uint32)cylinder_vertices.size(); + + int num_parts = 1 << inLevel; + for (int i = 0; i <= num_parts; ++i) + { + // Calculate top and bottom vertex + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vt(inTopRadius * s, inTop, inTopRadius * c); + Float3 vb(inBottomRadius * s, inBottom, inBottomRadius * c); + + // Calculate normal + Vec3 edge = Vec3(vt) - Vec3(vb); + Float3 n; + edge.Cross(Vec3(s, 0, c).Cross(edge)).Normalized().StoreFloat3(&n); + + cylinder_vertices.push_back({ vt, nt, uv, Color::sWhite }); + cylinder_vertices.push_back({ vb, nb, uv, Color::sWhite }); + cylinder_vertices.push_back({ vt, n, uv, Color::sWhite }); + cylinder_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = vtx_start_idx + 4 * i; + + // Top + cylinder_indices.push_back(center_start_idx); + cylinder_indices.push_back(start); + cylinder_indices.push_back(start + 4); + + // Bottom + cylinder_indices.push_back(center_start_idx + 1); + cylinder_indices.push_back(start + 5); + cylinder_indices.push_back(start + 1); + + // Side + cylinder_indices.push_back(start + 2); + cylinder_indices.push_back(start + 3); + cylinder_indices.push_back(start + 7); + + cylinder_indices.push_back(start + 2); + cylinder_indices.push_back(start + 7); + cylinder_indices.push_back(start + 6); + } + } + + return CreateTriangleBatch(cylinder_vertices, cylinder_indices); +} + +void DebugRenderer::CreateQuad(Array &ioIndices, Array &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4) +{ + // Make room + uint32 start_idx = uint32(ioVertices.size()); + ioVertices.resize(start_idx + 4); + Vertex *vertices = &ioVertices[start_idx]; + + // Set position + inV1.StoreFloat3(&vertices[0].mPosition); + inV2.StoreFloat3(&vertices[1].mPosition); + inV3.StoreFloat3(&vertices[2].mPosition); + inV4.StoreFloat3(&vertices[3].mPosition); + + // Set color + vertices[0].mColor = vertices[1].mColor = vertices[2].mColor = vertices[3].mColor = Color::sWhite; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1).Normalized(); + Float3 normal3; + normal.StoreFloat3(&normal3); + vertices[0].mNormal = vertices[1].mNormal = vertices[2].mNormal = vertices[3].mNormal = normal3; + + // Set UV's + vertices[0].mUV = { 0, 0 }; + vertices[1].mUV = { 2, 0 }; + vertices[2].mUV = { 2, 2 }; + vertices[3].mUV = { 0, 2 }; + + // Set indices + ioIndices.push_back(start_idx); + ioIndices.push_back(start_idx + 1); + ioIndices.push_back(start_idx + 2); + + ioIndices.push_back(start_idx); + ioIndices.push_back(start_idx + 2); + ioIndices.push_back(start_idx + 3); +} + +void DebugRenderer::Initialize() +{ + // Box + { + Array box_vertices; + Array box_indices; + + // Get corner points + Vec3 v0 = Vec3(-1, 1, -1); + Vec3 v1 = Vec3( 1, 1, -1); + Vec3 v2 = Vec3( 1, 1, 1); + Vec3 v3 = Vec3(-1, 1, 1); + Vec3 v4 = Vec3(-1, -1, -1); + Vec3 v5 = Vec3( 1, -1, -1); + Vec3 v6 = Vec3( 1, -1, 1); + Vec3 v7 = Vec3(-1, -1, 1); + + // Top + CreateQuad(box_indices, box_vertices, v0, v3, v2, v1); + + // Bottom + CreateQuad(box_indices, box_vertices, v4, v5, v6, v7); + + // Left + CreateQuad(box_indices, box_vertices, v0, v4, v7, v3); + + // Right + CreateQuad(box_indices, box_vertices, v2, v6, v5, v1); + + // Front + CreateQuad(box_indices, box_vertices, v3, v7, v6, v2); + + // Back + CreateQuad(box_indices, box_vertices, v0, v1, v5, v4); + + mBox = new Geometry(CreateTriangleBatch(box_vertices, box_indices), AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + } + + // Support function that returns a unit sphere + auto sphere_support = [](Vec3Arg inDirection) { return inDirection; }; + + // Construct geometries + mSphere = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + mCapsuleBottom = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 0, 1))); + mCapsuleTop = new Geometry(AABox(Vec3(-1, 0, -1), Vec3(1, 1, 1))); + mCapsuleMid = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + mOpenCone = new Geometry(AABox(Vec3(-1, 0, -1), Vec3(1, 1, 1))); + mCylinder = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + + // Iterate over levels + for (int level = sMaxLevel; level >= 1; --level) + { + // Determine at which distance this level should be active + float distance = sLODDistanceForLevel[sMaxLevel - level]; + + // Sphere + mSphere->mLODs.push_back({ CreateTriangleBatchForConvex(sphere_support, level), distance }); + + // Capsule bottom half sphere + { + Array capsule_bottom_vertices; + Array capsule_bottom_indices; + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + mCapsuleBottom->mLODs.push_back({ CreateTriangleBatch(capsule_bottom_vertices, capsule_bottom_indices), distance }); + } + + // Capsule top half sphere + { + Array capsule_top_vertices; + Array capsule_top_indices; + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + mCapsuleTop->mLODs.push_back({ CreateTriangleBatch(capsule_top_vertices, capsule_top_indices), distance }); + } + + // Capsule middle part + { + Array capsule_mid_vertices; + Array capsule_mid_indices; + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.25f) : Float2(0.25f, 0.75f); + + uint32 start_idx = (uint32)capsule_mid_vertices.size(); + + int num_parts = 1 << level; + for (int i = 0; i <= num_parts; ++i) + { + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vt(s, 1.0f, c); + Float3 vb(s, -1.0f, c); + Float3 n(s, 0, c); + + capsule_mid_vertices.push_back({ vt, n, uv, Color::sWhite }); + capsule_mid_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = start_idx + 2 * i; + + capsule_mid_indices.push_back(start); + capsule_mid_indices.push_back(start + 1); + capsule_mid_indices.push_back(start + 3); + + capsule_mid_indices.push_back(start); + capsule_mid_indices.push_back(start + 3); + capsule_mid_indices.push_back(start + 2); + } + } + mCapsuleMid->mLODs.push_back({ CreateTriangleBatch(capsule_mid_vertices, capsule_mid_indices), distance }); + } + + // Open cone + { + Array open_cone_vertices; + Array open_cone_indices; + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.25f) : Float2(0.25f, 0.75f); + + uint32 start_idx = (uint32)open_cone_vertices.size(); + + int num_parts = 2 << level; + Float3 vt(0, 0, 0); + for (int i = 0; i <= num_parts; ++i) + { + // Calculate bottom vertex + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vb(s, 1.0f, c); + + // Calculate normal + // perpendicular = Y cross vb (perpendicular to the plane in which 0, y and vb exists) + // normal = perpendicular cross vb (normal to the edge 0 vb) + Vec3 normal = Vec3(s, -Square(s) - Square(c), c).Normalized(); + Float3 n; normal.StoreFloat3(&n); + + open_cone_vertices.push_back({ vt, n, uv, Color::sWhite }); + open_cone_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = start_idx + 2 * i; + + open_cone_indices.push_back(start); + open_cone_indices.push_back(start + 1); + open_cone_indices.push_back(start + 3); + } + } + mOpenCone->mLODs.push_back({ CreateTriangleBatch(open_cone_vertices, open_cone_indices), distance }); + } + + // Cylinder + mCylinder->mLODs.push_back({ CreateCylinder(1.0f, -1.0f, 1.0f, 1.0f, level), distance }); + } +} + +AABox DebugRenderer::sCalculateBounds(const Vertex *inVertices, int inVertexCount) +{ + AABox bounds; + for (const Vertex *v = inVertices, *v_end = inVertices + inVertexCount; v < v_end; ++v) + bounds.Encapsulate(Vec3(v->mPosition)); + return bounds; +} + +DebugRenderer::Batch DebugRenderer::CreateTriangleBatch(const VertexList &inVertices, const IndexedTriangleNoMaterialList &inTriangles) +{ + JPH_PROFILE_FUNCTION(); + + Array vertices; + + // Create render vertices + vertices.resize(inVertices.size()); + for (size_t v = 0; v < inVertices.size(); ++v) + { + vertices[v].mPosition = inVertices[v]; + vertices[v].mNormal = Float3(0, 0, 0); + vertices[v].mUV = Float2(0, 0); + vertices[v].mColor = Color::sWhite; + } + + // Calculate normals + for (size_t i = 0; i < inTriangles.size(); ++i) + { + const IndexedTriangleNoMaterial &tri = inTriangles[i]; + + // Calculate normal of face + Vec3 vtx[3]; + for (int j = 0; j < 3; ++j) + vtx[j] = Vec3::sLoadFloat3Unsafe(vertices[tri.mIdx[j]].mPosition); + Vec3 normal = ((vtx[1] - vtx[0]).Cross(vtx[2] - vtx[0])).Normalized(); + + // Add normal to all vertices in face + for (int j = 0; j < 3; ++j) + (Vec3::sLoadFloat3Unsafe(vertices[tri.mIdx[j]].mNormal) + normal).StoreFloat3(&vertices[tri.mIdx[j]].mNormal); + } + + // Renormalize vertex normals + for (size_t i = 0; i < vertices.size(); ++i) + Vec3::sLoadFloat3Unsafe(vertices[i].mNormal).Normalized().StoreFloat3(&vertices[i].mNormal); + + return CreateTriangleBatch(&vertices[0], (int)vertices.size(), &inTriangles[0].mIdx[0], (int)(3 * inTriangles.size())); +} + +DebugRenderer::Batch DebugRenderer::CreateTriangleBatchForConvex(SupportFunction inGetSupport, int inLevel, AABox *outBounds) +{ + JPH_PROFILE_FUNCTION(); + + Array vertices; + Array indices; + Create8thSphere(indices, vertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + + if (outBounds != nullptr) + *outBounds = sCalculateBounds(&vertices[0], (int)vertices.size()); + + return CreateTriangleBatch(vertices, indices); +} + +DebugRenderer::GeometryRef DebugRenderer::CreateTriangleGeometryForConvex(SupportFunction inGetSupport) +{ + GeometryRef geometry; + + // Iterate over levels + for (int level = sMaxLevel; level >= 1; --level) + { + // Determine at which distance this level should be active + float distance = sLODDistanceForLevel[sMaxLevel - level]; + + // Create triangle batch and only calculate bounds for highest LOD level + AABox bounds; + Batch batch = CreateTriangleBatchForConvex(inGetSupport, level, geometry == nullptr? &bounds : nullptr); + + // Construct geometry in the first iteration + if (geometry == nullptr) + geometry = new Geometry(bounds); + + // Add the LOD + geometry->mLODs.push_back({ batch, distance }); + } + + return geometry; +} + +void DebugRenderer::DrawBox(const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + RMat44 m = RMat44::sScale(Vec3::sMax(inBox.GetExtent(), Vec3::sReplicate(1.0e-6f))); // Prevent div by zero when one of the edges has length 0 + m.SetTranslation(RVec3(inBox.GetCenter())); + DrawGeometry(m, inColor, mBox, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 m = Mat44::sScale(Vec3::sMax(inBox.GetExtent(), Vec3::sReplicate(1.0e-6f))); // Prevent div by zero when one of the edges has length 0 + m.SetTranslation(inBox.GetCenter()); + DrawGeometry(inMatrix * m, inColor, mBox, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + RMat44 matrix = RMat44::sTranslation(inCenter) * Mat44::sScale(inRadius); + + DrawUnitSphere(matrix, inColor, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawUnitSphere(RMat44Arg inMatrix, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + DrawGeometry(inMatrix, inColor, mSphere, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawCapsule(RMat44Arg inMatrix, float inHalfHeightOfCylinder, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 scale_matrix = Mat44::sScale(inRadius); + + // Calculate world space bounding box + AABox local_bounds(Vec3(-inRadius, -inHalfHeightOfCylinder - inRadius, -inRadius), Vec3(inRadius, inHalfHeightOfCylinder + inRadius, inRadius)); + AABox world_bounds = local_bounds.Transformed(inMatrix); + + float radius_sq = Square(inRadius); + + // Draw bottom half sphere + RMat44 bottom_matrix = inMatrix * Mat44::sTranslation(Vec3(0, -inHalfHeightOfCylinder, 0)) * scale_matrix; + DrawGeometry(bottom_matrix, world_bounds, radius_sq, inColor, mCapsuleBottom, ECullMode::CullBackFace, inCastShadow, inDrawMode); + + // Draw top half sphere + RMat44 top_matrix = inMatrix * Mat44::sTranslation(Vec3(0, inHalfHeightOfCylinder, 0)) * scale_matrix; + DrawGeometry(top_matrix, world_bounds, radius_sq, inColor, mCapsuleTop, ECullMode::CullBackFace, inCastShadow, inDrawMode); + + // Draw middle part + DrawGeometry(inMatrix * Mat44::sScale(Vec3(inRadius, inHalfHeightOfCylinder, inRadius)), world_bounds, radius_sq, inColor, mCapsuleMid, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawCylinder(RMat44Arg inMatrix, float inHalfHeight, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 local_transform(Vec4(inRadius, 0, 0, 0), Vec4(0, inHalfHeight, 0, 0), Vec4(0, 0, inRadius, 0), Vec4(0, 0, 0, 1)); + RMat44 transform = inMatrix * local_transform; + + DrawGeometry(transform, mCylinder->mBounds.Transformed(transform), Square(inRadius), inColor, mCylinder, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inPerpendicular.IsNormalized(1.0e-4f)); + JPH_ASSERT(abs(inPerpendicular.Dot(inAxis)) < 1.0e-4f); + + Vec3 axis = Sign(inHalfAngle) * inLength * inAxis; + float scale = inLength * Tan(abs(inHalfAngle)); + if (scale != 0.0f) + { + Vec3 perp1 = scale * inPerpendicular; + Vec3 perp2 = scale * inAxis.Cross(inPerpendicular); + RMat44 transform(Vec4(perp1, 0), Vec4(axis, 0), Vec4(perp2, 0), inTop); + DrawGeometry(transform, inColor, mOpenCone, ECullMode::Off, inCastShadow, inDrawMode); + } +} + +DebugRenderer::Geometry *DebugRenderer::CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices) +{ + // Allocate space for vertices + int num_vertices = 2 * inNumSegments; + Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex)); + Vertex *vertices = vertices_start; + + for (int i = 0; i < inNumSegments; ++i) + { + // Get output vertices + Vertex &top = *(vertices++); + Vertex &bottom = *(vertices++); + + // Get local position + const Vec3 &pos = inVertices[i]; + + // Get local normal + const Vec3 &prev_pos = inVertices[(i + inNumSegments - 1) % inNumSegments]; + const Vec3 &next_pos = inVertices[(i + 1) % inNumSegments]; + Vec3 normal = 0.5f * (next_pos.Cross(pos).NormalizedOr(Vec3::sZero()) + pos.Cross(prev_pos).NormalizedOr(Vec3::sZero())); + + // Store top vertex + top.mPosition = { 0, 0, 0 }; + normal.StoreFloat3(&top.mNormal); + top.mColor = Color::sWhite; + top.mUV = { 0, 0 }; + + // Store bottom vertex + pos.StoreFloat3(&bottom.mPosition); + normal.StoreFloat3(&bottom.mNormal); + bottom.mColor = Color::sWhite; + bottom.mUV = { 0, 0 }; + } + + // Allocate space for indices + int num_indices = 3 * inNumSegments; + uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32)); + uint32 *indices = indices_start; + + // Calculate indices + for (int i = 0; i < inNumSegments; ++i) + { + int first = 2 * i; + int second = (first + 3) % num_vertices; + int third = first + 1; + + // Triangle + *indices++ = first; + *indices++ = second; + *indices++ = third; + } + + // Convert to triangle batch + return new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices)); +} + +void DebugRenderer::DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + // Assert sane input + JPH_ASSERT(inSwingYHalfAngle >= 0.0f && inSwingYHalfAngle <= JPH_PI); + JPH_ASSERT(inSwingZHalfAngle >= 0.0f && inSwingZHalfAngle <= JPH_PI); + JPH_ASSERT(inEdgeLength > 0.0f); + + // Check cache + SwingConeLimits limits { inSwingYHalfAngle, inSwingZHalfAngle }; + GeometryRef &geometry = mSwingConeLimits[limits]; + if (geometry == nullptr) + { + SwingConeBatches::iterator it = mPrevSwingConeLimits.find(limits); + if (it != mPrevSwingConeLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + // Number of segments to draw the cone with + const int num_segments = 64; + int half_num_segments = num_segments / 2; + + // The y and z values of the quaternion are limited to an ellipse, e1 and e2 are the radii of this ellipse + float e1 = Sin(0.5f * inSwingZHalfAngle); + float e2 = Sin(0.5f * inSwingYHalfAngle); + + // Check if the limits will draw something + if ((e1 <= 0.0f && e2 <= 0.0f) || (e2 >= 1.0f && e1 >= 1.0f)) + return; + + // Calculate squared values + float e1_sq = Square(e1); + float e2_sq = Square(e2); + + // Calculate local space vertices for shape + Vec3 ls_vertices[num_segments]; + int tgt_vertex = 0; + for (int side_iter = 0; side_iter < 2; ++side_iter) + for (int segment_iter = 0; segment_iter < half_num_segments; ++segment_iter) + { + float y, z; + if (e2_sq > e1_sq) + { + // Trace the y value of the quaternion + y = e2 - 2.0f * segment_iter * e2 / half_num_segments; + + // Calculate the corresponding z value of the quaternion + float z_sq = e1_sq - e1_sq / e2_sq * Square(y); + z = z_sq <= 0.0f? 0.0f : sqrt(z_sq); + } + else + { + // Trace the z value of the quaternion + z = -e1 + 2.0f * segment_iter * e1 / half_num_segments; + + // Calculate the corresponding y value of the quaternion + float y_sq = e2_sq - e2_sq / e1_sq * Square(z); + y = y_sq <= 0.0f? 0.0f : sqrt(y_sq); + } + + // If we're tracing the opposite side, flip the values + if (side_iter == 1) + { + z = -z; + y = -y; + } + + // Create quaternion + Vec3 q_xyz(0, y, z); + float w = sqrt(1.0f - q_xyz.LengthSq()); + Quat q(Vec4(q_xyz, w)); + + // Store vertex + ls_vertices[tgt_vertex++] = q.RotateAxisX(); + } + + geometry = CreateSwingLimitGeometry(num_segments, ls_vertices); + } + + DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + // Assert sane input + JPH_ASSERT(inMinSwingYAngle <= inMaxSwingYAngle && inMinSwingZAngle <= inMaxSwingZAngle); + JPH_ASSERT(inEdgeLength > 0.0f); + + // Check cache + SwingPyramidLimits limits { inMinSwingYAngle, inMaxSwingYAngle, inMinSwingZAngle, inMaxSwingZAngle }; + GeometryRef &geometry = mSwingPyramidLimits[limits]; + if (geometry == nullptr) + { + SwingPyramidBatches::iterator it = mPrevSwingPyramidLimits.find(limits); + if (it != mPrevSwingPyramidLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + // Number of segments to draw the cone with + const int num_segments = 64; + int quarter_num_segments = num_segments / 4; + + // Note that this is q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y) with q.x set to zero so we don't introduce twist + // This matches the calculation in SwingTwistConstraintPart::ClampSwingTwist + auto get_axis = [](float inY, float inZ) { + float hy = 0.5f * inY; + float hz = 0.5f * inZ; + float cos_hy = Cos(hy); + float cos_hz = Cos(hz); + return Quat(0, Sin(hy) * cos_hz, cos_hy * Sin(hz), cos_hy * cos_hz).Normalized().RotateAxisX(); + }; + + // Calculate local space vertices for shape + Vec3 ls_vertices[num_segments]; + int tgt_vertex = 0; + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle, inMaxSwingZAngle - (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle + (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMinSwingZAngle); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle, inMinSwingZAngle + (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle - (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMaxSwingZAngle); + + geometry = CreateSwingLimitGeometry(num_segments, ls_vertices); + } + + DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + if (inMinAngle >= inMaxAngle) + return; + + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inNormal.IsNormalized(1.0e-4f)); + JPH_ASSERT(abs(inNormal.Dot(inAxis)) < 1.0e-4f); + + // Pies have a unique batch based on the difference between min and max angle + float delta_angle = inMaxAngle - inMinAngle; + GeometryRef &geometry = mPieLimits[delta_angle]; + if (geometry == nullptr) + { + PieBatces::iterator it = mPrevPieLimits.find(delta_angle); + if (it != mPrevPieLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + int num_parts = (int)ceil(64.0f * delta_angle / (2.0f * JPH_PI)); + + Float3 normal = { 0, 1, 0 }; + Float3 center = { 0, 0, 0 }; + + // Allocate space for vertices + int num_vertices = num_parts + 2; + Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex)); + Vertex *vertices = vertices_start; + + // Center of circle + *vertices++ = { center, normal, { 0, 0 }, Color::sWhite }; + + // Outer edge of pie + for (int i = 0; i <= num_parts; ++i) + { + float angle = float(i) / float(num_parts) * delta_angle; + + Float3 pos = { Cos(angle), 0, Sin(angle) }; + *vertices++ = { pos, normal, { 0, 0 }, Color::sWhite }; + } + + // Allocate space for indices + int num_indices = num_parts * 3; + uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32)); + uint32 *indices = indices_start; + + for (int i = 0; i < num_parts; ++i) + { + *indices++ = 0; + *indices++ = i + 1; + *indices++ = i + 2; + } + + // Convert to triangle batch + geometry = new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices)); + } + + // Construct matrix that transforms pie into world space + RMat44 matrix = RMat44(Vec4(inRadius * inAxis, 0), Vec4(inRadius * inNormal, 0), Vec4(inRadius * inNormal.Cross(inAxis), 0), inCenter) * Mat44::sRotationY(-inMinAngle); + + DrawGeometry(matrix, inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawTaperedCylinder(RMat44Arg inMatrix, float inTop, float inBottom, float inTopRadius, float inBottomRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + TaperedCylinder tapered_cylinder { inTop, inBottom, inTopRadius, inBottomRadius }; + + GeometryRef &geometry = mTaperedCylinders[tapered_cylinder]; + if (geometry == nullptr) + { + TaperedCylinderBatces::iterator it = mPrevTaperedCylinders.find(tapered_cylinder); + if (it != mPrevTaperedCylinders.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + float max_radius = max(inTopRadius, inBottomRadius); + geometry = new Geometry(AABox(Vec3(-max_radius, inBottom, -max_radius), Vec3(max_radius, inTop, max_radius))); + + for (int level = sMaxLevel; level >= 1; --level) + geometry->mLODs.push_back({ CreateCylinder(inTop, inBottom, inTopRadius, inBottomRadius, level), sLODDistanceForLevel[sMaxLevel - level] }); + } + + DrawGeometry(inMatrix, inColor, geometry, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::NextFrame() +{ + mPrevSwingConeLimits.clear(); + std::swap(mSwingConeLimits, mPrevSwingConeLimits); + + mPrevSwingPyramidLimits.clear(); + std::swap(mSwingPyramidLimits, mPrevSwingPyramidLimits); + + mPrevPieLimits.clear(); + std::swap(mPieLimits, mPrevPieLimits); + + mPrevTaperedCylinders.clear(); + std::swap(mTaperedCylinders, mPrevTaperedCylinders); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h new file mode 100644 index 000000000000..7e06ed49a4aa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h @@ -0,0 +1,383 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#ifndef JPH_DEBUG_RENDERER_EXPORT + // By default export the debug renderer + #define JPH_DEBUG_RENDERER_EXPORT JPH_EXPORT +#endif // !JPH_DEBUG_RENDERER_EXPORT + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class OrientedBox; + +/// Simple triangle renderer for debugging purposes. +/// +/// Inherit from this class to provide your own implementation. +/// +/// Implement the following virtual functions: +/// - DrawLine +/// - DrawTriangle +/// - DrawText3D +/// - CreateTriangleBatch +/// - DrawGeometry +/// +/// Make sure you call Initialize() from the constructor of your implementation. +/// +/// The CreateTriangleBatch is used to prepare a batch of triangles to be drawn by a single DrawGeometry call, +/// which means that Jolt can render a complex scene much more efficiently than when each triangle in that scene would have been drawn through DrawTriangle. +/// +/// Note that an implementation that implements CreateTriangleBatch and DrawGeometry is provided by DebugRendererSimple which can be used to start quickly. +class JPH_DEBUG_RENDERER_EXPORT DebugRenderer : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRenderer(); + virtual ~DebugRenderer(); + + /// Call once after frame is complete. Releases unused dynamically generated geometry assets. + void NextFrame(); + + /// Draw line + virtual void DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) = 0; + + /// Draw wireframe box + void DrawWireBox(const AABox &inBox, ColorArg inColor); + void DrawWireBox(const OrientedBox &inBox, ColorArg inColor); + void DrawWireBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor); + + /// Draw a marker on a position + void DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize); + + /// Draw an arrow + void DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize); + + /// Draw coordinate system (3 arrows, x = red, y = green, z = blue) + void DrawCoordinateSystem(RMat44Arg inTransform, float inSize = 1.0f); + + /// Draw a plane through inPoint with normal inNormal + void DrawPlane(RVec3Arg inPoint, Vec3Arg inNormal, ColorArg inColor, float inSize); + + /// Draw wireframe triangle + void DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor); + + /// Draw a wireframe polygon + template + void DrawWirePolygon(RMat44Arg inTransform, const VERTEX_ARRAY &inVertices, ColorArg inColor, float inArrowSize = 0.0f) { for (typename VERTEX_ARRAY::size_type i = 0; i < inVertices.size(); ++i) DrawArrow(inTransform * inVertices[i], inTransform * inVertices[(i + 1) % inVertices.size()], inColor, inArrowSize); } + + /// Draw wireframe sphere + void DrawWireSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, int inLevel = 3); + void DrawWireUnitSphere(RMat44Arg inMatrix, ColorArg inColor, int inLevel = 3); + + /// Enum that determines if a shadow should be cast or not + enum class ECastShadow + { + On, ///< This shape should cast a shadow + Off ///< This shape should not cast a shadow + }; + + /// Determines how triangles are drawn + enum class EDrawMode + { + Solid, ///< Draw as a solid shape + Wireframe, ///< Draw as wireframe + }; + + /// Draw a single back face culled triangle + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::Off) = 0; + + /// Draw a box + void DrawBox(const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + void DrawBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a sphere + void DrawSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + void DrawUnitSphere(RMat44Arg inMatrix, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a capsule with one half sphere at (0, -inHalfHeightOfCylinder, 0) and the other half sphere at (0, inHalfHeightOfCylinder, 0) and radius inRadius. + /// The capsule will be transformed by inMatrix. + void DrawCapsule(RMat44Arg inMatrix, float inHalfHeightOfCylinder, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a cylinder with top (0, inHalfHeight, 0) and bottom (0, -inHalfHeight, 0) and radius inRadius. + /// The cylinder will be transformed by inMatrix + void DrawCylinder(RMat44Arg inMatrix, float inHalfHeight, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a bottomless cone. + /// @param inTop Top of cone, center of base is at inTop + inAxis. + /// @param inAxis Height and direction of cone + /// @param inPerpendicular Perpendicular vector to inAxis. + /// @param inHalfAngle Specifies the cone angle in radians (angle measured between inAxis and cone surface). + /// @param inLength The length of the cone. + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draws cone rotation limits as used by the SwingTwistConstraintPart. + /// @param inMatrix Matrix that transforms from constraint space to world space + /// @param inSwingYHalfAngle See SwingTwistConstraintPart + /// @param inSwingZHalfAngle See SwingTwistConstraintPart + /// @param inEdgeLength Size of the edge of the cone shape + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draws rotation limits as used by the SwingTwistConstraintPart. + /// @param inMatrix Matrix that transforms from constraint space to world space + /// @param inMinSwingYAngle See SwingTwistConstraintPart + /// @param inMaxSwingYAngle See SwingTwistConstraintPart + /// @param inMinSwingZAngle See SwingTwistConstraintPart + /// @param inMaxSwingZAngle See SwingTwistConstraintPart + /// @param inEdgeLength Size of the edge of the cone shape + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a pie (part of a circle). + /// @param inCenter The center of the circle. + /// @param inRadius Radius of the circle. + /// @param inNormal The plane normal in which the pie resides. + /// @param inAxis The axis that defines an angle of 0 radians. + /// @param inMinAngle The pie will be drawn between [inMinAngle, inMaxAngle] (in radians). + /// @param inMaxAngle The pie will be drawn between [inMinAngle, inMaxAngle] (in radians). + /// @param inColor Color to use for drawing the pie. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a tapered cylinder + /// @param inMatrix Matrix that transforms the cylinder to world space. + /// @param inTop Top of cylinder (along Y axis) + /// @param inBottom Bottom of cylinder (along Y axis) + /// @param inTopRadius Radius at the top + /// @param inBottomRadius Radius at the bottom + /// @param inColor Color to use for drawing the pie. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawTaperedCylinder(RMat44Arg inMatrix, float inTop, float inBottom, float inTopRadius, float inBottomRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Singleton instance + static DebugRenderer * sInstance; + + /// Vertex format used by the triangle renderer + class Vertex + { + public: + Float3 mPosition; + Float3 mNormal; + Float2 mUV; + Color mColor; + }; + + /// A single triangle + class JPH_DEBUG_RENDERER_EXPORT Triangle + { + public: + Triangle() = default; + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor); + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor, Vec3Arg inUVOrigin, Vec3Arg inUVDirection); + + Vertex mV[3]; + }; + + /// Handle for a batch of triangles + using Batch = Ref; + + /// A single level of detail + class LOD + { + public: + Batch mTriangleBatch; + float mDistance; + }; + + /// A geometry primitive containing triangle batches for various lods + class Geometry : public RefTarget + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Geometry(const AABox &inBounds) : mBounds(inBounds) { } + Geometry(const Batch &inBatch, const AABox &inBounds) : mBounds(inBounds) { mLODs.push_back({ inBatch, FLT_MAX }); } + + /// Determine which LOD to render + /// @param inCameraPosition Current position of the camera + /// @param inWorldSpaceBounds World space bounds for this geometry (transform mBounds by model space matrix) + /// @param inLODScaleSq is the squared scale of the model matrix, it is multiplied with the LOD distances in inGeometry to calculate the real LOD distance (so a number > 1 will force a higher LOD). + /// @return The selected LOD. + const LOD & GetLOD(Vec3Arg inCameraPosition, const AABox &inWorldSpaceBounds, float inLODScaleSq) const + { + float dist_sq = inWorldSpaceBounds.GetSqDistanceTo(inCameraPosition); + for (const LOD &lod : mLODs) + if (dist_sq <= inLODScaleSq * Square(lod.mDistance)) + return lod; + + return mLODs.back(); + } + + /// All level of details for this mesh + Array mLODs; + + /// Bounding box that encapsulates all LODs + AABox mBounds; + }; + + /// Handle for a lodded triangle batch + using GeometryRef = Ref; + + /// Calculate bounding box for a batch of triangles + static AABox sCalculateBounds(const Vertex *inVertices, int inVertexCount); + + /// Create a batch of triangles that can be drawn efficiently + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) = 0; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) = 0; + Batch CreateTriangleBatch(const Array &inTriangles) { return CreateTriangleBatch(inTriangles.empty()? nullptr : &inTriangles[0], int(inTriangles.size())); } + Batch CreateTriangleBatch(const Array &inVertices, const Array &inIndices) { return CreateTriangleBatch(inVertices.empty()? nullptr : &inVertices[0], int(inVertices.size()), inIndices.empty()? nullptr : &inIndices[0], int(inIndices.size())); } + Batch CreateTriangleBatch(const VertexList &inVertices, const IndexedTriangleNoMaterialList &inTriangles); + + /// Create a primitive for a convex shape using its support function + using SupportFunction = function; + Batch CreateTriangleBatchForConvex(SupportFunction inGetSupport, int inLevel, AABox *outBounds = nullptr); + GeometryRef CreateTriangleGeometryForConvex(SupportFunction inGetSupport); + + /// Determines which polygons are culled + enum class ECullMode + { + CullBackFace, ///< Don't draw backfacing polygons + CullFrontFace, ///< Don't draw front facing polygons + Off ///< Don't do culling and draw both sides + }; + + /// Draw some geometry + /// @param inModelMatrix is the matrix that transforms the geometry to world space. + /// @param inWorldSpaceBounds is the bounding box of the geometry after transforming it into world space. + /// @param inLODScaleSq is the squared scale of the model matrix, it is multiplied with the LOD distances in inGeometry to calculate the real LOD distance (so a number > 1 will force a higher LOD). + /// @param inModelColor is the color with which to multiply the vertex colors in inGeometry. + /// @param inGeometry The geometry to draw. + /// @param inCullMode determines which polygons are culled. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode = ECullMode::CullBackFace, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid) = 0; + void DrawGeometry(RMat44Arg inModelMatrix, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode = ECullMode::CullBackFace, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid) { DrawGeometry(inModelMatrix, inGeometry->mBounds.Transformed(inModelMatrix), max(max(inModelMatrix.GetAxisX().LengthSq(), inModelMatrix.GetAxisY().LengthSq()), inModelMatrix.GetAxisZ().LengthSq()), inModelColor, inGeometry, inCullMode, inCastShadow, inDrawMode); } + + /// Draw text + virtual void DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor = Color::sWhite, float inHeight = 0.5f) = 0; + +protected: + /// Initialize the system, must be called from the constructor of the DebugRenderer implementation + void Initialize(); + +private: + /// Recursive helper function for DrawWireUnitSphere + void DrawWireUnitSphereRecursive(RMat44Arg inMatrix, ColorArg inColor, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel); + + /// Helper functions to create a box + void CreateQuad(Array &ioIndices, Array &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4); + + /// Helper functions to create a vertex and index buffer for a sphere + void Create8thSphereRecursive(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel); + void Create8thSphere(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel); + + /// Helper functions to create a vertex and index buffer for a cylinder + Batch CreateCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, int inLevel); + + /// Helper function for DrawSwingConeLimits and DrawSwingPyramidLimits + Geometry * CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices); + + // Predefined shapes + GeometryRef mBox; + GeometryRef mSphere; + GeometryRef mCapsuleTop; + GeometryRef mCapsuleMid; + GeometryRef mCapsuleBottom; + GeometryRef mOpenCone; + GeometryRef mCylinder; + + struct SwingConeLimits + { + bool operator == (const SwingConeLimits &inRHS) const + { + return mSwingYHalfAngle == inRHS.mSwingYHalfAngle + && mSwingZHalfAngle == inRHS.mSwingZHalfAngle; + } + + float mSwingYHalfAngle; + float mSwingZHalfAngle; + }; + + JPH_MAKE_HASH_STRUCT(SwingConeLimits, SwingConeLimitsHasher, t.mSwingYHalfAngle, t.mSwingZHalfAngle) + + using SwingConeBatches = UnorderedMap; + SwingConeBatches mSwingConeLimits; + SwingConeBatches mPrevSwingConeLimits; + + struct SwingPyramidLimits + { + bool operator == (const SwingPyramidLimits &inRHS) const + { + return mMinSwingYAngle == inRHS.mMinSwingYAngle + && mMaxSwingYAngle == inRHS.mMaxSwingYAngle + && mMinSwingZAngle == inRHS.mMinSwingZAngle + && mMaxSwingZAngle == inRHS.mMaxSwingZAngle; + } + + float mMinSwingYAngle; + float mMaxSwingYAngle; + float mMinSwingZAngle; + float mMaxSwingZAngle; + }; + + JPH_MAKE_HASH_STRUCT(SwingPyramidLimits, SwingPyramidLimitsHasher, t.mMinSwingYAngle, t.mMaxSwingYAngle, t.mMinSwingZAngle, t.mMaxSwingZAngle) + + using SwingPyramidBatches = UnorderedMap; + SwingPyramidBatches mSwingPyramidLimits; + SwingPyramidBatches mPrevSwingPyramidLimits; + + using PieBatces = UnorderedMap; + PieBatces mPieLimits; + PieBatces mPrevPieLimits; + + struct TaperedCylinder + { + bool operator == (const TaperedCylinder &inRHS) const + { + return mTop == inRHS.mTop + && mBottom == inRHS.mBottom + && mTopRadius == inRHS.mTopRadius + && mBottomRadius == inRHS.mBottomRadius; + } + + float mTop; + float mBottom; + float mTopRadius; + float mBottomRadius; + }; + + JPH_MAKE_HASH_STRUCT(TaperedCylinder, TaperedCylinderHasher, t.mTop, t.mBottom, t.mTopRadius, t.mBottomRadius) + + using TaperedCylinderBatces = UnorderedMap; + TaperedCylinderBatces mTaperedCylinders; + TaperedCylinderBatces mPrevTaperedCylinders; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp new file mode 100644 index 000000000000..bea7e4e08f87 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp @@ -0,0 +1,168 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +void DebugRendererPlayback::Parse(StreamIn &inStream) +{ + using ECommand = DebugRendererRecorder::ECommand; + + for (;;) + { + // Read the next command + ECommand command; + inStream.Read(command); + + if (inStream.IsEOF() || inStream.IsFailed()) + return; + + if (command == ECommand::CreateBatch) + { + uint32 id; + inStream.Read(id); + + uint32 triangle_count; + inStream.Read(triangle_count); + + DebugRenderer::Triangle *triangles = new DebugRenderer::Triangle [triangle_count]; + inStream.ReadBytes(triangles, triangle_count * sizeof(DebugRenderer::Triangle)); + + mBatches.insert({ id, mRenderer.CreateTriangleBatch(triangles, triangle_count) }); + + delete [] triangles; + } + else if (command == ECommand::CreateBatchIndexed) + { + uint32 id; + inStream.Read(id); + + uint32 vertex_count; + inStream.Read(vertex_count); + + DebugRenderer::Vertex *vertices = new DebugRenderer::Vertex [vertex_count]; + inStream.ReadBytes(vertices, vertex_count * sizeof(DebugRenderer::Vertex)); + + uint32 index_count; + inStream.Read(index_count); + + uint32 *indices = new uint32 [index_count]; + inStream.ReadBytes(indices, index_count * sizeof(uint32)); + + mBatches.insert({ id, mRenderer.CreateTriangleBatch(vertices, vertex_count, indices, index_count) }); + + delete [] indices; + delete [] vertices; + } + else if (command == ECommand::CreateGeometry) + { + uint32 geometry_id; + inStream.Read(geometry_id); + + AABox bounds; + inStream.Read(bounds.mMin); + inStream.Read(bounds.mMax); + + DebugRenderer::GeometryRef geometry = new DebugRenderer::Geometry(bounds); + mGeometries[geometry_id] = geometry; + + uint32 num_lods; + inStream.Read(num_lods); + for (uint32 l = 0; l < num_lods; ++l) + { + DebugRenderer::LOD lod; + inStream.Read(lod.mDistance); + + uint32 batch_id; + inStream.Read(batch_id); + lod.mTriangleBatch = mBatches.find(batch_id)->second; + + geometry->mLODs.push_back(lod); + } + } + else if (command == ECommand::EndFrame) + { + mFrames.push_back({}); + Frame &frame = mFrames.back(); + + // Read all lines + uint32 num_lines = 0; + inStream.Read(num_lines); + frame.mLines.resize(num_lines); + for (DebugRendererRecorder::LineBlob &line : frame.mLines) + { + inStream.Read(line.mFrom); + inStream.Read(line.mTo); + inStream.Read(line.mColor); + } + + // Read all triangles + uint32 num_triangles = 0; + inStream.Read(num_triangles); + frame.mTriangles.resize(num_triangles); + for (DebugRendererRecorder::TriangleBlob &triangle : frame.mTriangles) + { + inStream.Read(triangle.mV1); + inStream.Read(triangle.mV2); + inStream.Read(triangle.mV3); + inStream.Read(triangle.mColor); + inStream.Read(triangle.mCastShadow); + } + + // Read all texts + uint32 num_texts = 0; + inStream.Read(num_texts); + frame.mTexts.resize(num_texts); + for (DebugRendererRecorder::TextBlob &text : frame.mTexts) + { + inStream.Read(text.mPosition); + inStream.Read(text.mString); + inStream.Read(text.mColor); + inStream.Read(text.mHeight); + } + + // Read all geometries + uint32 num_geometries = 0; + inStream.Read(num_geometries); + frame.mGeometries.resize(num_geometries); + for (DebugRendererRecorder::GeometryBlob &geom : frame.mGeometries) + { + inStream.Read(geom.mModelMatrix); + inStream.Read(geom.mModelColor); + inStream.Read(geom.mGeometryID); + inStream.Read(geom.mCullMode); + inStream.Read(geom.mCastShadow); + inStream.Read(geom.mDrawMode); + } + } + else + JPH_ASSERT(false); + } +} + +void DebugRendererPlayback::DrawFrame(uint inFrameNumber) const +{ + const Frame &frame = mFrames[inFrameNumber]; + + for (const DebugRendererRecorder::LineBlob &line : frame.mLines) + mRenderer.DrawLine(line.mFrom, line.mTo, line.mColor); + + for (const DebugRendererRecorder::TriangleBlob &triangle : frame.mTriangles) + mRenderer.DrawTriangle(triangle.mV1, triangle.mV2, triangle.mV3, triangle.mColor, triangle.mCastShadow); + + for (const DebugRendererRecorder::TextBlob &text : frame.mTexts) + mRenderer.DrawText3D(text.mPosition, text.mString, text.mColor, text.mHeight); + + for (const DebugRendererRecorder::GeometryBlob &geom : frame.mGeometries) + mRenderer.DrawGeometry(geom.mModelMatrix, geom.mModelColor, mGeometries.find(geom.mGeometryID)->second, geom.mCullMode, geom.mCastShadow, geom.mDrawMode); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h new file mode 100644 index 000000000000..23ed45423898 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that can read a recorded stream from DebugRendererRecorder and plays it back trough a DebugRenderer +class JPH_DEBUG_RENDERER_EXPORT DebugRendererPlayback +{ +public: + /// Constructor + DebugRendererPlayback(DebugRenderer &inRenderer) : mRenderer(inRenderer) { } + + /// Parse a stream of frames + void Parse(StreamIn &inStream); + + /// Get the number of parsed frames + uint GetNumFrames() const { return (uint)mFrames.size(); } + + /// Draw a frame + void DrawFrame(uint inFrameNumber) const; + +private: + /// The debug renderer we're using to do the actual rendering + DebugRenderer & mRenderer; + + /// Mapping of ID to batch + UnorderedMap mBatches; + + /// Mapping of ID to geometry + UnorderedMap mGeometries; + + /// The list of parsed frames + using Frame = DebugRendererRecorder::Frame; + Array mFrames; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp new file mode 100644 index 000000000000..2e25912957fa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +void DebugRendererRecorder::DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mLines.push_back({ inFrom, inTo, inColor }); +} + +void DebugRendererRecorder::DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mTriangles.push_back({ inV1, inV2, inV3, inColor, inCastShadow }); +} + +DebugRenderer::Batch DebugRendererRecorder::CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) +{ + if (inTriangles == nullptr || inTriangleCount == 0) + return new BatchImpl(0); + + lock_guard lock(mMutex); + + mStream.Write(ECommand::CreateBatch); + + uint32 batch_id = mNextBatchID++; + JPH_ASSERT(batch_id != 0); + mStream.Write(batch_id); + mStream.Write((uint32)inTriangleCount); + mStream.WriteBytes(inTriangles, inTriangleCount * sizeof(Triangle)); + + return new BatchImpl(batch_id); +} + +DebugRenderer::Batch DebugRendererRecorder::CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) +{ + if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0) + return new BatchImpl(0); + + lock_guard lock(mMutex); + + mStream.Write(ECommand::CreateBatchIndexed); + + uint32 batch_id = mNextBatchID++; + JPH_ASSERT(batch_id != 0); + mStream.Write(batch_id); + mStream.Write((uint32)inVertexCount); + mStream.WriteBytes(inVertices, inVertexCount * sizeof(Vertex)); + mStream.Write((uint32)inIndexCount); + mStream.WriteBytes(inIndices, inIndexCount * sizeof(uint32)); + + return new BatchImpl(batch_id); +} + +void DebugRendererRecorder::DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + lock_guard lock(mMutex); + + // See if this geometry was used before + uint32 &geometry_id = mGeometries[inGeometry]; + if (geometry_id == 0) + { + mStream.Write(ECommand::CreateGeometry); + + // Create a new ID + geometry_id = mNextGeometryID++; + JPH_ASSERT(geometry_id != 0); + mStream.Write(geometry_id); + + // Save bounds + mStream.Write(inGeometry->mBounds.mMin); + mStream.Write(inGeometry->mBounds.mMax); + + // Save the LODs + mStream.Write((uint32)inGeometry->mLODs.size()); + for (const LOD & lod : inGeometry->mLODs) + { + mStream.Write(lod.mDistance); + mStream.Write(static_cast(lod.mTriangleBatch.GetPtr())->mID); + } + } + + mCurrentFrame.mGeometries.push_back({ inModelMatrix, inModelColor, geometry_id, inCullMode, inCastShadow, inDrawMode }); +} + +void DebugRendererRecorder::DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mTexts.push_back({ inPosition, inString, inColor, inHeight }); +} + +void DebugRendererRecorder::EndFrame() +{ + lock_guard lock(mMutex); + + mStream.Write(ECommand::EndFrame); + + // Write all lines + mStream.Write((uint32)mCurrentFrame.mLines.size()); + for (const LineBlob &line : mCurrentFrame.mLines) + { + mStream.Write(line.mFrom); + mStream.Write(line.mTo); + mStream.Write(line.mColor); + } + mCurrentFrame.mLines.clear(); + + // Write all triangles + mStream.Write((uint32)mCurrentFrame.mTriangles.size()); + for (const TriangleBlob &triangle : mCurrentFrame.mTriangles) + { + mStream.Write(triangle.mV1); + mStream.Write(triangle.mV2); + mStream.Write(triangle.mV3); + mStream.Write(triangle.mColor); + mStream.Write(triangle.mCastShadow); + } + mCurrentFrame.mTriangles.clear(); + + // Write all texts + mStream.Write((uint32)mCurrentFrame.mTexts.size()); + for (const TextBlob &text : mCurrentFrame.mTexts) + { + mStream.Write(text.mPosition); + mStream.Write(text.mString); + mStream.Write(text.mColor); + mStream.Write(text.mHeight); + } + mCurrentFrame.mTexts.clear(); + + // Write all geometries + mStream.Write((uint32)mCurrentFrame.mGeometries.size()); + for (const GeometryBlob &geom : mCurrentFrame.mGeometries) + { + mStream.Write(geom.mModelMatrix); + mStream.Write(geom.mModelColor); + mStream.Write(geom.mGeometryID); + mStream.Write(geom.mCullMode); + mStream.Write(geom.mCastShadow); + mStream.Write(geom.mDrawMode); + } + mCurrentFrame.mGeometries.clear(); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h new file mode 100644 index 000000000000..9608e03c908b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of DebugRenderer that records the API invocations to be played back later +class JPH_DEBUG_RENDERER_EXPORT DebugRendererRecorder final : public DebugRenderer +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRendererRecorder(StreamOut &inStream) : mStream(inStream) { Initialize(); } + + /// Implementation of DebugRenderer interface + virtual void DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) override; + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) override; + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) override; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) override; + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override; + virtual void DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) override; + + /// Mark the end of a frame + void EndFrame(); + + /// Control commands written into the stream + enum class ECommand : uint8 + { + CreateBatch, + CreateBatchIndexed, + CreateGeometry, + EndFrame + }; + + /// Holds a single line segment + struct LineBlob + { + RVec3 mFrom; + RVec3 mTo; + Color mColor; + }; + + /// Holds a single triangle + struct TriangleBlob + { + RVec3 mV1; + RVec3 mV2; + RVec3 mV3; + Color mColor; + ECastShadow mCastShadow; + }; + + /// Holds a single text entry + struct TextBlob + { + TextBlob() = default; + TextBlob(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) : mPosition(inPosition), mString(inString), mColor(inColor), mHeight(inHeight) { } + + RVec3 mPosition; + String mString; + Color mColor; + float mHeight; + }; + + /// Holds a single geometry draw call + struct GeometryBlob + { + RMat44 mModelMatrix; + Color mModelColor; + uint32 mGeometryID; + ECullMode mCullMode; + ECastShadow mCastShadow; + EDrawMode mDrawMode; + }; + + /// All information for a single frame + struct Frame + { + Array mLines; + Array mTriangles; + Array mTexts; + Array mGeometries; + }; + +private: + /// Implementation specific batch object + class BatchImpl : public RefTargetVirtual + { + public: + JPH_OVERRIDE_NEW_DELETE + + BatchImpl(uint32 inID) : mID(inID) { } + + virtual void AddRef() override { ++mRefCount; } + virtual void Release() override { if (--mRefCount == 0) delete this; } + + atomic mRefCount = 0; + uint32 mID; + }; + + /// Lock that prevents concurrent access to the internal structures + Mutex mMutex; + + /// Stream that recorded data will be sent to + StreamOut & mStream; + + /// Next available ID + uint32 mNextBatchID = 1; + uint32 mNextGeometryID = 1; + + /// Cached geometries and their IDs + UnorderedMap mGeometries; + + /// Data that is being accumulated for the current frame + Frame mCurrentFrame; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp new file mode 100644 index 000000000000..a404d95a002c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +DebugRendererSimple::DebugRendererSimple() +{ + Initialize(); +} + +DebugRenderer::Batch DebugRendererSimple::CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) +{ + BatchImpl *batch = new BatchImpl; + if (inTriangles == nullptr || inTriangleCount == 0) + return batch; + + batch->mTriangles.assign(inTriangles, inTriangles + inTriangleCount); + return batch; +} + +DebugRenderer::Batch DebugRendererSimple::CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) +{ + BatchImpl *batch = new BatchImpl; + if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0) + return batch; + + // Convert indexed triangle list to triangle list + batch->mTriangles.resize(inIndexCount / 3); + for (size_t t = 0; t < batch->mTriangles.size(); ++t) + { + Triangle &triangle = batch->mTriangles[t]; + triangle.mV[0] = inVertices[inIndices[t * 3 + 0]]; + triangle.mV[1] = inVertices[inIndices[t * 3 + 1]]; + triangle.mV[2] = inVertices[inIndices[t * 3 + 2]]; + } + + return batch; +} + +void DebugRendererSimple::DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + // Figure out which LOD to use + const LOD *lod = inGeometry->mLODs.data(); + if (mCameraPosSet) + lod = &inGeometry->GetLOD(Vec3(mCameraPos), inWorldSpaceBounds, inLODScaleSq); + + // Draw the batch + const BatchImpl *batch = static_cast(lod->mTriangleBatch.GetPtr()); + for (const Triangle &triangle : batch->mTriangles) + { + RVec3 v0 = inModelMatrix * Vec3(triangle.mV[0].mPosition); + RVec3 v1 = inModelMatrix * Vec3(triangle.mV[1].mPosition); + RVec3 v2 = inModelMatrix * Vec3(triangle.mV[2].mPosition); + Color color = inModelColor * triangle.mV[0].mColor; + + switch (inDrawMode) + { + case EDrawMode::Wireframe: + DrawLine(v0, v1, color); + DrawLine(v1, v2, color); + DrawLine(v2, v0, color); + break; + + case EDrawMode::Solid: + DrawTriangle(v0, v1, v2, color, inCastShadow); + break; + } + } +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h new file mode 100644 index 000000000000..4a23ab758b71 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h @@ -0,0 +1,88 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +/// Inherit from this class to simplify implementing a debug renderer, start with this implementation: +/// +/// class MyDebugRenderer : public JPH::DebugRendererSimple +/// { +/// public: +/// virtual void DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) override +/// { +/// // Implement +/// } +/// +/// virtual void DrawTriangle(JPH::RVec3Arg inV1, JPH::RVec3Arg inV2, JPH::RVec3Arg inV3, JPH::ColorArg inColor, ECastShadow inCastShadow) override +/// { +/// // Implement +/// } +/// +/// virtual void DrawText3D(JPH::RVec3Arg inPosition, const string_view &inString, JPH::ColorArg inColor, float inHeight) override +/// { +/// // Implement +/// } +/// }; +/// +/// Note that this class is meant to be a quick start for implementing a debug renderer, it is not the most efficient way to implement a debug renderer. +class JPH_DEBUG_RENDERER_EXPORT DebugRendererSimple : public DebugRenderer +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRendererSimple(); + + /// Should be called every frame by the application to provide the camera position. + /// This is used to determine the correct LOD for rendering. + void SetCameraPos(RVec3Arg inCameraPos) + { + mCameraPos = inCameraPos; + mCameraPosSet = true; + } + + /// Fallback implementation that uses DrawLine to draw a triangle (override this if you have a version that renders solid triangles) + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) override + { + DrawLine(inV1, inV2, inColor); + DrawLine(inV2, inV3, inColor); + DrawLine(inV3, inV1, inColor); + } + +protected: + /// Implementation of DebugRenderer interface + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) override; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) override; + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override; + +private: + /// Implementation specific batch object + class BatchImpl : public RefTargetVirtual + { + public: + JPH_OVERRIDE_NEW_DELETE + + virtual void AddRef() override { ++mRefCount; } + virtual void Release() override { if (--mRefCount == 0) delete this; } + + Array mTriangles; + + private: + atomic mRefCount = 0; + }; + + /// Last provided camera position + RVec3 mCameraPos; + bool mCameraPosSet = false; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp new file mode 100644 index 000000000000..4bf87007e9a8 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::JointState) +{ + JPH_ADD_ATTRIBUTE(JointState, mRotation) + JPH_ADD_ATTRIBUTE(JointState, mTranslation) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::Keyframe) +{ + JPH_ADD_BASE_CLASS(Keyframe, JointState) + + JPH_ADD_ATTRIBUTE(Keyframe, mTime) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::AnimatedJoint) +{ + JPH_ADD_ATTRIBUTE(AnimatedJoint, mJointName) + JPH_ADD_ATTRIBUTE(AnimatedJoint, mKeyframes) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation) +{ + JPH_ADD_ATTRIBUTE(SkeletalAnimation, mAnimatedJoints) + JPH_ADD_ATTRIBUTE(SkeletalAnimation, mIsLooping) +} + + +void SkeletalAnimation::JointState::FromMatrix(Mat44Arg inMatrix) +{ + mRotation = inMatrix.GetQuaternion(); + mTranslation = inMatrix.GetTranslation(); +} + +float SkeletalAnimation::GetDuration() const +{ + if (!mAnimatedJoints.empty() && !mAnimatedJoints[0].mKeyframes.empty()) + return mAnimatedJoints[0].mKeyframes.back().mTime; + else + return 0.0f; +} + +void SkeletalAnimation::ScaleJoints(float inScale) +{ + for (SkeletalAnimation::AnimatedJoint &j : mAnimatedJoints) + for (SkeletalAnimation::Keyframe &k : j.mKeyframes) + k.mTranslation *= inScale; +} + +void SkeletalAnimation::Sample(float inTime, SkeletonPose &ioPose) const +{ + // Correct time when animation is looping + JPH_ASSERT(inTime >= 0.0f); + float duration = GetDuration(); + float time = duration > 0.0f && mIsLooping? fmod(inTime, duration) : inTime; + + for (const AnimatedJoint &aj : mAnimatedJoints) + { + // Do binary search for keyframe + int high = (int)aj.mKeyframes.size(), low = -1; + while (high - low > 1) + { + int probe = (high + low) / 2; + if (aj.mKeyframes[probe].mTime < time) + low = probe; + else + high = probe; + } + + JointState &state = ioPose.GetJoint(ioPose.GetSkeleton()->GetJointIndex(aj.mJointName)); + + if (low == -1) + { + // Before first key, return first key + state = static_cast(aj.mKeyframes.front()); + } + else if (high == (int)aj.mKeyframes.size()) + { + // Beyond last key, return last key + state = static_cast(aj.mKeyframes.back()); + } + else + { + // Interpolate + const Keyframe &s1 = aj.mKeyframes[low]; + const Keyframe &s2 = aj.mKeyframes[low + 1]; + + float fraction = (time - s1.mTime) / (s2.mTime - s1.mTime); + JPH_ASSERT(fraction >= 0.0f && fraction <= 1.0f); + + state.mTranslation = (1.0f - fraction) * s1.mTranslation + fraction * s2.mTranslation; + JPH_ASSERT(s1.mRotation.IsNormalized()); + JPH_ASSERT(s2.mRotation.IsNormalized()); + state.mRotation = s1.mRotation.SLERP(s2.mRotation, fraction); + JPH_ASSERT(state.mRotation.IsNormalized()); + } + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h new file mode 100644 index 000000000000..344c6046fa10 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h @@ -0,0 +1,77 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class SkeletonPose; + +/// Resource for a skinned animation +class JPH_EXPORT SkeletalAnimation : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkeletalAnimation) + +public: + /// Contains the current state of a joint, a local space transformation relative to its parent joint + class JointState + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, JointState) + + public: + /// Convert from a local space matrix + void FromMatrix(Mat44Arg inMatrix); + + /// Convert to matrix representation + inline Mat44 ToMatrix() const { return Mat44::sRotationTranslation(mRotation, mTranslation); } + + Quat mRotation = Quat::sIdentity(); ///< Local space rotation of the joint + Vec3 mTranslation = Vec3::sZero(); ///< Local space translation of the joint + }; + + /// Contains the state of a single joint at a particular time + class Keyframe : public JointState + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Keyframe) + + public: + float mTime = 0.0f; ///< Time of keyframe in seconds + }; + + using KeyframeVector = Array; + + /// Contains the animation for a single joint + class AnimatedJoint + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AnimatedJoint) + + public: + String mJointName; ///< Name of the joint + KeyframeVector mKeyframes; ///< List of keyframes over time + }; + + using AnimatedJointVector = Array; + + /// Get the length (in seconds) of this animation + float GetDuration() const; + + /// Scale the size of all joints by inScale + void ScaleJoints(float inScale); + + /// Get the (interpolated) joint transforms at time inTime + void Sample(float inTime, SkeletonPose &ioPose) const; + + /// Get joint samples + const AnimatedJointVector & GetAnimatedJoints() const { return mAnimatedJoints; } + AnimatedJointVector & GetAnimatedJoints() { return mAnimatedJoints; } + +private: + AnimatedJointVector mAnimatedJoints; ///< List of joints and keyframes + bool mIsLooping = true; ///< If this animation loops back to start +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp new file mode 100644 index 000000000000..5ab7f56e792a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp @@ -0,0 +1,82 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton::Joint) +{ + JPH_ADD_ATTRIBUTE(Joint, mName) + JPH_ADD_ATTRIBUTE(Joint, mParentName) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton) +{ + JPH_ADD_ATTRIBUTE(Skeleton, mJoints) +} + +int Skeleton::GetJointIndex(const string_view &inName) const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + if (mJoints[i].mName == inName) + return i; + + return -1; +} + +void Skeleton::CalculateParentJointIndices() +{ + for (Joint &j : mJoints) + j.mParentJointIndex = GetJointIndex(j.mParentName); +} + +bool Skeleton::AreJointsCorrectlyOrdered() const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + if (mJoints[i].mParentJointIndex >= i) + return false; + + return true; +} + +void Skeleton::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write((uint32)mJoints.size()); + for (const Joint &j : mJoints) + { + inStream.Write(j.mName); + inStream.Write(j.mParentJointIndex); + inStream.Write(j.mParentName); + } +} + +Skeleton::SkeletonResult Skeleton::sRestoreFromBinaryState(StreamIn &inStream) +{ + Ref skeleton = new Skeleton; + + uint32 len = 0; + inStream.Read(len); + skeleton->mJoints.resize(len); + for (Joint &j : skeleton->mJoints) + { + inStream.Read(j.mName); + inStream.Read(j.mParentJointIndex); + inStream.Read(j.mParentName); + } + + SkeletonResult result; + if (inStream.IsEOF() || inStream.IsFailed()) + result.SetError("Failed to read skeleton from stream"); + else + result.Set(skeleton); + return result; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h new file mode 100644 index 000000000000..f53df31dfdaa --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Resource that contains the joint hierarchy for a skeleton +class JPH_EXPORT Skeleton : public RefTarget +{ + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skeleton) + +public: + using SkeletonResult = Result>; + + /// Declare internal structure for a joint + class Joint + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Joint) + + public: + Joint() = default; + Joint(const string_view &inName, const string_view &inParentName, int inParentJointIndex) : mName(inName), mParentName(inParentName), mParentJointIndex(inParentJointIndex) { } + + String mName; ///< Name of the joint + String mParentName; ///< Name of parent joint + int mParentJointIndex = -1; ///< Index of parent joint (in mJoints) or -1 if it has no parent + }; + + using JointVector = Array; + + ///@name Access to the joints + ///@{ + const JointVector & GetJoints() const { return mJoints; } + JointVector & GetJoints() { return mJoints; } + int GetJointCount() const { return (int)mJoints.size(); } + const Joint & GetJoint(int inJoint) const { return mJoints[inJoint]; } + Joint & GetJoint(int inJoint) { return mJoints[inJoint]; } + uint AddJoint(const string_view &inName, const string_view &inParentName = string_view()) { mJoints.emplace_back(inName, inParentName, -1); return (uint)mJoints.size() - 1; } + uint AddJoint(const string_view &inName, int inParentIndex) { mJoints.emplace_back(inName, inParentIndex >= 0? mJoints[inParentIndex].mName : String(), inParentIndex); return (uint)mJoints.size() - 1; } + ///@} + + /// Find joint by name + int GetJointIndex(const string_view &inName) const; + + /// Fill in parent joint indices based on name + void CalculateParentJointIndices(); + + /// Many of the algorithms that use the Skeleton class require that parent joints are in the mJoints array before their children. + /// This function returns true if this is the case, false if not. + bool AreJointsCorrectlyOrdered() const; + + /// Saves the state of this object in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. + static SkeletonResult sRestoreFromBinaryState(StreamIn &inStream); + +private: + /// Joints + JointVector mJoints; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp new file mode 100644 index 000000000000..add2956d541b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp @@ -0,0 +1,237 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void SkeletonMapper::Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint) +{ + JPH_ASSERT(mMappings.empty() && mChains.empty() && mUnmapped.empty()); // Should not be initialized yet + + // Count joints + int n1 = inSkeleton1->GetJointCount(); + int n2 = inSkeleton2->GetJointCount(); + JPH_ASSERT(n1 <= n2, "Skeleton 1 should be the low detail skeleton!"); + + // Keep track of mapped joints (initialize to false) + Array mapped1(n1, false); + Array mapped2(n2, false); + + // Find joints that can be mapped directly + for (int j1 = 0; j1 < n1; ++j1) + for (int j2 = 0; j2 < n2; ++j2) + if (inCanMapJoint(inSkeleton1, j1, inSkeleton2, j2)) + { + // Calculate the transform that takes this joint from skeleton 1 to 2 + Mat44 joint_1_to_2 = inNeutralPose1[j1].Inversed() * inNeutralPose2[j2]; + + // Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so) + joint_1_to_2(3, 3) = 1.0f; + + mMappings.emplace_back(j1, j2, joint_1_to_2); + mapped1[j1] = true; + mapped2[j2] = true; + break; + } + + Array cur_chain; // Taken out of the loop to minimize amount of allocations + + // Find joint chains + for (int m1 = 0; m1 < (int)mMappings.size(); ++m1) + { + Array chain2; + int chain2_m = -1; + + for (int m2 = m1 + 1; m2 < (int)mMappings.size(); ++m2) + { + // Find the chain from back from m2 to m1 + int start = mMappings[m1].mJointIdx2; + int end = mMappings[m2].mJointIdx2; + int cur = end; + cur_chain.clear(); // Should preserve memory + do + { + cur_chain.push_back(cur); + cur = inSkeleton2->GetJoint(cur).mParentJointIndex; + } + while (cur >= 0 && cur != start && !mapped2[cur]); + cur_chain.push_back(start); + + if (cur == start // This should be the correct chain + && cur_chain.size() > 2 // It should have joints between the mapped joints + && cur_chain.size() > chain2.size()) // And it should be the longest so far + { + chain2.swap(cur_chain); + chain2_m = m2; + } + } + + if (!chain2.empty()) + { + // Get the chain for 1 + Array chain1; + int start = mMappings[m1].mJointIdx1; + int cur = mMappings[chain2_m].mJointIdx1; + do + { + chain1.push_back(cur); + cur = inSkeleton1->GetJoint(cur).mParentJointIndex; + } + while (cur >= 0 && cur != start && !mapped1[cur]); + chain1.push_back(start); + + // If the chain exists in 1 too + if (cur == start) + { + // Reverse the chains + std::reverse(chain1.begin(), chain1.end()); + std::reverse(chain2.begin(), chain2.end()); + + // Mark elements mapped + for (int j1 : chain1) + mapped1[j1] = true; + for (int j2 : chain2) + mapped2[j2] = true; + + // Insert the chain + mChains.emplace_back(std::move(chain1), std::move(chain2)); + } + } + } + + // Collect unmapped joints from 2 + for (int j2 = 0; j2 < n2; ++j2) + if (!mapped2[j2]) + mUnmapped.emplace_back(j2, inSkeleton2->GetJoint(j2).mParentJointIndex); +} + +void SkeletonMapper::LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2) +{ + JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered()); + + int n = inSkeleton2->GetJointCount(); + + // Copy locked joints to array but don't actually include the first joint (this is physics driven) + for (int i = 0; i < n; ++i) + if (inLockedTranslations[i]) + { + Locked l; + l.mJointIdx = i; + l.mParentJointIdx = inSkeleton2->GetJoint(i).mParentJointIndex; + if (l.mParentJointIdx >= 0) + l.mTranslation = inNeutralPose2[l.mParentJointIdx].Inversed() * inNeutralPose2[i].GetTranslation(); + else + l.mTranslation = inNeutralPose2[i].GetTranslation(); + mLockedTranslations.push_back(l); + } +} + +void SkeletonMapper::LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2) +{ + JPH_ASSERT(!mMappings.empty(), "Call Initialize first!"); + JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered()); + + // The first mapping is the top most one (remember that joints should be ordered so that parents go before children). + // Because we created the mappings from the lowest joint first, this should contain the first mappable joint. + int root_idx = mMappings[0].mJointIdx2; + + // Create temp array to hold locked joints + int n = inSkeleton2->GetJointCount(); + bool *locked_translations = (bool *)JPH_STACK_ALLOC(n * sizeof(bool)); + memset(locked_translations, 0, n * sizeof(bool)); + + // Mark root as locked + locked_translations[root_idx] = true; + + // Loop over all joints and propagate the locked flag to all children + for (int i = root_idx + 1; i < n; ++i) + { + int parent_idx = inSkeleton2->GetJoint(i).mParentJointIndex; + if (parent_idx >= 0) + locked_translations[i] = locked_translations[parent_idx]; + } + + // Unmark root because we don't actually want to include this (this determines the position of the entire ragdoll) + locked_translations[root_idx] = false; + + // Call the generic function + LockTranslations(inSkeleton2, locked_translations, inNeutralPose2); +} + +void SkeletonMapper::Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const +{ + // Apply direct mappings + for (const Mapping &m : mMappings) + outPose2ModelSpace[m.mJointIdx2] = inPose1ModelSpace[m.mJointIdx1] * m.mJoint1To2; + + // Apply chain mappings + for (const Chain &c : mChains) + { + // Calculate end of chain given local space transforms of the joints of the chain + Mat44 &chain_start = outPose2ModelSpace[c.mJointIndices2.front()]; + Mat44 chain_end = chain_start; + for (int j = 1; j < (int)c.mJointIndices2.size(); ++j) + chain_end = chain_end * inPose2LocalSpace[c.mJointIndices2[j]]; + + // Calculate the direction in world space for skeleton 1 and skeleton 2 and the rotation between them + Vec3 actual = chain_end.GetTranslation() - chain_start.GetTranslation(); + Vec3 desired = inPose1ModelSpace[c.mJointIndices1.back()].GetTranslation() - inPose1ModelSpace[c.mJointIndices1.front()].GetTranslation(); + Quat rotation = Quat::sFromTo(actual, desired); + + // Rotate the start of the chain + chain_start.SetRotation(Mat44::sRotation(rotation) * chain_start.GetRotation()); + + // Update all joints but the first and the last joint using their local space transforms + for (int j = 1; j < (int)c.mJointIndices2.size() - 1; ++j) + { + int parent = c.mJointIndices2[j - 1]; + int child = c.mJointIndices2[j]; + outPose2ModelSpace[child] = outPose2ModelSpace[parent] * inPose2LocalSpace[child]; + } + } + + // All unmapped joints take the local pose and convert it to model space + for (const Unmapped &u : mUnmapped) + if (u.mParentJointIdx >= 0) + { + JPH_ASSERT(u.mParentJointIdx < u.mJointIdx, "Joints must be ordered: parents first"); + outPose2ModelSpace[u.mJointIdx] = outPose2ModelSpace[u.mParentJointIdx] * inPose2LocalSpace[u.mJointIdx]; + } + else + outPose2ModelSpace[u.mJointIdx] = inPose2LocalSpace[u.mJointIdx]; + + // Update all locked joint translations + for (const Locked &l : mLockedTranslations) + outPose2ModelSpace[l.mJointIdx].SetTranslation(outPose2ModelSpace[l.mParentJointIdx] * l.mTranslation); +} + +void SkeletonMapper::MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const +{ + // Normally each joint in skeleton 1 should be present in the mapping, so we only need to apply the direct mappings + for (const Mapping &m : mMappings) + outPose1ModelSpace[m.mJointIdx1] = inPose2ModelSpace[m.mJointIdx2] * m.mJoint2To1; +} + +int SkeletonMapper::GetMappedJointIdx(int inJoint1Idx) const +{ + for (const Mapping &m : mMappings) + if (m.mJointIdx1 == inJoint1Idx) + return m.mJointIdx2; + + return -1; +} + +bool SkeletonMapper::IsJointTranslationLocked(int inJoint2Idx) const +{ + for (const Locked &l : mLockedTranslations) + if (l.mJointIdx == inJoint2Idx) + return true; + + return false; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h new file mode 100644 index 000000000000..05cc8866a4b7 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h @@ -0,0 +1,145 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that is able to map a low detail (ragdoll) skeleton to a high detail (animation) skeleton and vice versa +class JPH_EXPORT SkeletonMapper : public RefTarget +{ +public: + /// A joint that maps 1-on-1 to a joint in the other skeleton + class Mapping + { + public: + Mapping() = default; + Mapping(int inJointIdx1, int inJointIdx2, Mat44Arg inJoint1To2) : mJointIdx1(inJointIdx1), mJointIdx2(inJointIdx2), mJoint1To2(inJoint1To2), mJoint2To1(inJoint1To2.Inversed()) + { + // Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so) + mJoint2To1(3, 3) = 1.0f; + } + + int mJointIdx1; ///< Index of joint from skeleton 1 + int mJointIdx2; ///< Corresponding index of joint from skeleton 2 + Mat44 mJoint1To2; ///< Transforms this joint from skeleton 1 to 2 + Mat44 mJoint2To1; ///< Inverse of the transform above + }; + + /// A joint chain that starts with a 1-on-1 mapped joint and ends with a 1-on-1 mapped joint with intermediate joints that cannot be mapped + class Chain + { + public: + Chain() = default; + Chain(Array &&inJointIndices1, Array &&inJointIndices2) : mJointIndices1(std::move(inJointIndices1)), mJointIndices2(std::move(inJointIndices2)) { } + + Array mJointIndices1; ///< Joint chain from skeleton 1 + Array mJointIndices2; ///< Corresponding joint chain from skeleton 2 + }; + + /// Joints that could not be mapped from skeleton 1 to 2 + class Unmapped + { + public: + Unmapped() = default; + Unmapped(int inJointIdx, int inParentJointIdx) : mJointIdx(inJointIdx), mParentJointIdx(inParentJointIdx) { } + + int mJointIdx; ///< Joint index of unmappable joint + int mParentJointIdx; ///< Parent joint index of unmappable joint + }; + + /// Joints that should have their translation locked (fixed) + class Locked + { + public: + int mJointIdx; ///< Joint index of joint with locked translation (in skeleton 2) + int mParentJointIdx; ///< Parent joint index of joint with locked translation (in skeleton 2) + Vec3 mTranslation; ///< Translation of neutral pose + }; + + /// A function that is called to determine if a joint can be mapped from source to target skeleton + using CanMapJoint = function; + + /// Default function that checks if the names of the joints are equal + static bool sDefaultCanMapJoint(const Skeleton *inSkeleton1, int inIndex1, const Skeleton *inSkeleton2, int inIndex2) + { + return inSkeleton1->GetJoint(inIndex1).mName == inSkeleton2->GetJoint(inIndex2).mName; + } + + /// Initialize the skeleton mapper. Skeleton 1 should be the (low detail) ragdoll skeleton and skeleton 2 the (high detail) animation skeleton. + /// We assume that each joint in skeleton 1 can be mapped to a joint in skeleton 2 (if not mapping from animation skeleton to ragdoll skeleton will be undefined). + /// Skeleton 2 should have the same hierarchy as skeleton 1 but can contain extra joints between those in skeleton 1 and it can have extra joints at the root and leaves of the skeleton. + /// @param inSkeleton1 Source skeleton to map from. + /// @param inNeutralPose1 Neutral pose of the source skeleton (model space) + /// @param inSkeleton2 Target skeleton to map to. + /// @param inNeutralPose2 Neutral pose of the target skeleton (model space), inNeutralPose1 and inNeutralPose2 must match as closely as possible, preferably the position of the mappable joints should be identical. + /// @param inCanMapJoint Function that checks if joints in skeleton 1 and skeleton 2 are equal. + void Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint = sDefaultCanMapJoint); + + /// This can be called so lock the translation of a specified set of joints in skeleton 2. + /// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress. + /// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the + /// physical simulation and the visual representation. + /// @param inSkeleton2 Target skeleton to map to. + /// @param inLockedTranslations An array of bools the size of inSkeleton2->GetJointCount(), for each joint indicating if the joint is locked. + /// @param inNeutralPose2 Neutral pose to take reference translations from + void LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2); + + /// After Initialize(), this can be called to lock the translation of all joints in skeleton 2 below the first mapped joint to those of the neutral pose. + /// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress. + /// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the + /// physical simulation and the visual representation. + /// @param inSkeleton2 Target skeleton to map to. + /// @param inNeutralPose2 Neutral pose to take reference translations from + void LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2); + + /// Map a pose. Joints that were directly mappable will be copied in model space from pose 1 to pose 2. Any joints that are only present in skeleton 2 + /// will get their model space transform calculated through the local space transforms of pose 2. Joints that are part of a joint chain between two + /// mapped joints will be reoriented towards the next joint in skeleton 1. This means that it is possible for unmapped joints to have some animation, + /// but very extreme animation poses will show artifacts. + /// @param inPose1ModelSpace Pose on skeleton 1 in model space + /// @param inPose2LocalSpace Pose on skeleton 2 in local space (used for the joints that cannot be mapped) + /// @param outPose2ModelSpace Model space pose on skeleton 2 (the output of the mapping) + void Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const; + + /// Reverse map a pose, this will only use the mappings and not the chains (it assumes that all joints in skeleton 1 are mapped) + /// @param inPose2ModelSpace Model space pose on skeleton 2 + /// @param outPose1ModelSpace When the function returns this will contain the model space pose for skeleton 1 + void MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const; + + /// Search through the directly mapped joints (mMappings) and find inJoint1Idx, returns the corresponding Joint2Idx or -1 if not found. + int GetMappedJointIdx(int inJoint1Idx) const; + + /// Search through the locked translations (mLockedTranslations) and find if joint inJoint2Idx is locked. + bool IsJointTranslationLocked(int inJoint2Idx) const; + + using MappingVector = Array; + using ChainVector = Array; + using UnmappedVector = Array; + using LockedVector = Array; + + ///@name Access to the mapped joints + ///@{ + const MappingVector & GetMappings() const { return mMappings; } + MappingVector & GetMappings() { return mMappings; } + const ChainVector & GetChains() const { return mChains; } + ChainVector & GetChains() { return mChains; } + const UnmappedVector & GetUnmapped() const { return mUnmapped; } + UnmappedVector & GetUnmapped() { return mUnmapped; } + const LockedVector & GetLockedTranslations() const { return mLockedTranslations; } + LockedVector & GetLockedTranslations() { return mLockedTranslations; } + ///@} + +private: + /// Joint mappings + MappingVector mMappings; + ChainVector mChains; + UnmappedVector mUnmapped; ///< Joint indices that could not be mapped from 1 to 2 (these are indices in 2) + LockedVector mLockedTranslations; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp new file mode 100644 index 000000000000..c64bf049716a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +void SkeletonPose::SetSkeleton(const Skeleton *inSkeleton) +{ + mSkeleton = inSkeleton; + + mJoints.resize(mSkeleton->GetJointCount()); + mJointMatrices.resize(mSkeleton->GetJointCount()); +} + +void SkeletonPose::CalculateJointMatrices() +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + { + mJointMatrices[i] = mJoints[i].ToMatrix(); + + int parent = mSkeleton->GetJoint(i).mParentJointIndex; + if (parent >= 0) + { + JPH_ASSERT(parent < i, "Joints must be ordered: parents first"); + mJointMatrices[i] = mJointMatrices[parent] * mJointMatrices[i]; + } + } +} + +void SkeletonPose::CalculateJointStates() +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + { + Mat44 local_transform; + int parent = mSkeleton->GetJoint(i).mParentJointIndex; + if (parent >= 0) + local_transform = mJointMatrices[parent].Inversed() * mJointMatrices[i]; + else + local_transform = mJointMatrices[i]; + + JointState &joint = mJoints[i]; + joint.mTranslation = local_transform.GetTranslation(); + joint.mRotation = local_transform.GetQuaternion(); + } +} + +void SkeletonPose::CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + outMatrices[i] = mJoints[i].ToMatrix(); +} + +#ifdef JPH_DEBUG_RENDERER +void SkeletonPose::Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset) const +{ + RMat44 offset = inOffset * RMat44::sTranslation(mRootOffset); + + const Skeleton::JointVector &joints = mSkeleton->GetJoints(); + + for (int b = 0; b < mSkeleton->GetJointCount(); ++b) + { + RMat44 joint_transform = offset * mJointMatrices[b]; + + if (inDrawSettings.mDrawJoints) + { + int parent = joints[b].mParentJointIndex; + if (parent >= 0) + inRenderer->DrawLine(offset * mJointMatrices[parent].GetTranslation(), joint_transform.GetTranslation(), Color::sGreen); + } + + if (inDrawSettings.mDrawJointOrientations) + inRenderer->DrawCoordinateSystem(joint_transform, 0.05f); + + if (inDrawSettings.mDrawJointNames) + inRenderer->DrawText3D(joint_transform.GetTranslation(), joints[b].mName, Color::sWhite, 0.05f); + } +} +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h new file mode 100644 index 000000000000..326227e447a9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h @@ -0,0 +1,82 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Instance of a skeleton, contains the pose the current skeleton is in +class JPH_EXPORT SkeletonPose +{ +public: + JPH_OVERRIDE_NEW_DELETE + + using JointState = SkeletalAnimation::JointState; + using JointStateVector = Array; + using Mat44Vector = Array; + + ///@name Skeleton + ///@{ + void SetSkeleton(const Skeleton *inSkeleton); + const Skeleton * GetSkeleton() const { return mSkeleton; } + ///@} + + /// Extra offset applied to the root (and therefore also to all of its children) + void SetRootOffset(RVec3Arg inOffset) { mRootOffset = inOffset; } + RVec3 GetRootOffset() const { return mRootOffset; } + + ///@name Properties of the joints + ///@{ + uint GetJointCount() const { return (uint)mJoints.size(); } + const JointStateVector & GetJoints() const { return mJoints; } + JointStateVector & GetJoints() { return mJoints; } + const JointState & GetJoint(int inJoint) const { return mJoints[inJoint]; } + JointState & GetJoint(int inJoint) { return mJoints[inJoint]; } + ///@} + + ///@name Joint matrices + ///@{ + const Mat44Vector & GetJointMatrices() const { return mJointMatrices; } + Mat44Vector & GetJointMatrices() { return mJointMatrices; } + const Mat44 & GetJointMatrix(int inJoint) const { return mJointMatrices[inJoint]; } + Mat44 & GetJointMatrix(int inJoint) { return mJointMatrices[inJoint]; } + ///@} + + /// Convert the joint states to joint matrices + void CalculateJointMatrices(); + + /// Convert joint matrices to joint states + void CalculateJointStates(); + + /// Outputs the joint matrices in local space (ensure that outMatrices has GetJointCount() elements, assumes that values in GetJoints() is up to date) + void CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const; + +#ifdef JPH_DEBUG_RENDERER + /// Draw settings + struct DrawSettings + { + bool mDrawJoints = true; + bool mDrawJointOrientations = true; + bool mDrawJointNames = false; + }; + + /// Draw current pose + void Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset = RMat44::sIdentity()) const; +#endif // JPH_DEBUG_RENDERER + +private: + RefConst mSkeleton; ///< Skeleton definition + RVec3 mRootOffset { RVec3::sZero() }; ///< Extra offset applied to the root (and therefore also to all of its children) + JointStateVector mJoints; ///< Local joint orientations (local to parent Joint) + Mat44Vector mJointMatrices; ///< Local joint matrices (local to world matrix) +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h new file mode 100644 index 000000000000..9d75691f68c1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h @@ -0,0 +1,27 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N (according to closeness) +class JPH_EXPORT TriangleGrouper : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~TriangleGrouper() = default; + + /// Group a batch of indexed triangles + /// @param inVertices The list of vertices + /// @param inTriangles The list of indexed triangles (indexes into inVertices) + /// @param inGroupSize How big each group should be + /// @param outGroupedTriangleIndices An ordered list of indices (indexing into inTriangles), contains groups of inGroupSize large worth of indices to triangles that are grouped together. If the triangle count is not an exact multiple of inGroupSize the last batch will be smaller. + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) = 0; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp new file mode 100644 index 000000000000..800160818e28 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp @@ -0,0 +1,95 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void TriangleGrouperClosestCentroid::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) +{ + const uint triangle_count = (uint)inTriangles.size(); + const uint num_batches = (triangle_count + inGroupSize - 1) / inGroupSize; + + Array centroids; + centroids.resize(triangle_count); + + outGroupedTriangleIndices.resize(triangle_count); + + for (uint t = 0; t < triangle_count; ++t) + { + // Store centroid + centroids[t] = inTriangles[t].GetCentroid(inVertices); + + // Initialize sort table + outGroupedTriangleIndices[t] = t; + } + + Array::const_iterator triangles_end = outGroupedTriangleIndices.end(); + + // Sort per batch + for (uint b = 0; b < num_batches - 1; ++b) + { + // Get iterators + Array::iterator batch_begin = outGroupedTriangleIndices.begin() + b * inGroupSize; + Array::iterator batch_end = batch_begin + inGroupSize; + Array::iterator batch_begin_plus_1 = batch_begin + 1; + Array::iterator batch_end_minus_1 = batch_end - 1; + + // Find triangle with centroid with lowest X coordinate + Array::iterator lowest_iter = batch_begin; + float lowest_val = centroids[*lowest_iter].GetX(); + for (Array::iterator other = batch_begin; other != triangles_end; ++other) + { + float val = centroids[*other].GetX(); + if (val < lowest_val) + { + lowest_iter = other; + lowest_val = val; + } + } + + // Make this triangle the first in a new batch + std::swap(*batch_begin, *lowest_iter); + Vec3 first_centroid = centroids[*batch_begin]; + + // Sort remaining triangles in batch on distance to first triangle + QuickSort(batch_begin_plus_1, batch_end, + [&first_centroid, ¢roids](uint inLHS, uint inRHS) + { + return (centroids[inLHS] - first_centroid).LengthSq() < (centroids[inRHS] - first_centroid).LengthSq(); + }); + + // Loop over remaining triangles + float furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq(); + for (Array::iterator other = batch_end; other != triangles_end; ++other) + { + // Check if this triangle is closer than the furthest triangle in the batch + float dist = (centroids[*other] - first_centroid).LengthSq(); + if (dist < furthest_dist) + { + // Replace furthest triangle + uint other_val = *other; + *other = *batch_end_minus_1; + + // Find first element that is bigger than this one and insert the current item before it + Array::iterator upper = std::upper_bound(batch_begin_plus_1, batch_end, dist, + [&first_centroid, ¢roids](float inLHS, uint inRHS) + { + return inLHS < (centroids[inRHS] - first_centroid).LengthSq(); + }); + std::copy_backward(upper, batch_end_minus_1, batch_end); + *upper = other_val; + + // Calculate new furthest distance + furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq(); + } + } + } +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h new file mode 100644 index 000000000000..58322741646c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h @@ -0,0 +1,21 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N. +/// Starts with centroid with lowest X coordinate and finds N closest centroids, this repeats until all groups have been found. +/// Time complexity: O(N^2) +class JPH_EXPORT TriangleGrouperClosestCentroid : public TriangleGrouper +{ +public: + // See: TriangleGrouper::Group + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) override; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp new file mode 100644 index 000000000000..a6e5a564426c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void TriangleGrouperMorton::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) +{ + const uint triangle_count = (uint)inTriangles.size(); + + Array centroids; + centroids.resize(triangle_count); + + outGroupedTriangleIndices.resize(triangle_count); + + for (uint t = 0; t < triangle_count; ++t) + { + // Store centroid + centroids[t] = inTriangles[t].GetCentroid(inVertices); + + // Initialize sort table + outGroupedTriangleIndices[t] = t; + } + + // Get bounding box of all centroids + AABox centroid_bounds; + for (uint t = 0; t < triangle_count; ++t) + centroid_bounds.Encapsulate(centroids[t]); + + // Make sure box is not degenerate + centroid_bounds.EnsureMinimalEdgeLength(1.0e-5f); + + // Calculate morton code for each centroid + Array morton_codes; + morton_codes.resize(triangle_count); + for (uint t = 0; t < triangle_count; ++t) + morton_codes[t] = MortonCode::sGetMortonCode(centroids[t], centroid_bounds); + + // Sort triangles based on morton code + QuickSort(outGroupedTriangleIndices.begin(), outGroupedTriangleIndices.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; }); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h new file mode 100644 index 000000000000..a35f9af004ee --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h @@ -0,0 +1,20 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N according to morton code of centroid. +/// Time complexity: O(N log(N)) +class JPH_EXPORT TriangleGrouperMorton : public TriangleGrouper +{ +public: + // See: TriangleGrouper::Group + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) override; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp new file mode 100644 index 000000000000..50d15117d322 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp @@ -0,0 +1,67 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitter::TriangleSplitter(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + mVertices(inVertices), + mTriangles(inTriangles) +{ + mSortedTriangleIdx.resize(inTriangles.size()); + mCentroids.resize(inTriangles.size()); + + for (uint t = 0; t < inTriangles.size(); ++t) + { + // Initially triangles start unsorted + mSortedTriangleIdx[t] = t; + + // Calculate centroid + inTriangles[t].GetCentroid(inVertices).StoreFloat3(&mCentroids[t]); + } +} + +bool TriangleSplitter::SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight) +{ + // Divide triangles + uint start = inTriangles.mBegin, end = inTriangles.mEnd; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && mCentroids[mSortedTriangleIdx[start]][inDimension] < inSplit) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && mCentroids[mSortedTriangleIdx[end - 1]][inDimension] >= inSplit) + --end; + + if (start < end) + { + // Swap the two elements + std::swap(mSortedTriangleIdx[start], mSortedTriangleIdx[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + +#ifdef JPH_ENABLE_ASSERTS + // Validate division algorithm + JPH_ASSERT(inTriangles.mBegin <= start); + JPH_ASSERT(start <= inTriangles.mEnd); + for (uint i = inTriangles.mBegin; i < start; ++i) + JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] < inSplit); + for (uint i = start; i < inTriangles.mEnd; ++i) + JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] >= inSplit); +#endif + + outLeft = Range(inTriangles.mBegin, start); + outRight = Range(start, inTriangles.mEnd); + return outLeft.Count() > 0 && outRight.Count() > 0; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h new file mode 100644 index 000000000000..a66672238fcd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h @@ -0,0 +1,84 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A class that splits a triangle list into two parts for building a tree +class JPH_EXPORT TriangleSplitter : public NonCopyable +{ +public: + /// Constructor + TriangleSplitter(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + /// Virtual destructor + virtual ~TriangleSplitter() = default; + + struct Stats + { + const char * mSplitterName = nullptr; + int mLeafSize = 0; + }; + + /// Get stats of splitter + virtual void GetStats(Stats &outStats) const = 0; + + /// Helper struct to indicate triangle range before and after the split + struct Range + { + /// Constructor + Range() = default; + Range(uint inBegin, uint inEnd) : mBegin(inBegin), mEnd(inEnd) { } + + /// Get number of triangles in range + uint Count() const + { + return mEnd - mBegin; + } + + /// Start and end index (end = 1 beyond end) + uint mBegin; + uint mEnd; + }; + + /// Range of triangles to start with + Range GetInitialRange() const + { + return Range(0, (uint)mSortedTriangleIdx.size()); + } + + /// Split triangles into two groups left and right, returns false if no split could be made + /// @param inTriangles The range of triangles (in mSortedTriangleIdx) to process + /// @param outLeft On return this will contain the ranges for the left subpart. mSortedTriangleIdx may have been shuffled. + /// @param outRight On return this will contain the ranges for the right subpart. mSortedTriangleIdx may have been shuffled. + /// @return Returns true when a split was found + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) = 0; + + /// Get the list of vertices + const VertexList & GetVertices() const + { + return mVertices; + } + + /// Get triangle by index + const IndexedTriangle & GetTriangle(uint inIdx) const + { + return mTriangles[mSortedTriangleIdx[inIdx]]; + } + +protected: + /// Helper function to split triangles based on dimension and split value + bool SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight); + + const VertexList & mVertices; ///< Vertices of the indexed triangles + const IndexedTriangleList & mTriangles; ///< Unsorted triangles + Array mCentroids; ///< Unsorted centroids of triangles + Array mSortedTriangleIdx; ///< Indices to sort triangles +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp new file mode 100644 index 000000000000..cdeb79bf750d --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp @@ -0,0 +1,136 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + + JPH_NAMESPACE_BEGIN + +TriangleSplitterBinning::TriangleSplitterBinning(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) : + TriangleSplitter(inVertices, inTriangles), + mMinNumBins(inMinNumBins), + mMaxNumBins(inMaxNumBins), + mNumTrianglesPerBin(inNumTrianglesPerBin) +{ + mBins.resize(mMaxNumBins * 3); // mMaxNumBins per dimension +} + +bool TriangleSplitterBinning::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate bounds for this range + AABox centroid_bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + centroid_bounds.Encapsulate(Vec3(mCentroids[mSortedTriangleIdx[t]])); + + // Convert bounds to min coordinate and size + // Prevent division by zero if one of the dimensions is zero + constexpr float cMinSize = 1.0e-5f; + Vec3 bounds_min = centroid_bounds.mMin; + Vec3 bounds_size = Vec3::sMax(centroid_bounds.mMax - bounds_min, Vec3::sReplicate(cMinSize)); + + float best_cp = FLT_MAX; + uint best_dim = 0xffffffff; + float best_split = 0; + + // Bin in all dimensions + uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins); + + // Initialize bins + for (uint dim = 0; dim < 3; ++dim) + { + // Get bounding box size for this dimension + float bounds_min_dim = bounds_min[dim]; + float bounds_size_dim = bounds_size[dim]; + + // Get the bins for this dimension + Bin *bins_dim = &mBins[num_bins * dim]; + + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins_dim[b]; + bin.mBounds.SetEmpty(); + bin.mMinCentroid = bounds_min_dim + bounds_size_dim * (b + 1) / num_bins; + bin.mNumTriangles = 0; + } + } + + // Bin all triangles in all dimensions at once + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + { + Vec3 centroid_pos(mCentroids[mSortedTriangleIdx[t]]); + + AABox triangle_bounds = AABox::sFromTriangle(mVertices, GetTriangle(t)); + + Vec3 bin_no_f = (centroid_pos - bounds_min) / bounds_size * float(num_bins); + UVec4 bin_no = UVec4::sMin(bin_no_f.ToInt(), UVec4::sReplicate(num_bins - 1)); + + for (uint dim = 0; dim < 3; ++dim) + { + // Select bin + Bin &bin = mBins[num_bins * dim + bin_no[dim]]; + + // Accumulate triangle in bin + bin.mBounds.Encapsulate(triangle_bounds); + bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos[dim]); + bin.mNumTriangles++; + } + } + + for (uint dim = 0; dim < 3; ++dim) + { + // Skip axis if too small + if (bounds_size[dim] <= cMinSize) + continue; + + // Get the bins for this dimension + Bin *bins_dim = &mBins[num_bins * dim]; + + // Calculate totals left to right + AABox prev_bounds; + int prev_triangles = 0; + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins_dim[b]; + bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin + bin.mNumTrianglesAccumulatedLeft = prev_triangles; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + } + + // Calculate totals right to left + prev_bounds.SetEmpty(); + prev_triangles = 0; + for (int b = num_bins - 1; b >= 0; --b) + { + Bin &bin = bins_dim[b]; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + bin.mBoundsAccumulatedRight = prev_bounds; + bin.mNumTrianglesAccumulatedRight = prev_triangles; + } + + // Get best splitting plane + for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side + { + // Calculate surface area heuristic and see if it is better than the current best + const Bin &bin = bins_dim[b]; + float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight; + if (cp < best_cp) + { + best_cp = cp; + best_dim = dim; + best_split = bin.mMinCentroid; + } + } + } + + // No split found? + if (best_dim == 0xffffffff) + return false; + + return SplitInternal(inTriangles, best_dim, best_split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h new file mode 100644 index 000000000000..2cb35c9cc7bd --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Binning splitter approach taken from: Realtime Ray Tracing on GPU with BVH-based Packet Traversal by Johannes Gunther et al. +class JPH_EXPORT TriangleSplitterBinning : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterBinning(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterBinning"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + // Configuration + const uint mMinNumBins; + const uint mMaxNumBins; + const uint mNumTrianglesPerBin; + + struct Bin + { + // Properties of this bin + AABox mBounds; + float mMinCentroid; + uint mNumTriangles; + + // Accumulated data from left most / right most bin to current (including this bin) + AABox mBoundsAccumulatedLeft; + AABox mBoundsAccumulatedRight; + uint mNumTrianglesAccumulatedLeft; + uint mNumTrianglesAccumulatedRight; + }; + + // Scratch area to store the bins + Array mBins; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp new file mode 100644 index 000000000000..333def988922 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp @@ -0,0 +1,170 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterFixedLeafSize::TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) : + TriangleSplitter(inVertices, inTriangles), + mLeafSize(inLeafSize), + mMinNumBins(inMinNumBins), + mMaxNumBins(inMaxNumBins), + mNumTrianglesPerBin(inNumTrianglesPerBin) +{ + // Group the triangles + TriangleGrouperClosestCentroid grouper; + grouper.Group(inVertices, inTriangles, mLeafSize, mSortedTriangleIdx); + + // Pad triangles so that we have a multiple of mLeafSize + const uint num_triangles = (uint)inTriangles.size(); + const uint num_groups = (num_triangles + mLeafSize - 1) / mLeafSize; + const uint last_triangle_idx = mSortedTriangleIdx.back(); + for (uint t = num_triangles, t_end = num_groups * mLeafSize; t < t_end; ++t) + mSortedTriangleIdx.push_back(last_triangle_idx); +} + +Vec3 TriangleSplitterFixedLeafSize::GetCentroidForGroup(uint inFirstTriangleInGroup) +{ + JPH_ASSERT(inFirstTriangleInGroup % mLeafSize == 0); + AABox box; + for (uint g = 0; g < mLeafSize; ++g) + box.Encapsulate(mVertices, GetTriangle(inFirstTriangleInGroup + g)); + return box.GetCenter(); +} + +bool TriangleSplitterFixedLeafSize::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Cannot split anything smaller than leaf size + JPH_ASSERT(inTriangles.Count() > mLeafSize); + JPH_ASSERT(inTriangles.Count() % mLeafSize == 0); + + // Calculate bounds for this range + AABox centroid_bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize) + centroid_bounds.Encapsulate(GetCentroidForGroup(t)); + + float best_cp = FLT_MAX; + uint best_dim = 0xffffffff; + float best_split = 0; + + // Bin in all dimensions + uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins); + Array bins(num_bins); + for (uint dim = 0; dim < 3; ++dim) + { + float bounds_min = centroid_bounds.mMin[dim]; + float bounds_size = centroid_bounds.mMax[dim] - bounds_min; + + // Skip axis if too small + if (bounds_size < 1.0e-5f) + continue; + + // Initialize bins + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins[b]; + bin.mBounds.SetEmpty(); + bin.mMinCentroid = bounds_min + bounds_size * (b + 1) / num_bins; + bin.mNumTriangles = 0; + } + + // Bin all triangles + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize) + { + // Calculate average centroid for group + float centroid_pos = GetCentroidForGroup(t)[dim]; + + // Select bin + uint bin_no = min(uint((centroid_pos - bounds_min) / bounds_size * num_bins), num_bins - 1); + Bin &bin = bins[bin_no]; + + // Put all triangles of group in same bin + for (uint g = 0; g < mLeafSize; ++g) + bin.mBounds.Encapsulate(mVertices, GetTriangle(t + g)); + bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos); + bin.mNumTriangles += mLeafSize; + } + + // Calculate totals left to right + AABox prev_bounds; + int prev_triangles = 0; + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins[b]; + bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin + bin.mNumTrianglesAccumulatedLeft = prev_triangles; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + } + + // Calculate totals right to left + prev_bounds.SetEmpty(); + prev_triangles = 0; + for (int b = num_bins - 1; b >= 0; --b) + { + Bin &bin = bins[b]; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + bin.mBoundsAccumulatedRight = prev_bounds; + bin.mNumTrianglesAccumulatedRight = prev_triangles; + } + + // Get best splitting plane + for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side + { + // Calculate surface area heuristic and see if it is better than the current best + const Bin &bin = bins[b]; + float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight; + if (cp < best_cp) + { + best_cp = cp; + best_dim = dim; + best_split = bin.mMinCentroid; + } + } + } + + // No split found? + if (best_dim == 0xffffffff) + return false; + + // Divide triangles + uint start = inTriangles.mBegin, end = inTriangles.mEnd; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && GetCentroidForGroup(start)[best_dim] < best_split) + start += mLeafSize; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && GetCentroidForGroup(end - mLeafSize)[best_dim] >= best_split) + end -= mLeafSize; + + if (start < end) + { + // Swap the two elements + for (uint g = 0; g < mLeafSize; ++g) + std::swap(mSortedTriangleIdx[start + g], mSortedTriangleIdx[end - mLeafSize + g]); + start += mLeafSize; + end -= mLeafSize; + } + } + JPH_ASSERT(start == end); + + // No suitable split found, doing random split in half + if (start == inTriangles.mBegin || start == inTriangles.mEnd) + start = inTriangles.mBegin + (inTriangles.Count() / mLeafSize + 1) / 2 * mLeafSize; + + outLeft = Range(inTriangles.mBegin, start); + outRight = Range(start, inTriangles.mEnd); + JPH_ASSERT(outLeft.mEnd > outLeft.mBegin && outRight.mEnd > outRight.mBegin); + JPH_ASSERT(outLeft.Count() % mLeafSize == 0 && outRight.Count() % mLeafSize == 0); + return true; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h new file mode 100644 index 000000000000..029121daea56 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Same as TriangleSplitterBinning, but ensuring that leaves have a fixed amount of triangles +/// The resulting tree should be suitable for processing on GPU where we want all threads to process an equal amount of triangles +class JPH_EXPORT TriangleSplitterFixedLeafSize : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterFixedLeafSize"; + outStats.mLeafSize = mLeafSize; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + /// Get centroid for group + Vec3 GetCentroidForGroup(uint inFirstTriangleInGroup); + + // Configuration + const uint mLeafSize; + const uint mMinNumBins; + const uint mMaxNumBins; + const uint mNumTrianglesPerBin; + + struct Bin + { + // Properties of this bin + AABox mBounds; + float mMinCentroid; + uint mNumTriangles; + + // Accumulated data from left most / right most bin to current (including this bin) + AABox mBoundsAccumulatedLeft; + AABox mBoundsAccumulatedRight; + uint mNumTrianglesAccumulatedLeft; + uint mNumTrianglesAccumulatedRight; + }; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp new file mode 100644 index 000000000000..f8115ab8999c --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterLongestAxis::TriangleSplitterLongestAxis(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ +} + +bool TriangleSplitterLongestAxis::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate bounding box for triangles + AABox bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + bounds.Encapsulate(mVertices, GetTriangle(t)); + + // Calculate split plane + uint dimension = bounds.GetExtent().GetHighestComponentIndex(); + float split = bounds.GetCenter()[dimension]; + + return SplitInternal(inTriangles, dimension, split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h new file mode 100644 index 000000000000..daf0d437019b --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using center of bounding box with longest axis +class JPH_EXPORT TriangleSplitterLongestAxis : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterLongestAxis(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterLongestAxis"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp new file mode 100644 index 000000000000..e884246fea59 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp @@ -0,0 +1,40 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterMean::TriangleSplitterMean(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ +} + +bool TriangleSplitterMean::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate mean value for these triangles + Vec3 mean = Vec3::sZero(); + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + mean += Vec3(mCentroids[mSortedTriangleIdx[t]]); + mean *= 1.0f / inTriangles.Count(); + + // Calculate deviation + Vec3 deviation = Vec3::sZero(); + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + { + Vec3 delta = Vec3(mCentroids[mSortedTriangleIdx[t]]) - mean; + deviation += delta * delta; + } + deviation *= 1.0f / inTriangles.Count(); + + // Calculate split plane + uint dimension = deviation.GetHighestComponentIndex(); + float split = mean[dimension]; + + return SplitInternal(inTriangles, dimension, split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h new file mode 100644 index 000000000000..737d76e1c1f9 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using mean of axis with biggest centroid deviation +class JPH_EXPORT TriangleSplitterMean : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterMean(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterMean"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp new file mode 100644 index 000000000000..35b0f4212b71 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp @@ -0,0 +1,63 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterMorton::TriangleSplitterMorton(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ + // Calculate bounds of centroids + AABox bounds; + for (uint t = 0; t < inTriangles.size(); ++t) + bounds.Encapsulate(Vec3(mCentroids[t])); + + // Make sure box is not degenerate + bounds.EnsureMinimalEdgeLength(1.0e-5f); + + // Calculate morton codes + mMortonCodes.resize(inTriangles.size()); + for (uint t = 0; t < inTriangles.size(); ++t) + mMortonCodes[t] = MortonCode::sGetMortonCode(Vec3(mCentroids[t]), bounds); + + // Sort triangles on morton code + const Array &morton_codes = mMortonCodes; + QuickSort(mSortedTriangleIdx.begin(), mSortedTriangleIdx.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; }); +} + +bool TriangleSplitterMorton::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + uint32 first_code = mMortonCodes[mSortedTriangleIdx[inTriangles.mBegin]]; + uint32 last_code = mMortonCodes[mSortedTriangleIdx[inTriangles.mEnd - 1]]; + + uint common_prefix = CountLeadingZeros(first_code ^ last_code); + + // Use binary search to find where the next bit differs + uint split = inTriangles.mBegin; // Initial guess + uint step = inTriangles.Count(); + do + { + step = (step + 1) >> 1; // Exponential decrease + uint new_split = split + step; // Proposed new position + if (new_split < inTriangles.mEnd) + { + uint32 split_code = mMortonCodes[mSortedTriangleIdx[new_split]]; + uint split_prefix = CountLeadingZeros(first_code ^ split_code); + if (split_prefix > common_prefix) + split = new_split; // Accept proposal + } + } + while (step > 1); + + outLeft = Range(inTriangles.mBegin, split + 1); + outRight = Range(split + 1, inTriangles.mEnd); + return outLeft.Count() > 0 && outRight.Count() > 0; +} + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h new file mode 100644 index 000000000000..2f48c0ea96c1 --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using Morton codes, see: http://devblogs.nvidia.com/parallelforall/thinking-parallel-part-iii-tree-construction-gpu/ +class JPH_EXPORT TriangleSplitterMorton : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterMorton(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterMorton"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + // Precalculated Morton codes + Array mMortonCodes; +}; + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/LICENSE b/thirdparty/jolt_physics/LICENSE new file mode 100644 index 000000000000..4f0976848529 --- /dev/null +++ b/thirdparty/jolt_physics/LICENSE @@ -0,0 +1,7 @@ +Copyright 2021 Jorrit Rouwe + +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/thirdparty/libbacktrace/patches/patch_big_files.diff b/thirdparty/libbacktrace/patches/patch_big_files.diff new file mode 100644 index 000000000000..6c3185c8d160 --- /dev/null +++ b/thirdparty/libbacktrace/patches/patch_big_files.diff @@ -0,0 +1,47 @@ +diff --git a/thirdparty/libbacktrace/read.c b/thirdparty/libbacktrace/read.c +index 1811c8d2e0..fda8e222d4 100644 +--- a/thirdparty/libbacktrace/read.c ++++ b/thirdparty/libbacktrace/read.c +@@ -52,14 +52,9 @@ backtrace_get_view (struct backtrace_state *state, int descriptor, + { + uint64_t got; + ssize_t r; +- +- if ((uint64_t) (size_t) size != size) +- { +- error_callback (data, "file size too large", 0); +- return 0; +- } +- +- if (lseek (descriptor, offset, SEEK_SET) < 0) ++/* GODOT start */ ++ if (_lseeki64 (descriptor, offset, SEEK_SET) < 0) ++/* GODOT end */ + { + error_callback (data, "lseek", errno); + return 0; +@@ -74,7 +69,13 @@ backtrace_get_view (struct backtrace_state *state, int descriptor, + got = 0; + while (got < size) + { +- r = read (descriptor, view->base, size - got); ++/* GODOT start */ ++ uint64_t sz = size - got; ++ if (sz > INT_MAX) { ++ sz = INT_MAX; ++ } ++ r = _read (descriptor, view->base, sz); ++/* GODOT end */ + if (r < 0) + { + error_callback (data, "read", errno); +@@ -84,6 +85,9 @@ backtrace_get_view (struct backtrace_state *state, int descriptor, + if (r == 0) + break; + got += (uint64_t) r; ++/* GODOT start */ ++ view->base += r; ++/* GODOT end */ + } + + if (got < size) diff --git a/thirdparty/libbacktrace/read.c b/thirdparty/libbacktrace/read.c index 1811c8d2e086..f5e01f01b0bf 100644 --- a/thirdparty/libbacktrace/read.c +++ b/thirdparty/libbacktrace/read.c @@ -52,14 +52,9 @@ backtrace_get_view (struct backtrace_state *state, int descriptor, { uint64_t got; ssize_t r; - - if ((uint64_t) (size_t) size != size) - { - error_callback (data, "file size too large", 0); - return 0; - } - - if (lseek (descriptor, offset, SEEK_SET) < 0) +/* GODOT start */ + if (_lseeki64 (descriptor, offset, SEEK_SET) < 0) +/* GODOT end */ { error_callback (data, "lseek", errno); return 0; @@ -74,7 +69,13 @@ backtrace_get_view (struct backtrace_state *state, int descriptor, got = 0; while (got < size) { - r = read (descriptor, view->base, size - got); +/* GODOT start */ + uint64_t sz = size - got; + if (sz > INT_MAX) { + sz = INT_MAX; + } + r = _read (descriptor, view->base, sz); +/* GODOT end */ if (r < 0) { error_callback (data, "read", errno); @@ -84,6 +85,9 @@ backtrace_get_view (struct backtrace_state *state, int descriptor, if (r == 0) break; got += (uint64_t) r; +/* GODOT start */ + view->base += r; +/* GODOT end */ } if (got < size) diff --git a/thirdparty/linuxbsd_headers/alsa/asoundlib.h b/thirdparty/linuxbsd_headers/alsa/asoundlib.h index a5461943825c..598175403cda 100644 --- a/thirdparty/linuxbsd_headers/alsa/asoundlib.h +++ b/thirdparty/linuxbsd_headers/alsa/asoundlib.h @@ -38,7 +38,11 @@ #include #include #include +#ifdef __FreeBSD__ +#include +#else #include +#endif // __FreeBSD__ #ifndef __GNUC__ #define __inline__ inline diff --git a/thirdparty/linuxbsd_headers/alsa/patches/freebsd_endian.diff b/thirdparty/linuxbsd_headers/alsa/patches/freebsd_endian.diff new file mode 100644 index 000000000000..f104d9df8547 --- /dev/null +++ b/thirdparty/linuxbsd_headers/alsa/patches/freebsd_endian.diff @@ -0,0 +1,16 @@ +diff --git a/thirdparty/linuxbsd_headers/alsa/asoundlib.h b/thirdparty/linuxbsd_headers/alsa/asoundlib.h +index a546194382..598175403c 100644 +--- a/thirdparty/linuxbsd_headers/alsa/asoundlib.h ++++ b/thirdparty/linuxbsd_headers/alsa/asoundlib.h +@@ -38,7 +38,11 @@ + #include + #include + #include ++#ifdef __FreeBSD__ ++#include ++#else + #include ++#endif // __FreeBSD__ + + #ifndef __GNUC__ + #define __inline__ inline diff --git a/thirdparty/manifold/AUTHORS b/thirdparty/manifold/AUTHORS new file mode 100644 index 000000000000..a3e3c587f416 --- /dev/null +++ b/thirdparty/manifold/AUTHORS @@ -0,0 +1,10 @@ +# This is the list of Manifold's significant contributors. +# +# This does not necessarily list everyone who has contributed code, +# especially since many employees of one corporation may be contributing. +# To see the full list of contributors, see the revision history in +# source control. +Emmett Lalish +Chun Kit LAM +Geoff deRosenroll +Google LLC diff --git a/thirdparty/manifold/LICENSE b/thirdparty/manifold/LICENSE new file mode 100644 index 000000000000..261eeb9e9f8b --- /dev/null +++ b/thirdparty/manifold/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/thirdparty/manifold/include/manifold/common.h b/thirdparty/manifold/include/manifold/common.h new file mode 100644 index 000000000000..f1200f4654b2 --- /dev/null +++ b/thirdparty/manifold/include/manifold/common.h @@ -0,0 +1,650 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include +#include + +#ifdef MANIFOLD_DEBUG +#include +#endif + +#include "manifold/linalg.h" + +namespace manifold { +/** @addtogroup Math + * @ingroup Core + * @brief Simple math operations. + * */ + +/** @addtogroup LinAlg + * @{ + */ +namespace la = linalg; +using vec2 = la::vec; +using vec3 = la::vec; +using vec4 = la::vec; +using bvec4 = la::vec; +using mat2 = la::mat; +using mat3x2 = la::mat; +using mat4x2 = la::mat; +using mat2x3 = la::mat; +using mat3 = la::mat; +using mat4x3 = la::mat; +using mat3x4 = la::mat; +using mat4 = la::mat; +using ivec2 = la::vec; +using ivec3 = la::vec; +using ivec4 = la::vec; +using quat = la::vec; +/** @} */ + +/** @addtogroup Scalar + * @ingroup Math + * @brief Simple scalar operations. + * @{ + */ + +constexpr double kPi = 3.14159265358979323846264338327950288; +constexpr double kTwoPi = 6.28318530717958647692528676655900576; +constexpr double kHalfPi = 1.57079632679489661923132169163975144; + +/** + * Convert degrees to radians. + * + * @param a Angle in degrees. + */ +constexpr double radians(double a) { return a * kPi / 180; } + +/** + * Convert radians to degrees. + * + * @param a Angle in radians. + */ +constexpr double degrees(double a) { return a * 180 / kPi; } + +/** + * Performs smooth Hermite interpolation between 0 and 1 when edge0 < x < edge1. + * + * @param edge0 Specifies the value of the lower edge of the Hermite function. + * @param edge1 Specifies the value of the upper edge of the Hermite function. + * @param a Specifies the source value for interpolation. + */ +constexpr double smoothstep(double edge0, double edge1, double a) { + const double x = la::clamp((a - edge0) / (edge1 - edge0), 0, 1); + return x * x * (3 - 2 * x); +} + +/** + * Sine function where multiples of 90 degrees come out exact. + * + * @param x Angle in degrees. + */ +inline double sind(double x) { + if (!la::isfinite(x)) return sin(x); + if (x < 0.0) return -sind(-x); + int quo; + x = remquo(fabs(x), 90.0, &quo); + switch (quo % 4) { + case 0: + return sin(radians(x)); + case 1: + return cos(radians(x)); + case 2: + return -sin(radians(x)); + case 3: + return -cos(radians(x)); + } + return 0.0; +} + +/** + * Cosine function where multiples of 90 degrees come out exact. + * + * @param x Angle in degrees. + */ +inline double cosd(double x) { return sind(x + 90.0); } +/** @} */ + +/** @addtogroup Structs + * @ingroup Core + * @brief Miscellaneous data structures for interfacing with this library. + * @{ + */ + +/** + * @brief Single polygon contour, wound CCW. First and last point are implicitly + * connected. Should ensure all input is + * [ε-valid](https://github.com/elalish/manifold/wiki/Manifold-Library#definition-of-%CE%B5-valid). + */ +using SimplePolygon = std::vector; + +/** + * @brief Set of polygons with holes. Order of contours is arbitrary. Can + * contain any depth of nested holes and any number of separate polygons. Should + * ensure all input is + * [ε-valid](https://github.com/elalish/manifold/wiki/Manifold-Library#definition-of-%CE%B5-valid). + */ +using Polygons = std::vector; + +/** + * @brief Defines which edges to sharpen and how much for the Manifold.Smooth() + * constructor. + */ +struct Smoothness { + /// The halfedge index = 3 * tri + i, referring to Mesh.triVerts[tri][i]. + size_t halfedge; + /// A value between 0 and 1, where 0 is sharp and 1 is the default and the + /// curvature is interpolated between these values. The two paired halfedges + /// can have different values while maintaining C-1 continuity (except for 0). + double smoothness; +}; + +/** + * @brief Axis-aligned 3D box, primarily for bounding. + */ +struct Box { + vec3 min = vec3(std::numeric_limits::infinity()); + vec3 max = vec3(-std::numeric_limits::infinity()); + + /** + * Default constructor is an infinite box that contains all space. + */ + constexpr Box() {} + + /** + * Creates a box that contains the two given points. + */ + constexpr Box(const vec3 p1, const vec3 p2) { + min = la::min(p1, p2); + max = la::max(p1, p2); + } + + /** + * Returns the dimensions of the Box. + */ + constexpr vec3 Size() const { return max - min; } + + /** + * Returns the center point of the Box. + */ + constexpr vec3 Center() const { return 0.5 * (max + min); } + + /** + * Returns the absolute-largest coordinate value of any contained + * point. + */ + constexpr double Scale() const { + vec3 absMax = la::max(la::abs(min), la::abs(max)); + return la::max(absMax.x, la::max(absMax.y, absMax.z)); + } + + /** + * Does this box contain (includes equal) the given point? + */ + constexpr bool Contains(const vec3& p) const { + return la::all(la::gequal(p, min)) && la::all(la::gequal(max, p)); + } + + /** + * Does this box contain (includes equal) the given box? + */ + constexpr bool Contains(const Box& box) const { + return la::all(la::gequal(box.min, min)) && + la::all(la::gequal(max, box.max)); + } + + /** + * Expand this box to include the given point. + */ + void Union(const vec3 p) { + min = la::min(min, p); + max = la::max(max, p); + } + + /** + * Expand this box to include the given box. + */ + constexpr Box Union(const Box& box) const { + Box out; + out.min = la::min(min, box.min); + out.max = la::max(max, box.max); + return out; + } + + /** + * Transform the given box by the given axis-aligned affine transform. + * + * Ensure the transform passed in is axis-aligned (rotations are all + * multiples of 90 degrees), or else the resulting bounding box will no longer + * bound properly. + */ + constexpr Box Transform(const mat3x4& transform) const { + Box out; + vec3 minT = transform * vec4(min, 1.0); + vec3 maxT = transform * vec4(max, 1.0); + out.min = la::min(minT, maxT); + out.max = la::max(minT, maxT); + return out; + } + + /** + * Shift this box by the given vector. + */ + constexpr Box operator+(vec3 shift) const { + Box out; + out.min = min + shift; + out.max = max + shift; + return out; + } + + /** + * Shift this box in-place by the given vector. + */ + Box& operator+=(vec3 shift) { + min += shift; + max += shift; + return *this; + } + + /** + * Scale this box by the given vector. + */ + constexpr Box operator*(vec3 scale) const { + Box out; + out.min = min * scale; + out.max = max * scale; + return out; + } + + /** + * Scale this box in-place by the given vector. + */ + Box& operator*=(vec3 scale) { + min *= scale; + max *= scale; + return *this; + } + + /** + * Does this box overlap the one given (including equality)? + */ + constexpr bool DoesOverlap(const Box& box) const { + return min.x <= box.max.x && min.y <= box.max.y && min.z <= box.max.z && + max.x >= box.min.x && max.y >= box.min.y && max.z >= box.min.z; + } + + /** + * Does the given point project within the XY extent of this box + * (including equality)? + */ + constexpr bool DoesOverlap(vec3 p) const { // projected in z + return p.x <= max.x && p.x >= min.x && p.y <= max.y && p.y >= min.y; + } + + /** + * Does this box have finite bounds? + */ + constexpr bool IsFinite() const { + return la::all(la::isfinite(min)) && la::all(la::isfinite(max)); + } +}; + +/** + * @brief Axis-aligned 2D box, primarily for bounding. + */ +struct Rect { + vec2 min = vec2(std::numeric_limits::infinity()); + vec2 max = vec2(-std::numeric_limits::infinity()); + + /** + * Default constructor is an empty rectangle.. + */ + constexpr Rect() {} + + /** + * Create a rectangle that contains the two given points. + */ + constexpr Rect(const vec2 a, const vec2 b) { + min = la::min(a, b); + max = la::max(a, b); + } + + /** @name Information + * Details of the rectangle + */ + ///@{ + + /** + * Return the dimensions of the rectangle. + */ + constexpr vec2 Size() const { return max - min; } + + /** + * Return the area of the rectangle. + */ + constexpr double Area() const { + auto sz = Size(); + return sz.x * sz.y; + } + + /** + * Returns the absolute-largest coordinate value of any contained + * point. + */ + constexpr double Scale() const { + vec2 absMax = la::max(la::abs(min), la::abs(max)); + return la::max(absMax.x, absMax.y); + } + + /** + * Returns the center point of the rectangle. + */ + constexpr vec2 Center() const { return 0.5 * (max + min); } + + /** + * Does this rectangle contain (includes on border) the given point? + */ + constexpr bool Contains(const vec2& p) const { + return la::all(la::gequal(p, min)) && la::all(la::gequal(max, p)); + } + + /** + * Does this rectangle contain (includes equal) the given rectangle? + */ + constexpr bool Contains(const Rect& rect) const { + return la::all(la::gequal(rect.min, min)) && + la::all(la::gequal(max, rect.max)); + } + + /** + * Does this rectangle overlap the one given (including equality)? + */ + constexpr bool DoesOverlap(const Rect& rect) const { + return min.x <= rect.max.x && min.y <= rect.max.y && max.x >= rect.min.x && + max.y >= rect.min.y; + } + + /** + * Is the rectangle empty (containing no space)? + */ + constexpr bool IsEmpty() const { return max.y <= min.y || max.x <= min.x; }; + + /** + * Does this recangle have finite bounds? + */ + constexpr bool IsFinite() const { + return la::all(la::isfinite(min)) && la::all(la::isfinite(max)); + } + + ///@} + + /** @name Modification + */ + ///@{ + + /** + * Expand this rectangle (in place) to include the given point. + */ + void Union(const vec2 p) { + min = la::min(min, p); + max = la::max(max, p); + } + + /** + * Expand this rectangle to include the given Rect. + */ + constexpr Rect Union(const Rect& rect) const { + Rect out; + out.min = la::min(min, rect.min); + out.max = la::max(max, rect.max); + return out; + } + + /** + * Shift this rectangle by the given vector. + */ + constexpr Rect operator+(const vec2 shift) const { + Rect out; + out.min = min + shift; + out.max = max + shift; + return out; + } + + /** + * Shift this rectangle in-place by the given vector. + */ + Rect& operator+=(const vec2 shift) { + min += shift; + max += shift; + return *this; + } + + /** + * Scale this rectangle by the given vector. + */ + constexpr Rect operator*(const vec2 scale) const { + Rect out; + out.min = min * scale; + out.max = max * scale; + return out; + } + + /** + * Scale this rectangle in-place by the given vector. + */ + Rect& operator*=(const vec2 scale) { + min *= scale; + max *= scale; + return *this; + } + + /** + * Transform the rectangle by the given axis-aligned affine transform. + * + * Ensure the transform passed in is axis-aligned (rotations are all + * multiples of 90 degrees), or else the resulting rectangle will no longer + * bound properly. + */ + constexpr Rect Transform(const mat2x3& m) const { + Rect rect; + rect.min = m * vec3(min, 1); + rect.max = m * vec3(max, 1); + return rect; + } + ///@} +}; + +/** + * @brief Boolean operation type: Add (Union), Subtract (Difference), and + * Intersect. + */ +enum class OpType { Add, Subtract, Intersect }; + +constexpr int DEFAULT_SEGMENTS = 0; +constexpr double DEFAULT_ANGLE = 10.0; +constexpr double DEFAULT_LENGTH = 1.0; +/** + * @brief These static properties control how circular shapes are quantized by + * default on construction. + * + * If circularSegments is specified, it takes + * precedence. If it is zero, then instead the minimum is used of the segments + * calculated based on edge length and angle, rounded up to the nearest + * multiple of four. To get numbers not divisible by four, circularSegments + * must be specified. + */ +class Quality { + private: + inline static int circularSegments_ = DEFAULT_SEGMENTS; + inline static double circularAngle_ = DEFAULT_ANGLE; + inline static double circularEdgeLength_ = DEFAULT_LENGTH; + + public: + /** + * Sets an angle constraint the default number of circular segments for the + * CrossSection::Circle(), Manifold::Cylinder(), Manifold::Sphere(), and + * Manifold::Revolve() constructors. The number of segments will be rounded up + * to the nearest factor of four. + * + * @param angle The minimum angle in degrees between consecutive segments. The + * angle will increase if the the segments hit the minimum edge length. + * Default is 10 degrees. + */ + static void SetMinCircularAngle(double angle) { + if (angle <= 0) return; + circularAngle_ = angle; + } + + /** + * Sets a length constraint the default number of circular segments for the + * CrossSection::Circle(), Manifold::Cylinder(), Manifold::Sphere(), and + * Manifold::Revolve() constructors. The number of segments will be rounded up + * to the nearest factor of four. + * + * @param length The minimum length of segments. The length will + * increase if the the segments hit the minimum angle. Default is 1.0. + */ + static void SetMinCircularEdgeLength(double length) { + if (length <= 0) return; + circularEdgeLength_ = length; + } + + /** + * Sets the default number of circular segments for the + * CrossSection::Circle(), Manifold::Cylinder(), Manifold::Sphere(), and + * Manifold::Revolve() constructors. Overrides the edge length and angle + * constraints and sets the number of segments to exactly this value. + * + * @param number Number of circular segments. Default is 0, meaning no + * constraint is applied. + */ + static void SetCircularSegments(int number) { + if (number < 3 && number != 0) return; + circularSegments_ = number; + } + + /** + * Determine the result of the SetMinCircularAngle(), + * SetMinCircularEdgeLength(), and SetCircularSegments() defaults. + * + * @param radius For a given radius of circle, determine how many default + * segments there will be. + */ + static int GetCircularSegments(double radius) { + if (circularSegments_ > 0) return circularSegments_; + int nSegA = 360.0 / circularAngle_; + int nSegL = 2.0 * radius * kPi / circularEdgeLength_; + int nSeg = fmin(nSegA, nSegL) + 3; + nSeg -= nSeg % 4; + return std::max(nSeg, 3); + } + + /** + * Resets the circular construction parameters to their defaults if + * SetMinCircularAngle, SetMinCircularEdgeLength, or SetCircularSegments have + * been called. + */ + static void ResetToDefaults() { + circularSegments_ = DEFAULT_SEGMENTS; + circularAngle_ = DEFAULT_ANGLE; + circularEdgeLength_ = DEFAULT_LENGTH; + } +}; +/** @} */ + +/** @addtogroup Debug + * @ingroup Optional + * @{ + */ + +/** + * @brief Global parameters that control debugging output. Only has an + * effect when compiled with the MANIFOLD_DEBUG flag. + */ +struct ExecutionParams { + /// Perform extra sanity checks and assertions on the intermediate data + /// structures. + bool intermediateChecks = false; + /// Verbose output primarily of the Boolean, including timing info and vector + /// sizes. + bool verbose = false; + /// If processOverlaps is false, a geometric check will be performed to assert + /// all triangles are CCW. + bool processOverlaps = true; + /// Suppresses printed errors regarding CW triangles. Has no effect if + /// processOverlaps is true. + bool suppressErrors = false; + /// Perform optional but recommended triangle cleanups in SimplifyTopology() + bool cleanupTriangles = true; +}; +/** @} */ + +#ifdef MANIFOLD_DEBUG +inline std::ostream& operator<<(std::ostream& stream, const Box& box) { + return stream << "min: " << box.min << ", " + << "max: " << box.max; +} + +inline std::ostream& operator<<(std::ostream& stream, const Rect& box) { + return stream << "min: " << box.min << ", " + << "max: " << box.max; +} + +/** + * Print the contents of this vector to standard output. Only exists if compiled + * with MANIFOLD_DEBUG flag. + */ +template +void Dump(const std::vector& vec) { + std::cout << "Vec = " << std::endl; + for (size_t i = 0; i < vec.size(); ++i) { + std::cout << i << ", " << vec[i] << ", " << std::endl; + } + std::cout << std::endl; +} + +template +void Diff(const std::vector& a, const std::vector& b) { + std::cout << "Diff = " << std::endl; + if (a.size() != b.size()) { + std::cout << "a and b must have the same length, aborting Diff" + << std::endl; + return; + } + for (size_t i = 0; i < a.size(); ++i) { + if (a[i] != b[i]) + std::cout << i << ": " << a[i] << ", " << b[i] << std::endl; + } + std::cout << std::endl; +} + +struct Timer { + std::chrono::high_resolution_clock::time_point start, end; + + void Start() { start = std::chrono::high_resolution_clock::now(); } + + void Stop() { end = std::chrono::high_resolution_clock::now(); } + + float Elapsed() { + return std::chrono::duration_cast(end - start) + .count(); + } + void Print(std::string message) { + std::cout << "----------- " << std::round(Elapsed()) << " ms for " + << message << std::endl; + } +}; +#endif +} // namespace manifold diff --git a/thirdparty/manifold/include/manifold/linalg.h b/thirdparty/manifold/include/manifold/linalg.h new file mode 100644 index 000000000000..dfcd305ba8fd --- /dev/null +++ b/thirdparty/manifold/include/manifold/linalg.h @@ -0,0 +1,2601 @@ +// Copyright 2024 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Based on linalg.h - 2.2 - Single-header public domain linear algebra library +// +// The intent of this library is to provide the bulk of the functionality +// you need to write programs that frequently use small, fixed-size vectors +// and matrices, in domains such as computational geometry or computer +// graphics. It strives for terse, readable source code. +// +// The original author of this software is Sterling Orsten, and its permanent +// home is . If you find this software +// useful, an acknowledgement in your source text and/or product documentation +// is appreciated, but not required. +// +// The author acknowledges significant insights and contributions by: +// Stan Melax +// Dimitri Diakopoulos +// +// Some features are deprecated. Define LINALG_FORWARD_COMPATIBLE to remove +// them. + +#pragma once +#ifndef LINALG_H +#define LINALG_H + +#include // For std::array +#include // For various unary math functions, such as std::sqrt +#include // For implementing namespace linalg::aliases +#include // To resolve std::abs ambiguity on clang +#include // For std::hash declaration +#include // For forward definitions of std::ostream +#include // For std::enable_if, std::is_same, std::declval + +#ifdef MANIFOLD_DEBUG +#include +#include +#endif + +// In Visual Studio 2015, `constexpr` applied to a member function implies +// `const`, which causes ambiguous overload resolution +#if defined(_MSC_VER) && (_MSC_VER <= 1900) +#define LINALG_CONSTEXPR14 +#else +#define LINALG_CONSTEXPR14 constexpr +#endif + +namespace linalg { +// Small, fixed-length vector type, consisting of exactly M elements of type T, +// and presumed to be a column-vector unless otherwise noted. +template +struct vec; + +// Small, fixed-size matrix type, consisting of exactly M rows and N columns of +// type T, stored in column-major order. +template +struct mat; + +// Specialize converter with a function application operator that converts +// type U to type T to enable implicit conversions +template +struct converter {}; +namespace detail { +template +using conv_t = typename std::enable_if::value, + decltype(converter{}( + std::declval()))>::type; + +// Trait for retrieving scalar type of any linear algebra object +template +struct scalar_type {}; +template +struct scalar_type> { + using type = T; +}; +template +struct scalar_type> { + using type = T; +}; + +// Type returned by the compare(...) function which supports all six comparison +// operators against 0 +template +struct ord { + T a, b; +}; +template +constexpr bool operator==(const ord &o, std::nullptr_t) { + return o.a == o.b; +} +template +constexpr bool operator!=(const ord &o, std::nullptr_t) { + return !(o.a == o.b); +} +template +constexpr bool operator<(const ord &o, std::nullptr_t) { + return o.a < o.b; +} +template +constexpr bool operator>(const ord &o, std::nullptr_t) { + return o.b < o.a; +} +template +constexpr bool operator<=(const ord &o, std::nullptr_t) { + return !(o.b < o.a); +} +template +constexpr bool operator>=(const ord &o, std::nullptr_t) { + return !(o.a < o.b); +} + +// Patterns which can be used with the compare(...) function +template +struct any_compare {}; +template +struct any_compare, vec> { + using type = ord; + constexpr ord operator()(const vec &a, const vec &b) const { + return ord{a.x, b.x}; + } +}; +template +struct any_compare, vec> { + using type = ord; + constexpr ord operator()(const vec &a, const vec &b) const { + return !(a.x == b.x) ? ord{a.x, b.x} : ord{a.y, b.y}; + } +}; +template +struct any_compare, vec> { + using type = ord; + constexpr ord operator()(const vec &a, const vec &b) const { + return !(a.x == b.x) ? ord{a.x, b.x} + : !(a.y == b.y) ? ord{a.y, b.y} + : ord{a.z, b.z}; + } +}; +template +struct any_compare, vec> { + using type = ord; + constexpr ord operator()(const vec &a, const vec &b) const { + return !(a.x == b.x) ? ord{a.x, b.x} + : !(a.y == b.y) ? ord{a.y, b.y} + : !(a.z == b.z) ? ord{a.z, b.z} + : ord{a.w, b.w}; + } +}; +template +struct any_compare, mat> { + using type = ord; + constexpr ord operator()(const mat &a, + const mat &b) const { + return compare(a.x, b.x); + } +}; +template +struct any_compare, mat> { + using type = ord; + constexpr ord operator()(const mat &a, + const mat &b) const { + return a.x != b.x ? compare(a.x, b.x) : compare(a.y, b.y); + } +}; +template +struct any_compare, mat> { + using type = ord; + constexpr ord operator()(const mat &a, + const mat &b) const { + return a.x != b.x ? compare(a.x, b.x) + : a.y != b.y ? compare(a.y, b.y) + : compare(a.z, b.z); + } +}; +template +struct any_compare, mat> { + using type = ord; + constexpr ord operator()(const mat &a, + const mat &b) const { + return a.x != b.x ? compare(a.x, b.x) + : a.y != b.y ? compare(a.y, b.y) + : a.z != b.z ? compare(a.z, b.z) + : compare(a.w, b.w); + } +}; + +// Helper for compile-time index-based access to members of vector and matrix +// types +template +struct getter; +template <> +struct getter<0> { + template + constexpr auto operator()(A &a) const -> decltype(a.x) { + return a.x; + } +}; +template <> +struct getter<1> { + template + constexpr auto operator()(A &a) const -> decltype(a.y) { + return a.y; + } +}; +template <> +struct getter<2> { + template + constexpr auto operator()(A &a) const -> decltype(a.z) { + return a.z; + } +}; +template <> +struct getter<3> { + template + constexpr auto operator()(A &a) const -> decltype(a.w) { + return a.w; + } +}; + +// Stand-in for std::integer_sequence/std::make_integer_sequence +template +struct seq {}; +template +struct make_seq_impl; +template +struct make_seq_impl { + using type = seq<>; +}; +template +struct make_seq_impl { + using type = seq; +}; +template +struct make_seq_impl { + using type = seq; +}; +template +struct make_seq_impl { + using type = seq; +}; +template +struct make_seq_impl { + using type = seq; +}; +template +using make_seq = typename make_seq_impl::type; +template +vec constexpr swizzle(const vec &v, seq i) { + return {getter{}(v)...}; +} +template +mat constexpr swizzle(const mat &m, + seq i, seq j) { + return {swizzle(getter{}(m), i)...}; +} + +// SFINAE helpers to determine result of function application +template +using ret_t = decltype(std::declval()(std::declval()...)); + +// SFINAE helper which is defined if all provided types are scalars +struct empty {}; +template +struct scalars; +template <> +struct scalars<> { + using type = void; +}; +template +struct scalars : std::conditional::value, + scalars, empty>::type {}; +template +using scalars_t = typename scalars::type; + +// Helpers which indicate how apply(F, ...) should be called for various +// arguments +template +struct apply {}; // Patterns which contain only vectors or scalars +template +struct apply, vec> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, const vec &a) { + return {f(getter{}(a))...}; + } +}; +template +struct apply, vec, vec> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, const vec &a, + const vec &b) { + return {f(getter{}(a), getter{}(b))...}; + } +}; +template +struct apply, vec, B> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, const vec &a, B b) { + return {f(getter{}(a), b)...}; + } +}; +template +struct apply, A, vec> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, A a, const vec &b) { + return {f(a, getter{}(b))...}; + } +}; +template +struct apply, vec, vec, vec> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, const vec &a, + const vec &b, const vec &c) { + return {f(getter{}(a), getter{}(b), getter{}(c))...}; + } +}; +template +struct apply, vec, vec, C> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, const vec &a, + const vec &b, C c) { + return {f(getter{}(a), getter{}(b), c)...}; + } +}; +template +struct apply, vec, B, vec> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, const vec &a, B b, + const vec &c) { + return {f(getter{}(a), b, getter{}(c))...}; + } +}; +template +struct apply, vec, B, C> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, const vec &a, B b, C c) { + return {f(getter{}(a), b, c)...}; + } +}; +template +struct apply, A, vec, vec> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, A a, const vec &b, + const vec &c) { + return {f(a, getter{}(b), getter{}(c))...}; + } +}; +template +struct apply, A, vec, C> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, A a, const vec &b, C c) { + return {f(a, getter{}(b), c)...}; + } +}; +template +struct apply, A, B, vec> { + using type = vec, M>; + enum { size = M, mm = 0 }; + template + static constexpr type impl(seq, F f, A a, B b, const vec &c) { + return {f(a, b, getter{}(c))...}; + } +}; +template +struct apply, mat> { + using type = mat, M, N>; + enum { size = N, mm = 0 }; + template + static constexpr type impl(seq, F f, const mat &a) { + return {apply>::impl(make_seq<0, M>{}, f, + getter{}(a))...}; + } +}; +template +struct apply, mat, mat> { + using type = mat, M, N>; + enum { size = N, mm = 1 }; + template + static constexpr type impl(seq, F f, const mat &a, + const mat &b) { + return {apply, vec>::impl( + make_seq<0, M>{}, f, getter{}(a), getter{}(b))...}; + } +}; +template +struct apply, mat, B> { + using type = mat, M, N>; + enum { size = N, mm = 0 }; + template + static constexpr type impl(seq, F f, const mat &a, B b) { + return {apply, B>::impl(make_seq<0, M>{}, f, + getter{}(a), b)...}; + } +}; +template +struct apply, A, mat> { + using type = mat, M, N>; + enum { size = N, mm = 0 }; + template + static constexpr type impl(seq, F f, A a, const mat &b) { + return {apply>::impl(make_seq<0, M>{}, f, a, + getter{}(b))...}; + } +}; +template +struct apply, A...> { + using type = ret_t; + enum { size = 0, mm = 0 }; + static constexpr type impl(seq<>, F f, A... a) { return f(a...); } +}; + +// Function objects for selecting between alternatives +struct min { + template + constexpr auto operator()(A a, B b) const -> + typename std::remove_reference::type { + return a < b ? a : b; + } +}; +struct max { + template + constexpr auto operator()(A a, B b) const -> + typename std::remove_reference::type { + return a < b ? b : a; + } +}; +struct clamp { + template + constexpr auto operator()(A a, B b, C c) const -> + typename std::remove_reference::type { + return a < b ? b : a < c ? a : c; + } +}; +struct select { + template + constexpr auto operator()(A a, B b, C c) const -> + typename std::remove_reference::type { + return a ? b : c; + } +}; +struct lerp { + template + constexpr auto operator()(A a, B b, + C c) const -> decltype(a * (1 - c) + b * c) { + return a * (1 - c) + b * c; + } +}; + +// Function objects for applying operators +struct op_pos { + template + constexpr auto operator()(A a) const -> decltype(+a) { + return +a; + } +}; +struct op_neg { + template + constexpr auto operator()(A a) const -> decltype(-a) { + return -a; + } +}; +struct op_not { + template + constexpr auto operator()(A a) const -> decltype(!a) { + return !a; + } +}; +struct op_cmp { + template + constexpr auto operator()(A a) const -> decltype(~(a)) { + return ~a; + } +}; +struct op_mul { + template + constexpr auto operator()(A a, B b) const -> decltype(a * b) { + return a * b; + } +}; +struct op_div { + template + constexpr auto operator()(A a, B b) const -> decltype(a / b) { + return a / b; + } +}; +struct op_mod { + template + constexpr auto operator()(A a, B b) const -> decltype(a % b) { + return a % b; + } +}; +struct op_add { + template + constexpr auto operator()(A a, B b) const -> decltype(a + b) { + return a + b; + } +}; +struct op_sub { + template + constexpr auto operator()(A a, B b) const -> decltype(a - b) { + return a - b; + } +}; +struct op_lsh { + template + constexpr auto operator()(A a, B b) const -> decltype(a << b) { + return a << b; + } +}; +struct op_rsh { + template + constexpr auto operator()(A a, B b) const -> decltype(a >> b) { + return a >> b; + } +}; +struct op_lt { + template + constexpr auto operator()(A a, B b) const -> decltype(a < b) { + return a < b; + } +}; +struct op_gt { + template + constexpr auto operator()(A a, B b) const -> decltype(a > b) { + return a > b; + } +}; +struct op_le { + template + constexpr auto operator()(A a, B b) const -> decltype(a <= b) { + return a <= b; + } +}; +struct op_ge { + template + constexpr auto operator()(A a, B b) const -> decltype(a >= b) { + return a >= b; + } +}; +struct op_eq { + template + constexpr auto operator()(A a, B b) const -> decltype(a == b) { + return a == b; + } +}; +struct op_ne { + template + constexpr auto operator()(A a, B b) const -> decltype(a != b) { + return a != b; + } +}; +struct op_int { + template + constexpr auto operator()(A a, B b) const -> decltype(a & b) { + return a & b; + } +}; +struct op_xor { + template + constexpr auto operator()(A a, B b) const -> decltype(a ^ b) { + return a ^ b; + } +}; +struct op_un { + template + constexpr auto operator()(A a, B b) const -> decltype(a | b) { + return a | b; + } +}; +struct op_and { + template + constexpr auto operator()(A a, B b) const -> decltype(a && b) { + return a && b; + } +}; +struct op_or { + template + constexpr auto operator()(A a, B b) const -> decltype(a || b) { + return a || b; + } +}; + +// Function objects for applying standard library math functions +struct std_isfinite { + template + constexpr auto operator()(A a) const -> decltype(std::isfinite(a)) { + return std::isfinite(a); + } +}; +struct std_abs { + template + constexpr auto operator()(A a) const -> decltype(std::abs(a)) { + return std::abs(a); + } +}; +struct std_floor { + template + constexpr auto operator()(A a) const -> decltype(std::floor(a)) { + return std::floor(a); + } +}; +struct std_ceil { + template + constexpr auto operator()(A a) const -> decltype(std::ceil(a)) { + return std::ceil(a); + } +}; +struct std_exp { + template + constexpr auto operator()(A a) const -> decltype(std::exp(a)) { + return std::exp(a); + } +}; +struct std_log { + template + constexpr auto operator()(A a) const -> decltype(std::log(a)) { + return std::log(a); + } +}; +struct std_log2 { + template + constexpr auto operator()(A a) const -> decltype(std::log2(a)) { + return std::log2(a); + } +}; +struct std_log10 { + template + constexpr auto operator()(A a) const -> decltype(std::log10(a)) { + return std::log10(a); + } +}; +struct std_sqrt { + template + constexpr auto operator()(A a) const -> decltype(std::sqrt(a)) { + return std::sqrt(a); + } +}; +struct std_sin { + template + constexpr auto operator()(A a) const -> decltype(std::sin(a)) { + return std::sin(a); + } +}; +struct std_cos { + template + constexpr auto operator()(A a) const -> decltype(std::cos(a)) { + return std::cos(a); + } +}; +struct std_tan { + template + constexpr auto operator()(A a) const -> decltype(std::tan(a)) { + return std::tan(a); + } +}; +struct std_asin { + template + constexpr auto operator()(A a) const -> decltype(std::asin(a)) { + return std::asin(a); + } +}; +struct std_acos { + template + constexpr auto operator()(A a) const -> decltype(std::acos(a)) { + return std::acos(a); + } +}; +struct std_atan { + template + constexpr auto operator()(A a) const -> decltype(std::atan(a)) { + return std::atan(a); + } +}; +struct std_sinh { + template + constexpr auto operator()(A a) const -> decltype(std::sinh(a)) { + return std::sinh(a); + } +}; +struct std_cosh { + template + constexpr auto operator()(A a) const -> decltype(std::cosh(a)) { + return std::cosh(a); + } +}; +struct std_tanh { + template + constexpr auto operator()(A a) const -> decltype(std::tanh(a)) { + return std::tanh(a); + } +}; +struct std_round { + template + constexpr auto operator()(A a) const -> decltype(std::round(a)) { + return std::round(a); + } +}; +struct std_fmod { + template + constexpr auto operator()(A a, B b) const -> decltype(std::fmod(a, b)) { + return std::fmod(a, b); + } +}; +struct std_pow { + template + constexpr auto operator()(A a, B b) const -> decltype(std::pow(a, b)) { + return std::pow(a, b); + } +}; +struct std_atan2 { + template + constexpr auto operator()(A a, B b) const -> decltype(std::atan2(a, b)) { + return std::atan2(a, b); + } +}; +struct std_copysign { + template + constexpr auto operator()(A a, B b) const -> decltype(std::copysign(a, b)) { + return std::copysign(a, b); + } +}; +} // namespace detail + +/** @addtogroup LinAlg + * @ingroup Math + */ + +/** @addtogroup vec + * @ingroup LinAlg + * @brief `linalg::vec` defines a fixed-length vector containing exactly + `M` elements of type `T`. + +This data structure can be used to store a wide variety of types of data, +including geometric vectors, points, homogeneous coordinates, plane equations, +colors, texture coordinates, or any other situation where you need to manipulate +a small sequence of numbers. As such, `vec` is supported by a set of +algebraic and component-wise functions, as well as a set of standard reductions. + +`vec`: +- is + [`DefaultConstructible`](https://en.cppreference.com/w/cpp/named_req/DefaultConstructible): + ```cpp + float3 v; // v contains 0,0,0 + ``` +- is constructible from `M` elements of type `T`: + ```cpp + float3 v {1,2,3}; // v contains 1,2,3 + ``` +- is + [`CopyConstructible`](https://en.cppreference.com/w/cpp/named_req/CopyConstructible) + and + [`CopyAssignable`](https://en.cppreference.com/w/cpp/named_req/CopyAssignable): + ```cpp + float3 v {1,2,3}; // v contains 1,2,3 + float3 u {v}; // u contains 1,2,3 + float3 w; // w contains 0,0,0 + w = u; // w contains 1,2,3 + ``` +- is + [`EqualityComparable`](https://en.cppreference.com/w/cpp/named_req/EqualityComparable) + and + [`LessThanComparable`](https://en.cppreference.com/w/cpp/named_req/LessThanComparable): + ```cpp + if(v == y) cout << "v and u contain equal elements in the same positions" << + endl; if(v < u) cout << "v precedes u lexicographically" << endl; + ``` +- is **explicitly** constructible from a single element of type `T`: + ```cpp + float3 v = float3{4}; // v contains 4,4,4 + ``` +- is **explicitly** constructible from a `vec` of some other type `U`: + ```cpp + float3 v {1.1f,2.3f,3.5f}; // v contains 1.1,2.3,3.5 + int3 u = int3{v}; // u contains 1,2,3 + ``` +- has fields `x,y,z,w`: + ```cpp + float y = point.y; // y contains second element of point + pixel.w = 0.5; // fourth element of pixel set to 0.5 + float s = tc.x; // s contains first element of tc + ``` +- supports indexing: + ```cpp + float x = v[0]; // x contains first element of v + v[2] = 5; // third element of v set to 5 + ``` +- supports unary operators `+`, `-`, `!` and `~` in component-wise fashion: + ```cpp + auto v = -float{2,3}; // v is float2{-2,-3} + ``` +- supports binary operators `+`, `-`, `*`, `/`, `%`, `|`, `&`, `^`, `<<` and + `>>` in component-wise fashion: + ```cpp + auto v = float2{1,1} + float2{2,3}; // v is float2{3,4} + ``` +- supports binary operators with a scalar on the left or the right: + ```cpp + auto v = 2 * float3{1,2,3}; // v is float3{2,4,6} + auto u = float3{1,2,3} + 1; // u is float3{2,3,4} + ``` +- supports operators `+=`, `-=`, `*=`, `/=`, `%=`, `|=`, `&=`, `^=`, `<<=` and + `>>=` with vectors or scalars on the right: + ```cpp + float2 v {1,2}; v *= 3; // v is float2{3,6} + ``` +- supports operations on mixed element types: + ```cpp + auto v = float3{1,2,3} + int3{4,5,6}; // v is float3{5,7,9} + ``` +- supports [range-based + for](https://en.cppreference.com/w/cpp/language/range-for): + ```cpp + for(auto elem : float3{1,2,3}) cout << elem << ' '; // prints "1 2 3 " + ``` +- has a flat memory layout: + ```cpp + float3 v {1,2,3}; + float * p = v.data(); // &v[i] == p+i + p[1] = 4; // v contains 1,4,3 + ``` + * @{ + */ +template +struct vec { + T x; + constexpr vec() : x() {} + constexpr vec(const T &x_) : x(x_) {} + // NOTE: vec does NOT have a constructor from pointer, this can conflict + // with initializing its single element from zero + template + constexpr explicit vec(const vec &v) : vec(static_cast(v.x)) {} + constexpr const T &operator[](int i) const { return x; } + LINALG_CONSTEXPR14 T &operator[](int i) { return x; } + + template > + constexpr vec(const U &u) : vec(converter{}(u)) {} + template > + constexpr operator U() const { + return converter{}(*this); + } +}; +template +struct vec { + T x, y; + constexpr vec() : x(), y() {} + constexpr vec(const T &x_, const T &y_) : x(x_), y(y_) {} + constexpr explicit vec(const T &s) : vec(s, s) {} + constexpr explicit vec(const T *p) : vec(p[0], p[1]) {} + template + constexpr explicit vec(const vec &v) + : vec(static_cast(v.x), static_cast(v.y)) { + static_assert( + N >= 2, + "You must give extra arguments if your input vector is shorter."); + } + constexpr const T &operator[](int i) const { return i == 0 ? x : y; } + LINALG_CONSTEXPR14 T &operator[](int i) { return i == 0 ? x : y; } + + template > + constexpr vec(const U &u) : vec(converter{}(u)) {} + template > + constexpr operator U() const { + return converter{}(*this); + } +}; +template +struct vec { + T x, y, z; + constexpr vec() : x(), y(), z() {} + constexpr vec(const T &x_, const T &y_, const T &z_) : x(x_), y(y_), z(z_) {} + constexpr vec(const vec &xy, const T &z_) : vec(xy.x, xy.y, z_) {} + constexpr explicit vec(const T &s) : vec(s, s, s) {} + constexpr explicit vec(const T *p) : vec(p[0], p[1], p[2]) {} + template + constexpr explicit vec(const vec &v) + : vec(static_cast(v.x), static_cast(v.y), static_cast(v.z)) { + static_assert( + N >= 3, + "You must give extra arguments if your input vector is shorter."); + } + constexpr const T &operator[](int i) const { + return i == 0 ? x : i == 1 ? y : z; + } + LINALG_CONSTEXPR14 T &operator[](int i) { + return i == 0 ? x : i == 1 ? y : z; + } + constexpr const vec &xy() const { + return *reinterpret_cast *>(this); + } + vec &xy() { return *reinterpret_cast *>(this); } + + template > + constexpr vec(const U &u) : vec(converter{}(u)) {} + template > + constexpr operator U() const { + return converter{}(*this); + } +}; +template +struct vec { + T x, y, z, w; + constexpr vec() : x(), y(), z(), w() {} + constexpr vec(const T &x_, const T &y_, const T &z_, const T &w_) + : x(x_), y(y_), z(z_), w(w_) {} + constexpr vec(const vec &xy, const T &z_, const T &w_) + : vec(xy.x, xy.y, z_, w_) {} + constexpr vec(const vec &xyz, const T &w_) + : vec(xyz.x, xyz.y, xyz.z, w_) {} + constexpr explicit vec(const T &s) : vec(s, s, s, s) {} + constexpr explicit vec(const T *p) : vec(p[0], p[1], p[2], p[3]) {} + template + constexpr explicit vec(const vec &v) + : vec(static_cast(v.x), static_cast(v.y), static_cast(v.z), + static_cast(v.w)) { + static_assert( + N >= 4, + "You must give extra arguments if your input vector is shorter."); + } + constexpr const T &operator[](int i) const { + return i == 0 ? x : i == 1 ? y : i == 2 ? z : w; + } + LINALG_CONSTEXPR14 T &operator[](int i) { + return i == 0 ? x : i == 1 ? y : i == 2 ? z : w; + } + constexpr const vec &xy() const { + return *reinterpret_cast *>(this); + } + constexpr const vec &xyz() const { + return *reinterpret_cast *>(this); + } + vec &xy() { return *reinterpret_cast *>(this); } + vec &xyz() { return *reinterpret_cast *>(this); } + + template > + constexpr vec(const U &u) : vec(converter{}(u)) {} + template > + constexpr operator U() const { + return converter{}(*this); + } +}; +/** @} */ + +/** @addtogroup mat + * @ingroup LinAlg + * @brief `linalg::mat` defines a fixed-size matrix containing exactly + `M` rows and `N` columns of type `T`, in column-major order. + +This data structure is supported by a set of algebraic and component-wise +functions, as well as a set of standard reductions. + +`mat`: +- is + [`DefaultConstructible`](https://en.cppreference.com/w/cpp/named_req/DefaultConstructible): + ```cpp + float2x2 m; // m contains columns 0,0; 0,0 + ``` +- is constructible from `N` columns of type `vec`: + ```cpp + float2x2 m {{1,2},{3,4}}; // m contains columns 1,2; 3,4 + ``` +- is constructible from `linalg::identity`: + ```cpp + float3x3 m = linalg::identity; // m contains columns 1,0,0; 0,1,0; 0,0,1 + ``` +- is + [`CopyConstructible`](https://en.cppreference.com/w/cpp/named_req/CopyConstructible) + and + [`CopyAssignable`](https://en.cppreference.com/w/cpp/named_req/CopyAssignable): + ```cpp + float2x2 m {{1,2},{3,4}}; // m contains columns 1,2; 3,4 + float2x2 n {m}; // n contains columns 1,2; 3,4 + float2x2 p; // p contains columns 0,0; 0,0 + p = n; // p contains columns 1,2; 3,4 + ``` +- is + [`EqualityComparable`](https://en.cppreference.com/w/cpp/named_req/EqualityComparable) + and + [`LessThanComparable`](https://en.cppreference.com/w/cpp/named_req/LessThanComparable): + ```cpp + if(m == n) cout << "m and n contain equal elements in the same positions" << + endl; if(m < n) cout << "m precedes n lexicographically when compared in + column-major order" << endl; + ``` +- is **explicitly** constructible from a single element of type `T`: + ```cpp + float2x2 m {5}; // m contains columns 5,5; 5,5 + ``` +- is **explicitly** constructible from a `mat` of some other type `U`: + ```cpp + float2x2 m {int2x2{{5,6},{7,8}}}; // m contains columns 5,6; 7,8 + ``` +- supports indexing into *columns*: + ```cpp + float2x3 m {{1,2},{3,4},{5,6}}; // m contains columns 1,2; 3,4; 5,6 + float2 c = m[0]; // c contains 1,2 + m[1] = {7,8}; // m contains columns 1,2; 7,8; 5,6 + ``` +- supports retrieval (but not assignment) of rows: + ```cpp + float2x3 m {{1,2},{3,4},{5,6}}; // m contains columns 1,2; 3,4; 5,6 + float3 r = m.row(1); // r contains 2,4,6 + ``` + +- supports unary operators `+`, `-`, `!` and `~` in component-wise fashion: + ```cpp + float2x2 m {{1,2},{3,4}}; // m contains columns 1,2; 3,4 + float2x2 n = -m; // n contains columns -1,-2; -3,-4 + ``` +- supports binary operators `+`, `-`, `*`, `/`, `%`, `|`, `&`, `^`, `<<` and + `>>` in component-wise fashion: + ```cpp + float2x2 a {{0,0},{2,2}}; // a contains columns 0,0; 2,2 + float2x2 b {{1,2},{1,2}}; // b contains columns 1,2; 1,2 + float2x2 c = a + b; // c contains columns 1,2; 3,4 + ``` + +- supports binary operators with a scalar on the left or the right: + ```cpp + auto m = 2 * float2x2{{1,2},{3,4}}; // m is float2x2{{2,4},{6,8}} + ``` + +- supports operators `+=`, `-=`, `*=`, `/=`, `%=`, `|=`, `&=`, `^=`, `<<=` and + `>>=` with matrices or scalars on the right: + ```cpp + float2x2 v {{5,4},{3,2}}; + v *= 3; // v is float2x2{{15,12},{9,6}} + ``` + +- supports operations on mixed element types: + +- supports [range-based + for](https://en.cppreference.com/w/cpp/language/range-for) over columns + +- has a flat memory layout + * @{ + */ +template +struct mat { + typedef vec V; + V x; + constexpr mat() : x() {} + constexpr mat(const V &x_) : x(x_) {} + constexpr explicit mat(const T &s) : x(s) {} + constexpr explicit mat(const T *p) : x(p + M * 0) {} + template + constexpr explicit mat(const mat &m) : mat(V(m.x)) {} + constexpr vec row(int i) const { return {x[i]}; } + constexpr const V &operator[](int j) const { return x; } + LINALG_CONSTEXPR14 V &operator[](int j) { return x; } + + template > + constexpr mat(const U &u) : mat(converter{}(u)) {} + template > + constexpr operator U() const { + return converter{}(*this); + } +}; +template +struct mat { + typedef vec V; + V x, y; + constexpr mat() : x(), y() {} + constexpr mat(const V &x_, const V &y_) : x(x_), y(y_) {} + constexpr explicit mat(const T &s) : x(s), y(s) {} + constexpr explicit mat(const T *p) : x(p + M * 0), y(p + M * 1) {} + template + constexpr explicit mat(const mat &m) : mat(V(m.x), V(m.y)) { + static_assert(P >= 2, "Input matrix dimensions must be at least as big."); + } + constexpr vec row(int i) const { return {x[i], y[i]}; } + constexpr const V &operator[](int j) const { return j == 0 ? x : y; } + LINALG_CONSTEXPR14 V &operator[](int j) { return j == 0 ? x : y; } + + template > + constexpr mat(const U &u) : mat(converter{}(u)) {} + template > + constexpr operator U() const { + return converter{}(*this); + } +}; +template +struct mat { + typedef vec V; + V x, y, z; + constexpr mat() : x(), y(), z() {} + constexpr mat(const V &x_, const V &y_, const V &z_) : x(x_), y(y_), z(z_) {} + constexpr mat(const mat &m_, const V &z_) + : x(m_.x), y(m_.y), z(z_) {} + constexpr explicit mat(const T &s) : x(s), y(s), z(s) {} + constexpr explicit mat(const T *p) + : x(p + M * 0), y(p + M * 1), z(p + M * 2) {} + template + constexpr explicit mat(const mat &m) : mat(V(m.x), V(m.y), V(m.z)) { + static_assert(P >= 3, "Input matrix dimensions must be at least as big."); + } + constexpr vec row(int i) const { return {x[i], y[i], z[i]}; } + constexpr const V &operator[](int j) const { + return j == 0 ? x : j == 1 ? y : z; + } + LINALG_CONSTEXPR14 V &operator[](int j) { + return j == 0 ? x : j == 1 ? y : z; + } + + template > + constexpr mat(const U &u) : mat(converter{}(u)) {} + template > + constexpr operator U() const { + return converter{}(*this); + } +}; +template +struct mat { + typedef vec V; + V x, y, z, w; + constexpr mat() : x(), y(), z(), w() {} + constexpr mat(const V &x_, const V &y_, const V &z_, const V &w_) + : x(x_), y(y_), z(z_), w(w_) {} + constexpr mat(const mat &m_, const V &w_) + : x(m_.x), y(m_.y), z(m_.z), w(w_) {} + constexpr explicit mat(const T &s) : x(s), y(s), z(s), w(s) {} + constexpr explicit mat(const T *p) + : x(p + M * 0), y(p + M * 1), z(p + M * 2), w(p + M * 3) {} + template + constexpr explicit mat(const mat &m) + : mat(V(m.x), V(m.y), V(m.z), V(m.w)) { + static_assert(P >= 4, "Input matrix dimensions must be at least as big."); + } + + constexpr vec row(int i) const { return {x[i], y[i], z[i], w[i]}; } + constexpr const V &operator[](int j) const { + return j == 0 ? x : j == 1 ? y : j == 2 ? z : w; + } + LINALG_CONSTEXPR14 V &operator[](int j) { + return j == 0 ? x : j == 1 ? y : j == 2 ? z : w; + } + + template > + constexpr mat(const U &u) : mat(converter{}(u)) {} + template > + constexpr operator U() const { + return converter{}(*this); + } +}; +/** @} */ + +/** @addtogroup identity + * @ingroup LinAlg + * @brief Define a type which will convert to the multiplicative identity of any + * square matrix. + * @{ + */ +struct identity_t { + constexpr explicit identity_t(int) {} +}; +template +struct converter, identity_t> { + constexpr mat operator()(identity_t) const { return {vec{1}}; } +}; +template +struct converter, identity_t> { + constexpr mat operator()(identity_t) const { + return {{1, 0}, {0, 1}}; + } +}; +template +struct converter, identity_t> { + constexpr mat operator()(identity_t) const { + return {{1, 0}, {0, 1}, {0, 0}}; + } +}; +template +struct converter, identity_t> { + constexpr mat operator()(identity_t) const { + return {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + } +}; +template +struct converter, identity_t> { + constexpr mat operator()(identity_t) const { + return {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, 0, 0}}; + } +}; +template +struct converter, identity_t> { + constexpr mat operator()(identity_t) const { + return {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; + } +}; +constexpr identity_t identity{1}; +/** @} */ + +/** @addtogroup fold + * @ingroup LinAlg + * @brief Produce a scalar by applying f(A,B) -> A to adjacent pairs of elements + * from a vec/mat in left-to-right/column-major order (matching the + * associativity of arithmetic and logical operators). + * @{ + */ +template +constexpr A fold(F f, A a, const vec &b) { + return f(a, b.x); +} +template +constexpr A fold(F f, A a, const vec &b) { + return f(f(a, b.x), b.y); +} +template +constexpr A fold(F f, A a, const vec &b) { + return f(f(f(a, b.x), b.y), b.z); +} +template +constexpr A fold(F f, A a, const vec &b) { + return f(f(f(f(a, b.x), b.y), b.z), b.w); +} +template +constexpr A fold(F f, A a, const mat &b) { + return fold(f, a, b.x); +} +template +constexpr A fold(F f, A a, const mat &b) { + return fold(f, fold(f, a, b.x), b.y); +} +template +constexpr A fold(F f, A a, const mat &b) { + return fold(f, fold(f, fold(f, a, b.x), b.y), b.z); +} +template +constexpr A fold(F f, A a, const mat &b) { + return fold(f, fold(f, fold(f, fold(f, a, b.x), b.y), b.z), b.w); +} +/** @} */ + +/** @addtogroup apply + * @ingroup LinAlg + * @brief apply(f,...) applies the provided function in an elementwise fashion + * to its arguments, producing an object of the same dimensions. + * @{ + */ + +// Type aliases for the result of calling apply(...) with various arguments, can +// be used with return type SFINAE to constrain overload sets +template +using apply_t = typename detail::apply::type; +template +using mm_apply_t = typename std::enable_if::mm, + apply_t>::type; +template +using no_mm_apply_t = typename std::enable_if::mm, + apply_t>::type; +template +using scalar_t = + typename detail::scalar_type::type; // Underlying scalar type when + // performing elementwise operations + +// apply(f,...) applies the provided function in an elementwise fashion to its +// arguments, producing an object of the same dimensions +template +constexpr apply_t apply(F func, const A &...args) { + return detail::apply::impl( + detail::make_seq<0, detail::apply::size>{}, func, args...); +} + +// map(a,f) is equivalent to apply(f,a) +template +constexpr apply_t map(const A &a, F func) { + return apply(func, a); +} + +// zip(a,b,f) is equivalent to apply(f,a,b) +template +constexpr apply_t zip(const A &a, const B &b, F func) { + return apply(func, a, b); +} +/** @} */ + +/** @addtogroup comparison_ops + * @ingroup LinAlg + * @brief Relational operators are defined to compare the elements of two + * vectors or matrices lexicographically, in column-major order. + * @{ + */ +template +constexpr typename detail::any_compare::type compare(const A &a, + const B &b) { + return detail::any_compare()(a, b); +} +template +constexpr auto operator==(const A &a, + const B &b) -> decltype(compare(a, b) == 0) { + return compare(a, b) == 0; +} +template +constexpr auto operator!=(const A &a, + const B &b) -> decltype(compare(a, b) != 0) { + return compare(a, b) != 0; +} +template +constexpr auto operator<(const A &a, + const B &b) -> decltype(compare(a, b) < 0) { + return compare(a, b) < 0; +} +template +constexpr auto operator>(const A &a, + const B &b) -> decltype(compare(a, b) > 0) { + return compare(a, b) > 0; +} +template +constexpr auto operator<=(const A &a, + const B &b) -> decltype(compare(a, b) <= 0) { + return compare(a, b) <= 0; +} +template +constexpr auto operator>=(const A &a, + const B &b) -> decltype(compare(a, b) >= 0) { + return compare(a, b) >= 0; +} +/** @} */ + +/** @addtogroup reductions + * @ingroup LinAlg + * @brief Functions for coalescing scalar values. + * @{ + */ +template +constexpr bool any(const A &a) { + return fold(detail::op_or{}, false, a); +} +template +constexpr bool all(const A &a) { + return fold(detail::op_and{}, true, a); +} +template +constexpr scalar_t sum(const A &a) { + return fold(detail::op_add{}, scalar_t(0), a); +} +template +constexpr scalar_t product(const A &a) { + return fold(detail::op_mul{}, scalar_t(1), a); +} +template +constexpr scalar_t minelem(const A &a) { + return fold(detail::min{}, a.x, a); +} +template +constexpr scalar_t maxelem(const A &a) { + return fold(detail::max{}, a.x, a); +} +template +int argmin(const vec &a) { + int j = 0; + for (int i = 1; i < M; ++i) + if (a[i] < a[j]) j = i; + return j; +} +template +int argmax(const vec &a) { + int j = 0; + for (int i = 1; i < M; ++i) + if (a[i] > a[j]) j = i; + return j; +} +/** @} */ + +/** @addtogroup unary_ops + * @ingroup LinAlg + * @brief Unary operators are defined component-wise for linalg types. + * @{ + */ +template +constexpr apply_t operator+(const A &a) { + return apply(detail::op_pos{}, a); +} +template +constexpr apply_t operator-(const A &a) { + return apply(detail::op_neg{}, a); +} +template +constexpr apply_t operator~(const A &a) { + return apply(detail::op_cmp{}, a); +} +template +constexpr apply_t operator!(const A &a) { + return apply(detail::op_not{}, a); +} +/** @} */ + +/** @addtogroup binary_ops + * @ingroup LinAlg + * @brief Binary operators are defined component-wise for linalg types, EXCEPT + * for `operator *`, which does standard matrix multiplication, scalar + * multiplication, and component-wise multiplication for same-size vectors. Use + * `cmul` for the matrix Hadamard product. + * @{ + */ +template +constexpr apply_t operator+(const A &a, const B &b) { + return apply(detail::op_add{}, a, b); +} +template +constexpr apply_t operator-(const A &a, const B &b) { + return apply(detail::op_sub{}, a, b); +} +template +constexpr apply_t cmul(const A &a, const B &b) { + return apply(detail::op_mul{}, a, b); +} +template +constexpr apply_t operator/(const A &a, const B &b) { + return apply(detail::op_div{}, a, b); +} +template +constexpr apply_t operator%(const A &a, const B &b) { + return apply(detail::op_mod{}, a, b); +} +template +constexpr apply_t operator|(const A &a, const B &b) { + return apply(detail::op_un{}, a, b); +} +template +constexpr apply_t operator^(const A &a, const B &b) { + return apply(detail::op_xor{}, a, b); +} +template +constexpr apply_t operator&(const A &a, const B &b) { + return apply(detail::op_int{}, a, b); +} +template +constexpr apply_t operator<<(const A &a, const B &b) { + return apply(detail::op_lsh{}, a, b); +} +template +constexpr apply_t operator>>(const A &a, const B &b) { + return apply(detail::op_rsh{}, a, b); +} + +// Binary `operator *` represents the algebraic matrix product - use cmul(a, b) +// for the Hadamard (component-wise) product. +template +constexpr auto operator*(const A &a, const B &b) { + return mul(a, b); +} + +// Binary assignment operators a $= b is always defined as though it were +// explicitly written a = a $ b +template +constexpr auto operator+=(A &a, const B &b) -> decltype(a = a + b) { + return a = a + b; +} +template +constexpr auto operator-=(A &a, const B &b) -> decltype(a = a - b) { + return a = a - b; +} +template +constexpr auto operator*=(A &a, const B &b) -> decltype(a = a * b) { + return a = a * b; +} +template +constexpr auto operator/=(A &a, const B &b) -> decltype(a = a / b) { + return a = a / b; +} +template +constexpr auto operator%=(A &a, const B &b) -> decltype(a = a % b) { + return a = a % b; +} +template +constexpr auto operator|=(A &a, const B &b) -> decltype(a = a | b) { + return a = a | b; +} +template +constexpr auto operator^=(A &a, const B &b) -> decltype(a = a ^ b) { + return a = a ^ b; +} +template +constexpr auto operator&=(A &a, const B &b) -> decltype(a = a & b) { + return a = a & b; +} +template +constexpr auto operator<<=(A &a, const B &b) -> decltype(a = a << b) { + return a = a << b; +} +template +constexpr auto operator>>=(A &a, const B &b) -> decltype(a = a >> b) { + return a = a >> b; +} +/** @} */ + +/** @addtogroup swizzles + * @ingroup LinAlg + * @brief Swizzles and subobjects. + * @{ + */ +/** + * @brief Returns a vector containing the specified ordered indices, e.g. + * linalg::swizzle<1, 2, 0>(vec4(4, 5, 6, 7)) == vec3(5, 6, 4) + */ +template +constexpr vec swizzle(const vec &a) { + return {detail::getter{}(a)...}; +} +/** + * @brief Returns a vector containing the specified index range, e.g. + * linalg::subvec<1, 4>(vec4(4, 5, 6, 7)) == vec3(5, 6, 7) + */ +template +constexpr vec subvec(const vec &a) { + return detail::swizzle(a, detail::make_seq{}); +} +/** + * @brief Returns a matrix containing the specified row and column range: + * linalg::submat + */ +template +constexpr mat submat(const mat &a) { + return detail::swizzle(a, detail::make_seq{}, + detail::make_seq{}); +} +/** @} */ + +/** @addtogroup unary_STL + * @ingroup LinAlg + * @brief Component-wise standard library math functions. + * @{ + */ +template +constexpr apply_t isfinite(const A &a) { + return apply(detail::std_isfinite{}, a); +} +template +constexpr apply_t abs(const A &a) { + return apply(detail::std_abs{}, a); +} +template +constexpr apply_t floor(const A &a) { + return apply(detail::std_floor{}, a); +} +template +constexpr apply_t ceil(const A &a) { + return apply(detail::std_ceil{}, a); +} +template +constexpr apply_t exp(const A &a) { + return apply(detail::std_exp{}, a); +} +template +constexpr apply_t log(const A &a) { + return apply(detail::std_log{}, a); +} +template +constexpr apply_t log2(const A &a) { + return apply(detail::std_log2{}, a); +} +template +constexpr apply_t log10(const A &a) { + return apply(detail::std_log10{}, a); +} +template +constexpr apply_t sqrt(const A &a) { + return apply(detail::std_sqrt{}, a); +} +template +constexpr apply_t sin(const A &a) { + return apply(detail::std_sin{}, a); +} +template +constexpr apply_t cos(const A &a) { + return apply(detail::std_cos{}, a); +} +template +constexpr apply_t tan(const A &a) { + return apply(detail::std_tan{}, a); +} +template +constexpr apply_t asin(const A &a) { + return apply(detail::std_asin{}, a); +} +template +constexpr apply_t acos(const A &a) { + return apply(detail::std_acos{}, a); +} +template +constexpr apply_t atan(const A &a) { + return apply(detail::std_atan{}, a); +} +template +constexpr apply_t sinh(const A &a) { + return apply(detail::std_sinh{}, a); +} +template +constexpr apply_t cosh(const A &a) { + return apply(detail::std_cosh{}, a); +} +template +constexpr apply_t tanh(const A &a) { + return apply(detail::std_tanh{}, a); +} +template +constexpr apply_t round(const A &a) { + return apply(detail::std_round{}, a); +} +/** @} */ + +/** @addtogroup binary_STL + * @ingroup LinAlg + * @brief Component-wise standard library math functions. Either argument can be + * a vector or a scalar. + * @{ + */ +template +constexpr apply_t fmod(const A &a, const B &b) { + return apply(detail::std_fmod{}, a, b); +} +template +constexpr apply_t pow(const A &a, const B &b) { + return apply(detail::std_pow{}, a, b); +} +template +constexpr apply_t atan2(const A &a, const B &b) { + return apply(detail::std_atan2{}, a, b); +} +template +constexpr apply_t copysign(const A &a, const B &b) { + return apply(detail::std_copysign{}, a, b); +} +/** @} */ + +/** @addtogroup relational + * @ingroup LinAlg + * @brief Component-wise relational functions on vectors. Either argument can be + * a vector or a scalar. + * @{ + */ +template +constexpr apply_t equal(const A &a, const B &b) { + return apply(detail::op_eq{}, a, b); +} +template +constexpr apply_t nequal(const A &a, const B &b) { + return apply(detail::op_ne{}, a, b); +} +template +constexpr apply_t less(const A &a, const B &b) { + return apply(detail::op_lt{}, a, b); +} +template +constexpr apply_t greater(const A &a, const B &b) { + return apply(detail::op_gt{}, a, b); +} +template +constexpr apply_t lequal(const A &a, const B &b) { + return apply(detail::op_le{}, a, b); +} +template +constexpr apply_t gequal(const A &a, const B &b) { + return apply(detail::op_ge{}, a, b); +} +/** @} */ + +/** @addtogroup selection + * @ingroup LinAlg + * @brief Component-wise selection functions on vectors. Either argument can be + * a vector or a scalar. + * @{ + */ +template +constexpr apply_t min(const A &a, const B &b) { + return apply(detail::min{}, a, b); +} +template +constexpr apply_t max(const A &a, const B &b) { + return apply(detail::max{}, a, b); +} +/** + * @brief Clamps the components of x between l and h, provided l[i] < h[i]. + */ +template +constexpr apply_t clamp(const X &x, const L &l, + const H &h) { + return apply(detail::clamp{}, x, l, h); +} +/** + * @brief Returns the component from a if the corresponding component of p is + * true and from b otherwise. + */ +template +constexpr apply_t select(const P &p, const A &a, + const B &b) { + return apply(detail::select{}, p, a, b); +} +/** + * @brief Linear interpolation from a to b as t goes from 0 -> 1. Values beyond + * [a, b] will result if t is outside [0, 1]. + */ +template +constexpr apply_t lerp(const A &a, const B &b, + const T &t) { + return apply(detail::lerp{}, a, b, t); +} +/** @} */ + +/** @addtogroup vec_algebra + * @ingroup LinAlg + * @brief Support for vector algebra. + * @{ + */ +/** + * @brief shorthand for `cross({a.x,a.y,0}, {b.x,b.y,0}).z` + */ +template +constexpr T cross(const vec &a, const vec &b) { + return a.x * b.y - a.y * b.x; +} +/** + * @brief shorthand for `cross({0,0,a.z}, {b.x,b.y,0}).xy()` + */ +template +constexpr vec cross(T a, const vec &b) { + return {-a * b.y, a * b.x}; +} +/** + * @brief shorthand for `cross({a.x,a.y,0}, {0,0,b.z}).xy()` + */ +template +constexpr vec cross(const vec &a, T b) { + return {a.y * b, -a.x * b}; +} +/** + * @brief the [cross or vector + * product](https://en.wikipedia.org/wiki/Cross_product) of vectors `a` and `b` + */ +template +constexpr vec cross(const vec &a, const vec &b) { + return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x}; +} +/** + * @brief [dot or inner product](https://en.wikipedia.org/wiki/Dot_product) of + * vectors `a` and `b` + */ +template +constexpr T dot(const vec &a, const vec &b) { + return sum(a * b); +} +/** + * @brief *square* of the length or magnitude of vector `a` + */ +template +constexpr T length2(const vec &a) { + return dot(a, a); +} +/** + * @brief length or magnitude of a vector `a` + */ +template +T length(const vec &a) { + return std::sqrt(length2(a)); +} +/** + * @brief unit length vector in the same direction as `a` (undefined for + zero-length vectors) + + */ +template +vec normalize(const vec &a) { + return a / length(a); +} +/** + * @brief *square* of the [Euclidean + * distance](https://en.wikipedia.org/wiki/Euclidean_distance) between points + * `a` and `b` + */ +template +constexpr T distance2(const vec &a, const vec &b) { + return length2(b - a); +} +/** + * @brief [Euclidean distance](https://en.wikipedia.org/wiki/Euclidean_distance) + * between points `a` and `b` + */ +template +T distance(const vec &a, const vec &b) { + return length(b - a); +} +/** + * @brief Return the angle in radians between two unit vectors. + */ +template +T uangle(const vec &a, const vec &b) { + T d = dot(a, b); + return d > 1 ? 0 : std::acos(d < -1 ? -1 : d); +} +/** + * @brief Return the angle in radians between two non-unit vectors. + */ +template +T angle(const vec &a, const vec &b) { + return uangle(normalize(a), normalize(b)); +} +/** + * @brief vector `v` rotated counter-clockwise by the angle `a` in + * [radians](https://en.wikipedia.org/wiki/Radian) + */ +template +vec rot(T a, const vec &v) { + const T s = std::sin(a), c = std::cos(a); + return {v.x * c - v.y * s, v.x * s + v.y * c}; +} +/** + * @brief vector `v` rotated counter-clockwise by the angle `a` in + * [radians](https://en.wikipedia.org/wiki/Radian) around the X axis + */ +template +vec rotx(T a, const vec &v) { + const T s = std::sin(a), c = std::cos(a); + return {v.x, v.y * c - v.z * s, v.y * s + v.z * c}; +} +/** + * @brief vector `v` rotated counter-clockwise by the angle `a` in + * [radians](https://en.wikipedia.org/wiki/Radian) around the Y axis + */ +template +vec roty(T a, const vec &v) { + const T s = std::sin(a), c = std::cos(a); + return {v.x * c + v.z * s, v.y, -v.x * s + v.z * c}; +} +/** + * @brief vector `v` rotated counter-clockwise by the angle `a` in + * [radians](https://en.wikipedia.org/wiki/Radian) around the Z axis + */ +template +vec rotz(T a, const vec &v) { + const T s = std::sin(a), c = std::cos(a); + return {v.x * c - v.y * s, v.x * s + v.y * c, v.z}; +} +/** + * @brief shorthand for `normalize(lerp(a,b,t))` + */ +template +vec nlerp(const vec &a, const vec &b, T t) { + return normalize(lerp(a, b, t)); +} +/** + * @brief [spherical linear interpolation](https://en.wikipedia.org/wiki/Slerp) + * between unit vectors `a` and `b` (undefined for non-unit vectors) by + * parameter `t` + */ +template +vec slerp(const vec &a, const vec &b, T t) { + T th = uangle(a, b); + return th == 0 ? a + : a * (std::sin(th * (1 - t)) / std::sin(th)) + + b * (std::sin(th * t) / std::sin(th)); +} +/** @} */ + +/** @addtogroup quaternions + * @ingroup LinAlg + * @brief Support for quaternion algebra using 4D vectors of arbitrary length, + * representing xi + yj + zk + w. + * @{ + */ +/** + * @brief + * [conjugate](https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal) + * of quaternion `q` + */ +template +constexpr vec qconj(const vec &q) { + return {-q.x, -q.y, -q.z, q.w}; +} +/** + * @brief [inverse or + * reciprocal](https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal) + * of quaternion `q` (undefined for zero-length quaternions) + */ +template +vec qinv(const vec &q) { + return qconj(q) / length2(q); +} +/** + * @brief + * [exponential](https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions) + * of quaternion `q` + */ +template +vec qexp(const vec &q) { + const auto v = q.xyz(); + const auto vv = length(v); + return std::exp(q.w) * + vec{v * (vv > 0 ? std::sin(vv) / vv : 0), std::cos(vv)}; +} +/** + * @brief + * [logarithm](https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions) + * of quaternion `q` + */ +template +vec qlog(const vec &q) { + const auto v = q.xyz(); + const auto vv = length(v), qq = length(q); + return {v * (vv > 0 ? std::acos(q.w / qq) / vv : 0), std::log(qq)}; +} +/** + * @brief quaternion `q` raised to the exponent `p` + */ +template +vec qpow(const vec &q, const T &p) { + const auto v = q.xyz(); + const auto vv = length(v), qq = length(q), th = std::acos(q.w / qq); + return std::pow(qq, p) * + vec{v * (vv > 0 ? std::sin(p * th) / vv : 0), std::cos(p * th)}; +} +/** + * @brief [Hamilton + * product](https://en.wikipedia.org/wiki/Quaternion#Hamilton_product) of + * quaternions `a` and `b` + */ +template +constexpr vec qmul(const vec &a, const vec &b) { + return {a.x * b.w + a.w * b.x + a.y * b.z - a.z * b.y, + a.y * b.w + a.w * b.y + a.z * b.x - a.x * b.z, + a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x, + a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z}; +} +/** + * @brief Multiply as many input quaternions together as desired. + */ +template +constexpr vec qmul(const vec &a, R... r) { + return qmul(a, qmul(r...)); +} +/** @} */ + +/** @addtogroup quaternion_rotation + * @ingroup LinAlg + * @brief Support for 3D spatial rotations using normalized quaternions. + * @{ + */ +/** + * @brief efficient shorthand for `qrot(q, {1,0,0})` + */ +template +constexpr vec qxdir(const vec &q) { + return {q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, + (q.x * q.y + q.z * q.w) * 2, (q.z * q.x - q.y * q.w) * 2}; +} +/** + * @brief efficient shorthand for `qrot(q, {0,1,0})` + */ +template +constexpr vec qydir(const vec &q) { + return {(q.x * q.y - q.z * q.w) * 2, + q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, + (q.y * q.z + q.x * q.w) * 2}; +} +/** + * @brief efficient shorthand for `qrot(q, {0,0,1})` + */ +template +constexpr vec qzdir(const vec &q) { + return {(q.z * q.x + q.y * q.w) * 2, (q.y * q.z - q.x * q.w) * 2, + q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z}; +} +/** + * @brief Create an equivalent mat3 rotation matrix from the input quaternion. + */ +template +constexpr mat qmat(const vec &q) { + return {qxdir(q), qydir(q), qzdir(q)}; +} +/** + * @brief Rotate a vector by a quaternion. + */ +template +constexpr vec qrot(const vec &q, const vec &v) { + return qxdir(q) * v.x + qydir(q) * v.y + qzdir(q) * v.z; +} +/** + * @brief Return the angle in radians of the axis-angle representation of the + * input normalized quaternion. + */ +template +T qangle(const vec &q) { + return std::atan2(length(q.xyz()), q.w) * 2; +} +/** + * @brief Return the normalized axis of the axis-angle representation of the + * input normalized quaternion. + */ +template +vec qaxis(const vec &q) { + return normalize(q.xyz()); +} +/** + * @brief Linear interpolation that takes the shortest path - this is not + * geometrically sensible, consider qslerp instead. + */ +template +vec qnlerp(const vec &a, const vec &b, T t) { + return nlerp(a, dot(a, b) < 0 ? -b : b, t); +} +/** + * @brief Spherical linear interpolation that takes the shortest path. + */ +template +vec qslerp(const vec &a, const vec &b, T t) { + return slerp(a, dot(a, b) < 0 ? -b : b, t); +} +/** + * @brief Returns a normalized quaternion representing a rotation by angle in + * radians about the provided axis. + */ +template +vec constexpr rotation_quat(const vec &axis, T angle) { + return {axis * std::sin(angle / 2), std::cos(angle / 2)}; +} +/** + * @brief Returns a normalized quaternion representing the shortest rotation + * from orig vector to dest vector. + */ +template +vec rotation_quat(const vec &orig, const vec &dest); +/** + * @brief Returns a normalized quaternion representing the input rotation + * matrix, which should be orthonormal. + */ +template +vec rotation_quat(const mat &m); +/** @} */ + +/** @addtogroup mat_algebra + * @ingroup LinAlg + * @brief Support for matrix algebra. + * @{ + */ +template +constexpr vec mul(const vec &a, const T &b) { + return cmul(a, b); +} +template +constexpr vec mul(const T &b, const vec &a) { + return cmul(b, a); +} +template +constexpr mat mul(const mat &a, const T &b) { + return cmul(a, b); +} +template +constexpr mat mul(const T &b, const mat &a) { + return cmul(b, a); +} +template +constexpr vec mul(const vec &a, const vec &b) { + return cmul(a, b); +} +template +constexpr vec mul(const mat &a, const vec &b) { + return a.x * b.x; +} +template +constexpr vec mul(const mat &a, const vec &b) { + return a.x * b.x + a.y * b.y; +} +template +constexpr vec mul(const mat &a, const vec &b) { + return a.x * b.x + a.y * b.y + a.z * b.z; +} +template +constexpr vec mul(const mat &a, const vec &b) { + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} +template +constexpr mat mul(const mat &a, const mat &b) { + return {mul(a, b.x)}; +} +template +constexpr mat mul(const mat &a, const mat &b) { + return {mul(a, b.x), mul(a, b.y)}; +} +template +constexpr mat mul(const mat &a, const mat &b) { + return {mul(a, b.x), mul(a, b.y), mul(a, b.z)}; +} +template +constexpr mat mul(const mat &a, const mat &b) { + return {mul(a, b.x), mul(a, b.y), mul(a, b.z), mul(a, b.w)}; +} +template +constexpr vec mul(const mat &a, const mat &b, + const vec &c) { + return mul(mul(a, b), c); +} +template +constexpr mat mul(const mat &a, const mat &b, + const mat &c) { + return mul(mul(a, b), c); +} +template +constexpr vec mul(const mat &a, const mat &b, + const mat &c, const vec &d) { + return mul(mul(a, b, c), d); +} +template +constexpr mat mul(const mat &a, const mat &b, + const mat &c, const mat &d) { + return mul(mul(a, b, c), d); +} +template +constexpr mat outerprod(const vec &a, const vec &b) { + return {a * b.x}; +} +template +constexpr mat outerprod(const vec &a, const vec &b) { + return {a * b.x, a * b.y}; +} +template +constexpr mat outerprod(const vec &a, const vec &b) { + return {a * b.x, a * b.y, a * b.z}; +} +template +constexpr mat outerprod(const vec &a, const vec &b) { + return {a * b.x, a * b.y, a * b.z, a * b.w}; +} +template +constexpr vec diagonal(const mat &a) { + return {a.x.x}; +} +template +constexpr vec diagonal(const mat &a) { + return {a.x.x, a.y.y}; +} +template +constexpr vec diagonal(const mat &a) { + return {a.x.x, a.y.y, a.z.z}; +} +template +constexpr vec diagonal(const mat &a) { + return {a.x.x, a.y.y, a.z.z, a.w.w}; +} +template +constexpr T trace(const mat &a) { + return sum(diagonal(a)); +} +template +constexpr mat transpose(const mat &m) { + return {m.row(0)}; +} +template +constexpr mat transpose(const mat &m) { + return {m.row(0), m.row(1)}; +} +template +constexpr mat transpose(const mat &m) { + return {m.row(0), m.row(1), m.row(2)}; +} +template +constexpr mat transpose(const mat &m) { + return {m.row(0), m.row(1), m.row(2), m.row(3)}; +} +template +constexpr mat transpose(const vec &m) { + return transpose(mat(m)); +} +template +constexpr mat adjugate(const mat &a) { + return {vec{1}}; +} +template +constexpr mat adjugate(const mat &a) { + return {{a.y.y, -a.x.y}, {-a.y.x, a.x.x}}; +} +template +constexpr mat adjugate(const mat &a); +template +constexpr mat adjugate(const mat &a); +template +constexpr mat comatrix(const mat &a) { + return transpose(adjugate(a)); +} +template +constexpr T determinant(const mat &a) { + return a.x.x; +} +template +constexpr T determinant(const mat &a) { + return a.x.x * a.y.y - a.x.y * a.y.x; +} +template +constexpr T determinant(const mat &a) { + return a.x.x * (a.y.y * a.z.z - a.z.y * a.y.z) + + a.x.y * (a.y.z * a.z.x - a.z.z * a.y.x) + + a.x.z * (a.y.x * a.z.y - a.z.x * a.y.y); +} +template +constexpr T determinant(const mat &a); +template +constexpr mat inverse(const mat &a) { + return adjugate(a) / determinant(a); +} +/** @} */ + +/** @addtogroup iterators + * @ingroup LinAlg + * @brief Vectors and matrices can be used as ranges. + * @{ + */ +template +T *begin(vec &a) { + return &a.x; +} +template +const T *begin(const vec &a) { + return &a.x; +} +template +T *end(vec &a) { + return begin(a) + M; +} +template +const T *end(const vec &a) { + return begin(a) + M; +} +template +vec *begin(mat &a) { + return &a.x; +} +template +const vec *begin(const mat &a) { + return &a.x; +} +template +vec *end(mat &a) { + return begin(a) + N; +} +template +const vec *end(const mat &a) { + return begin(a) + N; +} +/** @} */ + +/** @addtogroup transforms + * @ingroup LinAlg + * @brief Factory functions for 3D spatial transformations. + * @{ + */ +enum fwd_axis { + neg_z, + pos_z +}; // Should projection matrices be generated assuming forward is {0,0,-1} or + // {0,0,1} +enum z_range { + neg_one_to_one, + zero_to_one +}; // Should projection matrices map z into the range of [-1,1] or [0,1]? +template +mat translation_matrix(const vec &translation) { + return {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {translation, 1}}; +} +template +mat rotation_matrix(const vec &rotation) { + return {{qxdir(rotation), 0}, + {qydir(rotation), 0}, + {qzdir(rotation), 0}, + {0, 0, 0, 1}}; +} +template +mat scaling_matrix(const vec &scaling) { + return {{scaling.x, 0, 0, 0}, + {0, scaling.y, 0, 0}, + {0, 0, scaling.z, 0}, + {0, 0, 0, 1}}; +} +template +mat pose_matrix(const vec &q, const vec &p) { + return {{qxdir(q), 0}, {qydir(q), 0}, {qzdir(q), 0}, {p, 1}}; +} +template +mat lookat_matrix(const vec &eye, const vec ¢er, + const vec &view_y_dir, fwd_axis fwd = neg_z); +template +mat frustum_matrix(T x0, T x1, T y0, T y1, T n, T f, + fwd_axis a = neg_z, z_range z = neg_one_to_one); +template +mat perspective_matrix(T fovy, T aspect, T n, T f, fwd_axis a = neg_z, + z_range z = neg_one_to_one) { + T y = n * std::tan(fovy / 2), x = y * aspect; + return frustum_matrix(-x, x, -y, y, n, f, a, z); +} +/** @} */ + +/** @addtogroup array + * @ingroup LinAlg + * @brief Provide implicit conversion between linalg::vec and + * std::array. + * @{ + */ +template +struct converter, std::array> { + vec operator()(const std::array &a) const { return {a[0]}; } +}; +template +struct converter, std::array> { + vec operator()(const std::array &a) const { return {a[0], a[1]}; } +}; +template +struct converter, std::array> { + vec operator()(const std::array &a) const { + return {a[0], a[1], a[2]}; + } +}; +template +struct converter, std::array> { + vec operator()(const std::array &a) const { + return {a[0], a[1], a[2], a[3]}; + } +}; + +template +struct converter, vec> { + std::array operator()(const vec &a) const { return {a[0]}; } +}; +template +struct converter, vec> { + std::array operator()(const vec &a) const { return {a[0], a[1]}; } +}; +template +struct converter, vec> { + std::array operator()(const vec &a) const { + return {a[0], a[1], a[2]}; + } +}; +template +struct converter, vec> { + std::array operator()(const vec &a) const { + return {a[0], a[1], a[2], a[3]}; + } +}; +/** @} */ + +#ifdef MANIFOLD_DEBUG +template +std::ostream &operator<<(std::ostream &out, const vec &v) { + return out << '{' << v[0] << '}'; +} +template +std::ostream &operator<<(std::ostream &out, const vec &v) { + return out << '{' << v[0] << ',' << v[1] << '}'; +} +template +std::ostream &operator<<(std::ostream &out, const vec &v) { + return out << '{' << v[0] << ',' << v[1] << ',' << v[2] << '}'; +} +template +std::ostream &operator<<(std::ostream &out, const vec &v) { + return out << '{' << v[0] << ',' << v[1] << ',' << v[2] << ',' << v[3] << '}'; +} + +template +std::ostream &operator<<(std::ostream &out, const mat &m) { + return out << '{' << m[0] << '}'; +} +template +std::ostream &operator<<(std::ostream &out, const mat &m) { + return out << '{' << m[0] << ',' << m[1] << '}'; +} +template +std::ostream &operator<<(std::ostream &out, const mat &m) { + return out << '{' << m[0] << ',' << m[1] << ',' << m[2] << '}'; +} +template +std::ostream &operator<<(std::ostream &out, const mat &m) { + return out << '{' << m[0] << ',' << m[1] << ',' << m[2] << ',' << m[3] << '}'; +} +#endif +} // namespace linalg + +namespace std { +/** @addtogroup hash + * @ingroup LinAlg + * @brief Provide specializations for std::hash<...> with linalg types. + * @{ + */ +template +struct hash> { + std::size_t operator()(const linalg::vec &v) const { + std::hash h; + return h(v.x); + } +}; +template +struct hash> { + std::size_t operator()(const linalg::vec &v) const { + std::hash h; + return h(v.x) ^ (h(v.y) << 1); + } +}; +template +struct hash> { + std::size_t operator()(const linalg::vec &v) const { + std::hash h; + return h(v.x) ^ (h(v.y) << 1) ^ (h(v.z) << 2); + } +}; +template +struct hash> { + std::size_t operator()(const linalg::vec &v) const { + std::hash h; + return h(v.x) ^ (h(v.y) << 1) ^ (h(v.z) << 2) ^ (h(v.w) << 3); + } +}; + +template +struct hash> { + std::size_t operator()(const linalg::mat &v) const { + std::hash> h; + return h(v.x); + } +}; +template +struct hash> { + std::size_t operator()(const linalg::mat &v) const { + std::hash> h; + return h(v.x) ^ (h(v.y) << M); + } +}; +template +struct hash> { + std::size_t operator()(const linalg::mat &v) const { + std::hash> h; + return h(v.x) ^ (h(v.y) << M) ^ (h(v.z) << (M * 2)); + } +}; +template +struct hash> { + std::size_t operator()(const linalg::mat &v) const { + std::hash> h; + return h(v.x) ^ (h(v.y) << M) ^ (h(v.z) << (M * 2)) ^ (h(v.w) << (M * 3)); + } +}; +/** @} */ +} // namespace std + +// Definitions of functions too long to be defined inline +template +constexpr linalg::mat linalg::adjugate(const mat &a) { + return {{a.y.y * a.z.z - a.z.y * a.y.z, a.z.y * a.x.z - a.x.y * a.z.z, + a.x.y * a.y.z - a.y.y * a.x.z}, + {a.y.z * a.z.x - a.z.z * a.y.x, a.z.z * a.x.x - a.x.z * a.z.x, + a.x.z * a.y.x - a.y.z * a.x.x}, + {a.y.x * a.z.y - a.z.x * a.y.y, a.z.x * a.x.y - a.x.x * a.z.y, + a.x.x * a.y.y - a.y.x * a.x.y}}; +} + +template +constexpr linalg::mat linalg::adjugate(const mat &a) { + return {{a.y.y * a.z.z * a.w.w + a.w.y * a.y.z * a.z.w + + a.z.y * a.w.z * a.y.w - a.y.y * a.w.z * a.z.w - + a.z.y * a.y.z * a.w.w - a.w.y * a.z.z * a.y.w, + a.x.y * a.w.z * a.z.w + a.z.y * a.x.z * a.w.w + + a.w.y * a.z.z * a.x.w - a.w.y * a.x.z * a.z.w - + a.z.y * a.w.z * a.x.w - a.x.y * a.z.z * a.w.w, + a.x.y * a.y.z * a.w.w + a.w.y * a.x.z * a.y.w + + a.y.y * a.w.z * a.x.w - a.x.y * a.w.z * a.y.w - + a.y.y * a.x.z * a.w.w - a.w.y * a.y.z * a.x.w, + a.x.y * a.z.z * a.y.w + a.y.y * a.x.z * a.z.w + + a.z.y * a.y.z * a.x.w - a.x.y * a.y.z * a.z.w - + a.z.y * a.x.z * a.y.w - a.y.y * a.z.z * a.x.w}, + {a.y.z * a.w.w * a.z.x + a.z.z * a.y.w * a.w.x + + a.w.z * a.z.w * a.y.x - a.y.z * a.z.w * a.w.x - + a.w.z * a.y.w * a.z.x - a.z.z * a.w.w * a.y.x, + a.x.z * a.z.w * a.w.x + a.w.z * a.x.w * a.z.x + + a.z.z * a.w.w * a.x.x - a.x.z * a.w.w * a.z.x - + a.z.z * a.x.w * a.w.x - a.w.z * a.z.w * a.x.x, + a.x.z * a.w.w * a.y.x + a.y.z * a.x.w * a.w.x + + a.w.z * a.y.w * a.x.x - a.x.z * a.y.w * a.w.x - + a.w.z * a.x.w * a.y.x - a.y.z * a.w.w * a.x.x, + a.x.z * a.y.w * a.z.x + a.z.z * a.x.w * a.y.x + + a.y.z * a.z.w * a.x.x - a.x.z * a.z.w * a.y.x - + a.y.z * a.x.w * a.z.x - a.z.z * a.y.w * a.x.x}, + {a.y.w * a.z.x * a.w.y + a.w.w * a.y.x * a.z.y + + a.z.w * a.w.x * a.y.y - a.y.w * a.w.x * a.z.y - + a.z.w * a.y.x * a.w.y - a.w.w * a.z.x * a.y.y, + a.x.w * a.w.x * a.z.y + a.z.w * a.x.x * a.w.y + + a.w.w * a.z.x * a.x.y - a.x.w * a.z.x * a.w.y - + a.w.w * a.x.x * a.z.y - a.z.w * a.w.x * a.x.y, + a.x.w * a.y.x * a.w.y + a.w.w * a.x.x * a.y.y + + a.y.w * a.w.x * a.x.y - a.x.w * a.w.x * a.y.y - + a.y.w * a.x.x * a.w.y - a.w.w * a.y.x * a.x.y, + a.x.w * a.z.x * a.y.y + a.y.w * a.x.x * a.z.y + + a.z.w * a.y.x * a.x.y - a.x.w * a.y.x * a.z.y - + a.z.w * a.x.x * a.y.y - a.y.w * a.z.x * a.x.y}, + {a.y.x * a.w.y * a.z.z + a.z.x * a.y.y * a.w.z + + a.w.x * a.z.y * a.y.z - a.y.x * a.z.y * a.w.z - + a.w.x * a.y.y * a.z.z - a.z.x * a.w.y * a.y.z, + a.x.x * a.z.y * a.w.z + a.w.x * a.x.y * a.z.z + + a.z.x * a.w.y * a.x.z - a.x.x * a.w.y * a.z.z - + a.z.x * a.x.y * a.w.z - a.w.x * a.z.y * a.x.z, + a.x.x * a.w.y * a.y.z + a.y.x * a.x.y * a.w.z + + a.w.x * a.y.y * a.x.z - a.x.x * a.y.y * a.w.z - + a.w.x * a.x.y * a.y.z - a.y.x * a.w.y * a.x.z, + a.x.x * a.y.y * a.z.z + a.z.x * a.x.y * a.y.z + + a.y.x * a.z.y * a.x.z - a.x.x * a.z.y * a.y.z - + a.y.x * a.x.y * a.z.z - a.z.x * a.y.y * a.x.z}}; +} + +template +constexpr T linalg::determinant(const mat &a) { + return a.x.x * (a.y.y * a.z.z * a.w.w + a.w.y * a.y.z * a.z.w + + a.z.y * a.w.z * a.y.w - a.y.y * a.w.z * a.z.w - + a.z.y * a.y.z * a.w.w - a.w.y * a.z.z * a.y.w) + + a.x.y * (a.y.z * a.w.w * a.z.x + a.z.z * a.y.w * a.w.x + + a.w.z * a.z.w * a.y.x - a.y.z * a.z.w * a.w.x - + a.w.z * a.y.w * a.z.x - a.z.z * a.w.w * a.y.x) + + a.x.z * (a.y.w * a.z.x * a.w.y + a.w.w * a.y.x * a.z.y + + a.z.w * a.w.x * a.y.y - a.y.w * a.w.x * a.z.y - + a.z.w * a.y.x * a.w.y - a.w.w * a.z.x * a.y.y) + + a.x.w * (a.y.x * a.w.y * a.z.z + a.z.x * a.y.y * a.w.z + + a.w.x * a.z.y * a.y.z - a.y.x * a.z.y * a.w.z - + a.w.x * a.y.y * a.z.z - a.z.x * a.w.y * a.y.z); +} + +template +linalg::vec linalg::rotation_quat(const vec &orig, + const vec &dest) { + T cosTheta = dot(orig, dest); + if (cosTheta >= 1 - std::numeric_limits::epsilon()) { + return {0, 0, 0, 1}; + } + if (cosTheta < -1 + std::numeric_limits::epsilon()) { + vec axis = cross(vec(0, 0, 1), orig); + if (length2(axis) < std::numeric_limits::epsilon()) + axis = cross(vec(1, 0, 0), orig); + return rotation_quat(normalize(axis), + 3.14159265358979323846264338327950288); + } + vec axis = cross(orig, dest); + T s = std::sqrt((1 + cosTheta) * 2); + return {axis * (1 / s), s * 0.5}; +} + +template +linalg::vec linalg::rotation_quat(const mat &m) { + const vec q{m.x.x - m.y.y - m.z.z, m.y.y - m.x.x - m.z.z, + m.z.z - m.x.x - m.y.y, m.x.x + m.y.y + m.z.z}, + s[]{{1, m.x.y + m.y.x, m.z.x + m.x.z, m.y.z - m.z.y}, + {m.x.y + m.y.x, 1, m.y.z + m.z.y, m.z.x - m.x.z}, + {m.x.z + m.z.x, m.y.z + m.z.y, 1, m.x.y - m.y.x}, + {m.y.z - m.z.y, m.z.x - m.x.z, m.x.y - m.y.x, 1}}; + return copysign(normalize(sqrt(max(T(0), T(1) + q))), s[argmax(q)]); +} + +template +linalg::mat linalg::lookat_matrix(const vec &eye, + const vec ¢er, + const vec &view_y_dir, + fwd_axis a) { + const vec f = normalize(center - eye), z = a == pos_z ? f : -f, + x = normalize(cross(view_y_dir, z)), y = cross(z, x); + return inverse(mat{{x, 0}, {y, 0}, {z, 0}, {eye, 1}}); +} + +template +linalg::mat linalg::frustum_matrix(T x0, T x1, T y0, T y1, T n, T f, + fwd_axis a, z_range z) { + const T s = a == pos_z ? T(1) : T(-1), o = z == neg_one_to_one ? n : 0; + return {{2 * n / (x1 - x0), 0, 0, 0}, + {0, 2 * n / (y1 - y0), 0, 0}, + {-s * (x0 + x1) / (x1 - x0), -s * (y0 + y1) / (y1 - y0), + s * (f + o) / (f - n), s}, + {0, 0, -(n + o) * f / (f - n), 0}}; +} +#endif diff --git a/thirdparty/manifold/include/manifold/manifold.h b/thirdparty/manifold/include/manifold/manifold.h new file mode 100644 index 000000000000..a4f65e9c4488 --- /dev/null +++ b/thirdparty/manifold/include/manifold/manifold.h @@ -0,0 +1,435 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include +#include + +#include "manifold/common.h" +#include "manifold/vec_view.h" + +namespace manifold { + +/** + * @ingroup Debug + * + * Allows modification of the assertions checked in MANIFOLD_DEBUG mode. + * + * @return ExecutionParams& + */ +ExecutionParams& ManifoldParams(); + +class CsgNode; +class CsgLeafNode; + +/** @addtogroup Core + * @brief The central classes of the library + * @{ + */ + +/** + * @brief Mesh input/output suitable for pushing directly into graphics + * libraries. + * + * This may not be manifold since the verts are duplicated along property + * boundaries that do not match. The additional merge vectors store this missing + * information, allowing the manifold to be reconstructed. MeshGL is an alias + * for the standard single-precision version. Use MeshGL64 to output the full + * double precision that Manifold uses internally. + */ +template +struct MeshGLP { + /// Number of property vertices + I NumVert() const { return vertProperties.size() / numProp; }; + /// Number of triangles + I NumTri() const { return triVerts.size() / 3; }; + /// Number of properties per vertex, always >= 3. + I numProp = 3; + /// Flat, GL-style interleaved list of all vertex properties: propVal = + /// vertProperties[vert * numProp + propIdx]. The first three properties are + /// always the position x, y, z. + std::vector vertProperties; + /// The vertex indices of the three triangle corners in CCW (from the outside) + /// order, for each triangle. + std::vector triVerts; + /// Optional: A list of only the vertex indicies that need to be merged to + /// reconstruct the manifold. + std::vector mergeFromVert; + /// Optional: The same length as mergeFromVert, and the corresponding value + /// contains the vertex to merge with. It will have an identical position, but + /// the other properties may differ. + std::vector mergeToVert; + /// Optional: Indicates runs of triangles that correspond to a particular + /// input mesh instance. The runs encompass all of triVerts and are sorted + /// by runOriginalID. Run i begins at triVerts[runIndex[i]] and ends at + /// triVerts[runIndex[i+1]]. All runIndex values are divisible by 3. Returned + /// runIndex will always be 1 longer than runOriginalID, but same length is + /// also allowed as input: triVerts.size() will be automatically appended in + /// this case. + std::vector runIndex; + /// Optional: The OriginalID of the mesh this triangle run came from. This ID + /// is ideal for reapplying materials to the output mesh. Multiple runs may + /// have the same ID, e.g. representing different copies of the same input + /// mesh. If you create an input MeshGL that you want to be able to reference + /// as one or more originals, be sure to set unique values from ReserveIDs(). + std::vector runOriginalID; + /// Optional: For each run, a 3x4 transform is stored representing how the + /// corresponding original mesh was transformed to create this triangle run. + /// This matrix is stored in column-major order and the length of the overall + /// vector is 12 * runOriginalID.size(). + std::vector runTransform; + /// Optional: Length NumTri, contains the source face ID this + /// triangle comes from. When auto-generated, this ID will be a triangle index + /// into the original mesh. This index/ID is purely for external use (e.g. + /// recreating polygonal faces) and will not affect Manifold's algorithms. + std::vector faceID; + /// Optional: The X-Y-Z-W weighted tangent vectors for smooth Refine(). If + /// non-empty, must be exactly four times as long as Mesh.triVerts. Indexed + /// as 4 * (3 * tri + i) + j, i < 3, j < 4, representing the tangent value + /// Mesh.triVerts[tri][i] along the CCW edge. If empty, mesh is faceted. + std::vector halfedgeTangent; + /// Tolerance for mesh simplification. When creating a Manifold, the tolerance + /// used will be the maximum of this and a baseline tolerance from the size of + /// the bounding box. Any edge shorter than tolerance may be collapsed. + /// Tolerance may be enlarged when floating point error accumulates. + Precision tolerance = 0; + + MeshGLP() = default; + + /** + * Updates the mergeFromVert and mergeToVert vectors in order to create a + * manifold solid. If the MeshGL is already manifold, no change will occur and + * the function will return false. Otherwise, this will merge verts along open + * edges within tolerance (the maximum of the MeshGL tolerance and the + * baseline bounding-box tolerance), keeping any from the existing merge + * vectors, and return true. + * + * There is no guarantee the result will be manifold - this is a best-effort + * helper function designed primarily to aid in the case where a manifold + * multi-material MeshGL was produced, but its merge vectors were lost due to + * a round-trip through a file format. Constructing a Manifold from the result + * will report an error status if it is not manifold. + */ + bool Merge(); + + /** + * Returns the x, y, z position of the ith vertex. + * + * @param v vertex index. + */ + la::vec GetVertPos(size_t v) const { + size_t offset = v * numProp; + return la::vec(vertProperties[offset], + vertProperties[offset + 1], + vertProperties[offset + 2]); + } + + /** + * Returns the three vertex indices of the ith triangle. + * + * @param t triangle index. + */ + la::vec GetTriVerts(size_t t) const { + size_t offset = 3 * t; + return la::vec(triVerts[offset], triVerts[offset + 1], + triVerts[offset + 2]); + } + + /** + * Returns the x, y, z, w tangent of the ith halfedge. + * + * @param h halfedge index (3 * triangle_index + [0|1|2]). + */ + la::vec GetTangent(size_t h) const { + size_t offset = 4 * h; + return la::vec( + halfedgeTangent[offset], halfedgeTangent[offset + 1], + halfedgeTangent[offset + 2], halfedgeTangent[offset + 3]); + } +}; + +/** + * @brief Single-precision - ideal for most uses, especially graphics. + */ +using MeshGL = MeshGLP; +/** + * @brief Double-precision, 64-bit indices - best for huge meshes. + */ +using MeshGL64 = MeshGLP; + +/** + * @brief This library's internal representation of an oriented, 2-manifold, + * triangle mesh - a simple boundary-representation of a solid object. Use this + * class to store and operate on solids, and use MeshGL for input and output. + * + * In addition to storing geometric data, a Manifold can also store an arbitrary + * number of vertex properties. These could be anything, e.g. normals, UV + * coordinates, colors, etc, but this library is completely agnostic. All + * properties are merely float values indexed by channel number. It is up to the + * user to associate channel numbers with meaning. + * + * Manifold allows vertex properties to be shared for efficient storage, or to + * have multiple property verts associated with a single geometric vertex, + * allowing sudden property changes, e.g. at Boolean intersections, without + * sacrificing manifoldness. + * + * Manifolds also keep track of their relationships to their inputs, via + * OriginalIDs and the faceIDs and transforms accessible through MeshGL. This + * allows object-level properties to be re-associated with the output after many + * operations, particularly useful for materials. Since separate object's + * properties are not mixed, there is no requirement that channels have + * consistent meaning between different inputs. + */ +class Manifold { + public: + /** @name Basics + * Copy / move / assignment + */ + ///@{ + Manifold(); + ~Manifold(); + Manifold(const Manifold& other); + Manifold& operator=(const Manifold& other); + Manifold(Manifold&&) noexcept; + Manifold& operator=(Manifold&&) noexcept; + ///@} + + /** @name Input & Output + * Create and retrieve arbitrary manifolds + */ + ///@{ + Manifold(const MeshGL&); + Manifold(const MeshGL64&); + MeshGL GetMeshGL(int normalIdx = -1) const; + MeshGL64 GetMeshGL64(int normalIdx = -1) const; + ///@} + + /** @name Constructors + * Topological ops, primitives, and SDF + */ + ///@{ + std::vector Decompose() const; + static Manifold Compose(const std::vector&); + static Manifold Tetrahedron(); + static Manifold Cube(vec3 size = vec3(1.0), bool center = false); + static Manifold Cylinder(double height, double radiusLow, + double radiusHigh = -1.0, int circularSegments = 0, + bool center = false); + static Manifold Sphere(double radius, int circularSegments = 0); + static Manifold LevelSet(std::function sdf, Box bounds, + double edgeLength, double level = 0, + double tolerance = -1, bool canParallel = true); + ///@} + + /** @name Polygons + * 3D to 2D and 2D to 3D + */ + ///@{ + Polygons Slice(double height = 0) const; + Polygons Project() const; + static Manifold Extrude(const Polygons& crossSection, double height, + int nDivisions = 0, double twistDegrees = 0.0, + vec2 scaleTop = vec2(1.0)); + static Manifold Revolve(const Polygons& crossSection, + int circularSegments = 0, + double revolveDegrees = 360.0f); + ///@} + + enum class Error { + NoError, + NonFiniteVertex, + NotManifold, + VertexOutOfBounds, + PropertiesWrongLength, + MissingPositionProperties, + MergeVectorsDifferentLengths, + MergeIndexOutOfBounds, + TransformWrongLength, + RunIndexWrongLength, + FaceIDWrongLength, + InvalidConstruction, + }; + + /** @name Information + * Details of the manifold + */ + ///@{ + Error Status() const; + bool IsEmpty() const; + size_t NumVert() const; + size_t NumEdge() const; + size_t NumTri() const; + size_t NumProp() const; + size_t NumPropVert() const; + Box BoundingBox() const; + int Genus() const; + double GetTolerance() const; + ///@} + + /** @name Measurement + */ + ///@{ + double SurfaceArea() const; + double Volume() const; + double MinGap(const Manifold& other, double searchLength) const; + ///@} + + /** @name Mesh ID + * Details of the manifold's relation to its input meshes, for the purposes + * of reapplying mesh properties. + */ + ///@{ + int OriginalID() const; + Manifold AsOriginal() const; + static uint32_t ReserveIDs(uint32_t); + ///@} + + /** @name Transformations + */ + ///@{ + Manifold Translate(vec3) const; + Manifold Scale(vec3) const; + Manifold Rotate(double xDegrees, double yDegrees = 0.0, + double zDegrees = 0.0) const; + Manifold Mirror(vec3) const; + Manifold Transform(const mat3x4&) const; + Manifold Warp(std::function) const; + Manifold WarpBatch(std::function)>) const; + Manifold SetTolerance(double) const; + ///@} + + /** @name Boolean + * Combine two manifolds + */ + ///@{ + Manifold Boolean(const Manifold& second, OpType op) const; + static Manifold BatchBoolean(const std::vector& manifolds, + OpType op); + // Boolean operation shorthand + Manifold operator+(const Manifold&) const; // Add (Union) + Manifold& operator+=(const Manifold&); + Manifold operator-(const Manifold&) const; // Subtract (Difference) + Manifold& operator-=(const Manifold&); + Manifold operator^(const Manifold&) const; // Intersect + Manifold& operator^=(const Manifold&); + std::pair Split(const Manifold&) const; + std::pair SplitByPlane(vec3 normal, + double originOffset) const; + Manifold TrimByPlane(vec3 normal, double originOffset) const; + ///@} + + /** @name Properties + * Create and modify vertex properties. + */ + ///@{ + Manifold SetProperties( + int numProp, + std::function propFunc) const; + Manifold CalculateCurvature(int gaussianIdx, int meanIdx) const; + Manifold CalculateNormals(int normalIdx, double minSharpAngle = 60) const; + ///@} + + /** @name Smoothing + * Smooth meshes by calculating tangent vectors and refining to a higher + * triangle count. + */ + ///@{ + Manifold Refine(int) const; + Manifold RefineToLength(double) const; + Manifold RefineToTolerance(double) const; + Manifold SmoothByNormals(int normalIdx) const; + Manifold SmoothOut(double minSharpAngle = 60, double minSmoothness = 0) const; + static Manifold Smooth(const MeshGL&, + const std::vector& sharpenedEdges = {}); + static Manifold Smooth(const MeshGL64&, + const std::vector& sharpenedEdges = {}); + ///@} + + /** @name Convex Hull + */ + ///@{ + Manifold Hull() const; + static Manifold Hull(const std::vector& manifolds); + static Manifold Hull(const std::vector& pts); + ///@} + + /** @name Testing Hooks + * These are just for internal testing. + */ + ///@{ + bool MatchesTriNormals() const; + size_t NumDegenerateTris() const; + size_t NumOverlaps(const Manifold& second) const; + double GetEpsilon() const; + ///@} + + struct Impl; + + private: + Manifold(std::shared_ptr pNode_); + Manifold(std::shared_ptr pImpl_); + static Manifold Invalid(); + mutable std::shared_ptr pNode_; + + CsgLeafNode& GetCsgLeafNode() const; +}; +/** @} */ + +/** @addtogroup Debug + * @ingroup Optional + * @brief Debugging features + * + * The features require compiler flags to be enabled. Assertions are enabled + * with the MANIFOLD_DEBUG flag and then controlled with ExecutionParams. + * @{ + */ +#ifdef MANIFOLD_DEBUG +inline std::string ToString(const Manifold::Error& error) { + switch (error) { + case Manifold::Error::NoError: + return "No Error"; + case Manifold::Error::NonFiniteVertex: + return "Non Finite Vertex"; + case Manifold::Error::NotManifold: + return "Not Manifold"; + case Manifold::Error::VertexOutOfBounds: + return "Vertex Out Of Bounds"; + case Manifold::Error::PropertiesWrongLength: + return "Properties Wrong Length"; + case Manifold::Error::MissingPositionProperties: + return "Missing Position Properties"; + case Manifold::Error::MergeVectorsDifferentLengths: + return "Merge Vectors Different Lengths"; + case Manifold::Error::MergeIndexOutOfBounds: + return "Merge Index Out Of Bounds"; + case Manifold::Error::TransformWrongLength: + return "Transform Wrong Length"; + case Manifold::Error::RunIndexWrongLength: + return "Run Index Wrong Length"; + case Manifold::Error::FaceIDWrongLength: + return "Face ID Wrong Length"; + case Manifold::Error::InvalidConstruction: + return "Invalid Construction"; + default: + return "Unknown Error"; + }; +} + +inline std::ostream& operator<<(std::ostream& stream, + const Manifold::Error& error) { + return stream << ToString(error); +} +#endif +/** @} */ +} // namespace manifold diff --git a/thirdparty/manifold/include/manifold/optional_assert.h b/thirdparty/manifold/include/manifold/optional_assert.h new file mode 100644 index 000000000000..905746cca38b --- /dev/null +++ b/thirdparty/manifold/include/manifold/optional_assert.h @@ -0,0 +1,66 @@ +// Copyright 2022 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#ifdef MANIFOLD_DEBUG +#include +#include +#include +#include + +/** @addtogroup Debug + * @{ + */ +struct userErr : public virtual std::runtime_error { + using std::runtime_error::runtime_error; +}; +struct topologyErr : public virtual std::runtime_error { + using std::runtime_error::runtime_error; +}; +struct geometryErr : public virtual std::runtime_error { + using std::runtime_error::runtime_error; +}; +using logicErr = std::logic_error; + +template +void AssertFail(const char* file, int line, const char* cond, const char* msg) { + std::ostringstream output; + output << "Error in file: " << file << " (" << line << "): \'" << cond + << "\' is false: " << msg; + throw Ex(output.str()); +} + +template +void AssertFail(const char* file, int line, const std::string& cond, + const std::string& msg) { + std::ostringstream output; + output << "Error in file: " << file << " (" << line << "): \'" << cond + << "\' is false: " << msg; + throw Ex(output.str()); +} + +// DEBUG_ASSERT is slightly slower due to the function call, but gives more +// detailed info. +#define DEBUG_ASSERT(condition, EX, msg) \ + if (!(condition)) AssertFail(__FILE__, __LINE__, #condition, msg); +// ASSERT has almost no overhead, so better to use for frequent calls like +// vector bounds checking. +#define ASSERT(condition, EX) \ + if (!(condition)) throw(EX); +#else +#define DEBUG_ASSERT(condition, EX, msg) +#define ASSERT(condition, EX) +#endif +/** @} */ diff --git a/thirdparty/manifold/include/manifold/polygon.h b/thirdparty/manifold/include/manifold/polygon.h new file mode 100644 index 000000000000..2902524ce489 --- /dev/null +++ b/thirdparty/manifold/include/manifold/polygon.h @@ -0,0 +1,62 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include "manifold/common.h" + +namespace manifold { + +/** @addtogroup Structs + * @{ + */ + +/** + * @brief Polygon vertex. + */ +struct PolyVert { + /// X-Y position + vec2 pos; + /// ID or index into another vertex vector + int idx; +}; + +/** + * @brief Single polygon contour, wound CCW, with indices. First and last point + * are implicitly connected. Should ensure all input is + * [ε-valid](https://github.com/elalish/manifold/wiki/Manifold-Library#definition-of-%CE%B5-valid). + */ +using SimplePolygonIdx = std::vector; + +/** + * @brief Set of indexed polygons with holes. Order of contours is arbitrary. + * Can contain any depth of nested holes and any number of separate polygons. + * Should ensure all input is + * [ε-valid](https://github.com/elalish/manifold/wiki/Manifold-Library#definition-of-%CE%B5-valid). + */ +using PolygonsIdx = std::vector; +/** @} */ + +/** @addtogroup Triangulation + * @ingroup Core + * @brief Polygon triangulation + * @{ + */ +std::vector TriangulateIdx(const PolygonsIdx &polys, + double epsilon = -1); + +std::vector Triangulate(const Polygons &polygons, double epsilon = -1); + +ExecutionParams &PolygonParams(); +/** @} */ +} // namespace manifold diff --git a/thirdparty/manifold/include/manifold/vec_view.h b/thirdparty/manifold/include/manifold/vec_view.h new file mode 100644 index 000000000000..4dc5f7344519 --- /dev/null +++ b/thirdparty/manifold/include/manifold/vec_view.h @@ -0,0 +1,151 @@ +// Copyright 2023 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include + +#include "manifold/optional_assert.h" + +namespace manifold { + +/** + * View for Vec, can perform offset operation. + * This will be invalidated when the original vector is dropped or changes + * length. Roughly equivalent to std::span from c++20 + */ +template +class VecView { + public: + using Iter = T *; + using IterC = const T *; + + VecView() : ptr_(nullptr), size_(0) {} + + VecView(T *ptr, size_t size) : ptr_(ptr), size_(size) {} + + VecView(const std::vector> &v) + : ptr_(v.data()), size_(v.size()) {} + + VecView(const VecView &other) { + ptr_ = other.ptr_; + size_ = other.size_; + } + + VecView &operator=(const VecView &other) { + ptr_ = other.ptr_; + size_ = other.size_; + return *this; + } + + // allows conversion to a const VecView + operator VecView() const { return {ptr_, size_}; } + + inline const T &operator[](size_t i) const { + ASSERT(i < size_, std::out_of_range("Vec out of range")); + return ptr_[i]; + } + + inline T &operator[](size_t i) { + ASSERT(i < size_, std::out_of_range("Vec out of range")); + return ptr_[i]; + } + + IterC cbegin() const { return ptr_; } + IterC cend() const { return ptr_ + size_; } + + IterC begin() const { return cbegin(); } + IterC end() const { return cend(); } + + Iter begin() { return ptr_; } + Iter end() { return ptr_ + size_; } + + const T &front() const { + ASSERT(size_ != 0, + std::out_of_range("Attempt to take the front of an empty vector")); + return ptr_[0]; + } + + const T &back() const { + ASSERT(size_ != 0, + std::out_of_range("Attempt to take the back of an empty vector")); + return ptr_[size_ - 1]; + } + + T &front() { + ASSERT(size_ != 0, + std::out_of_range("Attempt to take the front of an empty vector")); + return ptr_[0]; + } + + T &back() { + ASSERT(size_ != 0, + std::out_of_range("Attempt to take the back of an empty vector")); + return ptr_[size_ - 1]; + } + + size_t size() const { return size_; } + + bool empty() const { return size_ == 0; } + + VecView view(size_t offset = 0, + size_t length = std::numeric_limits::max()) { + if (length == std::numeric_limits::max()) + length = this->size_ - offset; + ASSERT(length >= 0, std::out_of_range("Vec::view out of range")); + ASSERT(offset + length <= this->size_ && offset >= 0, + std::out_of_range("Vec::view out of range")); + return VecView(this->ptr_ + offset, length); + } + + VecView cview( + size_t offset = 0, + size_t length = std::numeric_limits::max()) const { + if (length == std::numeric_limits::max()) + length = this->size_ - offset; + ASSERT(length >= 0, std::out_of_range("Vec::cview out of range")); + ASSERT(offset + length <= this->size_ && offset >= 0, + std::out_of_range("Vec::cview out of range")); + return VecView(this->ptr_ + offset, length); + } + + VecView view( + size_t offset = 0, + size_t length = std::numeric_limits::max()) const { + return cview(offset, length); + } + + T *data() { return this->ptr_; } + + const T *data() const { return this->ptr_; } + +#ifdef MANIFOLD_DEBUG + void Dump() const { + std::cout << "Vec = " << std::endl; + for (size_t i = 0; i < size(); ++i) { + std::cout << i << ", " << ptr_[i] << ", " << std::endl; + } + std::cout << std::endl; + } +#endif + + protected: + T *ptr_ = nullptr; + size_t size_ = 0; +}; + +} // namespace manifold diff --git a/thirdparty/manifold/src/boolean3.cpp b/thirdparty/manifold/src/boolean3.cpp new file mode 100644 index 000000000000..1fca99db451e --- /dev/null +++ b/thirdparty/manifold/src/boolean3.cpp @@ -0,0 +1,599 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./boolean3.h" + +#include + +#include "./parallel.h" + +using namespace manifold; + +namespace { + +// These two functions (Interpolate and Intersect) are the only places where +// floating-point operations take place in the whole Boolean function. These +// are carefully designed to minimize rounding error and to eliminate it at edge +// cases to ensure consistency. + +vec2 Interpolate(vec3 pL, vec3 pR, double x) { + const double dxL = x - pL.x; + const double dxR = x - pR.x; + DEBUG_ASSERT(dxL * dxR <= 0, logicErr, + "Boolean manifold error: not in domain"); + const bool useL = fabs(dxL) < fabs(dxR); + const vec3 dLR = pR - pL; + const double lambda = (useL ? dxL : dxR) / dLR.x; + if (!std::isfinite(lambda) || !std::isfinite(dLR.y) || !std::isfinite(dLR.z)) + return vec2(pL.y, pL.z); + vec2 yz; + yz[0] = fma(lambda, dLR.y, useL ? pL.y : pR.y); + yz[1] = fma(lambda, dLR.z, useL ? pL.z : pR.z); + return yz; +} + +vec4 Intersect(const vec3 &pL, const vec3 &pR, const vec3 &qL, const vec3 &qR) { + const double dyL = qL.y - pL.y; + const double dyR = qR.y - pR.y; + DEBUG_ASSERT(dyL * dyR <= 0, logicErr, + "Boolean manifold error: no intersection"); + const bool useL = fabs(dyL) < fabs(dyR); + const double dx = pR.x - pL.x; + double lambda = (useL ? dyL : dyR) / (dyL - dyR); + if (!std::isfinite(lambda)) lambda = 0.0; + vec4 xyzz; + xyzz.x = fma(lambda, dx, useL ? pL.x : pR.x); + const double pDy = pR.y - pL.y; + const double qDy = qR.y - qL.y; + const bool useP = fabs(pDy) < fabs(qDy); + xyzz.y = fma(lambda, useP ? pDy : qDy, + useL ? (useP ? pL.y : qL.y) : (useP ? pR.y : qR.y)); + xyzz.z = fma(lambda, pR.z - pL.z, useL ? pL.z : pR.z); + xyzz.w = fma(lambda, qR.z - qL.z, useL ? qL.z : qR.z); + return xyzz; +} + +template +struct CopyFaceEdges { + const SparseIndices &p1q1; + // x can be either vert or edge (0 or 1). + SparseIndices &pXq1; + VecView halfedgesQ; + const size_t offset; + + void operator()(const size_t i) { + int idx = 3 * (i + offset); + int pX = p1q1.Get(i, inverted); + int q2 = p1q1.Get(i, !inverted); + + for (const int j : {0, 1, 2}) { + const int q1 = 3 * q2 + j; + const Halfedge edge = halfedgesQ[q1]; + int a = pX; + int b = edge.IsForward() ? q1 : edge.pairedHalfedge; + if (inverted) std::swap(a, b); + pXq1.Set(idx + static_cast(j), a, b); + } + } +}; + +SparseIndices Filter11(const Manifold::Impl &inP, const Manifold::Impl &inQ, + const SparseIndices &p1q2, const SparseIndices &p2q1) { + ZoneScoped; + SparseIndices p1q1(3 * p1q2.size() + 3 * p2q1.size()); + for_each_n(autoPolicy(p1q2.size(), 1e5), countAt(0_uz), p1q2.size(), + CopyFaceEdges({p1q2, p1q1, inQ.halfedge_, 0_uz})); + for_each_n(autoPolicy(p2q1.size(), 1e5), countAt(0_uz), p2q1.size(), + CopyFaceEdges({p2q1, p1q1, inP.halfedge_, p1q2.size()})); + p1q1.Unique(); + return p1q1; +} + +inline bool Shadows(double p, double q, double dir) { + return p == q ? dir < 0 : p < q; +} + +inline std::pair Shadow01( + const int p0, const int q1, VecView vertPosP, + VecView vertPosQ, VecView halfedgeQ, + const double expandP, VecView normalP, const bool reverse) { + const int q1s = halfedgeQ[q1].startVert; + const int q1e = halfedgeQ[q1].endVert; + const double p0x = vertPosP[p0].x; + const double q1sx = vertPosQ[q1s].x; + const double q1ex = vertPosQ[q1e].x; + int s01 = reverse ? Shadows(q1sx, p0x, expandP * normalP[q1s].x) - + Shadows(q1ex, p0x, expandP * normalP[q1e].x) + : Shadows(p0x, q1ex, expandP * normalP[p0].x) - + Shadows(p0x, q1sx, expandP * normalP[p0].x); + vec2 yz01(NAN); + + if (s01 != 0) { + yz01 = Interpolate(vertPosQ[q1s], vertPosQ[q1e], vertPosP[p0].x); + if (reverse) { + vec3 diff = vertPosQ[q1s] - vertPosP[p0]; + const double start2 = la::dot(diff, diff); + diff = vertPosQ[q1e] - vertPosP[p0]; + const double end2 = la::dot(diff, diff); + const double dir = start2 < end2 ? normalP[q1s].y : normalP[q1e].y; + if (!Shadows(yz01[0], vertPosP[p0].y, expandP * dir)) s01 = 0; + } else { + if (!Shadows(vertPosP[p0].y, yz01[0], expandP * normalP[p0].y)) s01 = 0; + } + } + return std::make_pair(s01, yz01); +} + +// https://github.com/scandum/binary_search/blob/master/README.md +// much faster than standard binary search on large arrays +size_t monobound_quaternary_search(VecView array, int64_t key) { + if (array.size() == 0) { + return std::numeric_limits::max(); + } + size_t bot = 0; + size_t top = array.size(); + while (top >= 65536) { + size_t mid = top / 4; + top -= mid * 3; + if (key < array[bot + mid * 2]) { + if (key >= array[bot + mid]) { + bot += mid; + } + } else { + bot += mid * 2; + if (key >= array[bot + mid]) { + bot += mid; + } + } + } + + while (top > 3) { + size_t mid = top / 2; + if (key >= array[bot + mid]) { + bot += mid; + } + top -= mid; + } + + while (top--) { + if (key == array[bot + top]) { + return bot + top; + } + } + return -1; +} + +struct Kernel11 { + VecView xyzz; + VecView s; + VecView vertPosP; + VecView vertPosQ; + VecView halfedgeP; + VecView halfedgeQ; + const double expandP; + VecView normalP; + const SparseIndices &p1q1; + + void operator()(const size_t idx) { + const int p1 = p1q1.Get(idx, false); + const int q1 = p1q1.Get(idx, true); + vec4 &xyzz11 = xyzz[idx]; + int &s11 = s[idx]; + + // For pRL[k], qRL[k], k==0 is the left and k==1 is the right. + int k = 0; + vec3 pRL[2], qRL[2]; + // Either the left or right must shadow, but not both. This ensures the + // intersection is between the left and right. + bool shadows = false; + s11 = 0; + + const int p0[2] = {halfedgeP[p1].startVert, halfedgeP[p1].endVert}; + for (int i : {0, 1}) { + const auto syz01 = Shadow01(p0[i], q1, vertPosP, vertPosQ, halfedgeQ, + expandP, normalP, false); + const int s01 = syz01.first; + const vec2 yz01 = syz01.second; + // If the value is NaN, then these do not overlap. + if (std::isfinite(yz01[0])) { + s11 += s01 * (i == 0 ? -1 : 1); + if (k < 2 && (k == 0 || (s01 != 0) != shadows)) { + shadows = s01 != 0; + pRL[k] = vertPosP[p0[i]]; + qRL[k] = vec3(pRL[k].x, yz01.x, yz01.y); + ++k; + } + } + } + + const int q0[2] = {halfedgeQ[q1].startVert, halfedgeQ[q1].endVert}; + for (int i : {0, 1}) { + const auto syz10 = Shadow01(q0[i], p1, vertPosQ, vertPosP, halfedgeP, + expandP, normalP, true); + const int s10 = syz10.first; + const vec2 yz10 = syz10.second; + // If the value is NaN, then these do not overlap. + if (std::isfinite(yz10[0])) { + s11 += s10 * (i == 0 ? -1 : 1); + if (k < 2 && (k == 0 || (s10 != 0) != shadows)) { + shadows = s10 != 0; + qRL[k] = vertPosQ[q0[i]]; + pRL[k] = vec3(qRL[k].x, yz10.x, yz10.y); + ++k; + } + } + } + + if (s11 == 0) { // No intersection + xyzz11 = vec4(NAN); + } else { + DEBUG_ASSERT(k == 2, logicErr, "Boolean manifold error: s11"); + xyzz11 = Intersect(pRL[0], pRL[1], qRL[0], qRL[1]); + + const int p1s = halfedgeP[p1].startVert; + const int p1e = halfedgeP[p1].endVert; + vec3 diff = vertPosP[p1s] - vec3(xyzz11); + const double start2 = la::dot(diff, diff); + diff = vertPosP[p1e] - vec3(xyzz11); + const double end2 = la::dot(diff, diff); + const double dir = start2 < end2 ? normalP[p1s].z : normalP[p1e].z; + + if (!Shadows(xyzz11.z, xyzz11.w, expandP * dir)) s11 = 0; + } + } +}; + +std::tuple, Vec> Shadow11(SparseIndices &p1q1, + const Manifold::Impl &inP, + const Manifold::Impl &inQ, + double expandP) { + ZoneScoped; + Vec s11(p1q1.size()); + Vec xyzz11(p1q1.size()); + + for_each_n(autoPolicy(p1q1.size(), 1e4), countAt(0_uz), p1q1.size(), + Kernel11({xyzz11, s11, inP.vertPos_, inQ.vertPos_, inP.halfedge_, + inQ.halfedge_, expandP, inP.vertNormal_, p1q1})); + + p1q1.KeepFinite(xyzz11, s11); + + return std::make_tuple(s11, xyzz11); +}; + +struct Kernel02 { + VecView s; + VecView z; + VecView vertPosP; + VecView halfedgeQ; + VecView vertPosQ; + const double expandP; + VecView vertNormalP; + const SparseIndices &p0q2; + const bool forward; + + void operator()(const size_t idx) { + const int p0 = p0q2.Get(idx, !forward); + const int q2 = p0q2.Get(idx, forward); + int &s02 = s[idx]; + double &z02 = z[idx]; + + // For yzzLR[k], k==0 is the left and k==1 is the right. + int k = 0; + vec3 yzzRL[2]; + // Either the left or right must shadow, but not both. This ensures the + // intersection is between the left and right. + bool shadows = false; + int closestVert = -1; + double minMetric = std::numeric_limits::infinity(); + s02 = 0; + + const vec3 posP = vertPosP[p0]; + for (const int i : {0, 1, 2}) { + const int q1 = 3 * q2 + i; + const Halfedge edge = halfedgeQ[q1]; + const int q1F = edge.IsForward() ? q1 : edge.pairedHalfedge; + + if (!forward) { + const int qVert = halfedgeQ[q1F].startVert; + const vec3 diff = posP - vertPosQ[qVert]; + const double metric = la::dot(diff, diff); + if (metric < minMetric) { + minMetric = metric; + closestVert = qVert; + } + } + + const auto syz01 = Shadow01(p0, q1F, vertPosP, vertPosQ, halfedgeQ, + expandP, vertNormalP, !forward); + const int s01 = syz01.first; + const vec2 yz01 = syz01.second; + // If the value is NaN, then these do not overlap. + if (std::isfinite(yz01[0])) { + s02 += s01 * (forward == edge.IsForward() ? -1 : 1); + if (k < 2 && (k == 0 || (s01 != 0) != shadows)) { + shadows = s01 != 0; + yzzRL[k++] = vec3(yz01[0], yz01[1], yz01[1]); + } + } + } + + if (s02 == 0) { // No intersection + z02 = NAN; + } else { + DEBUG_ASSERT(k == 2, logicErr, "Boolean manifold error: s02"); + vec3 vertPos = vertPosP[p0]; + z02 = Interpolate(yzzRL[0], yzzRL[1], vertPos.y)[1]; + if (forward) { + if (!Shadows(vertPos.z, z02, expandP * vertNormalP[p0].z)) s02 = 0; + } else { + // DEBUG_ASSERT(closestVert != -1, topologyErr, "No closest vert"); + if (!Shadows(z02, vertPos.z, expandP * vertNormalP[closestVert].z)) + s02 = 0; + } + } + } +}; + +std::tuple, Vec> Shadow02(const Manifold::Impl &inP, + const Manifold::Impl &inQ, + SparseIndices &p0q2, bool forward, + double expandP) { + ZoneScoped; + Vec s02(p0q2.size()); + Vec z02(p0q2.size()); + + auto vertNormalP = forward ? inP.vertNormal_ : inQ.vertNormal_; + for_each_n(autoPolicy(p0q2.size(), 1e4), countAt(0_uz), p0q2.size(), + Kernel02({s02, z02, inP.vertPos_, inQ.halfedge_, inQ.vertPos_, + expandP, vertNormalP, p0q2, forward})); + + p0q2.KeepFinite(z02, s02); + + return std::make_tuple(s02, z02); +}; + +struct Kernel12 { + VecView x; + VecView v; + VecView p0q2; + VecView s02; + VecView z02; + VecView p1q1; + VecView s11; + VecView xyzz11; + VecView halfedgesP; + VecView halfedgesQ; + VecView vertPosP; + const bool forward; + const SparseIndices &p1q2; + + void operator()(const size_t idx) { + int p1 = p1q2.Get(idx, !forward); + int q2 = p1q2.Get(idx, forward); + int &x12 = x[idx]; + vec3 &v12 = v[idx]; + + // For xzyLR-[k], k==0 is the left and k==1 is the right. + int k = 0; + vec3 xzyLR0[2]; + vec3 xzyLR1[2]; + // Either the left or right must shadow, but not both. This ensures the + // intersection is between the left and right. + bool shadows = false; + x12 = 0; + + const Halfedge edge = halfedgesP[p1]; + + for (int vert : {edge.startVert, edge.endVert}) { + const int64_t key = forward ? SparseIndices::EncodePQ(vert, q2) + : SparseIndices::EncodePQ(q2, vert); + const size_t idx = monobound_quaternary_search(p0q2, key); + if (idx != std::numeric_limits::max()) { + const int s = s02[idx]; + x12 += s * ((vert == edge.startVert) == forward ? 1 : -1); + if (k < 2 && (k == 0 || (s != 0) != shadows)) { + shadows = s != 0; + xzyLR0[k] = vertPosP[vert]; + std::swap(xzyLR0[k].y, xzyLR0[k].z); + xzyLR1[k] = xzyLR0[k]; + xzyLR1[k][1] = z02[idx]; + k++; + } + } + } + + for (const int i : {0, 1, 2}) { + const int q1 = 3 * q2 + i; + const Halfedge edge = halfedgesQ[q1]; + const int q1F = edge.IsForward() ? q1 : edge.pairedHalfedge; + const int64_t key = forward ? SparseIndices::EncodePQ(p1, q1F) + : SparseIndices::EncodePQ(q1F, p1); + const size_t idx = monobound_quaternary_search(p1q1, key); + if (idx != + std::numeric_limits::max()) { // s is implicitly zero for + // anything not found + const int s = s11[idx]; + x12 -= s * (edge.IsForward() ? 1 : -1); + if (k < 2 && (k == 0 || (s != 0) != shadows)) { + shadows = s != 0; + const vec4 xyzz = xyzz11[idx]; + xzyLR0[k][0] = xyzz.x; + xzyLR0[k][1] = xyzz.z; + xzyLR0[k][2] = xyzz.y; + xzyLR1[k] = xzyLR0[k]; + xzyLR1[k][1] = xyzz.w; + if (!forward) std::swap(xzyLR0[k][1], xzyLR1[k][1]); + k++; + } + } + } + + if (x12 == 0) { // No intersection + v12 = vec3(NAN); + } else { + DEBUG_ASSERT(k == 2, logicErr, "Boolean manifold error: v12"); + const vec4 xzyy = Intersect(xzyLR0[0], xzyLR0[1], xzyLR1[0], xzyLR1[1]); + v12.x = xzyy[0]; + v12.y = xzyy[2]; + v12.z = xzyy[1]; + } + } +}; + +std::tuple, Vec> Intersect12( + const Manifold::Impl &inP, const Manifold::Impl &inQ, const Vec &s02, + const SparseIndices &p0q2, const Vec &s11, const SparseIndices &p1q1, + const Vec &z02, const Vec &xyzz11, SparseIndices &p1q2, + bool forward) { + ZoneScoped; + Vec x12(p1q2.size()); + Vec v12(p1q2.size()); + + for_each_n( + autoPolicy(p1q2.size(), 1e4), countAt(0_uz), p1q2.size(), + Kernel12({x12, v12, p0q2.AsVec64(), s02, z02, p1q1.AsVec64(), s11, xyzz11, + inP.halfedge_, inQ.halfedge_, inP.vertPos_, forward, p1q2})); + + p1q2.KeepFinite(v12, x12); + + return std::make_tuple(x12, v12); +}; + +Vec Winding03(const Manifold::Impl &inP, Vec &vertices, Vec &s02, + bool reverse) { + ZoneScoped; + // verts that are not shadowed (not in p0q2) have winding number zero. + Vec w03(inP.NumVert(), 0); + if (vertices.size() <= 1e5) { + for_each_n(ExecutionPolicy::Seq, countAt(0), s02.size(), + [&w03, &vertices, &s02, reverse](const int i) { + w03[vertices[i]] += s02[i] * (reverse ? -1 : 1); + }); + } else { + for_each_n(ExecutionPolicy::Par, countAt(0), s02.size(), + [&w03, &vertices, &s02, reverse](const int i) { + AtomicAdd(w03[vertices[i]], s02[i] * (reverse ? -1 : 1)); + }); + } + return w03; +}; +} // namespace + +namespace manifold { +Boolean3::Boolean3(const Manifold::Impl &inP, const Manifold::Impl &inQ, + OpType op) + : inP_(inP), inQ_(inQ), expandP_(op == OpType::Add ? 1.0 : -1.0) { + // Symbolic perturbation: + // Union -> expand inP + // Difference, Intersection -> contract inP + +#ifdef MANIFOLD_DEBUG + Timer broad; + broad.Start(); +#endif + + if (inP.IsEmpty() || inQ.IsEmpty() || !inP.bBox_.DoesOverlap(inQ.bBox_)) { + PRINT("No overlap, early out"); + w03_.resize(inP.NumVert(), 0); + w30_.resize(inQ.NumVert(), 0); + return; + } + + // Level 3 + // Find edge-triangle overlaps (broad phase) + p1q2_ = inQ_.EdgeCollisions(inP_); + p2q1_ = inP_.EdgeCollisions(inQ_, true); // inverted + + p1q2_.Sort(); + PRINT("p1q2 size = " << p1q2_.size()); + + p2q1_.Sort(); + PRINT("p2q1 size = " << p2q1_.size()); + + // Level 2 + // Find vertices that overlap faces in XY-projection + SparseIndices p0q2 = inQ.VertexCollisionsZ(inP.vertPos_); + p0q2.Sort(); + PRINT("p0q2 size = " << p0q2.size()); + + SparseIndices p2q0 = inP.VertexCollisionsZ(inQ.vertPos_, true); // inverted + p2q0.Sort(); + PRINT("p2q0 size = " << p2q0.size()); + + // Find involved edge pairs from Level 3 + SparseIndices p1q1 = Filter11(inP_, inQ_, p1q2_, p2q1_); + PRINT("p1q1 size = " << p1q1.size()); + +#ifdef MANIFOLD_DEBUG + broad.Stop(); + Timer intersections; + intersections.Start(); +#endif + + // Level 2 + // Build up XY-projection intersection of two edges, including the z-value for + // each edge, keeping only those whose intersection exists. + Vec s11; + Vec xyzz11; + std::tie(s11, xyzz11) = Shadow11(p1q1, inP, inQ, expandP_); + PRINT("s11 size = " << s11.size()); + + // Build up Z-projection of vertices onto triangles, keeping only those that + // fall inside the triangle. + Vec s02; + Vec z02; + std::tie(s02, z02) = Shadow02(inP, inQ, p0q2, true, expandP_); + PRINT("s02 size = " << s02.size()); + + Vec s20; + Vec z20; + std::tie(s20, z20) = Shadow02(inQ, inP, p2q0, false, expandP_); + PRINT("s20 size = " << s20.size()); + + // Level 3 + // Build up the intersection of the edges and triangles, keeping only those + // that intersect, and record the direction the edge is passing through the + // triangle. + std::tie(x12_, v12_) = + Intersect12(inP, inQ, s02, p0q2, s11, p1q1, z02, xyzz11, p1q2_, true); + PRINT("x12 size = " << x12_.size()); + + std::tie(x21_, v21_) = + Intersect12(inQ, inP, s20, p2q0, s11, p1q1, z20, xyzz11, p2q1_, false); + PRINT("x21 size = " << x21_.size()); + + s11.clear(); + xyzz11.clear(); + z02.clear(); + z20.clear(); + + Vec p0 = p0q2.Copy(false); + p0q2.Resize(0); + Vec q0 = p2q0.Copy(true); + p2q0.Resize(0); + // Sum up the winding numbers of all vertices. + w03_ = Winding03(inP, p0, s02, false); + + w30_ = Winding03(inQ, q0, s20, true); + +#ifdef MANIFOLD_DEBUG + intersections.Stop(); + + if (ManifoldParams().verbose) { + broad.Print("Broad phase"); + intersections.Print("Intersections"); + } +#endif +} +} // namespace manifold diff --git a/thirdparty/manifold/src/boolean3.h b/thirdparty/manifold/src/boolean3.h new file mode 100644 index 000000000000..7ee2ee12304a --- /dev/null +++ b/thirdparty/manifold/src/boolean3.h @@ -0,0 +1,60 @@ +// Copyright 2020 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include "./impl.h" + +#ifdef MANIFOLD_DEBUG +#define PRINT(msg) \ + if (ManifoldParams().verbose) std::cout << msg << std::endl; +#else +#define PRINT(msg) +#endif + +/** + * The notation in these files is abbreviated due to the complexity of the + * functions involved. The key is that the input manifolds are P and Q, while + * the output is R, and these letters in both upper and lower case refer to + * these objects. Operations are based on dimensionality: vert: 0, edge: 1, + * face: 2, solid: 3. X denotes a winding-number type quantity from the source + * paper of this algorithm, while S is closely related but includes only the + * subset of X values which "shadow" (are on the correct side of). + * + * Nearly everything here are sparse arrays, where for instance each pair in + * p2q1 refers to a face index of P interacting with a halfedge index of Q. + * Adjacent arrays like x21 refer to the values of X corresponding to each + * sparse index pair. + * + * Note many functions are designed to work symmetrically, for instance for both + * p2q1 and p1q2. Inside of these functions P and Q are marked as though the + * function is forwards, but it may include a Boolean "reverse" that indicates P + * and Q have been swapped. + */ + +namespace manifold { + +/** @ingroup Private */ +class Boolean3 { + public: + Boolean3(const Manifold::Impl& inP, const Manifold::Impl& inQ, OpType op); + Manifold::Impl Result(OpType op) const; + + private: + const Manifold::Impl &inP_, &inQ_; + const double expandP_; + SparseIndices p1q2_, p2q1_; + Vec x12_, x21_, w03_, w30_; + Vec v12_, v21_; +}; +} // namespace manifold diff --git a/thirdparty/manifold/src/boolean_result.cpp b/thirdparty/manifold/src/boolean_result.cpp new file mode 100644 index 000000000000..44faf73e97b8 --- /dev/null +++ b/thirdparty/manifold/src/boolean_result.cpp @@ -0,0 +1,889 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "./boolean3.h" +#include "./parallel.h" +#include "./utils.h" + +#if (MANIFOLD_PAR == 1) && __has_include() +#define TBB_PREVIEW_CONCURRENT_ORDERED_CONTAINERS 1 +#include +#include + +template +using concurrent_map = tbb::concurrent_map; +#else +template +// not really concurrent when tbb is disabled +using concurrent_map = std::map; +#endif + +using namespace manifold; + +template <> +struct std::hash> { + size_t operator()(const std::pair &p) const { + return std::hash()(p.first) ^ std::hash()(p.second); + } +}; + +namespace { + +constexpr int kParallelThreshold = 128; + +struct AbsSum { + int operator()(int a, int b) const { return abs(a) + abs(b); } +}; + +struct DuplicateVerts { + VecView vertPosR; + VecView inclusion; + VecView vertR; + VecView vertPosP; + + void operator()(const int vert) { + const int n = std::abs(inclusion[vert]); + for (int i = 0; i < n; ++i) { + vertPosR[vertR[vert] + i] = vertPosP[vert]; + } + } +}; + +template +struct CountVerts { + VecView halfedges; + VecView count; + VecView inclusion; + + void operator()(size_t i) { + if (atomic) + AtomicAdd(count[i / 3], std::abs(inclusion[halfedges[i].startVert])); + else + count[i / 3] += std::abs(inclusion[halfedges[i].startVert]); + } +}; + +template +struct CountNewVerts { + VecView countP; + VecView countQ; + VecView i12; + const SparseIndices &pq; + VecView halfedges; + + void operator()(const int idx) { + int edgeP = pq.Get(idx, inverted); + int faceQ = pq.Get(idx, !inverted); + int inclusion = std::abs(i12[idx]); + + if (atomic) { + AtomicAdd(countQ[faceQ], inclusion); + const Halfedge half = halfedges[edgeP]; + AtomicAdd(countP[edgeP / 3], inclusion); + AtomicAdd(countP[half.pairedHalfedge / 3], inclusion); + } else { + countQ[faceQ] += inclusion; + const Halfedge half = halfedges[edgeP]; + countP[edgeP / 3] += inclusion; + countP[half.pairedHalfedge / 3] += inclusion; + } + } +}; + +std::tuple, Vec> SizeOutput( + Manifold::Impl &outR, const Manifold::Impl &inP, const Manifold::Impl &inQ, + const Vec &i03, const Vec &i30, const Vec &i12, + const Vec &i21, const SparseIndices &p1q2, const SparseIndices &p2q1, + bool invertQ) { + ZoneScoped; + Vec sidesPerFacePQ(inP.NumTri() + inQ.NumTri(), 0); + // note: numFaceR <= facePQ2R.size() = sidesPerFacePQ.size() + 1 + + auto sidesPerFaceP = sidesPerFacePQ.view(0, inP.NumTri()); + auto sidesPerFaceQ = sidesPerFacePQ.view(inP.NumTri(), inQ.NumTri()); + + if (inP.halfedge_.size() >= 1e5) { + for_each(ExecutionPolicy::Par, countAt(0_uz), countAt(inP.halfedge_.size()), + CountVerts({inP.halfedge_, sidesPerFaceP, i03})); + for_each(ExecutionPolicy::Par, countAt(0_uz), countAt(inQ.halfedge_.size()), + CountVerts({inQ.halfedge_, sidesPerFaceQ, i30})); + } else { + for_each(ExecutionPolicy::Seq, countAt(0_uz), countAt(inP.halfedge_.size()), + CountVerts({inP.halfedge_, sidesPerFaceP, i03})); + for_each(ExecutionPolicy::Seq, countAt(0_uz), countAt(inQ.halfedge_.size()), + CountVerts({inQ.halfedge_, sidesPerFaceQ, i30})); + } + + if (i12.size() >= 1e5) { + for_each_n(ExecutionPolicy::Par, countAt(0), i12.size(), + CountNewVerts( + {sidesPerFaceP, sidesPerFaceQ, i12, p1q2, inP.halfedge_})); + for_each_n(ExecutionPolicy::Par, countAt(0), i21.size(), + CountNewVerts( + {sidesPerFaceQ, sidesPerFaceP, i21, p2q1, inQ.halfedge_})); + } else { + for_each_n(ExecutionPolicy::Seq, countAt(0), i12.size(), + CountNewVerts( + {sidesPerFaceP, sidesPerFaceQ, i12, p1q2, inP.halfedge_})); + for_each_n(ExecutionPolicy::Seq, countAt(0), i21.size(), + CountNewVerts( + {sidesPerFaceQ, sidesPerFaceP, i21, p2q1, inQ.halfedge_})); + } + + Vec facePQ2R(inP.NumTri() + inQ.NumTri() + 1, 0); + auto keepFace = TransformIterator(sidesPerFacePQ.begin(), + [](int x) { return x > 0 ? 1 : 0; }); + + inclusive_scan(keepFace, keepFace + sidesPerFacePQ.size(), + facePQ2R.begin() + 1); + int numFaceR = facePQ2R.back(); + facePQ2R.resize(inP.NumTri() + inQ.NumTri()); + + outR.faceNormal_.resize(numFaceR); + + Vec tmpBuffer(outR.faceNormal_.size()); + auto faceIds = TransformIterator(countAt(0_uz), [&sidesPerFacePQ](size_t i) { + if (sidesPerFacePQ[i] > 0) return i; + return std::numeric_limits::max(); + }); + + auto next = + copy_if(faceIds, faceIds + inP.faceNormal_.size(), tmpBuffer.begin(), + [](size_t v) { return v != std::numeric_limits::max(); }); + + gather(tmpBuffer.begin(), next, inP.faceNormal_.begin(), + outR.faceNormal_.begin()); + + auto faceIdsQ = + TransformIterator(countAt(0_uz), [&sidesPerFacePQ, &inP](size_t i) { + if (sidesPerFacePQ[i + inP.faceNormal_.size()] > 0) return i; + return std::numeric_limits::max(); + }); + auto end = + copy_if(faceIdsQ, faceIdsQ + inQ.faceNormal_.size(), next, + [](size_t v) { return v != std::numeric_limits::max(); }); + + if (invertQ) { + gather(next, end, + TransformIterator(inQ.faceNormal_.begin(), Negate()), + outR.faceNormal_.begin() + std::distance(tmpBuffer.begin(), next)); + } else { + gather(next, end, inQ.faceNormal_.begin(), + outR.faceNormal_.begin() + std::distance(tmpBuffer.begin(), next)); + } + + auto newEnd = remove(sidesPerFacePQ.begin(), sidesPerFacePQ.end(), 0); + Vec faceEdge(newEnd - sidesPerFacePQ.begin() + 1, 0); + inclusive_scan(sidesPerFacePQ.begin(), newEnd, faceEdge.begin() + 1); + outR.halfedge_.resize(faceEdge.back()); + + return std::make_tuple(faceEdge, facePQ2R); +} + +struct EdgePos { + int vert; + double edgePos; + bool isStart; +}; + +// thread sanitizer doesn't really know how to check when there are too many +// mutex +#if defined(__has_feature) +#if __has_feature(thread_sanitizer) +__attribute__((no_sanitize("thread"))) +#endif +#endif +void AddNewEdgeVerts( + // we need concurrent_map because we will be adding things concurrently + concurrent_map> &edgesP, + concurrent_map, std::vector> &edgesNew, + const SparseIndices &p1q2, const Vec &i12, const Vec &v12R, + const Vec &halfedgeP, bool forward) { + ZoneScoped; + // For each edge of P that intersects a face of Q (p1q2), add this vertex to + // P's corresponding edge vector and to the two new edges, which are + // intersections between the face of Q and the two faces of P attached to the + // edge. The direction and duplicity are given by i12, while v12R remaps to + // the output vert index. When forward is false, all is reversed. + auto process = [&](std::function lock, + std::function unlock, size_t i) { + const int edgeP = p1q2.Get(i, !forward); + const int faceQ = p1q2.Get(i, forward); + const int vert = v12R[i]; + const int inclusion = i12[i]; + + Halfedge halfedge = halfedgeP[edgeP]; + std::pair keyRight = {halfedge.pairedHalfedge / 3, faceQ}; + if (!forward) std::swap(keyRight.first, keyRight.second); + + std::pair keyLeft = {edgeP / 3, faceQ}; + if (!forward) std::swap(keyLeft.first, keyLeft.second); + + bool direction = inclusion < 0; + std::hash> pairHasher; + std::array *>, 3> edges = { + std::make_tuple(direction, std::hash{}(edgeP), &edgesP[edgeP]), + std::make_tuple(direction ^ !forward, // revert if not forward + pairHasher(keyRight), &edgesNew[keyRight]), + std::make_tuple(direction ^ forward, // revert if forward + pairHasher(keyLeft), &edgesNew[keyLeft])}; + for (const auto &tuple : edges) { + lock(std::get<1>(tuple)); + for (int j = 0; j < std::abs(inclusion); ++j) + std::get<2>(tuple)->push_back({vert + j, 0.0, std::get<0>(tuple)}); + unlock(std::get<1>(tuple)); + direction = !direction; + } + }; +#if (MANIFOLD_PAR == 1) && __has_include() + // parallelize operations, requires concurrent_map so we can only enable this + // with tbb + if (p1q2.size() > kParallelThreshold) { + // ideally we should have 1 mutex per key, but kParallelThreshold is enough + // to avoid contention for most of the cases + std::array mutexes; + static tbb::affinity_partitioner ap; + auto processFun = std::bind( + process, [&](size_t hash) { mutexes[hash % mutexes.size()].lock(); }, + [&](size_t hash) { mutexes[hash % mutexes.size()].unlock(); }, + std::placeholders::_1); + tbb::parallel_for( + tbb::blocked_range(0_uz, p1q2.size(), 32), + [&](const tbb::blocked_range &range) { + for (size_t i = range.begin(); i != range.end(); i++) processFun(i); + }, + ap); + return; + } +#endif + auto processFun = std::bind( + process, [](size_t _) {}, [](size_t _) {}, std::placeholders::_1); + for (size_t i = 0; i < p1q2.size(); ++i) processFun(i); +} + +std::vector PairUp(std::vector &edgePos) { + // Pair start vertices with end vertices to form edges. The choice of pairing + // is arbitrary for the manifoldness guarantee, but must be ordered to be + // geometrically valid. If the order does not go start-end-start-end... then + // the input and output are not geometrically valid and this algorithm becomes + // a heuristic. + DEBUG_ASSERT(edgePos.size() % 2 == 0, topologyErr, + "Non-manifold edge! Not an even number of points."); + size_t nEdges = edgePos.size() / 2; + auto middle = std::partition(edgePos.begin(), edgePos.end(), + [](EdgePos x) { return x.isStart; }); + DEBUG_ASSERT(static_cast(middle - edgePos.begin()) == nEdges, + topologyErr, "Non-manifold edge!"); + auto cmp = [](EdgePos a, EdgePos b) { return a.edgePos < b.edgePos; }; + std::stable_sort(edgePos.begin(), middle, cmp); + std::stable_sort(middle, edgePos.end(), cmp); + std::vector edges; + for (size_t i = 0; i < nEdges; ++i) + edges.push_back({edgePos[i].vert, edgePos[i + nEdges].vert, -1}); + return edges; +} + +void AppendPartialEdges(Manifold::Impl &outR, Vec &wholeHalfedgeP, + Vec &facePtrR, + concurrent_map> &edgesP, + Vec &halfedgeRef, const Manifold::Impl &inP, + const Vec &i03, const Vec &vP2R, + const Vec::IterC faceP2R, bool forward) { + ZoneScoped; + // Each edge in the map is partially retained; for each of these, look up + // their original verts and include them based on their winding number (i03), + // while remapping them to the output using vP2R. Use the verts position + // projected along the edge vector to pair them up, then distribute these + // edges to their faces. + Vec &halfedgeR = outR.halfedge_; + const Vec &vertPosP = inP.vertPos_; + const Vec &halfedgeP = inP.halfedge_; + + for (auto &value : edgesP) { + const int edgeP = value.first; + std::vector &edgePosP = value.second; + + const Halfedge &halfedge = halfedgeP[edgeP]; + wholeHalfedgeP[edgeP] = false; + wholeHalfedgeP[halfedge.pairedHalfedge] = false; + + const int vStart = halfedge.startVert; + const int vEnd = halfedge.endVert; + const vec3 edgeVec = vertPosP[vEnd] - vertPosP[vStart]; + // Fill in the edge positions of the old points. + for (EdgePos &edge : edgePosP) { + edge.edgePos = la::dot(outR.vertPos_[edge.vert], edgeVec); + } + + int inclusion = i03[vStart]; + EdgePos edgePos = {vP2R[vStart], + la::dot(outR.vertPos_[vP2R[vStart]], edgeVec), + inclusion > 0}; + for (int j = 0; j < std::abs(inclusion); ++j) { + edgePosP.push_back(edgePos); + ++edgePos.vert; + } + + inclusion = i03[vEnd]; + edgePos = {vP2R[vEnd], la::dot(outR.vertPos_[vP2R[vEnd]], edgeVec), + inclusion < 0}; + for (int j = 0; j < std::abs(inclusion); ++j) { + edgePosP.push_back(edgePos); + ++edgePos.vert; + } + + // sort edges into start/end pairs along length + std::vector edges = PairUp(edgePosP); + + // add halfedges to result + const int faceLeftP = edgeP / 3; + const int faceLeft = faceP2R[faceLeftP]; + const int faceRightP = halfedge.pairedHalfedge / 3; + const int faceRight = faceP2R[faceRightP]; + // Negative inclusion means the halfedges are reversed, which means our + // reference is now to the endVert instead of the startVert, which is one + // position advanced CCW. This is only valid if this is a retained vert; it + // will be ignored later if the vert is new. + const TriRef forwardRef = {forward ? 0 : 1, -1, faceLeftP}; + const TriRef backwardRef = {forward ? 0 : 1, -1, faceRightP}; + + for (Halfedge e : edges) { + const int forwardEdge = facePtrR[faceLeft]++; + const int backwardEdge = facePtrR[faceRight]++; + + e.pairedHalfedge = backwardEdge; + halfedgeR[forwardEdge] = e; + halfedgeRef[forwardEdge] = forwardRef; + + std::swap(e.startVert, e.endVert); + e.pairedHalfedge = forwardEdge; + halfedgeR[backwardEdge] = e; + halfedgeRef[backwardEdge] = backwardRef; + } + } +} + +void AppendNewEdges( + Manifold::Impl &outR, Vec &facePtrR, + concurrent_map, std::vector> &edgesNew, + Vec &halfedgeRef, const Vec &facePQ2R, const int numFaceP) { + ZoneScoped; + // Pair up each edge's verts and distribute to faces based on indices in key. + Vec &halfedgeR = outR.halfedge_; + Vec &vertPosR = outR.vertPos_; + + for (auto &value : edgesNew) { + const int faceP = value.first.first; + const int faceQ = value.first.second; + std::vector &edgePos = value.second; + + Box bbox; + for (auto edge : edgePos) { + bbox.Union(vertPosR[edge.vert]); + } + const vec3 size = bbox.Size(); + // Order the points along their longest dimension. + const int i = (size.x > size.y && size.x > size.z) ? 0 + : size.y > size.z ? 1 + : 2; + for (auto &edge : edgePos) { + edge.edgePos = vertPosR[edge.vert][i]; + } + + // sort edges into start/end pairs along length. + std::vector edges = PairUp(edgePos); + + // add halfedges to result + const int faceLeft = facePQ2R[faceP]; + const int faceRight = facePQ2R[numFaceP + faceQ]; + const TriRef forwardRef = {0, -1, faceP}; + const TriRef backwardRef = {1, -1, faceQ}; + for (Halfedge e : edges) { + const int forwardEdge = facePtrR[faceLeft]++; + const int backwardEdge = facePtrR[faceRight]++; + + e.pairedHalfedge = backwardEdge; + halfedgeR[forwardEdge] = e; + halfedgeRef[forwardEdge] = forwardRef; + + std::swap(e.startVert, e.endVert); + e.pairedHalfedge = forwardEdge; + halfedgeR[backwardEdge] = e; + halfedgeRef[backwardEdge] = backwardRef; + } + } +} + +struct DuplicateHalfedges { + VecView halfedgesR; + VecView halfedgeRef; + VecView facePtr; + VecView wholeHalfedgeP; + VecView halfedgesP; + VecView i03; + VecView vP2R; + VecView faceP2R; + const bool forward; + + void operator()(const int idx) { + if (!wholeHalfedgeP[idx]) return; + Halfedge halfedge = halfedgesP[idx]; + if (!halfedge.IsForward()) return; + + const int inclusion = i03[halfedge.startVert]; + if (inclusion == 0) return; + if (inclusion < 0) { // reverse + std::swap(halfedge.startVert, halfedge.endVert); + } + halfedge.startVert = vP2R[halfedge.startVert]; + halfedge.endVert = vP2R[halfedge.endVert]; + const int faceLeftP = idx / 3; + const int newFace = faceP2R[faceLeftP]; + const int faceRightP = halfedge.pairedHalfedge / 3; + const int faceRight = faceP2R[faceRightP]; + // Negative inclusion means the halfedges are reversed, which means our + // reference is now to the endVert instead of the startVert, which is one + // position advanced CCW. + const TriRef forwardRef = {forward ? 0 : 1, -1, faceLeftP}; + const TriRef backwardRef = {forward ? 0 : 1, -1, faceRightP}; + + for (int i = 0; i < std::abs(inclusion); ++i) { + int forwardEdge = AtomicAdd(facePtr[newFace], 1); + int backwardEdge = AtomicAdd(facePtr[faceRight], 1); + halfedge.pairedHalfedge = backwardEdge; + + halfedgesR[forwardEdge] = halfedge; + halfedgesR[backwardEdge] = {halfedge.endVert, halfedge.startVert, + forwardEdge}; + halfedgeRef[forwardEdge] = forwardRef; + halfedgeRef[backwardEdge] = backwardRef; + + ++halfedge.startVert; + ++halfedge.endVert; + } + } +}; + +void AppendWholeEdges(Manifold::Impl &outR, Vec &facePtrR, + Vec &halfedgeRef, const Manifold::Impl &inP, + const Vec wholeHalfedgeP, const Vec &i03, + const Vec &vP2R, VecView faceP2R, + bool forward) { + ZoneScoped; + for_each_n( + autoPolicy(inP.halfedge_.size()), countAt(0), inP.halfedge_.size(), + DuplicateHalfedges({outR.halfedge_, halfedgeRef, facePtrR, wholeHalfedgeP, + inP.halfedge_, i03, vP2R, faceP2R, forward})); +} + +struct MapTriRef { + VecView triRefP; + VecView triRefQ; + const int offsetQ; + + void operator()(TriRef &triRef) { + const int tri = triRef.tri; + const bool PQ = triRef.meshID == 0; + triRef = PQ ? triRefP[tri] : triRefQ[tri]; + if (!PQ) triRef.meshID += offsetQ; + } +}; + +void UpdateReference(Manifold::Impl &outR, const Manifold::Impl &inP, + const Manifold::Impl &inQ, bool invertQ) { + const int offsetQ = Manifold::Impl::meshIDCounter_; + for_each_n( + autoPolicy(outR.NumTri(), 1e5), outR.meshRelation_.triRef.begin(), + outR.NumTri(), + MapTriRef({inP.meshRelation_.triRef, inQ.meshRelation_.triRef, offsetQ})); + + for (const auto &pair : inP.meshRelation_.meshIDtransform) { + outR.meshRelation_.meshIDtransform[pair.first] = pair.second; + } + for (const auto &pair : inQ.meshRelation_.meshIDtransform) { + outR.meshRelation_.meshIDtransform[pair.first + offsetQ] = pair.second; + outR.meshRelation_.meshIDtransform[pair.first + offsetQ].backSide ^= + invertQ; + } +} + +struct Barycentric { + VecView uvw; + VecView ref; + VecView vertPosP; + VecView vertPosQ; + VecView vertPosR; + VecView halfedgeP; + VecView halfedgeQ; + VecView halfedgeR; + const double epsilon; + + void operator()(const int tri) { + const TriRef refPQ = ref[tri]; + if (halfedgeR[3 * tri].startVert < 0) return; + + const int triPQ = refPQ.tri; + const bool PQ = refPQ.meshID == 0; + const auto &vertPos = PQ ? vertPosP : vertPosQ; + const auto &halfedge = PQ ? halfedgeP : halfedgeQ; + + mat3 triPos; + for (const int j : {0, 1, 2}) + triPos[j] = vertPos[halfedge[3 * triPQ + j].startVert]; + + for (const int i : {0, 1, 2}) { + const int vert = halfedgeR[3 * tri + i].startVert; + uvw[3 * tri + i] = GetBarycentric(vertPosR[vert], triPos, epsilon); + } + } +}; + +void CreateProperties(Manifold::Impl &outR, const Manifold::Impl &inP, + const Manifold::Impl &inQ) { + ZoneScoped; + const int numPropP = inP.NumProp(); + const int numPropQ = inQ.NumProp(); + const int numProp = std::max(numPropP, numPropQ); + outR.meshRelation_.numProp = numProp; + if (numProp == 0) return; + + const int numTri = outR.NumTri(); + outR.meshRelation_.triProperties.resize(numTri); + + Vec bary(outR.halfedge_.size()); + for_each_n(autoPolicy(numTri, 1e4), countAt(0), numTri, + Barycentric({bary, outR.meshRelation_.triRef, inP.vertPos_, + inQ.vertPos_, outR.vertPos_, inP.halfedge_, + inQ.halfedge_, outR.halfedge_, outR.epsilon_})); + + using Entry = std::pair; + int idMissProp = outR.NumVert(); + std::vector> propIdx(outR.NumVert() + 1); + std::vector propMissIdx[2]; + propMissIdx[0].resize(inQ.NumPropVert(), -1); + propMissIdx[1].resize(inP.NumPropVert(), -1); + + outR.meshRelation_.properties.reserve(outR.NumVert() * numProp); + int idx = 0; + + for (int tri = 0; tri < numTri; ++tri) { + // Skip collapsed triangles + if (outR.halfedge_[3 * tri].startVert < 0) continue; + + const TriRef ref = outR.meshRelation_.triRef[tri]; + const bool PQ = ref.meshID == 0; + const int oldNumProp = PQ ? numPropP : numPropQ; + const auto &properties = + PQ ? inP.meshRelation_.properties : inQ.meshRelation_.properties; + const ivec3 &triProp = oldNumProp == 0 ? ivec3(-1) + : PQ ? inP.meshRelation_.triProperties[ref.tri] + : inQ.meshRelation_.triProperties[ref.tri]; + + for (const int i : {0, 1, 2}) { + const int vert = outR.halfedge_[3 * tri + i].startVert; + const vec3 &uvw = bary[3 * tri + i]; + + ivec4 key(PQ, idMissProp, -1, -1); + if (oldNumProp > 0) { + int edge = -2; + for (const int j : {0, 1, 2}) { + if (uvw[j] == 1) { + // On a retained vert, the propVert must also match + key[2] = triProp[j]; + edge = -1; + break; + } + if (uvw[j] == 0) edge = j; + } + if (edge >= 0) { + // On an edge, both propVerts must match + const int p0 = triProp[Next3(edge)]; + const int p1 = triProp[Prev3(edge)]; + key[1] = vert; + key[2] = std::min(p0, p1); + key[3] = std::max(p0, p1); + } else if (edge == -2) { + key[1] = vert; + } + } + + if (key.y == idMissProp && key.z >= 0) { + // only key.x/key.z matters + auto &entry = propMissIdx[key.x][key.z]; + if (entry >= 0) { + outR.meshRelation_.triProperties[tri][i] = entry; + continue; + } + entry = idx; + } else { + auto &bin = propIdx[key.y]; + bool bFound = false; + for (const auto &b : bin) { + if (b.first == ivec3(key.x, key.z, key.w)) { + bFound = true; + outR.meshRelation_.triProperties[tri][i] = b.second; + break; + } + } + if (bFound) continue; + bin.push_back(std::make_pair(ivec3(key.x, key.z, key.w), idx)); + } + + outR.meshRelation_.triProperties[tri][i] = idx++; + for (int p = 0; p < numProp; ++p) { + if (p < oldNumProp) { + vec3 oldProps; + for (const int j : {0, 1, 2}) + oldProps[j] = properties[oldNumProp * triProp[j] + p]; + outR.meshRelation_.properties.push_back(la::dot(uvw, oldProps)); + } else { + outR.meshRelation_.properties.push_back(0); + } + } + } + } +} +} // namespace + +namespace manifold { + +Manifold::Impl Boolean3::Result(OpType op) const { +#ifdef MANIFOLD_DEBUG + Timer assemble; + assemble.Start(); +#endif + + DEBUG_ASSERT((expandP_ > 0) == (op == OpType::Add), logicErr, + "Result op type not compatible with constructor op type."); + const int c1 = op == OpType::Intersect ? 0 : 1; + const int c2 = op == OpType::Add ? 1 : 0; + const int c3 = op == OpType::Intersect ? 1 : -1; + + if (inP_.status_ != Manifold::Error::NoError) { + auto impl = Manifold::Impl(); + impl.status_ = inP_.status_; + return impl; + } + if (inQ_.status_ != Manifold::Error::NoError) { + auto impl = Manifold::Impl(); + impl.status_ = inQ_.status_; + return impl; + } + + if (inP_.IsEmpty()) { + if (!inQ_.IsEmpty() && op == OpType::Add) { + return inQ_; + } + return Manifold::Impl(); + } else if (inQ_.IsEmpty()) { + if (op == OpType::Intersect) { + return Manifold::Impl(); + } + return inP_; + } + + const bool invertQ = op == OpType::Subtract; + + // Convert winding numbers to inclusion values based on operation type. + Vec i12(x12_.size()); + Vec i21(x21_.size()); + Vec i03(w03_.size()); + Vec i30(w30_.size()); + + transform(x12_.begin(), x12_.end(), i12.begin(), + [c3](int v) { return c3 * v; }); + transform(x21_.begin(), x21_.end(), i21.begin(), + [c3](int v) { return c3 * v; }); + transform(w03_.begin(), w03_.end(), i03.begin(), + [c1, c3](int v) { return c1 + c3 * v; }); + transform(w30_.begin(), w30_.end(), i30.begin(), + [c2, c3](int v) { return c2 + c3 * v; }); + + Vec vP2R(inP_.NumVert()); + exclusive_scan(i03.begin(), i03.end(), vP2R.begin(), 0, AbsSum()); + int numVertR = AbsSum()(vP2R.back(), i03.back()); + const int nPv = numVertR; + + Vec vQ2R(inQ_.NumVert()); + exclusive_scan(i30.begin(), i30.end(), vQ2R.begin(), numVertR, AbsSum()); + numVertR = AbsSum()(vQ2R.back(), i30.back()); + const int nQv = numVertR - nPv; + + Vec v12R(v12_.size()); + if (v12_.size() > 0) { + exclusive_scan(i12.begin(), i12.end(), v12R.begin(), numVertR, AbsSum()); + numVertR = AbsSum()(v12R.back(), i12.back()); + } + const int n12 = numVertR - nPv - nQv; + + Vec v21R(v21_.size()); + if (v21_.size() > 0) { + exclusive_scan(i21.begin(), i21.end(), v21R.begin(), numVertR, AbsSum()); + numVertR = AbsSum()(v21R.back(), i21.back()); + } + const int n21 = numVertR - nPv - nQv - n12; + + // Create the output Manifold + Manifold::Impl outR; + + if (numVertR == 0) return outR; + + outR.epsilon_ = std::max(inP_.epsilon_, inQ_.epsilon_); + outR.tolerance_ = std::max(inP_.tolerance_, inQ_.tolerance_); + + outR.vertPos_.resize(numVertR); + // Add vertices, duplicating for inclusion numbers not in [-1, 1]. + // Retained vertices from P and Q: + for_each_n(autoPolicy(inP_.NumVert(), 1e4), countAt(0), inP_.NumVert(), + DuplicateVerts({outR.vertPos_, i03, vP2R, inP_.vertPos_})); + for_each_n(autoPolicy(inQ_.NumVert(), 1e4), countAt(0), inQ_.NumVert(), + DuplicateVerts({outR.vertPos_, i30, vQ2R, inQ_.vertPos_})); + // New vertices created from intersections: + for_each_n(autoPolicy(i12.size(), 1e4), countAt(0), i12.size(), + DuplicateVerts({outR.vertPos_, i12, v12R, v12_})); + for_each_n(autoPolicy(i21.size(), 1e4), countAt(0), i21.size(), + DuplicateVerts({outR.vertPos_, i21, v21R, v21_})); + + PRINT(nPv << " verts from inP"); + PRINT(nQv << " verts from inQ"); + PRINT(n12 << " new verts from edgesP -> facesQ"); + PRINT(n21 << " new verts from facesP -> edgesQ"); + + // Build up new polygonal faces from triangle intersections. At this point the + // calculation switches from parallel to serial. + + // Level 3 + + // This key is the forward halfedge index of P or Q. Only includes intersected + // edges. + concurrent_map> edgesP, edgesQ; + // This key is the face index of + concurrent_map, std::vector> edgesNew; + + AddNewEdgeVerts(edgesP, edgesNew, p1q2_, i12, v12R, inP_.halfedge_, true); + AddNewEdgeVerts(edgesQ, edgesNew, p2q1_, i21, v21R, inQ_.halfedge_, false); + + v12R.clear(); + v21R.clear(); + + // Level 4 + Vec faceEdge; + Vec facePQ2R; + std::tie(faceEdge, facePQ2R) = + SizeOutput(outR, inP_, inQ_, i03, i30, i12, i21, p1q2_, p2q1_, invertQ); + + i12.clear(); + i21.clear(); + + // This gets incremented for each halfedge that's added to a face so that the + // next one knows where to slot in. + Vec facePtrR = faceEdge; + // Intersected halfedges are marked false. + Vec wholeHalfedgeP(inP_.halfedge_.size(), true); + Vec wholeHalfedgeQ(inQ_.halfedge_.size(), true); + // The halfedgeRef contains the data that will become triRef once the faces + // are triangulated. + Vec halfedgeRef(2 * outR.NumEdge()); + + AppendPartialEdges(outR, wholeHalfedgeP, facePtrR, edgesP, halfedgeRef, inP_, + i03, vP2R, facePQ2R.begin(), true); + AppendPartialEdges(outR, wholeHalfedgeQ, facePtrR, edgesQ, halfedgeRef, inQ_, + i30, vQ2R, facePQ2R.begin() + inP_.NumTri(), false); + + edgesP.clear(); + edgesQ.clear(); + + AppendNewEdges(outR, facePtrR, edgesNew, halfedgeRef, facePQ2R, + inP_.NumTri()); + + edgesNew.clear(); + + AppendWholeEdges(outR, facePtrR, halfedgeRef, inP_, wholeHalfedgeP, i03, vP2R, + facePQ2R.cview(0, inP_.NumTri()), true); + AppendWholeEdges(outR, facePtrR, halfedgeRef, inQ_, wholeHalfedgeQ, i30, vQ2R, + facePQ2R.cview(inP_.NumTri(), inQ_.NumTri()), false); + + wholeHalfedgeP.clear(); + wholeHalfedgeQ.clear(); + facePtrR.clear(); + facePQ2R.clear(); + i03.clear(); + i30.clear(); + vP2R.clear(); + vQ2R.clear(); + +#ifdef MANIFOLD_DEBUG + assemble.Stop(); + Timer triangulate; + triangulate.Start(); +#endif + + // Level 6 + + if (ManifoldParams().intermediateChecks) + DEBUG_ASSERT(outR.IsManifold(), logicErr, "polygon mesh is not manifold!"); + + outR.Face2Tri(faceEdge, halfedgeRef); + halfedgeRef.clear(); + faceEdge.clear(); + +#ifdef MANIFOLD_DEBUG + triangulate.Stop(); + Timer simplify; + simplify.Start(); +#endif + + if (ManifoldParams().intermediateChecks) + DEBUG_ASSERT(outR.IsManifold(), logicErr, + "triangulated mesh is not manifold!"); + + CreateProperties(outR, inP_, inQ_); + + UpdateReference(outR, inP_, inQ_, invertQ); + + outR.SimplifyTopology(); + + if (ManifoldParams().intermediateChecks) + DEBUG_ASSERT(outR.Is2Manifold(), logicErr, + "simplified mesh is not 2-manifold!"); + +#ifdef MANIFOLD_DEBUG + simplify.Stop(); + Timer sort; + sort.Start(); +#endif + + outR.Finish(); + outR.IncrementMeshIDs(); + +#ifdef MANIFOLD_DEBUG + sort.Stop(); + if (ManifoldParams().verbose) { + assemble.Print("Assembly"); + triangulate.Print("Triangulation"); + simplify.Print("Simplification"); + sort.Print("Sorting"); + std::cout << outR.NumVert() << " verts and " << outR.NumTri() << " tris" + << std::endl; + } +#endif + + return outR; +} + +} // namespace manifold diff --git a/thirdparty/manifold/src/collider.h b/thirdparty/manifold/src/collider.h new file mode 100644 index 000000000000..80de94b7c2e9 --- /dev/null +++ b/thirdparty/manifold/src/collider.h @@ -0,0 +1,382 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include "./parallel.h" +#include "./sparse.h" +#include "./utils.h" +#include "./vec.h" +#include "manifold/common.h" + +#ifdef _MSC_VER +#include +#endif + +#if (MANIFOLD_PAR == 1) +#include +#endif + +namespace manifold { + +namespace collider_internal { +// Adjustable parameters +constexpr int kInitialLength = 128; +constexpr int kLengthMultiple = 4; +constexpr int kSequentialThreshold = 512; +// Fundamental constants +constexpr int kRoot = 1; + +#ifdef _MSC_VER + +#ifndef _WINDEF_ +typedef unsigned long DWORD; +#endif + +uint32_t inline ctz(uint32_t value) { + DWORD trailing_zero = 0; + + if (_BitScanForward(&trailing_zero, value)) { + return trailing_zero; + } else { + // This is undefined, I better choose 32 than 0 + return 32; + } +} + +uint32_t inline clz(uint32_t value) { + DWORD leading_zero = 0; + + if (_BitScanReverse(&leading_zero, value)) { + return 31 - leading_zero; + } else { + // Same remarks as above + return 32; + } +} +#endif + +constexpr inline bool IsLeaf(int node) { return node % 2 == 0; } +constexpr inline bool IsInternal(int node) { return node % 2 == 1; } +constexpr inline int Node2Internal(int node) { return (node - 1) / 2; } +constexpr inline int Internal2Node(int internal) { return internal * 2 + 1; } +constexpr inline int Node2Leaf(int node) { return node / 2; } +constexpr inline int Leaf2Node(int leaf) { return leaf * 2; } + +struct CreateRadixTree { + VecView nodeParent_; + VecView> internalChildren_; + const VecView leafMorton_; + + int PrefixLength(uint32_t a, uint32_t b) const { +// count-leading-zeros is used to find the number of identical highest-order +// bits +#ifdef _MSC_VER + // return __lzcnt(a ^ b); + return clz(a ^ b); +#else + return __builtin_clz(a ^ b); +#endif + } + + int PrefixLength(int i, int j) const { + if (j < 0 || j >= static_cast(leafMorton_.size())) { + return -1; + } else { + int out; + if (leafMorton_[i] == leafMorton_[j]) + // use index to disambiguate + out = 32 + + PrefixLength(static_cast(i), static_cast(j)); + else + out = PrefixLength(leafMorton_[i], leafMorton_[j]); + return out; + } + } + + int RangeEnd(int i) const { + // Determine direction of range (+1 or -1) + int dir = PrefixLength(i, i + 1) - PrefixLength(i, i - 1); + dir = (dir > 0) - (dir < 0); + // Compute conservative range length with exponential increase + int commonPrefix = PrefixLength(i, i - dir); + int max_length = kInitialLength; + while (PrefixLength(i, i + dir * max_length) > commonPrefix) + max_length *= kLengthMultiple; + // Compute precise range length with binary search + int length = 0; + for (int step = max_length / 2; step > 0; step /= 2) { + if (PrefixLength(i, i + dir * (length + step)) > commonPrefix) + length += step; + } + return i + dir * length; + } + + int FindSplit(int first, int last) const { + int commonPrefix = PrefixLength(first, last); + // Find the furthest object that shares more than commonPrefix bits with the + // first one, using binary search. + int split = first; + int step = last - first; + do { + step = (step + 1) >> 1; // divide by 2, rounding up + int newSplit = split + step; + if (newSplit < last) { + int splitPrefix = PrefixLength(first, newSplit); + if (splitPrefix > commonPrefix) split = newSplit; + } + } while (step > 1); + return split; + } + + void operator()(int internal) { + int first = internal; + // Find the range of objects with a common prefix + int last = RangeEnd(first); + if (first > last) std::swap(first, last); + // Determine where the next-highest difference occurs + int split = FindSplit(first, last); + int child1 = split == first ? Leaf2Node(split) : Internal2Node(split); + ++split; + int child2 = split == last ? Leaf2Node(split) : Internal2Node(split); + // Record parent_child relationships. + internalChildren_[internal].first = child1; + internalChildren_[internal].second = child2; + int node = Internal2Node(internal); + nodeParent_[child1] = node; + nodeParent_[child2] = node; + } +}; + +template +struct FindCollision { + VecView queries; + VecView nodeBBox_; + VecView> internalChildren_; + Recorder recorder; + + inline int RecordCollision(int node, const int queryIdx, SparseIndices& ind) { + bool overlaps = nodeBBox_[node].DoesOverlap(queries[queryIdx]); + if (overlaps && IsLeaf(node)) { + int leafIdx = Node2Leaf(node); + if (!selfCollision || leafIdx != queryIdx) { + recorder.record(queryIdx, leafIdx, ind); + } + } + return overlaps && IsInternal(node); // Should traverse into node + } + + void operator()(const int queryIdx) { + // stack cannot overflow because radix tree has max depth 30 (Morton code) + + // 32 (index). + int stack[64]; + int top = -1; + // Depth-first search + int node = kRoot; + SparseIndices& ind = recorder.local(); + while (1) { + int internal = Node2Internal(node); + int child1 = internalChildren_[internal].first; + int child2 = internalChildren_[internal].second; + + int traverse1 = RecordCollision(child1, queryIdx, ind); + int traverse2 = RecordCollision(child2, queryIdx, ind); + + if (!traverse1 && !traverse2) { + if (top < 0) break; // done + node = stack[top--]; // get a saved node + } else { + node = traverse1 ? child1 : child2; // go here next + if (traverse1 && traverse2) { + stack[++top] = child2; // save the other for later + } + } + } + } +}; + +template +struct SeqCollisionRecorder { + SparseIndices& queryTri_; + inline void record(int queryIdx, int leafIdx, SparseIndices& ind) const { + if (inverted) + ind.Add(leafIdx, queryIdx); + else + ind.Add(queryIdx, leafIdx); + } + SparseIndices& local() { return queryTri_; } +}; + +#if (MANIFOLD_PAR == 1) +template +struct ParCollisionRecorder { + tbb::combinable& store; + inline void record(int queryIdx, int leafIdx, SparseIndices& ind) const { + // Add may invoke something in parallel, and it may return in + // another thread, making thread local unsafe + // we need to explicitly forbid parallelization by passing a flag + if (inverted) + ind.Add(leafIdx, queryIdx, true); + else + ind.Add(queryIdx, leafIdx, true); + } + SparseIndices& local() { return store.local(); } +}; +#endif + +struct BuildInternalBoxes { + VecView nodeBBox_; + VecView counter_; + const VecView nodeParent_; + const VecView> internalChildren_; + + void operator()(int leaf) { + int node = Leaf2Node(leaf); + do { + node = nodeParent_[node]; + int internal = Node2Internal(node); + if (AtomicAdd(counter_[internal], 1) == 0) return; + nodeBBox_[node] = nodeBBox_[internalChildren_[internal].first].Union( + nodeBBox_[internalChildren_[internal].second]); + } while (node != kRoot); + } +}; + +struct TransformBox { + const mat3x4 transform; + void operator()(Box& box) { box = box.Transform(transform); } +}; + +constexpr inline uint32_t SpreadBits3(uint32_t v) { + v = 0xFF0000FFu & (v * 0x00010001u); + v = 0x0F00F00Fu & (v * 0x00000101u); + v = 0xC30C30C3u & (v * 0x00000011u); + v = 0x49249249u & (v * 0x00000005u); + return v; +} +} // namespace collider_internal + +/** @ingroup Private */ +class Collider { + public: + Collider() {}; + + Collider(const VecView& leafBB, + const VecView& leafMorton) { + ZoneScoped; + DEBUG_ASSERT(leafBB.size() == leafMorton.size(), userErr, + "vectors must be the same length"); + int num_nodes = 2 * leafBB.size() - 1; + // assign and allocate members + nodeBBox_.resize(num_nodes); + nodeParent_.resize(num_nodes, -1); + internalChildren_.resize(leafBB.size() - 1, std::make_pair(-1, -1)); + // organize tree + for_each_n(autoPolicy(NumInternal(), 1e4), countAt(0), NumInternal(), + collider_internal::CreateRadixTree( + {nodeParent_, internalChildren_, leafMorton})); + UpdateBoxes(leafBB); + } + + bool Transform(mat3x4 transform) { + ZoneScoped; + bool axisAligned = true; + for (int row : {0, 1, 2}) { + int count = 0; + for (int col : {0, 1, 2}) { + if (transform[col][row] == 0.0) ++count; + } + if (count != 2) axisAligned = false; + } + if (axisAligned) { + for_each(autoPolicy(nodeBBox_.size(), 1e5), nodeBBox_.begin(), + nodeBBox_.end(), + [transform](Box& box) { box = box.Transform(transform); }); + } + return axisAligned; + } + + void UpdateBoxes(const VecView& leafBB) { + ZoneScoped; + DEBUG_ASSERT(leafBB.size() == NumLeaves(), userErr, + "must have the same number of updated boxes as original"); + // copy in leaf node Boxes + auto leaves = StridedRange(nodeBBox_.begin(), nodeBBox_.end(), 2); + copy(leafBB.cbegin(), leafBB.cend(), leaves.begin()); + // create global counters + Vec counter(NumInternal(), 0); + // kernel over leaves to save internal Boxes + for_each_n(autoPolicy(NumInternal(), 1e3), countAt(0), NumLeaves(), + collider_internal::BuildInternalBoxes( + {nodeBBox_, counter, nodeParent_, internalChildren_})); + } + + template + void Collisions(const VecView& queriesIn, + SparseIndices& queryTri) const { + ZoneScoped; + using collider_internal::FindCollision; +#if (MANIFOLD_PAR == 1) + if (queriesIn.size() > collider_internal::kSequentialThreshold) { + tbb::combinable store; + for_each_n( + ExecutionPolicy::Par, countAt(0), queriesIn.size(), + FindCollision>{ + queriesIn, nodeBBox_, internalChildren_, {store}}); + + std::vector tmp; + store.combine_each( + [&](SparseIndices& ind) { tmp.emplace_back(std::move(ind)); }); + queryTri.FromIndices(tmp); + return; + } +#endif + for_each_n(ExecutionPolicy::Seq, countAt(0), queriesIn.size(), + FindCollision>{ + queriesIn, nodeBBox_, internalChildren_, {queryTri}}); + } + + template + SparseIndices Collisions(const VecView& queriesIn) const { + SparseIndices result; + Collisions(queriesIn, result); + return result; + } + + static uint32_t MortonCode(vec3 position, Box bBox) { + using collider_internal::SpreadBits3; + vec3 xyz = (position - bBox.min) / (bBox.max - bBox.min); + xyz = la::min(vec3(1023.0), la::max(vec3(0.0), 1024.0 * xyz)); + uint32_t x = SpreadBits3(static_cast(xyz.x)); + uint32_t y = SpreadBits3(static_cast(xyz.y)); + uint32_t z = SpreadBits3(static_cast(xyz.z)); + return x * 4 + y * 2 + z; + } + + private: + Vec nodeBBox_; + Vec nodeParent_; + // even nodes are leaves, odd nodes are internal, root is 1 + Vec> internalChildren_; + + size_t NumInternal() const { return internalChildren_.size(); }; + size_t NumLeaves() const { + return internalChildren_.empty() ? 0 : (NumInternal() + 1); + }; +}; + +} // namespace manifold diff --git a/thirdparty/manifold/src/constructors.cpp b/thirdparty/manifold/src/constructors.cpp new file mode 100644 index 000000000000..2b3e528c6bbe --- /dev/null +++ b/thirdparty/manifold/src/constructors.cpp @@ -0,0 +1,503 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./csg_tree.h" +#include "./impl.h" +#include "./parallel.h" +#include "manifold/polygon.h" + +namespace manifold { +/** + * Constructs a smooth version of the input mesh by creating tangents; this + * method will throw if you have supplied tangents with your mesh already. The + * actual triangle resolution is unchanged; use the Refine() method to + * interpolate to a higher-resolution curve. + * + * By default, every edge is calculated for maximum smoothness (very much + * approximately), attempting to minimize the maximum mean Curvature magnitude. + * No higher-order derivatives are considered, as the interpolation is + * independent per triangle, only sharing constraints on their boundaries. + * + * @param meshGL input MeshGL. + * @param sharpenedEdges If desired, you can supply a vector of sharpened + * halfedges, which should in general be a small subset of all halfedges. Order + * of entries doesn't matter, as each one specifies the desired smoothness + * (between zero and one, with one the default for all unspecified halfedges) + * and the halfedge index (3 * triangle index + [0,1,2] where 0 is the edge + * between triVert 0 and 1, etc). + * + * At a smoothness value of zero, a sharp crease is made. The smoothness is + * interpolated along each edge, so the specified value should be thought of as + * an average. Where exactly two sharpened edges meet at a vertex, their + * tangents are rotated to be colinear so that the sharpened edge can be + * continuous. Vertices with only one sharpened edge are completely smooth, + * allowing sharpened edges to smoothly vanish at termination. A single vertex + * can be sharpened by sharping all edges that are incident on it, allowing + * cones to be formed. + */ +Manifold Manifold::Smooth(const MeshGL& meshGL, + const std::vector& sharpenedEdges) { + DEBUG_ASSERT(meshGL.halfedgeTangent.empty(), std::runtime_error, + "when supplying tangents, the normal constructor should be used " + "rather than Smooth()."); + + std::shared_ptr impl = std::make_shared(meshGL); + impl->CreateTangents(impl->UpdateSharpenedEdges(sharpenedEdges)); + return Manifold(impl); +} + +/** + * Constructs a smooth version of the input mesh by creating tangents; this + * method will throw if you have supplied tangents with your mesh already. The + * actual triangle resolution is unchanged; use the Refine() method to + * interpolate to a higher-resolution curve. + * + * By default, every edge is calculated for maximum smoothness (very much + * approximately), attempting to minimize the maximum mean Curvature magnitude. + * No higher-order derivatives are considered, as the interpolation is + * independent per triangle, only sharing constraints on their boundaries. + * + * @param meshGL64 input MeshGL64. + * @param sharpenedEdges If desired, you can supply a vector of sharpened + * halfedges, which should in general be a small subset of all halfedges. Order + * of entries doesn't matter, as each one specifies the desired smoothness + * (between zero and one, with one the default for all unspecified halfedges) + * and the halfedge index (3 * triangle index + [0,1,2] where 0 is the edge + * between triVert 0 and 1, etc). + * + * At a smoothness value of zero, a sharp crease is made. The smoothness is + * interpolated along each edge, so the specified value should be thought of as + * an average. Where exactly two sharpened edges meet at a vertex, their + * tangents are rotated to be colinear so that the sharpened edge can be + * continuous. Vertices with only one sharpened edge are completely smooth, + * allowing sharpened edges to smoothly vanish at termination. A single vertex + * can be sharpened by sharping all edges that are incident on it, allowing + * cones to be formed. + */ +Manifold Manifold::Smooth(const MeshGL64& meshGL64, + const std::vector& sharpenedEdges) { + DEBUG_ASSERT(meshGL64.halfedgeTangent.empty(), std::runtime_error, + "when supplying tangents, the normal constructor should be used " + "rather than Smooth()."); + + std::shared_ptr impl = std::make_shared(meshGL64); + impl->CreateTangents(impl->UpdateSharpenedEdges(sharpenedEdges)); + return Manifold(impl); +} + +/** + * Constructs a tetrahedron centered at the origin with one vertex at (1,1,1) + * and the rest at similarly symmetric points. + */ +Manifold Manifold::Tetrahedron() { + return Manifold(std::make_shared(Impl::Shape::Tetrahedron)); +} + +/** + * Constructs a unit cube (edge lengths all one), by default in the first + * octant, touching the origin. If any dimensions in size are negative, or if + * all are zero, an empty Manifold will be returned. + * + * @param size The X, Y, and Z dimensions of the box. + * @param center Set to true to shift the center to the origin. + */ +Manifold Manifold::Cube(vec3 size, bool center) { + if (size.x < 0.0 || size.y < 0.0 || size.z < 0.0 || la::length(size) == 0.) { + return Invalid(); + } + mat3x4 m({{size.x, 0.0, 0.0}, {0.0, size.y, 0.0}, {0.0, 0.0, size.z}}, + center ? (-size / 2.0) : vec3(0.0)); + return Manifold(std::make_shared(Manifold::Impl::Shape::Cube, m)); +} + +/** + * A convenience constructor for the common case of extruding a circle. Can also + * form cones if both radii are specified. + * + * @param height Z-extent + * @param radiusLow Radius of bottom circle. Must be positive. + * @param radiusHigh Radius of top circle. Can equal zero. Default is equal to + * radiusLow. + * @param circularSegments How many line segments to use around the circle. + * Default is calculated by the static Defaults. + * @param center Set to true to shift the center to the origin. Default is + * origin at the bottom. + */ +Manifold Manifold::Cylinder(double height, double radiusLow, double radiusHigh, + int circularSegments, bool center) { + if (height <= 0.0 || radiusLow <= 0.0) { + return Invalid(); + } + const double scale = radiusHigh >= 0.0 ? radiusHigh / radiusLow : 1.0; + const double radius = fmax(radiusLow, radiusHigh); + const int n = circularSegments > 2 ? circularSegments + : Quality::GetCircularSegments(radius); + + SimplePolygon circle(n); + const double dPhi = 360.0 / n; + for (int i = 0; i < n; ++i) { + circle[i] = {radiusLow * cosd(dPhi * i), radiusLow * sind(dPhi * i)}; + } + + Manifold cylinder = Manifold::Extrude({circle}, height, 0, 0.0, vec2(scale)); + if (center) + cylinder = cylinder.Translate(vec3(0.0, 0.0, -height / 2.0)).AsOriginal(); + return cylinder; +} + +/** + * Constructs a geodesic sphere of a given radius. + * + * @param radius Radius of the sphere. Must be positive. + * @param circularSegments Number of segments along its + * diameter. This number will always be rounded up to the nearest factor of + * four, as this sphere is constructed by refining an octahedron. This means + * there are a circle of vertices on all three of the axis planes. Default is + * calculated by the static Defaults. + */ +Manifold Manifold::Sphere(double radius, int circularSegments) { + if (radius <= 0.0) { + return Invalid(); + } + int n = circularSegments > 0 ? (circularSegments + 3) / 4 + : Quality::GetCircularSegments(radius) / 4; + auto pImpl_ = std::make_shared(Impl::Shape::Octahedron); + pImpl_->Subdivide( + [n](vec3 edge, vec4 tangentStart, vec4 tangentEnd) { return n - 1; }); + for_each_n(autoPolicy(pImpl_->NumVert(), 1e5), pImpl_->vertPos_.begin(), + pImpl_->NumVert(), [radius](vec3& v) { + v = la::cos(kHalfPi * (1.0 - v)); + v = radius * la::normalize(v); + if (std::isnan(v.x)) v = vec3(0.0); + }); + pImpl_->Finish(); + // Ignore preceding octahedron. + pImpl_->InitializeOriginal(); + return Manifold(pImpl_); +} + +/** + * Constructs a manifold from a set of polygons by extruding them along the + * Z-axis. + * Note that high twistDegrees with small nDivisions may cause + * self-intersection. This is not checked here and it is up to the user to + * choose the correct parameters. + * + * @param crossSection A set of non-overlapping polygons to extrude. + * @param height Z-extent of extrusion. + * @param nDivisions Number of extra copies of the crossSection to insert into + * the shape vertically; especially useful in combination with twistDegrees to + * avoid interpolation artifacts. Default is none. + * @param twistDegrees Amount to twist the top crossSection relative to the + * bottom, interpolated linearly for the divisions in between. + * @param scaleTop Amount to scale the top (independently in X and Y). If the + * scale is {0, 0}, a pure cone is formed with only a single vertex at the top. + * Note that scale is applied after twist. + * Default {1, 1}. + */ +Manifold Manifold::Extrude(const Polygons& crossSection, double height, + int nDivisions, double twistDegrees, vec2 scaleTop) { + ZoneScoped; + if (crossSection.size() == 0 || height <= 0.0) { + return Invalid(); + } + + scaleTop.x = std::max(scaleTop.x, 0.0); + scaleTop.y = std::max(scaleTop.y, 0.0); + + auto pImpl_ = std::make_shared(); + ++nDivisions; + auto& vertPos = pImpl_->vertPos_; + Vec triVertsDH; + auto& triVerts = triVertsDH; + int nCrossSection = 0; + bool isCone = scaleTop.x == 0.0 && scaleTop.y == 0.0; + size_t idx = 0; + PolygonsIdx polygonsIndexed; + for (auto& poly : crossSection) { + nCrossSection += poly.size(); + SimplePolygonIdx simpleIndexed; + for (const vec2& polyVert : poly) { + vertPos.push_back({polyVert.x, polyVert.y, 0.0}); + simpleIndexed.push_back({polyVert, static_cast(idx++)}); + } + polygonsIndexed.push_back(simpleIndexed); + } + for (int i = 1; i < nDivisions + 1; ++i) { + double alpha = i / double(nDivisions); + double phi = alpha * twistDegrees; + vec2 scale = la::lerp(vec2(1.0), scaleTop, alpha); + mat2 rotation({cosd(phi), sind(phi)}, {-sind(phi), cosd(phi)}); + mat2 transform = mat2({scale.x, 0.0}, {0.0, scale.y}) * rotation; + size_t j = 0; + size_t idx = 0; + for (const auto& poly : crossSection) { + for (size_t vert = 0; vert < poly.size(); ++vert) { + size_t offset = idx + nCrossSection * i; + size_t thisVert = vert + offset; + size_t lastVert = (vert == 0 ? poly.size() : vert) - 1 + offset; + if (i == nDivisions && isCone) { + triVerts.push_back(ivec3(nCrossSection * i + j, + lastVert - nCrossSection, + thisVert - nCrossSection)); + } else { + vec2 pos = transform * poly[vert]; + vertPos.push_back({pos.x, pos.y, height * alpha}); + triVerts.push_back( + ivec3(thisVert, lastVert, thisVert - nCrossSection)); + triVerts.push_back(ivec3(lastVert, lastVert - nCrossSection, + thisVert - nCrossSection)); + } + } + ++j; + idx += poly.size(); + } + } + if (isCone) + for (size_t j = 0; j < crossSection.size(); + ++j) // Duplicate vertex for Genus + vertPos.push_back({0.0, 0.0, height}); + std::vector top = TriangulateIdx(polygonsIndexed); + for (const ivec3& tri : top) { + triVerts.push_back({tri[0], tri[2], tri[1]}); + if (!isCone) triVerts.push_back(tri + nCrossSection * nDivisions); + } + + pImpl_->CreateHalfedges(triVertsDH); + pImpl_->Finish(); + pImpl_->InitializeOriginal(); + pImpl_->CreateFaces(); + return Manifold(pImpl_); +} + +/** + * Constructs a manifold from a set of polygons by revolving this cross-section + * around its Y-axis and then setting this as the Z-axis of the resulting + * manifold. If the polygons cross the Y-axis, only the part on the positive X + * side is used. Geometrically valid input will result in geometrically valid + * output. + * + * @param crossSection A set of non-overlapping polygons to revolve. + * @param circularSegments Number of segments along its diameter. Default is + * calculated by the static Defaults. + * @param revolveDegrees Number of degrees to revolve. Default is 360 degrees. + */ +Manifold Manifold::Revolve(const Polygons& crossSection, int circularSegments, + double revolveDegrees) { + ZoneScoped; + + Polygons polygons; + double radius = 0; + for (const SimplePolygon& poly : crossSection) { + size_t i = 0; + while (i < poly.size() && poly[i].x < 0) { + ++i; + } + if (i == poly.size()) { + continue; + } + polygons.push_back({}); + const size_t start = i; + do { + if (poly[i].x >= 0) { + polygons.back().push_back(poly[i]); + radius = std::max(radius, poly[i].x); + } + const size_t next = i + 1 == poly.size() ? 0 : i + 1; + if ((poly[next].x < 0) != (poly[i].x < 0)) { + const double y = poly[next].y + poly[next].x * + (poly[i].y - poly[next].y) / + (poly[i].x - poly[next].x); + polygons.back().push_back({0, y}); + } + i = next; + } while (i != start); + } + + if (polygons.empty()) { + return Invalid(); + } + + if (revolveDegrees > 360.0) { + revolveDegrees = 360.0; + } + const bool isFullRevolution = revolveDegrees == 360.0; + + const int nDivisions = + circularSegments > 2 + ? circularSegments + : Quality::GetCircularSegments(radius) * revolveDegrees / 360; + + auto pImpl_ = std::make_shared(); + auto& vertPos = pImpl_->vertPos_; + Vec triVertsDH; + auto& triVerts = triVertsDH; + + std::vector startPoses; + std::vector endPoses; + + const double dPhi = revolveDegrees / nDivisions; + // first and last slice are distinguished if not a full revolution. + const int nSlices = isFullRevolution ? nDivisions : nDivisions + 1; + + for (const auto& poly : polygons) { + std::size_t nPosVerts = 0; + std::size_t nRevolveAxisVerts = 0; + for (auto& pt : poly) { + if (pt.x > 0) { + nPosVerts++; + } else { + nRevolveAxisVerts++; + } + } + + for (size_t polyVert = 0; polyVert < poly.size(); ++polyVert) { + const size_t startPosIndex = vertPos.size(); + + if (!isFullRevolution) startPoses.push_back(startPosIndex); + + const vec2 currPolyVertex = poly[polyVert]; + const vec2 prevPolyVertex = + poly[polyVert == 0 ? poly.size() - 1 : polyVert - 1]; + + const int prevStartPosIndex = + startPosIndex + + (polyVert == 0 ? nRevolveAxisVerts + (nSlices * nPosVerts) : 0) + + (prevPolyVertex.x == 0.0 ? -1 : -nSlices); + + for (int slice = 0; slice < nSlices; ++slice) { + const double phi = slice * dPhi; + if (slice == 0 || currPolyVertex.x > 0) { + vertPos.push_back({currPolyVertex.x * cosd(phi), + currPolyVertex.x * sind(phi), currPolyVertex.y}); + } + + if (isFullRevolution || slice > 0) { + const int lastSlice = (slice == 0 ? nDivisions : slice) - 1; + if (currPolyVertex.x > 0.0) { + triVerts.push_back(ivec3( + startPosIndex + slice, startPosIndex + lastSlice, + // "Reuse" vertex of first slice if it lies on the revolve axis + (prevPolyVertex.x == 0.0 ? prevStartPosIndex + : prevStartPosIndex + lastSlice))); + } + + if (prevPolyVertex.x > 0.0) { + triVerts.push_back( + ivec3(prevStartPosIndex + lastSlice, prevStartPosIndex + slice, + (currPolyVertex.x == 0.0 ? startPosIndex + : startPosIndex + slice))); + } + } + } + if (!isFullRevolution) endPoses.push_back(vertPos.size() - 1); + } + } + + // Add front and back triangles if not a full revolution. + if (!isFullRevolution) { + std::vector frontTriangles = Triangulate(polygons, pImpl_->epsilon_); + for (auto& t : frontTriangles) { + triVerts.push_back({startPoses[t.x], startPoses[t.y], startPoses[t.z]}); + } + + for (auto& t : frontTriangles) { + triVerts.push_back({endPoses[t.z], endPoses[t.y], endPoses[t.x]}); + } + } + + pImpl_->CreateHalfedges(triVertsDH); + pImpl_->Finish(); + pImpl_->InitializeOriginal(); + pImpl_->CreateFaces(); + return Manifold(pImpl_); +} + +/** + * Constructs a new manifold from a vector of other manifolds. This is a purely + * topological operation, so care should be taken to avoid creating + * overlapping results. It is the inverse operation of Decompose(). + * + * @param manifolds A vector of Manifolds to lazy-union together. + */ +Manifold Manifold::Compose(const std::vector& manifolds) { + std::vector> children; + for (const auto& manifold : manifolds) { + children.push_back(manifold.pNode_->ToLeafNode()); + } + return Manifold(CsgLeafNode::Compose(children)); +} + +/** + * This operation returns a vector of Manifolds that are topologically + * disconnected. If everything is connected, the vector is length one, + * containing a copy of the original. It is the inverse operation of Compose(). + */ +std::vector Manifold::Decompose() const { + ZoneScoped; + UnionFind<> uf(NumVert()); + // Graph graph; + auto pImpl_ = GetCsgLeafNode().GetImpl(); + for (const Halfedge& halfedge : pImpl_->halfedge_) { + if (halfedge.IsForward()) uf.unionXY(halfedge.startVert, halfedge.endVert); + } + std::vector componentIndices; + const int numComponents = uf.connectedComponents(componentIndices); + + if (numComponents == 1) { + std::vector meshes(1); + meshes[0] = *this; + return meshes; + } + Vec vertLabel(componentIndices); + + const int numVert = NumVert(); + std::vector meshes; + for (int i = 0; i < numComponents; ++i) { + auto impl = std::make_shared(); + // inherit original object's precision + impl->epsilon_ = pImpl_->epsilon_; + impl->tolerance_ = pImpl_->tolerance_; + + Vec vertNew2Old(numVert); + const int nVert = + copy_if(countAt(0), countAt(numVert), vertNew2Old.begin(), + [i, &vertLabel](int v) { return vertLabel[v] == i; }) - + vertNew2Old.begin(); + impl->vertPos_.resize(nVert); + vertNew2Old.resize(nVert); + gather(vertNew2Old.begin(), vertNew2Old.end(), pImpl_->vertPos_.begin(), + impl->vertPos_.begin()); + + Vec faceNew2Old(NumTri()); + const auto& halfedge = pImpl_->halfedge_; + const int nFace = + copy_if(countAt(0_uz), countAt(NumTri()), faceNew2Old.begin(), + [i, &vertLabel, &halfedge](int face) { + return vertLabel[halfedge[3 * face].startVert] == i; + }) - + faceNew2Old.begin(); + + if (nFace == 0) continue; + faceNew2Old.resize(nFace); + + impl->GatherFaces(*pImpl_, faceNew2Old); + impl->ReindexVerts(vertNew2Old, pImpl_->NumVert()); + impl->Finish(); + + meshes.push_back(Manifold(impl)); + } + return meshes; +} +} // namespace manifold diff --git a/thirdparty/manifold/src/cross_section/cross_section.cpp b/thirdparty/manifold/src/cross_section/cross_section.cpp new file mode 100644 index 000000000000..ac45fc5233ec --- /dev/null +++ b/thirdparty/manifold/src/cross_section/cross_section.cpp @@ -0,0 +1,789 @@ +// Copyright 2023 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manifold/cross_section.h" + +#include "../utils.h" +#include "clipper2/clipper.core.h" +#include "clipper2/clipper.h" +#include "clipper2/clipper.offset.h" + +namespace C2 = Clipper2Lib; + +using namespace manifold; + +namespace manifold { +struct PathImpl { + PathImpl(const C2::PathsD paths_) : paths_(paths_) {} + operator const C2::PathsD&() const { return paths_; } + const C2::PathsD paths_; +}; +} // namespace manifold + +namespace { +const int precision_ = 8; + +C2::ClipType cliptype_of_op(OpType op) { + C2::ClipType ct = C2::ClipType::Union; + switch (op) { + case OpType::Add: + break; + case OpType::Subtract: + ct = C2::ClipType::Difference; + break; + case OpType::Intersect: + ct = C2::ClipType::Intersection; + break; + }; + return ct; +} + +C2::FillRule fr(CrossSection::FillRule fillrule) { + C2::FillRule fr = C2::FillRule::EvenOdd; + switch (fillrule) { + case CrossSection::FillRule::EvenOdd: + break; + case CrossSection::FillRule::NonZero: + fr = C2::FillRule::NonZero; + break; + case CrossSection::FillRule::Positive: + fr = C2::FillRule::Positive; + break; + case CrossSection::FillRule::Negative: + fr = C2::FillRule::Negative; + break; + }; + return fr; +} + +C2::JoinType jt(CrossSection::JoinType jointype) { + C2::JoinType jt = C2::JoinType::Square; + switch (jointype) { + case CrossSection::JoinType::Square: + break; + case CrossSection::JoinType::Round: + jt = C2::JoinType::Round; + break; + case CrossSection::JoinType::Miter: + jt = C2::JoinType::Miter; + break; + }; + return jt; +} + +vec2 v2_of_pd(const C2::PointD p) { return {p.x, p.y}; } + +C2::PointD v2_to_pd(const vec2 v) { return C2::PointD(v.x, v.y); } + +C2::PathD pathd_of_contour(const SimplePolygon& ctr) { + auto p = C2::PathD(); + p.reserve(ctr.size()); + for (auto v : ctr) { + p.push_back(v2_to_pd(v)); + } + return p; +} + +C2::PathsD transform(const C2::PathsD ps, const mat2x3 m) { + const bool invert = la::determinant(mat2(m)) < 0; + auto transformed = C2::PathsD(); + transformed.reserve(ps.size()); + for (auto path : ps) { + auto sz = path.size(); + auto s = C2::PathD(sz); + for (size_t i = 0; i < sz; ++i) { + auto idx = invert ? sz - 1 - i : i; + s[idx] = v2_to_pd(m * vec3(path[i].x, path[i].y, 1)); + } + transformed.push_back(s); + } + return transformed; +} + +std::shared_ptr shared_paths(const C2::PathsD& ps) { + return std::make_shared(ps); +} + +// forward declaration for mutual recursion +void decompose_hole(const C2::PolyTreeD* outline, + std::vector& polys, C2::PathsD& poly, + size_t n_holes, size_t j); + +void decompose_outline(const C2::PolyTreeD* tree, + std::vector& polys, size_t i) { + auto n_outlines = tree->Count(); + if (i < n_outlines) { + auto outline = tree->Child(i); + auto n_holes = outline->Count(); + auto poly = C2::PathsD(n_holes + 1); + poly[0] = outline->Polygon(); + decompose_hole(outline, polys, poly, n_holes, 0); + polys.push_back(poly); + if (i < n_outlines - 1) { + decompose_outline(tree, polys, i + 1); + } + } +} + +void decompose_hole(const C2::PolyTreeD* outline, + std::vector& polys, C2::PathsD& poly, + size_t n_holes, size_t j) { + if (j < n_holes) { + auto child = outline->Child(j); + decompose_outline(child, polys, 0); + poly[j + 1] = child->Polygon(); + decompose_hole(outline, polys, poly, n_holes, j + 1); + } +} + +void flatten(const C2::PolyTreeD* tree, C2::PathsD& polys, size_t i) { + auto n_outlines = tree->Count(); + if (i < n_outlines) { + auto outline = tree->Child(i); + flatten(outline, polys, 0); + polys.push_back(outline->Polygon()); + if (i < n_outlines - 1) { + flatten(tree, polys, i + 1); + } + } +} + +bool V2Lesser(vec2 a, vec2 b) { + if (a.x == b.x) return a.y < b.y; + return a.x < b.x; +} + +void HullBacktrack(const vec2& pt, std::vector& stack) { + auto sz = stack.size(); + while (sz >= 2 && CCW(stack[sz - 2], stack[sz - 1], pt, 0.0) <= 0.0) { + stack.pop_back(); + sz = stack.size(); + } +} + +// Based on method described here: +// https://www.hackerearth.com/practice/math/geometry/line-sweep-technique/tutorial/ +// Changed to follow: +// https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain +// This is the same algorithm (Andrew, also called Montone Chain). +C2::PathD HullImpl(SimplePolygon& pts) { + size_t len = pts.size(); + if (len < 3) return C2::PathD(); // not enough points to create a polygon + std::sort(pts.begin(), pts.end(), V2Lesser); + + auto lower = std::vector{}; + for (auto& pt : pts) { + HullBacktrack(pt, lower); + lower.push_back(pt); + } + auto upper = std::vector{}; + for (auto pt_iter = pts.rbegin(); pt_iter != pts.rend(); pt_iter++) { + HullBacktrack(*pt_iter, upper); + upper.push_back(*pt_iter); + } + + upper.pop_back(); + lower.pop_back(); + + auto path = C2::PathD(); + path.reserve(lower.size() + upper.size()); + for (const auto& l : lower) path.push_back(v2_to_pd(l)); + for (const auto& u : upper) path.push_back(v2_to_pd(u)); + return path; +} +} // namespace + +namespace manifold { + +/** + * The default constructor is an empty cross-section (containing no contours). + */ +CrossSection::CrossSection() { + paths_ = std::make_shared(C2::PathsD()); +} + +CrossSection::~CrossSection() = default; +CrossSection::CrossSection(CrossSection&&) noexcept = default; +CrossSection& CrossSection::operator=(CrossSection&&) noexcept = default; + +/** + * The copy constructor avoids copying the underlying paths vector (sharing + * with its parent via shared_ptr), however subsequent transformations, and + * their application will not be shared. It is generally recommended to avoid + * this, opting instead to simply create CrossSections with the available + * const methods. + */ +CrossSection::CrossSection(const CrossSection& other) { + paths_ = other.paths_; + transform_ = other.transform_; +} + +CrossSection& CrossSection::operator=(const CrossSection& other) { + if (this != &other) { + paths_ = other.paths_; + transform_ = other.transform_; + } + return *this; +}; + +// Private, skips unioning. +CrossSection::CrossSection(std::shared_ptr ps) { paths_ = ps; } + +/** + * Create a 2d cross-section from a single contour. A boolean union operation + * (with Positive filling rule by default) is performed to ensure the + * resulting CrossSection is free of self-intersections. + * + * @param contour A closed path outlining the desired cross-section. + * @param fillrule The filling rule used to interpret polygon sub-regions + * created by self-intersections in contour. + */ +CrossSection::CrossSection(const SimplePolygon& contour, FillRule fillrule) { + auto ps = C2::PathsD{(pathd_of_contour(contour))}; + paths_ = shared_paths(C2::Union(ps, fr(fillrule), precision_)); +} + +/** + * Create a 2d cross-section from a set of contours (complex polygons). A + * boolean union operation (with Positive filling rule by default) is + * performed to combine overlapping polygons and ensure the resulting + * CrossSection is free of intersections. + * + * @param contours A set of closed paths describing zero or more complex + * polygons. + * @param fillrule The filling rule used to interpret polygon sub-regions in + * contours. + */ +CrossSection::CrossSection(const Polygons& contours, FillRule fillrule) { + auto ps = C2::PathsD(); + ps.reserve(contours.size()); + for (auto ctr : contours) { + ps.push_back(pathd_of_contour(ctr)); + } + paths_ = shared_paths(C2::Union(ps, fr(fillrule), precision_)); +} + +/** + * Create a 2d cross-section from an axis-aligned rectangle (bounding box). + * + * @param rect An axis-aligned rectangular bounding box. + */ +CrossSection::CrossSection(const Rect& rect) { + C2::PathD p(4); + p[0] = C2::PointD(rect.min.x, rect.min.y); + p[1] = C2::PointD(rect.max.x, rect.min.y); + p[2] = C2::PointD(rect.max.x, rect.max.y); + p[3] = C2::PointD(rect.min.x, rect.max.y); + paths_ = shared_paths(C2::PathsD{p}); +} + +// Private +// All access to paths_ should be done through the GetPaths() method, which +// applies the accumulated transform_ +std::shared_ptr CrossSection::GetPaths() const { + if (transform_ == mat2x3(la::identity)) { + return paths_; + } + paths_ = shared_paths(::transform(paths_->paths_, transform_)); + transform_ = mat2x3(la::identity); + return paths_; +} + +/** + * Constructs a square with the given XY dimensions. By default it is + * positioned in the first quadrant, touching the origin. If any dimensions in + * size are negative, or if all are zero, an empty Manifold will be returned. + * + * @param size The X, and Y dimensions of the square. + * @param center Set to true to shift the center to the origin. + */ +CrossSection CrossSection::Square(const vec2 size, bool center) { + if (size.x < 0.0 || size.y < 0.0 || la::length(size) == 0.0) { + return CrossSection(); + } + + auto p = C2::PathD(4); + if (center) { + const auto w = size.x / 2; + const auto h = size.y / 2; + p[0] = C2::PointD(w, h); + p[1] = C2::PointD(-w, h); + p[2] = C2::PointD(-w, -h); + p[3] = C2::PointD(w, -h); + } else { + const double x = size.x; + const double y = size.y; + p[0] = C2::PointD(0.0, 0.0); + p[1] = C2::PointD(x, 0.0); + p[2] = C2::PointD(x, y); + p[3] = C2::PointD(0.0, y); + } + return CrossSection(shared_paths(C2::PathsD{p})); +} + +/** + * Constructs a circle of a given radius. + * + * @param radius Radius of the circle. Must be positive. + * @param circularSegments Number of segments along its diameter. Default is + * calculated by the static Quality defaults according to the radius. + */ +CrossSection CrossSection::Circle(double radius, int circularSegments) { + if (radius <= 0.0) { + return CrossSection(); + } + int n = circularSegments > 2 ? circularSegments + : Quality::GetCircularSegments(radius); + double dPhi = 360.0 / n; + auto circle = C2::PathD(n); + for (int i = 0; i < n; ++i) { + circle[i] = C2::PointD(radius * cosd(dPhi * i), radius * sind(dPhi * i)); + } + return CrossSection(shared_paths(C2::PathsD{circle})); +} + +/** + * Perform the given boolean operation between this and another CrossSection. + */ +CrossSection CrossSection::Boolean(const CrossSection& second, + OpType op) const { + auto ct = cliptype_of_op(op); + auto res = C2::BooleanOp(ct, C2::FillRule::Positive, GetPaths()->paths_, + second.GetPaths()->paths_, precision_); + return CrossSection(shared_paths(res)); +} + +/** + * Perform the given boolean operation on a list of CrossSections. In case of + * Subtract, all CrossSections in the tail are differenced from the head. + */ +CrossSection CrossSection::BatchBoolean( + const std::vector& crossSections, OpType op) { + if (crossSections.size() == 0) + return CrossSection(); + else if (crossSections.size() == 1) + return crossSections[0]; + + auto subjs = crossSections[0].GetPaths(); + int n_clips = 0; + for (size_t i = 1; i < crossSections.size(); ++i) { + n_clips += crossSections[i].GetPaths()->paths_.size(); + } + auto clips = C2::PathsD(); + clips.reserve(n_clips); + for (size_t i = 1; i < crossSections.size(); ++i) { + auto ps = crossSections[i].GetPaths(); + clips.insert(clips.end(), ps->paths_.begin(), ps->paths_.end()); + } + + auto ct = cliptype_of_op(op); + auto res = C2::BooleanOp(ct, C2::FillRule::Positive, subjs->paths_, clips, + precision_); + return CrossSection(shared_paths(res)); +} + +/** + * Compute the boolean union between two cross-sections. + */ +CrossSection CrossSection::operator+(const CrossSection& Q) const { + return Boolean(Q, OpType::Add); +} + +/** + * Compute the boolean union between two cross-sections, assigning the result + * to the first. + */ +CrossSection& CrossSection::operator+=(const CrossSection& Q) { + *this = *this + Q; + return *this; +} + +/** + * Compute the boolean difference of a (clip) cross-section from another + * (subject). + */ +CrossSection CrossSection::operator-(const CrossSection& Q) const { + return Boolean(Q, OpType::Subtract); +} + +/** + * Compute the boolean difference of a (clip) cross-section from a another + * (subject), assigning the result to the subject. + */ +CrossSection& CrossSection::operator-=(const CrossSection& Q) { + *this = *this - Q; + return *this; +} + +/** + * Compute the boolean intersection between two cross-sections. + */ +CrossSection CrossSection::operator^(const CrossSection& Q) const { + return Boolean(Q, OpType::Intersect); +} + +/** + * Compute the boolean intersection between two cross-sections, assigning the + * result to the first. + */ +CrossSection& CrossSection::operator^=(const CrossSection& Q) { + *this = *this ^ Q; + return *this; +} + +/** + * Construct a CrossSection from a vector of other CrossSections (batch + * boolean union). + */ +CrossSection CrossSection::Compose(std::vector& crossSections) { + return BatchBoolean(crossSections, OpType::Add); +} + +/** + * This operation returns a vector of CrossSections that are topologically + * disconnected, each containing one outline contour with zero or more + * holes. + */ +std::vector CrossSection::Decompose() const { + if (NumContour() < 2) { + return std::vector{CrossSection(*this)}; + } + + C2::PolyTreeD tree; + C2::BooleanOp(C2::ClipType::Union, C2::FillRule::Positive, GetPaths()->paths_, + C2::PathsD(), tree, precision_); + + auto polys = std::vector(); + decompose_outline(&tree, polys, 0); + + auto comps = std::vector(); + comps.reserve(polys.size()); + // reverse the stack while wrapping + for (auto poly = polys.rbegin(); poly != polys.rend(); ++poly) + comps.emplace_back(CrossSection(shared_paths(*poly))); + + return comps; +} + +/** + * Move this CrossSection in space. This operation can be chained. Transforms + * are combined and applied lazily. + * + * @param v The vector to add to every vertex. + */ +CrossSection CrossSection::Translate(const vec2 v) const { + mat2x3 m({1.0, 0.0}, // + {0.0, 1.0}, // + {v.x, v.y}); + return Transform(m); +} + +/** + * Applies a (Z-axis) rotation to the CrossSection, in degrees. This operation + * can be chained. Transforms are combined and applied lazily. + * + * @param degrees degrees about the Z-axis to rotate. + */ +CrossSection CrossSection::Rotate(double degrees) const { + auto s = sind(degrees); + auto c = cosd(degrees); + mat2x3 m({c, s}, // + {-s, c}, // + {0.0, 0.0}); + return Transform(m); +} + +/** + * Scale this CrossSection in space. This operation can be chained. Transforms + * are combined and applied lazily. + * + * @param scale The vector to multiply every vertex by per component. + */ +CrossSection CrossSection::Scale(const vec2 scale) const { + mat2x3 m({scale.x, 0.0}, // + {0.0, scale.y}, // + {0.0, 0.0}); + return Transform(m); +} + +/** + * Mirror this CrossSection over the arbitrary axis described by the unit form + * of the given vector. If the length of the vector is zero, an empty + * CrossSection is returned. This operation can be chained. Transforms are + * combined and applied lazily. + * + * @param ax the axis to be mirrored over + */ +CrossSection CrossSection::Mirror(const vec2 ax) const { + if (la::length(ax) == 0.) { + return CrossSection(); + } + auto n = la::normalize(la::abs(ax)); + auto m = mat2x3(mat2(la::identity) - 2.0 * la::outerprod(n, n), vec2(0.0)); + return Transform(m); +} + +/** + * Transform this CrossSection in space. The first two columns form a 2x2 + * matrix transform and the last is a translation vector. This operation can + * be chained. Transforms are combined and applied lazily. + * + * @param m The affine transform matrix to apply to all the vertices. + */ +CrossSection CrossSection::Transform(const mat2x3& m) const { + auto transformed = CrossSection(); + transformed.transform_ = m * Mat3(transform_); + transformed.paths_ = paths_; + return transformed; +} + +/** + * Move the vertices of this CrossSection (creating a new one) according to + * any arbitrary input function, followed by a union operation (with a + * Positive fill rule) that ensures any introduced intersections are not + * included in the result. + * + * @param warpFunc A function that modifies a given vertex position. + */ +CrossSection CrossSection::Warp(std::function warpFunc) const { + return WarpBatch([&warpFunc](VecView vecs) { + for (vec2& p : vecs) { + warpFunc(p); + } + }); +} + +/** + * Same as CrossSection::Warp but calls warpFunc with + * a VecView which is roughly equivalent to std::span + * pointing to all vec2 elements to be modified in-place + * + * @param warpFunc A function that modifies multiple vertex positions. + */ +CrossSection CrossSection::WarpBatch( + std::function)> warpFunc) const { + std::vector tmp_verts; + C2::PathsD paths = GetPaths()->paths_; // deep copy + for (C2::PathD const& path : paths) { + for (C2::PointD const& p : path) { + tmp_verts.push_back(v2_of_pd(p)); + } + } + + warpFunc(VecView(tmp_verts.data(), tmp_verts.size())); + + auto cursor = tmp_verts.begin(); + for (C2::PathD& path : paths) { + for (C2::PointD& p : path) { + p = v2_to_pd(*cursor); + ++cursor; + } + } + + return CrossSection( + shared_paths(C2::Union(paths, C2::FillRule::Positive, precision_))); +} + +/** + * Remove vertices from the contours in this CrossSection that are less than + * the specified distance epsilon from an imaginary line that passes through + * its two adjacent vertices. Near duplicate vertices and collinear points + * will be removed at lower epsilons, with elimination of line segments + * becoming increasingly aggressive with larger epsilons. + * + * It is recommended to apply this function following Offset, in order to + * clean up any spurious tiny line segments introduced that do not improve + * quality in any meaningful way. This is particularly important if further + * offseting operations are to be performed, which would compound the issue. + */ +CrossSection CrossSection::Simplify(double epsilon) const { + C2::PolyTreeD tree; + C2::BooleanOp(C2::ClipType::Union, C2::FillRule::Positive, GetPaths()->paths_, + C2::PathsD(), tree, precision_); + + C2::PathsD polys; + flatten(&tree, polys, 0); + + // Filter out contours less than epsilon wide. + C2::PathsD filtered; + for (C2::PathD poly : polys) { + auto area = C2::Area(poly); + Rect box; + for (auto vert : poly) { + box.Union(vec2(vert.x, vert.y)); + } + vec2 size = box.Size(); + if (std::abs(area) > std::max(size.x, size.y) * epsilon) { + filtered.push_back(poly); + } + } + + auto ps = SimplifyPaths(filtered, epsilon, true); + return CrossSection(shared_paths(ps)); +} + +/** + * Inflate the contours in CrossSection by the specified delta, handling + * corners according to the given JoinType. + * + * @param delta Positive deltas will cause the expansion of outlining contours + * to expand, and retraction of inner (hole) contours. Negative deltas will + * have the opposite effect. + * @param jointype The join type specifying the treatment of contour joins + * (corners). + * @param miter_limit The maximum distance in multiples of delta that vertices + * can be offset from their original positions with before squaring is + * applied, when the join type is Miter (default is 2, which is the + * minimum allowed). See the [Clipper2 + * MiterLimit](http://www.angusj.com/clipper2/Docs/Units/Clipper.Offset/Classes/ClipperOffset/Properties/MiterLimit.htm) + * page for a visual example. + * @param circularSegments Number of segments per 360 degrees of + * JoinType::Round corners (roughly, the number of vertices that + * will be added to each contour). Default is calculated by the static Quality + * defaults according to the radius. + */ +CrossSection CrossSection::Offset(double delta, JoinType jointype, + double miter_limit, + int circularSegments) const { + double arc_tol = 0.; + if (jointype == JoinType::Round) { + int n = circularSegments > 2 ? circularSegments + : Quality::GetCircularSegments(delta); + // This calculates tolerance as a function of circular segments and delta + // (radius) in order to get back the same number of segments in Clipper2: + // steps_per_360 = PI / acos(1 - arc_tol / abs_delta) + const double abs_delta = std::fabs(delta); + const double scaled_delta = abs_delta * std::pow(10, precision_); + arc_tol = (std::cos(Clipper2Lib::PI / n) - 1) * -scaled_delta; + } + auto ps = + C2::InflatePaths(GetPaths()->paths_, delta, jt(jointype), + C2::EndType::Polygon, miter_limit, precision_, arc_tol); + return CrossSection(shared_paths(ps)); +} + +/** + * Compute the convex hull enveloping a set of cross-sections. + * + * @param crossSections A vector of cross-sections over which to compute a + * convex hull. + */ +CrossSection CrossSection::Hull( + const std::vector& crossSections) { + int n = 0; + for (auto cs : crossSections) n += cs.NumVert(); + SimplePolygon pts; + pts.reserve(n); + for (auto cs : crossSections) { + auto paths = cs.GetPaths()->paths_; + for (auto path : paths) { + for (auto p : path) { + pts.push_back(v2_of_pd(p)); + } + } + } + return CrossSection(shared_paths(C2::PathsD{HullImpl(pts)})); +} + +/** + * Compute the convex hull of this cross-section. + */ +CrossSection CrossSection::Hull() const { + return Hull(std::vector{*this}); +} + +/** + * Compute the convex hull of a set of points. If the given points are fewer + * than 3, an empty CrossSection will be returned. + * + * @param pts A vector of 2-dimensional points over which to compute a convex + * hull. + */ +CrossSection CrossSection::Hull(SimplePolygon pts) { + return CrossSection(shared_paths(C2::PathsD{HullImpl(pts)})); +} + +/** + * Compute the convex hull of a set of points/polygons. If the given points are + * fewer than 3, an empty CrossSection will be returned. + * + * @param polys A vector of vectors of 2-dimensional points over which to + * compute a convex hull. + */ +CrossSection CrossSection::Hull(const Polygons polys) { + SimplePolygon pts; + for (auto poly : polys) { + for (auto p : poly) { + pts.push_back(p); + } + } + return Hull(pts); +} + +/** + * Return the total area covered by complex polygons making up the + * CrossSection. + */ +double CrossSection::Area() const { return C2::Area(GetPaths()->paths_); } + +/** + * Return the number of vertices in the CrossSection. + */ +int CrossSection::NumVert() const { + int n = 0; + auto paths = GetPaths()->paths_; + for (auto p : paths) { + n += p.size(); + } + return n; +} + +/** + * Return the number of contours (both outer and inner paths) in the + * CrossSection. + */ +int CrossSection::NumContour() const { return GetPaths()->paths_.size(); } + +/** + * Does the CrossSection contain any contours? + */ +bool CrossSection::IsEmpty() const { return GetPaths()->paths_.empty(); } + +/** + * Returns the axis-aligned bounding rectangle of all the CrossSections' + * vertices. + */ +Rect CrossSection::Bounds() const { + auto r = C2::GetBounds(GetPaths()->paths_); + return Rect({r.left, r.bottom}, {r.right, r.top}); +} + +/** + * Return the contours of this CrossSection as a Polygons. + */ +Polygons CrossSection::ToPolygons() const { + auto polys = Polygons(); + auto paths = GetPaths()->paths_; + polys.reserve(paths.size()); + for (auto p : paths) { + auto sp = SimplePolygon(); + sp.reserve(p.size()); + for (auto v : p) { + sp.push_back({v.x, v.y}); + } + polys.push_back(sp); + } + return polys; +} +} // namespace manifold diff --git a/thirdparty/manifold/src/csg_tree.cpp b/thirdparty/manifold/src/csg_tree.cpp new file mode 100644 index 000000000000..a05fce3aa6b6 --- /dev/null +++ b/thirdparty/manifold/src/csg_tree.cpp @@ -0,0 +1,662 @@ +// Copyright 2022 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if (MANIFOLD_PAR == 1) && __has_include() +#include +#define TBB_PREVIEW_CONCURRENT_ORDERED_CONTAINERS 1 +#include +#endif + +#include + +#include "./boolean3.h" +#include "./csg_tree.h" +#include "./impl.h" +#include "./mesh_fixes.h" +#include "./parallel.h" + +constexpr int kParallelThreshold = 4096; + +namespace { +using namespace manifold; +struct MeshCompare { + bool operator()(const std::shared_ptr &a, + const std::shared_ptr &b) { + return a->GetImpl()->NumVert() < b->GetImpl()->NumVert(); + } +}; + +} // namespace +namespace manifold { + +std::shared_ptr CsgNode::Boolean( + const std::shared_ptr &second, OpType op) { + if (second->GetNodeType() != CsgNodeType::Leaf) { + // "this" is not a CsgOpNode (which overrides Boolean), but if "second" is + // and the operation is commutative, we let it built the tree. + if ((op == OpType::Add || op == OpType::Intersect)) { + return std::static_pointer_cast(second)->Boolean( + shared_from_this(), op); + } + } + std::vector> children({shared_from_this(), second}); + return std::make_shared(children, op); +} + +std::shared_ptr CsgNode::Translate(const vec3 &t) const { + mat3x4 transform = la::identity; + transform[3] += t; + return Transform(transform); +} + +std::shared_ptr CsgNode::Scale(const vec3 &v) const { + mat3x4 transform; + for (int i : {0, 1, 2}) transform[i][i] = v[i]; + return Transform(transform); +} + +std::shared_ptr CsgNode::Rotate(double xDegrees, double yDegrees, + double zDegrees) const { + mat3 rX({1.0, 0.0, 0.0}, // + {0.0, cosd(xDegrees), sind(xDegrees)}, // + {0.0, -sind(xDegrees), cosd(xDegrees)}); + mat3 rY({cosd(yDegrees), 0.0, -sind(yDegrees)}, // + {0.0, 1.0, 0.0}, // + {sind(yDegrees), 0.0, cosd(yDegrees)}); + mat3 rZ({cosd(zDegrees), sind(zDegrees), 0.0}, // + {-sind(zDegrees), cosd(zDegrees), 0.0}, // + {0.0, 0.0, 1.0}); + mat3x4 transform(rZ * rY * rX, vec3()); + return Transform(transform); +} + +CsgLeafNode::CsgLeafNode() : pImpl_(std::make_shared()) {} + +CsgLeafNode::CsgLeafNode(std::shared_ptr pImpl_) + : pImpl_(pImpl_) {} + +CsgLeafNode::CsgLeafNode(std::shared_ptr pImpl_, + mat3x4 transform_) + : pImpl_(pImpl_), transform_(transform_) {} + +std::shared_ptr CsgLeafNode::GetImpl() const { + if (transform_ == mat3x4(la::identity)) return pImpl_; + pImpl_ = + std::make_shared(pImpl_->Transform(transform_)); + transform_ = la::identity; + return pImpl_; +} + +std::shared_ptr CsgLeafNode::ToLeafNode() const { + return std::make_shared(*this); +} + +std::shared_ptr CsgLeafNode::Transform(const mat3x4 &m) const { + return std::make_shared(pImpl_, m * Mat4(transform_)); +} + +CsgNodeType CsgLeafNode::GetNodeType() const { return CsgNodeType::Leaf; } + +std::shared_ptr ImplToLeaf(Manifold::Impl &&impl) { + return std::make_shared(std::make_shared(impl)); +} + +/** + * Efficient union of a set of pairwise disjoint meshes. + */ +std::shared_ptr CsgLeafNode::Compose( + const std::vector> &nodes) { + ZoneScoped; + double epsilon = -1; + double tolerance = -1; + int numVert = 0; + int numEdge = 0; + int numTri = 0; + int numPropVert = 0; + std::vector vertIndices; + std::vector edgeIndices; + std::vector triIndices; + std::vector propVertIndices; + int numPropOut = 0; + for (auto &node : nodes) { + if (node->pImpl_->status_ != Manifold::Error::NoError) { + Manifold::Impl impl; + impl.status_ = node->pImpl_->status_; + return ImplToLeaf(std::move(impl)); + } + double nodeOldScale = node->pImpl_->bBox_.Scale(); + double nodeNewScale = + node->pImpl_->bBox_.Transform(node->transform_).Scale(); + double nodeEpsilon = node->pImpl_->epsilon_; + nodeEpsilon *= std::max(1.0, nodeNewScale / nodeOldScale); + nodeEpsilon = std::max(nodeEpsilon, kPrecision * nodeNewScale); + if (!std::isfinite(nodeEpsilon)) nodeEpsilon = -1; + epsilon = std::max(epsilon, nodeEpsilon); + tolerance = std::max(tolerance, node->pImpl_->tolerance_); + + vertIndices.push_back(numVert); + edgeIndices.push_back(numEdge * 2); + triIndices.push_back(numTri); + propVertIndices.push_back(numPropVert); + numVert += node->pImpl_->NumVert(); + numEdge += node->pImpl_->NumEdge(); + numTri += node->pImpl_->NumTri(); + const int numProp = node->pImpl_->NumProp(); + numPropOut = std::max(numPropOut, numProp); + numPropVert += + numProp == 0 ? 1 + : node->pImpl_->meshRelation_.properties.size() / numProp; + } + + Manifold::Impl combined; + combined.epsilon_ = epsilon; + combined.tolerance_ = tolerance; + combined.vertPos_.resize(numVert); + combined.halfedge_.resize(2 * numEdge); + combined.faceNormal_.resize(numTri); + combined.halfedgeTangent_.resize(2 * numEdge); + combined.meshRelation_.triRef.resize(numTri); + if (numPropOut > 0) { + combined.meshRelation_.numProp = numPropOut; + combined.meshRelation_.properties.resize(numPropOut * numPropVert, 0); + combined.meshRelation_.triProperties.resize(numTri); + } + auto policy = autoPolicy(numTri); + + // if we are already parallelizing for each node, do not perform multithreaded + // copying as it will slightly hurt performance + if (nodes.size() > 1 && policy == ExecutionPolicy::Par) + policy = ExecutionPolicy::Seq; + + for_each_n( + nodes.size() > 1 ? ExecutionPolicy::Par : ExecutionPolicy::Seq, + countAt(0), nodes.size(), + [&nodes, &vertIndices, &edgeIndices, &triIndices, &propVertIndices, + numPropOut, &combined, policy](int i) { + auto &node = nodes[i]; + copy(node->pImpl_->halfedgeTangent_.begin(), + node->pImpl_->halfedgeTangent_.end(), + combined.halfedgeTangent_.begin() + edgeIndices[i]); + const int nextVert = vertIndices[i]; + const int nextEdge = edgeIndices[i]; + const int nextFace = triIndices[i]; + transform(node->pImpl_->halfedge_.begin(), + node->pImpl_->halfedge_.end(), + combined.halfedge_.begin() + edgeIndices[i], + [nextVert, nextEdge, nextFace](Halfedge edge) { + edge.startVert += nextVert; + edge.endVert += nextVert; + edge.pairedHalfedge += nextEdge; + return edge; + }); + + if (numPropOut > 0) { + auto start = + combined.meshRelation_.triProperties.begin() + triIndices[i]; + if (node->pImpl_->NumProp() > 0) { + auto &triProp = node->pImpl_->meshRelation_.triProperties; + const int nextProp = propVertIndices[i]; + transform(triProp.begin(), triProp.end(), start, + [nextProp](ivec3 tri) { + tri += nextProp; + return tri; + }); + + const int numProp = node->pImpl_->NumProp(); + auto &oldProp = node->pImpl_->meshRelation_.properties; + auto &newProp = combined.meshRelation_.properties; + for (int p = 0; p < numProp; ++p) { + auto oldRange = + StridedRange(oldProp.cbegin() + p, oldProp.cend(), numProp); + auto newRange = StridedRange( + newProp.begin() + numPropOut * propVertIndices[i] + p, + newProp.end(), numPropOut); + copy(oldRange.begin(), oldRange.end(), newRange.begin()); + } + } else { + // point all triangles at single new property of zeros. + fill(start, start + node->pImpl_->NumTri(), + ivec3(propVertIndices[i])); + } + } + + if (node->transform_ == mat3x4(la::identity)) { + copy(node->pImpl_->vertPos_.begin(), node->pImpl_->vertPos_.end(), + combined.vertPos_.begin() + vertIndices[i]); + copy(node->pImpl_->faceNormal_.begin(), + node->pImpl_->faceNormal_.end(), + combined.faceNormal_.begin() + triIndices[i]); + } else { + // no need to apply the transform to the node, just copy the vertices + // and face normals and apply transform on the fly + const mat3x4 transform = node->transform_; + auto vertPosBegin = TransformIterator( + node->pImpl_->vertPos_.begin(), [&transform](vec3 position) { + return transform * vec4(position, 1.0); + }); + mat3 normalTransform = + la::inverse(la::transpose(mat3(node->transform_))); + auto faceNormalBegin = + TransformIterator(node->pImpl_->faceNormal_.begin(), + TransformNormals({normalTransform})); + copy_n(vertPosBegin, node->pImpl_->vertPos_.size(), + combined.vertPos_.begin() + vertIndices[i]); + copy_n(faceNormalBegin, node->pImpl_->faceNormal_.size(), + combined.faceNormal_.begin() + triIndices[i]); + + const bool invert = la::determinant(mat3(node->transform_)) < 0; + for_each_n(policy, countAt(0), node->pImpl_->halfedgeTangent_.size(), + TransformTangents{combined.halfedgeTangent_, + edgeIndices[i], mat3(node->transform_), + invert, node->pImpl_->halfedgeTangent_, + node->pImpl_->halfedge_}); + if (invert) + for_each_n(policy, countAt(triIndices[i]), node->pImpl_->NumTri(), + FlipTris({combined.halfedge_})); + } + // Since the nodes may be copies containing the same meshIDs, it is + // important to add an offset so that each node instance gets + // unique meshIDs. + const int offset = i * Manifold::Impl::meshIDCounter_; + transform(node->pImpl_->meshRelation_.triRef.begin(), + node->pImpl_->meshRelation_.triRef.end(), + combined.meshRelation_.triRef.begin() + triIndices[i], + [offset](TriRef ref) { + ref.meshID += offset; + return ref; + }); + }); + + for (size_t i = 0; i < nodes.size(); i++) { + auto &node = nodes[i]; + const int offset = i * Manifold::Impl::meshIDCounter_; + + for (const auto &pair : node->pImpl_->meshRelation_.meshIDtransform) { + combined.meshRelation_.meshIDtransform[pair.first + offset] = pair.second; + } + } + + // required to remove parts that are smaller than the tolerance + combined.SimplifyTopology(); + combined.Finish(); + combined.IncrementMeshIDs(); + return ImplToLeaf(std::move(combined)); +} + +/** + * Efficient boolean operation on a set of nodes utilizing commutativity of the + * operation. Only supports union and intersection. + */ +std::shared_ptr BatchBoolean( + OpType operation, std::vector> &results) { + ZoneScoped; + DEBUG_ASSERT(operation != OpType::Subtract, logicErr, + "BatchBoolean doesn't support Difference."); + // common cases + if (results.size() == 0) return std::make_shared(); + if (results.size() == 1) return results.front(); + if (results.size() == 2) { + Boolean3 boolean(*results[0]->GetImpl(), *results[1]->GetImpl(), operation); + return ImplToLeaf(boolean.Result(operation)); + } +#if (MANIFOLD_PAR == 1) && __has_include() + tbb::task_group group; + tbb::concurrent_priority_queue, MeshCompare> + queue(results.size()); + for (auto result : results) { + queue.emplace(result); + } + results.clear(); + std::function process = [&]() { + while (queue.size() > 1) { + std::shared_ptr a, b; + if (!queue.try_pop(a)) continue; + if (!queue.try_pop(b)) { + queue.push(a); + continue; + } + group.run([&, a, b]() { + Boolean3 boolean(*a->GetImpl(), *b->GetImpl(), operation); + queue.emplace(ImplToLeaf(boolean.Result(operation))); + return group.run(process); + }); + } + }; + group.run_and_wait(process); + std::shared_ptr r; + queue.try_pop(r); + return r; +#endif + // apply boolean operations starting from smaller meshes + // the assumption is that boolean operations on smaller meshes is faster, + // due to less data being copied and processed + auto cmpFn = MeshCompare(); + std::make_heap(results.begin(), results.end(), cmpFn); + while (results.size() > 1) { + std::pop_heap(results.begin(), results.end(), cmpFn); + auto a = std::move(results.back()); + results.pop_back(); + std::pop_heap(results.begin(), results.end(), cmpFn); + auto b = std::move(results.back()); + results.pop_back(); + // boolean operation + Boolean3 boolean(*a->GetImpl(), *b->GetImpl(), operation); + auto result = ImplToLeaf(boolean.Result(operation)); + if (results.size() == 0) { + return result; + } + results.push_back(result); + std::push_heap(results.begin(), results.end(), cmpFn); + } + return results.front(); +} + +/** + * Efficient union operation on a set of nodes by doing Compose as much as + * possible. + */ +std::shared_ptr BatchUnion( + std::vector> &children) { + ZoneScoped; + // INVARIANT: children_ is a vector of leaf nodes + // this kMaxUnionSize is a heuristic to avoid the pairwise disjoint check + // with O(n^2) complexity to take too long. + // If the number of children exceeded this limit, we will operate on chunks + // with size kMaxUnionSize. + constexpr size_t kMaxUnionSize = 1000; + DEBUG_ASSERT(!children.empty(), logicErr, + "BatchUnion should not have empty children"); + while (children.size() > 1) { + const size_t start = (children.size() > kMaxUnionSize) + ? (children.size() - kMaxUnionSize) + : 0; + Vec boxes; + boxes.reserve(children.size() - start); + for (size_t i = start; i < children.size(); i++) { + boxes.push_back(children[i]->GetImpl()->bBox_); + } + // partition the children into a set of disjoint sets + // each set contains a set of children that are pairwise disjoint + std::vector> disjointSets; + for (size_t i = 0; i < boxes.size(); i++) { + auto lambda = [&boxes, i](const Vec &set) { + return std::find_if(set.begin(), set.end(), [&boxes, i](size_t j) { + return boxes[i].DoesOverlap(boxes[j]); + }) == set.end(); + }; + auto it = std::find_if(disjointSets.begin(), disjointSets.end(), lambda); + if (it == disjointSets.end()) { + disjointSets.push_back(std::vector{i}); + } else { + it->push_back(i); + } + } + // compose each set of disjoint children + std::vector> impls; + for (auto &set : disjointSets) { + if (set.size() == 1) { + impls.push_back(children[start + set[0]]); + } else { + std::vector> tmp; + for (size_t j : set) { + tmp.push_back(children[start + j]); + } + impls.push_back(CsgLeafNode::Compose(tmp)); + } + } + + children.erase(children.begin() + start, children.end()); + children.push_back(BatchBoolean(OpType::Add, impls)); + // move it to the front as we process from the back, and the newly added + // child should be quite complicated + std::swap(children.front(), children.back()); + } + return children.front(); +} + +CsgOpNode::CsgOpNode() {} + +CsgOpNode::CsgOpNode(const std::vector> &children, + OpType op) + : impl_(Impl{}), op_(op) { + auto impl = impl_.GetGuard(); + impl->children_ = children; +} + +std::shared_ptr CsgOpNode::Boolean( + const std::shared_ptr &second, OpType op) { + std::vector> children; + children.push_back(shared_from_this()); + children.push_back(second); + + return std::make_shared(children, op); +} + +std::shared_ptr CsgOpNode::Transform(const mat3x4 &m) const { + auto node = std::make_shared(); + node->impl_ = impl_; + node->transform_ = m * Mat4(transform_); + node->op_ = op_; + return node; +} + +struct CsgStackFrame { + bool finalize; + OpType parent_op; + mat3x4 transform; + std::vector> *destination; + std::shared_ptr op_node; + std::vector> positive_children; + std::vector> negative_children; + + CsgStackFrame(bool finalize, OpType parent_op, mat3x4 transform, + std::vector> *parent, + std::shared_ptr op_node) + : finalize(finalize), + parent_op(parent_op), + transform(transform), + destination(parent), + op_node(op_node) {} +}; + +std::shared_ptr CsgOpNode::ToLeafNode() const { + if (cache_ != nullptr) return cache_; + + // Note: We do need a pointer here to avoid vector pointers from being + // invalidated after pushing elements into the explicit stack. + // It is a `shared_ptr` because we may want to drop the stack frame while + // still referring to some of the elements inside the old frame. + // It is possible to use `unique_ptr`, extending the lifetime of the frame + // when we remove it from the stack, but it is a bit more complicated and + // there is no measurable overhead from using `shared_ptr` here... + std::vector> stack; + // initial node, destination is a nullptr because we don't need to put the + // result anywhere else (except in the cache_). + stack.push_back(std::make_shared( + false, op_, la::identity, nullptr, + std::static_pointer_cast(shared_from_this()))); + + // Instead of actually using recursion in the algorithm, we use an explicit + // stack, do DFS and store the intermediate states into `CsgStackFrame` to + // avoid stack overflow. + // + // Before performing boolean operations, we should make sure that all children + // are `CsgLeafNodes`, i.e. are actual meshes that can be operated on. Hence, + // we do it in two steps: + // 1. Populate `children` (`left_children` and `right_children`, see below) + // If the child is a `CsgOpNode`, we either collapse it or compute its + // boolean operation result. + // 2. Performs boolean after populating the `children` set. + // After a boolean operation is completed, we put the result back to its + // parent's `children` set. + // + // When we populate `children`, we perform collapsing on-the-fly. + // For example, we want to turn `(Union a (Union b c))` into `(Union a b c)`. + // This allows more efficient `BatchBoolean`/`BatchUnion` calls. + // We can do this when the child operation is the same as the parent + // operation, except when the operation is `Subtract` (see below). + // Note that to avoid repeating work, we will not collapse nodes that are + // reused. And in the special case where the children set only contains one + // element, we don't need any operation, so we can collapse that as well. + // Instead of moving `b` and `c` into the parent, and running this collapsing + // check until a fixed point, we remember the `destination` where we should + // put the `CsgLeafNode` into. Normally, the `destination` pointer point to + // the parent `children` set. However, when a child is being collapsed, we + // keep using the old `destination` pointer for the grandchildren. Hence, + // removing a node by collapsing takes O(1) time. We also need to store the + // parent operation type for checking if the node is eligible for collapsing, + // and transform matrix because we need to re-apply the transformation to the + // children. + // + // `Subtract` is handled differently from `Add` and `Intersect`. It is treated + // as two `Add` nodes, `positive_children` and `negative_children`, that + // should be subtracted later. This allows collapsing children `Add` nodes. + // For normal `Add` and `Intersect`, we only use `positive_children`. + // + // `impl->children_` should always contain either the raw set of children or + // the NOT transformed result, while `cache_` should contain the transformed + // result. This is because `impl` can be shared between `CsgOpNode` that + // differ in `transform_`, so we want it to be able to share the result. + // =========================================================================== + // Recursive version (pseudocode only): + // + // void f(CsgOpNode node, OpType parent_op, mat3x4 transform, + // std::vector *destination) { + // auto impl = node->impl_.GetGuard(); + // // can collapse when we have the same operation as the parent and is + // // unique, or when we have only one children. + // const bool canCollapse = (node->op_ == parent_op && IsUnique(node)) || + // impl->children_.size() == 1; + // const mat3x4 transform2 = canCollapse ? transform * node->transform_ + // : la::identity; + // std::vector positive_children, negative_children; + // // for subtract, we pretend the operation is Add for our children. + // auto op = node->op_ == OpType::Subtract ? OpType::Add : node->op_; + // for (size_t i = 0; i < impl->children_.size(); i++) { + // auto child = impl->children_[i]; + // // negative when it is the remaining operands for Subtract + // auto dest = node->op_ == OpType::Subtract && i != 0 ? + // negative_children : positive_children; + // if (canCollapse) dest = destination; + // if (child->GetNodeType() == CsgNodeType::Leaf) + // dest.push_back(child); + // else + // f(child, op, transform2, dest); + // } + // if (canCollapse) return; + // if (node->op_ == OpType::Add) + // impl->children_ = {BatchUnion(positive_children)}; + // else if (node->op_ == OpType::Intersect) + // impl->children_ = {BatchBoolean(Intersect, positive_children)}; + // else // subtract + // impl->children_ = { BatchUnion(positive_children) - + // BatchUnion(negative_children)}; + // // node local transform + // node->cache_ = impl->children_[0].Transform(node.transform); + // // collapsed node transforms + // if (destination) + // destination->push_back(node->cache_->Transform(transform)); + // } + while (!stack.empty()) { + std::shared_ptr frame = stack.back(); + auto impl = frame->op_node->impl_.GetGuard(); + if (frame->finalize) { + switch (frame->op_node->op_) { + case OpType::Add: + impl->children_ = {BatchUnion(frame->positive_children)}; + break; + case OpType::Intersect: { + impl->children_ = { + BatchBoolean(OpType::Intersect, frame->positive_children)}; + break; + }; + case OpType::Subtract: + if (frame->positive_children.empty()) { + // nothing to subtract from, so the result is empty. + impl->children_ = {std::make_shared()}; + } else { + auto positive = BatchUnion(frame->positive_children); + if (frame->negative_children.empty()) { + // nothing to subtract, result equal to the LHS. + impl->children_ = {frame->positive_children[0]}; + } else { + Boolean3 boolean(*positive->GetImpl(), + *BatchUnion(frame->negative_children)->GetImpl(), + OpType::Subtract); + impl->children_ = {ImplToLeaf(boolean.Result(OpType::Subtract))}; + } + } + break; + } + frame->op_node->cache_ = std::static_pointer_cast( + impl->children_[0]->Transform(frame->op_node->transform_)); + if (frame->destination != nullptr) + frame->destination->push_back(std::static_pointer_cast( + frame->op_node->cache_->Transform(frame->transform))); + stack.pop_back(); + } else { + auto add_children = [&stack](std::shared_ptr &node, OpType op, + mat3x4 transform, auto *destination) { + if (node->GetNodeType() == CsgNodeType::Leaf) + destination->push_back(std::static_pointer_cast( + node->Transform(transform))); + else + stack.push_back(std::make_shared( + false, op, transform, destination, + std::static_pointer_cast(node))); + }; + // op_node use_count == 2 because it is both inside one CsgOpNode + // and in our stack. + // if there is only one child, we can also collapse. + const bool canCollapse = frame->destination != nullptr && + ((frame->op_node->op_ == frame->parent_op && + frame->op_node.use_count() <= 2 && + frame->op_node->impl_.UseCount() == 1) || + impl->children_.size() == 1); + if (canCollapse) + stack.pop_back(); + else + frame->finalize = true; + + const mat3x4 transform = + canCollapse ? (frame->transform * Mat4(frame->op_node->transform_)) + : la::identity; + OpType op = frame->op_node->op_ == OpType::Subtract ? OpType::Add + : frame->op_node->op_; + for (size_t i = 0; i < impl->children_.size(); i++) { + auto dest = canCollapse ? frame->destination + : (frame->op_node->op_ == OpType::Subtract && i != 0) + ? &frame->negative_children + : &frame->positive_children; + add_children(impl->children_[i], op, transform, dest); + } + } + } + return cache_; +} + +CsgNodeType CsgOpNode::GetNodeType() const { + switch (op_) { + case OpType::Add: + return CsgNodeType::Union; + case OpType::Subtract: + return CsgNodeType::Difference; + case OpType::Intersect: + return CsgNodeType::Intersection; + } + // unreachable... + return CsgNodeType::Leaf; +} + +} // namespace manifold diff --git a/thirdparty/manifold/src/csg_tree.h b/thirdparty/manifold/src/csg_tree.h new file mode 100644 index 000000000000..5c24dc5a2050 --- /dev/null +++ b/thirdparty/manifold/src/csg_tree.h @@ -0,0 +1,89 @@ +// Copyright 2022 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include "./utils.h" +#include "manifold/manifold.h" + +namespace manifold { + +enum class CsgNodeType { Union, Intersection, Difference, Leaf }; + +class CsgLeafNode; + +class CsgNode : public std::enable_shared_from_this { + public: + virtual std::shared_ptr ToLeafNode() const = 0; + virtual std::shared_ptr Transform(const mat3x4 &m) const = 0; + virtual CsgNodeType GetNodeType() const = 0; + + virtual std::shared_ptr Boolean( + const std::shared_ptr &second, OpType op); + + std::shared_ptr Translate(const vec3 &t) const; + std::shared_ptr Scale(const vec3 &s) const; + std::shared_ptr Rotate(double xDegrees = 0, double yDegrees = 0, + double zDegrees = 0) const; +}; + +class CsgLeafNode final : public CsgNode { + public: + CsgLeafNode(); + CsgLeafNode(std::shared_ptr pImpl_); + CsgLeafNode(std::shared_ptr pImpl_, mat3x4 transform_); + + std::shared_ptr GetImpl() const; + + std::shared_ptr ToLeafNode() const override; + + std::shared_ptr Transform(const mat3x4 &m) const override; + + CsgNodeType GetNodeType() const override; + + static std::shared_ptr Compose( + const std::vector> &nodes); + + private: + mutable std::shared_ptr pImpl_; + mutable mat3x4 transform_ = la::identity; +}; + +class CsgOpNode final : public CsgNode { + public: + CsgOpNode(); + + CsgOpNode(const std::vector> &children, OpType op); + + std::shared_ptr Boolean(const std::shared_ptr &second, + OpType op) override; + + std::shared_ptr Transform(const mat3x4 &m) const override; + + std::shared_ptr ToLeafNode() const override; + + CsgNodeType GetNodeType() const override; + + private: + struct Impl { + std::vector> children_; + bool forcedToLeafNodes_ = false; + }; + mutable ConcurrentSharedPtr impl_ = ConcurrentSharedPtr(Impl{}); + OpType op_; + mat3x4 transform_ = la::identity; + // the following fields are for lazy evaluation, so they are mutable + mutable std::shared_ptr cache_ = nullptr; +}; + +} // namespace manifold diff --git a/thirdparty/manifold/src/edge_op.cpp b/thirdparty/manifold/src/edge_op.cpp new file mode 100644 index 000000000000..d22577a2a2eb --- /dev/null +++ b/thirdparty/manifold/src/edge_op.cpp @@ -0,0 +1,696 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./impl.h" +#include "./parallel.h" + +namespace { +using namespace manifold; + +ivec3 TriOf(int edge) { + ivec3 triEdge; + triEdge[0] = edge; + triEdge[1] = NextHalfedge(triEdge[0]); + triEdge[2] = NextHalfedge(triEdge[1]); + return triEdge; +} + +bool Is01Longest(vec2 v0, vec2 v1, vec2 v2) { + const vec2 e[3] = {v1 - v0, v2 - v1, v0 - v2}; + double l[3]; + for (int i : {0, 1, 2}) l[i] = la::dot(e[i], e[i]); + return l[0] > l[1] && l[0] > l[2]; +} + +struct DuplicateEdge { + const Halfedge* sortedHalfedge; + + bool operator()(int edge) { + const Halfedge& halfedge = sortedHalfedge[edge]; + const Halfedge& nextHalfedge = sortedHalfedge[edge + 1]; + return halfedge.startVert == nextHalfedge.startVert && + halfedge.endVert == nextHalfedge.endVert; + } +}; + +struct ShortEdge { + VecView halfedge; + VecView vertPos; + const double tolerance; + + bool operator()(int edge) const { + if (halfedge[edge].pairedHalfedge < 0) return false; + // Flag short edges + const vec3 delta = + vertPos[halfedge[edge].endVert] - vertPos[halfedge[edge].startVert]; + return la::dot(delta, delta) < tolerance * tolerance; + } +}; + +struct FlagEdge { + VecView halfedge; + VecView triRef; + + bool operator()(int edge) const { + if (halfedge[edge].pairedHalfedge < 0) return false; + // Flag redundant edges - those where the startVert is surrounded by only + // two original triangles. + const TriRef ref0 = triRef[edge / 3]; + int current = NextHalfedge(halfedge[edge].pairedHalfedge); + const TriRef ref1 = triRef[current / 3]; + while (current != edge) { + current = NextHalfedge(halfedge[current].pairedHalfedge); + int tri = current / 3; + const TriRef ref = triRef[tri]; + if (!ref.SameFace(ref0) && !ref.SameFace(ref1)) return false; + } + return true; + } +}; + +struct SwappableEdge { + VecView halfedge; + VecView vertPos; + VecView triNormal; + const double tolerance; + + bool operator()(int edge) const { + if (halfedge[edge].pairedHalfedge < 0) return false; + + int tri = edge / 3; + ivec3 triEdge = TriOf(edge); + mat2x3 projection = GetAxisAlignedProjection(triNormal[tri]); + vec2 v[3]; + for (int i : {0, 1, 2}) + v[i] = projection * vertPos[halfedge[triEdge[i]].startVert]; + if (CCW(v[0], v[1], v[2], tolerance) > 0 || !Is01Longest(v[0], v[1], v[2])) + return false; + + // Switch to neighbor's projection. + edge = halfedge[edge].pairedHalfedge; + tri = edge / 3; + triEdge = TriOf(edge); + projection = GetAxisAlignedProjection(triNormal[tri]); + for (int i : {0, 1, 2}) + v[i] = projection * vertPos[halfedge[triEdge[i]].startVert]; + return CCW(v[0], v[1], v[2], tolerance) > 0 || + Is01Longest(v[0], v[1], v[2]); + } +}; + +struct SortEntry { + int start; + int end; + size_t index; + inline bool operator<(const SortEntry& other) const { + return start == other.start ? end < other.end : start < other.start; + } +}; +} // namespace + +namespace manifold { + +/** + * Duplicates just enough verts to covert an even-manifold to a proper + * 2-manifold, splitting non-manifold verts and edges with too many triangles. + */ +void Manifold::Impl::CleanupTopology() { + if (!halfedge_.size()) return; + + // In the case of a very bad triangulation, it is possible to create pinched + // verts. They must be removed before edge collapse. + SplitPinchedVerts(); + + while (1) { + ZoneScopedN("DedupeEdge"); + + const size_t nbEdges = halfedge_.size(); + size_t numFlagged = 0; + + Vec entries; + entries.reserve(nbEdges / 2); + for (size_t i = 0; i < nbEdges; ++i) { + if (halfedge_[i].IsForward()) { + entries.push_back({halfedge_[i].startVert, halfedge_[i].endVert, i}); + } + } + + stable_sort(entries.begin(), entries.end()); + for (size_t i = 0; i < entries.size() - 1; ++i) { + const int h0 = entries[i].index; + const int h1 = entries[i + 1].index; + if (halfedge_[h0].startVert == halfedge_[h1].startVert && + halfedge_[h0].endVert == halfedge_[h1].endVert) { + DedupeEdge(entries[i].index); + numFlagged++; + } + } + + if (numFlagged == 0) break; + +#ifdef MANIFOLD_DEBUG + if (ManifoldParams().verbose) { + std::cout << "found " << numFlagged << " duplicate edges to split" + << std::endl; + } +#endif + } +} + +/** + * Collapses degenerate triangles by removing edges shorter than tolerance_ and + * any edge that is preceeded by an edge that joins the same two face relations. + * It also performs edge swaps on the long edges of degenerate triangles, though + * there are some configurations of degenerates that cannot be removed this way. + * + * Before collapsing edges, the mesh is checked for duplicate edges (more than + * one pair of triangles sharing the same edge), which are removed by + * duplicating one vert and adding two triangles. These degenerate triangles are + * likely to be collapsed again in the subsequent simplification. + * + * Note when an edge collapse would result in something non-manifold, the + * vertices are duplicated in such a way as to remove handles or separate + * meshes, thus decreasing the Genus(). It only increases when meshes that have + * collapsed to just a pair of triangles are removed entirely. + * + * Rather than actually removing the edges, this step merely marks them for + * removal, by setting vertPos to NaN and halfedge to {-1, -1, -1, -1}. + */ +void Manifold::Impl::SimplifyTopology() { + if (!halfedge_.size()) return; + + CleanupTopology(); + + if (!ManifoldParams().cleanupTriangles) { + return; + } + + const size_t nbEdges = halfedge_.size(); + auto policy = autoPolicy(nbEdges, 1e5); + size_t numFlagged = 0; + Vec bFlags(nbEdges); + + std::vector scratchBuffer; + scratchBuffer.reserve(10); + { + ZoneScopedN("CollapseShortEdge"); + numFlagged = 0; + ShortEdge se{halfedge_, vertPos_, epsilon_}; + for_each_n(policy, countAt(0_uz), nbEdges, + [&](size_t i) { bFlags[i] = se(i); }); + for (size_t i = 0; i < nbEdges; ++i) { + if (bFlags[i]) { + CollapseEdge(i, scratchBuffer); + scratchBuffer.resize(0); + numFlagged++; + } + } + } + +#ifdef MANIFOLD_DEBUG + if (ManifoldParams().verbose && numFlagged > 0) { + std::cout << "found " << numFlagged << " short edges to collapse" + << std::endl; + } +#endif + + { + ZoneScopedN("CollapseFlaggedEdge"); + numFlagged = 0; + FlagEdge se{halfedge_, meshRelation_.triRef}; + for_each_n(policy, countAt(0_uz), nbEdges, + [&](size_t i) { bFlags[i] = se(i); }); + for (size_t i = 0; i < nbEdges; ++i) { + if (bFlags[i]) { + CollapseEdge(i, scratchBuffer); + scratchBuffer.resize(0); + numFlagged++; + } + } + } + +#ifdef MANIFOLD_DEBUG + if (ManifoldParams().verbose && numFlagged > 0) { + std::cout << "found " << numFlagged << " colinear edges to collapse" + << std::endl; + } +#endif + + { + ZoneScopedN("RecursiveEdgeSwap"); + numFlagged = 0; + SwappableEdge se{halfedge_, vertPos_, faceNormal_, tolerance_}; + for_each_n(policy, countAt(0_uz), nbEdges, + [&](size_t i) { bFlags[i] = se(i); }); + std::vector edgeSwapStack; + std::vector visited(halfedge_.size(), -1); + int tag = 0; + for (size_t i = 0; i < nbEdges; ++i) { + if (bFlags[i]) { + numFlagged++; + tag++; + RecursiveEdgeSwap(i, tag, visited, edgeSwapStack, scratchBuffer); + while (!edgeSwapStack.empty()) { + int last = edgeSwapStack.back(); + edgeSwapStack.pop_back(); + RecursiveEdgeSwap(last, tag, visited, edgeSwapStack, scratchBuffer); + } + } + } + } + +#ifdef MANIFOLD_DEBUG + if (ManifoldParams().verbose && numFlagged > 0) { + std::cout << "found " << numFlagged << " edges to swap" << std::endl; + } +#endif +} + +// Deduplicate the given 4-manifold edge by duplicating endVert, thus making the +// edges distinct. Also duplicates startVert if it becomes pinched. +void Manifold::Impl::DedupeEdge(const int edge) { + // Orbit endVert + const int startVert = halfedge_[edge].startVert; + const int endVert = halfedge_[edge].endVert; + int current = halfedge_[NextHalfedge(edge)].pairedHalfedge; + while (current != edge) { + const int vert = halfedge_[current].startVert; + if (vert == startVert) { + // Single topological unit needs 2 faces added to be split + const int newVert = vertPos_.size(); + vertPos_.push_back(vertPos_[endVert]); + if (vertNormal_.size() > 0) vertNormal_.push_back(vertNormal_[endVert]); + current = halfedge_[NextHalfedge(current)].pairedHalfedge; + const int opposite = halfedge_[NextHalfedge(edge)].pairedHalfedge; + + UpdateVert(newVert, current, opposite); + + int newHalfedge = halfedge_.size(); + int newFace = newHalfedge / 3; + int oldFace = current / 3; + int outsideVert = halfedge_[current].startVert; + halfedge_.push_back({endVert, newVert, -1}); + halfedge_.push_back({newVert, outsideVert, -1}); + halfedge_.push_back({outsideVert, endVert, -1}); + PairUp(newHalfedge + 2, halfedge_[current].pairedHalfedge); + PairUp(newHalfedge + 1, current); + if (meshRelation_.triRef.size() > 0) + meshRelation_.triRef.push_back(meshRelation_.triRef[oldFace]); + if (meshRelation_.triProperties.size() > 0) + meshRelation_.triProperties.push_back( + meshRelation_.triProperties[oldFace]); + if (faceNormal_.size() > 0) faceNormal_.push_back(faceNormal_[oldFace]); + + newHalfedge += 3; + ++newFace; + oldFace = opposite / 3; + outsideVert = halfedge_[opposite].startVert; + halfedge_.push_back({newVert, endVert, -1}); + halfedge_.push_back({endVert, outsideVert, -1}); + halfedge_.push_back({outsideVert, newVert, -1}); + PairUp(newHalfedge + 2, halfedge_[opposite].pairedHalfedge); + PairUp(newHalfedge + 1, opposite); + PairUp(newHalfedge, newHalfedge - 3); + if (meshRelation_.triRef.size() > 0) + meshRelation_.triRef.push_back(meshRelation_.triRef[oldFace]); + if (meshRelation_.triProperties.size() > 0) + meshRelation_.triProperties.push_back( + meshRelation_.triProperties[oldFace]); + if (faceNormal_.size() > 0) faceNormal_.push_back(faceNormal_[oldFace]); + + break; + } + + current = halfedge_[NextHalfedge(current)].pairedHalfedge; + } + + if (current == edge) { + // Separate topological unit needs no new faces to be split + const int newVert = vertPos_.size(); + vertPos_.push_back(vertPos_[endVert]); + if (vertNormal_.size() > 0) vertNormal_.push_back(vertNormal_[endVert]); + + ForVert(NextHalfedge(current), [this, newVert](int e) { + halfedge_[e].startVert = newVert; + halfedge_[halfedge_[e].pairedHalfedge].endVert = newVert; + }); + } + + // Orbit startVert + const int pair = halfedge_[edge].pairedHalfedge; + current = halfedge_[NextHalfedge(pair)].pairedHalfedge; + while (current != pair) { + const int vert = halfedge_[current].startVert; + if (vert == endVert) { + break; // Connected: not a pinched vert + } + current = halfedge_[NextHalfedge(current)].pairedHalfedge; + } + + if (current == pair) { + // Split the pinched vert the previous split created. + const int newVert = vertPos_.size(); + vertPos_.push_back(vertPos_[endVert]); + if (vertNormal_.size() > 0) vertNormal_.push_back(vertNormal_[endVert]); + + ForVert(NextHalfedge(current), [this, newVert](int e) { + halfedge_[e].startVert = newVert; + halfedge_[halfedge_[e].pairedHalfedge].endVert = newVert; + }); + } +} + +void Manifold::Impl::PairUp(int edge0, int edge1) { + halfedge_[edge0].pairedHalfedge = edge1; + halfedge_[edge1].pairedHalfedge = edge0; +} + +// Traverses CW around startEdge.endVert from startEdge to endEdge +// (edgeEdge.endVert must == startEdge.endVert), updating each edge to point +// to vert instead. +void Manifold::Impl::UpdateVert(int vert, int startEdge, int endEdge) { + int current = startEdge; + while (current != endEdge) { + halfedge_[current].endVert = vert; + current = NextHalfedge(current); + halfedge_[current].startVert = vert; + current = halfedge_[current].pairedHalfedge; + DEBUG_ASSERT(current != startEdge, logicErr, "infinite loop in decimator!"); + } +} + +// In the event that the edge collapse would create a non-manifold edge, +// instead we duplicate the two verts and attach the manifolds the other way +// across this edge. +void Manifold::Impl::FormLoop(int current, int end) { + int startVert = vertPos_.size(); + vertPos_.push_back(vertPos_[halfedge_[current].startVert]); + int endVert = vertPos_.size(); + vertPos_.push_back(vertPos_[halfedge_[current].endVert]); + + int oldMatch = halfedge_[current].pairedHalfedge; + int newMatch = halfedge_[end].pairedHalfedge; + + UpdateVert(startVert, oldMatch, newMatch); + UpdateVert(endVert, end, current); + + halfedge_[current].pairedHalfedge = newMatch; + halfedge_[newMatch].pairedHalfedge = current; + halfedge_[end].pairedHalfedge = oldMatch; + halfedge_[oldMatch].pairedHalfedge = end; + + RemoveIfFolded(end); +} + +void Manifold::Impl::CollapseTri(const ivec3& triEdge) { + if (halfedge_[triEdge[1]].pairedHalfedge == -1) return; + int pair1 = halfedge_[triEdge[1]].pairedHalfedge; + int pair2 = halfedge_[triEdge[2]].pairedHalfedge; + halfedge_[pair1].pairedHalfedge = pair2; + halfedge_[pair2].pairedHalfedge = pair1; + for (int i : {0, 1, 2}) { + halfedge_[triEdge[i]] = {-1, -1, -1}; + } +} + +void Manifold::Impl::RemoveIfFolded(int edge) { + const ivec3 tri0edge = TriOf(edge); + const ivec3 tri1edge = TriOf(halfedge_[edge].pairedHalfedge); + if (halfedge_[tri0edge[1]].pairedHalfedge == -1) return; + if (halfedge_[tri0edge[1]].endVert == halfedge_[tri1edge[1]].endVert) { + if (halfedge_[tri0edge[1]].pairedHalfedge == tri1edge[2]) { + if (halfedge_[tri0edge[2]].pairedHalfedge == tri1edge[1]) { + for (int i : {0, 1, 2}) + vertPos_[halfedge_[tri0edge[i]].startVert] = vec3(NAN); + } else { + vertPos_[halfedge_[tri0edge[1]].startVert] = vec3(NAN); + } + } else { + if (halfedge_[tri0edge[2]].pairedHalfedge == tri1edge[1]) { + vertPos_[halfedge_[tri1edge[1]].startVert] = vec3(NAN); + } + } + PairUp(halfedge_[tri0edge[1]].pairedHalfedge, + halfedge_[tri1edge[2]].pairedHalfedge); + PairUp(halfedge_[tri0edge[2]].pairedHalfedge, + halfedge_[tri1edge[1]].pairedHalfedge); + for (int i : {0, 1, 2}) { + halfedge_[tri0edge[i]] = {-1, -1, -1}; + halfedge_[tri1edge[i]] = {-1, -1, -1}; + } + } +} + +// Collapses the given edge by removing startVert. May split the mesh +// topologically if the collapse would have resulted in a 4-manifold edge. Do +// not collapse an edge if startVert is pinched - the vert will be marked NaN, +// but other edges may still be pointing to it. +void Manifold::Impl::CollapseEdge(const int edge, std::vector& edges) { + Vec& triRef = meshRelation_.triRef; + Vec& triProp = meshRelation_.triProperties; + + const Halfedge toRemove = halfedge_[edge]; + if (toRemove.pairedHalfedge < 0) return; + + const int endVert = toRemove.endVert; + const ivec3 tri0edge = TriOf(edge); + const ivec3 tri1edge = TriOf(toRemove.pairedHalfedge); + + const vec3 pNew = vertPos_[endVert]; + const vec3 pOld = vertPos_[toRemove.startVert]; + const vec3 delta = pNew - pOld; + const bool shortEdge = la::dot(delta, delta) < tolerance_ * tolerance_; + + // Orbit endVert + int current = halfedge_[tri0edge[1]].pairedHalfedge; + while (current != tri1edge[2]) { + current = NextHalfedge(current); + edges.push_back(current); + current = halfedge_[current].pairedHalfedge; + } + + // Orbit startVert + int start = halfedge_[tri1edge[1]].pairedHalfedge; + if (!shortEdge) { + current = start; + TriRef refCheck = triRef[toRemove.pairedHalfedge / 3]; + vec3 pLast = vertPos_[halfedge_[tri1edge[1]].endVert]; + while (current != tri0edge[2]) { + current = NextHalfedge(current); + vec3 pNext = vertPos_[halfedge_[current].endVert]; + const int tri = current / 3; + const TriRef ref = triRef[tri]; + const mat2x3 projection = GetAxisAlignedProjection(faceNormal_[tri]); + // Don't collapse if the edge is not redundant (this may have changed due + // to the collapse of neighbors). + if (!ref.SameFace(refCheck)) { + refCheck = triRef[edge / 3]; + if (!ref.SameFace(refCheck)) { + return; + } else { + // Don't collapse if the edges separating the faces are not colinear + // (can happen when the two faces are coplanar). + if (CCW(projection * pOld, projection * pLast, projection * pNew, + epsilon_) != 0) + return; + } + } + + // Don't collapse edge if it would cause a triangle to invert. + if (CCW(projection * pNext, projection * pLast, projection * pNew, + epsilon_) < 0) + return; + + pLast = pNext; + current = halfedge_[current].pairedHalfedge; + } + } + + // Remove toRemove.startVert and replace with endVert. + vertPos_[toRemove.startVert] = vec3(NAN); + CollapseTri(tri1edge); + + // Orbit startVert + const int tri0 = edge / 3; + const int tri1 = toRemove.pairedHalfedge / 3; + const int triVert0 = (edge + 1) % 3; + const int triVert1 = toRemove.pairedHalfedge % 3; + current = start; + while (current != tri0edge[2]) { + current = NextHalfedge(current); + + if (triProp.size() > 0) { + // Update the shifted triangles to the vertBary of endVert + const int tri = current / 3; + const int vIdx = current - 3 * tri; + if (triRef[tri].SameFace(triRef[tri0])) { + triProp[tri][vIdx] = triProp[tri0][triVert0]; + } else if (triRef[tri].SameFace(triRef[tri1])) { + triProp[tri][vIdx] = triProp[tri1][triVert1]; + } + } + + const int vert = halfedge_[current].endVert; + const int next = halfedge_[current].pairedHalfedge; + for (size_t i = 0; i < edges.size(); ++i) { + if (vert == halfedge_[edges[i]].endVert) { + FormLoop(edges[i], current); + start = next; + edges.resize(i); + break; + } + } + current = next; + } + + UpdateVert(endVert, start, tri0edge[2]); + CollapseTri(tri0edge); + RemoveIfFolded(start); +} + +void Manifold::Impl::RecursiveEdgeSwap(const int edge, int& tag, + std::vector& visited, + std::vector& edgeSwapStack, + std::vector& edges) { + Vec& triRef = meshRelation_.triRef; + + if (edge < 0) return; + const int pair = halfedge_[edge].pairedHalfedge; + if (pair < 0) return; + + // avoid infinite recursion + if (visited[edge] == tag && visited[pair] == tag) return; + + const ivec3 tri0edge = TriOf(edge); + const ivec3 tri1edge = TriOf(pair); + const ivec3 perm0 = TriOf(edge % 3); + const ivec3 perm1 = TriOf(pair % 3); + + mat2x3 projection = GetAxisAlignedProjection(faceNormal_[edge / 3]); + vec2 v[4]; + for (int i : {0, 1, 2}) + v[i] = projection * vertPos_[halfedge_[tri0edge[i]].startVert]; + // Only operate on the long edge of a degenerate triangle. + if (CCW(v[0], v[1], v[2], tolerance_) > 0 || !Is01Longest(v[0], v[1], v[2])) + return; + + // Switch to neighbor's projection. + projection = GetAxisAlignedProjection(faceNormal_[pair / 3]); + for (int i : {0, 1, 2}) + v[i] = projection * vertPos_[halfedge_[tri0edge[i]].startVert]; + v[3] = projection * vertPos_[halfedge_[tri1edge[2]].startVert]; + + auto SwapEdge = [&]() { + // The 0-verts are swapped to the opposite 2-verts. + const int v0 = halfedge_[tri0edge[2]].startVert; + const int v1 = halfedge_[tri1edge[2]].startVert; + halfedge_[tri0edge[0]].startVert = v1; + halfedge_[tri0edge[2]].endVert = v1; + halfedge_[tri1edge[0]].startVert = v0; + halfedge_[tri1edge[2]].endVert = v0; + PairUp(tri0edge[0], halfedge_[tri1edge[2]].pairedHalfedge); + PairUp(tri1edge[0], halfedge_[tri0edge[2]].pairedHalfedge); + PairUp(tri0edge[2], tri1edge[2]); + // Both triangles are now subsets of the neighboring triangle. + const int tri0 = tri0edge[0] / 3; + const int tri1 = tri1edge[0] / 3; + faceNormal_[tri0] = faceNormal_[tri1]; + triRef[tri0] = triRef[tri1]; + const double l01 = la::length(v[1] - v[0]); + const double l02 = la::length(v[2] - v[0]); + const double a = std::max(0.0, std::min(1.0, l02 / l01)); + // Update properties if applicable + if (meshRelation_.properties.size() > 0) { + Vec& triProp = meshRelation_.triProperties; + Vec& prop = meshRelation_.properties; + triProp[tri0] = triProp[tri1]; + triProp[tri0][perm0[1]] = triProp[tri1][perm1[0]]; + triProp[tri0][perm0[0]] = triProp[tri1][perm1[2]]; + const int numProp = NumProp(); + const int newProp = prop.size() / numProp; + const int propIdx0 = triProp[tri1][perm1[0]]; + const int propIdx1 = triProp[tri1][perm1[1]]; + for (int p = 0; p < numProp; ++p) { + prop.push_back(a * prop[numProp * propIdx0 + p] + + (1 - a) * prop[numProp * propIdx1 + p]); + } + triProp[tri1][perm1[0]] = newProp; + triProp[tri0][perm0[2]] = newProp; + } + + // if the new edge already exists, duplicate the verts and split the mesh. + int current = halfedge_[tri1edge[0]].pairedHalfedge; + const int endVert = halfedge_[tri1edge[1]].endVert; + while (current != tri0edge[1]) { + current = NextHalfedge(current); + if (halfedge_[current].endVert == endVert) { + FormLoop(tri0edge[2], current); + RemoveIfFolded(tri0edge[2]); + return; + } + current = halfedge_[current].pairedHalfedge; + } + }; + + // Only operate if the other triangles are not degenerate. + if (CCW(v[1], v[0], v[3], tolerance_) <= 0) { + if (!Is01Longest(v[1], v[0], v[3])) return; + // Two facing, long-edge degenerates can swap. + SwapEdge(); + const vec2 e23 = v[3] - v[2]; + if (la::dot(e23, e23) < tolerance_ * tolerance_) { + tag++; + CollapseEdge(tri0edge[2], edges); + edges.resize(0); + } else { + visited[edge] = tag; + visited[pair] = tag; + edgeSwapStack.insert(edgeSwapStack.end(), {tri1edge[1], tri1edge[0], + tri0edge[1], tri0edge[0]}); + } + return; + } else if (CCW(v[0], v[3], v[2], tolerance_) <= 0 || + CCW(v[1], v[2], v[3], tolerance_) <= 0) { + return; + } + // Normal path + SwapEdge(); + visited[edge] = tag; + visited[pair] = tag; + edgeSwapStack.insert(edgeSwapStack.end(), + {halfedge_[tri1edge[0]].pairedHalfedge, + halfedge_[tri0edge[1]].pairedHalfedge}); +} + +void Manifold::Impl::SplitPinchedVerts() { + ZoneScoped; + std::vector vertProcessed(NumVert(), false); + std::vector halfedgeProcessed(halfedge_.size(), false); + for (size_t i = 0; i < halfedge_.size(); ++i) { + if (halfedgeProcessed[i]) continue; + int vert = halfedge_[i].startVert; + if (vertProcessed[vert]) { + vertPos_.push_back(vertPos_[vert]); + vert = NumVert() - 1; + } else { + vertProcessed[vert] = true; + } + ForVert(i, [this, &halfedgeProcessed, vert](int current) { + halfedgeProcessed[current] = true; + halfedge_[current].startVert = vert; + halfedge_[halfedge_[current].pairedHalfedge].endVert = vert; + }); + } +} +} // namespace manifold diff --git a/thirdparty/manifold/src/face_op.cpp b/thirdparty/manifold/src/face_op.cpp new file mode 100644 index 000000000000..b1a6b16c735e --- /dev/null +++ b/thirdparty/manifold/src/face_op.cpp @@ -0,0 +1,319 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if (MANIFOLD_PAR == 1) && __has_include() +#include +#define TBB_PREVIEW_CONCURRENT_ORDERED_CONTAINERS 1 +#include +#endif +#include + +#include "./impl.h" +#include "./parallel.h" +#include "manifold/polygon.h" + +namespace manifold { + +using GeneralTriangulation = std::function(int)>; +using AddTriangle = std::function; + +/** + * Triangulates the faces. In this case, the halfedge_ vector is not yet a set + * of triangles as required by this data structure, but is instead a set of + * general faces with the input faceEdge vector having length of the number of + * faces + 1. The values are indicies into the halfedge_ vector for the first + * edge of each face, with the final value being the length of the halfedge_ + * vector itself. Upon return, halfedge_ has been lengthened and properly + * represents the mesh as a set of triangles as usual. In this process the + * faceNormal_ values are retained, repeated as necessary. + */ +void Manifold::Impl::Face2Tri(const Vec& faceEdge, + const Vec& halfedgeRef) { + ZoneScoped; + Vec triVerts; + Vec triNormal; + Vec& triRef = meshRelation_.triRef; + triRef.resize(0); + auto processFace = [&](GeneralTriangulation general, AddTriangle addTri, + int face) { + const int firstEdge = faceEdge[face]; + const int lastEdge = faceEdge[face + 1]; + const int numEdge = lastEdge - firstEdge; + DEBUG_ASSERT(numEdge >= 3, topologyErr, "face has less than three edges."); + const vec3 normal = faceNormal_[face]; + + if (numEdge == 3) { // Single triangle + int mapping[3] = {halfedge_[firstEdge].startVert, + halfedge_[firstEdge + 1].startVert, + halfedge_[firstEdge + 2].startVert}; + ivec3 tri(halfedge_[firstEdge].startVert, + halfedge_[firstEdge + 1].startVert, + halfedge_[firstEdge + 2].startVert); + ivec3 ends(halfedge_[firstEdge].endVert, halfedge_[firstEdge + 1].endVert, + halfedge_[firstEdge + 2].endVert); + if (ends[0] == tri[2]) { + std::swap(tri[1], tri[2]); + std::swap(ends[1], ends[2]); + } + DEBUG_ASSERT(ends[0] == tri[1] && ends[1] == tri[2] && ends[2] == tri[0], + topologyErr, "These 3 edges do not form a triangle!"); + + addTri(face, tri, normal, halfedgeRef[firstEdge]); + } else if (numEdge == 4) { // Pair of triangles + int mapping[4] = {halfedge_[firstEdge].startVert, + halfedge_[firstEdge + 1].startVert, + halfedge_[firstEdge + 2].startVert, + halfedge_[firstEdge + 3].startVert}; + const mat2x3 projection = GetAxisAlignedProjection(normal); + auto triCCW = [&projection, this](const ivec3 tri) { + return CCW(projection * this->vertPos_[tri[0]], + projection * this->vertPos_[tri[1]], + projection * this->vertPos_[tri[2]], epsilon_) >= 0; + }; + + ivec3 tri0(halfedge_[firstEdge].startVert, halfedge_[firstEdge].endVert, + -1); + ivec3 tri1(-1, -1, tri0[0]); + for (const int i : {1, 2, 3}) { + if (halfedge_[firstEdge + i].startVert == tri0[1]) { + tri0[2] = halfedge_[firstEdge + i].endVert; + tri1[0] = tri0[2]; + } + if (halfedge_[firstEdge + i].endVert == tri0[0]) { + tri1[1] = halfedge_[firstEdge + i].startVert; + } + } + DEBUG_ASSERT(la::all(la::gequal(tri0, ivec3(0))) && + la::all(la::gequal(tri1, ivec3(0))), + topologyErr, "non-manifold quad!"); + bool firstValid = triCCW(tri0) && triCCW(tri1); + tri0[2] = tri1[1]; + tri1[2] = tri0[1]; + bool secondValid = triCCW(tri0) && triCCW(tri1); + + if (!secondValid) { + tri0[2] = tri1[0]; + tri1[2] = tri0[0]; + } else if (firstValid) { + vec3 firstCross = vertPos_[tri0[0]] - vertPos_[tri1[0]]; + vec3 secondCross = vertPos_[tri0[1]] - vertPos_[tri1[1]]; + if (la::dot(firstCross, firstCross) < + la::dot(secondCross, secondCross)) { + tri0[2] = tri1[0]; + tri1[2] = tri0[0]; + } + } + + for (const auto& tri : {tri0, tri1}) { + addTri(face, tri, normal, halfedgeRef[firstEdge]); + } + } else { // General triangulation + for (const auto& tri : general(face)) { + addTri(face, tri, normal, halfedgeRef[firstEdge]); + } + } + }; + auto generalTriangulation = [&](int face) { + const vec3 normal = faceNormal_[face]; + const mat2x3 projection = GetAxisAlignedProjection(normal); + const PolygonsIdx polys = + Face2Polygons(halfedge_.cbegin() + faceEdge[face], + halfedge_.cbegin() + faceEdge[face + 1], projection); + return TriangulateIdx(polys, epsilon_); + }; +#if (MANIFOLD_PAR == 1) && __has_include() + tbb::task_group group; + // map from face to triangle + tbb::concurrent_unordered_map> results; + Vec triCount(faceEdge.size()); + triCount.back() = 0; + // precompute number of triangles per face, and launch async tasks to + // triangulate complex faces + for_each(autoPolicy(faceEdge.size(), 1e5), countAt(0_uz), + countAt(faceEdge.size() - 1), [&](size_t face) { + triCount[face] = faceEdge[face + 1] - faceEdge[face] - 2; + DEBUG_ASSERT(triCount[face] >= 1, topologyErr, + "face has less than three edges."); + if (triCount[face] > 2) + group.run([&, face] { + std::vector newTris = generalTriangulation(face); + triCount[face] = newTris.size(); + results[face] = std::move(newTris); + }); + }); + group.wait(); + // prefix sum computation (assign unique index to each face) and preallocation + exclusive_scan(triCount.begin(), triCount.end(), triCount.begin(), 0_uz); + triVerts.resize(triCount.back()); + triNormal.resize(triCount.back()); + triRef.resize(triCount.back()); + + auto processFace2 = std::bind( + processFace, [&](size_t face) { return std::move(results[face]); }, + [&](size_t face, ivec3 tri, vec3 normal, TriRef r) { + triVerts[triCount[face]] = tri; + triNormal[triCount[face]] = normal; + triRef[triCount[face]] = r; + triCount[face]++; + }, + std::placeholders::_1); + // set triangles in parallel + for_each(autoPolicy(faceEdge.size(), 1e4), countAt(0_uz), + countAt(faceEdge.size() - 1), processFace2); +#else + triVerts.reserve(faceEdge.size()); + triNormal.reserve(faceEdge.size()); + triRef.reserve(faceEdge.size()); + auto processFace2 = std::bind( + processFace, generalTriangulation, + [&](size_t _face, ivec3 tri, vec3 normal, TriRef r) { + triVerts.push_back(tri); + triNormal.push_back(normal); + triRef.push_back(r); + }, + std::placeholders::_1); + for (size_t face = 0; face < faceEdge.size() - 1; ++face) { + processFace2(face); + } +#endif + + faceNormal_ = std::move(triNormal); + CreateHalfedges(triVerts); +} + +/** + * Returns a set of 2D polygons formed by the input projection of the vertices + * of the list of Halfedges, which must be an even-manifold, meaning each vert + * must be referenced the same number of times as a startVert and endVert. + */ +PolygonsIdx Manifold::Impl::Face2Polygons(VecView::IterC start, + VecView::IterC end, + mat2x3 projection) const { + std::multimap vert_edge; + for (auto edge = start; edge != end; ++edge) { + vert_edge.emplace( + std::make_pair(edge->startVert, static_cast(edge - start))); + } + + PolygonsIdx polys; + int startEdge = 0; + int thisEdge = startEdge; + while (1) { + if (thisEdge == startEdge) { + if (vert_edge.empty()) break; + startEdge = vert_edge.begin()->second; + thisEdge = startEdge; + polys.push_back({}); + } + int vert = (start + thisEdge)->startVert; + polys.back().push_back({projection * vertPos_[vert], vert}); + const auto result = vert_edge.find((start + thisEdge)->endVert); + DEBUG_ASSERT(result != vert_edge.end(), topologyErr, "non-manifold edge"); + thisEdge = result->second; + vert_edge.erase(result); + } + return polys; +} + +Polygons Manifold::Impl::Slice(double height) const { + Box plane = bBox_; + plane.min.z = plane.max.z = height; + Vec query; + query.push_back(plane); + const SparseIndices collisions = + collider_.Collisions(query.cview()); + + std::unordered_set tris; + for (size_t i = 0; i < collisions.size(); ++i) { + const int tri = collisions.Get(i, 1); + double min = std::numeric_limits::infinity(); + double max = -std::numeric_limits::infinity(); + for (const int j : {0, 1, 2}) { + const double z = vertPos_[halfedge_[3 * tri + j].startVert].z; + min = std::min(min, z); + max = std::max(max, z); + } + + if (min <= height && max > height) { + tris.insert(tri); + } + } + + Polygons polys; + while (!tris.empty()) { + const int startTri = *tris.begin(); + SimplePolygon poly; + + int k = 0; + for (const int j : {0, 1, 2}) { + if (vertPos_[halfedge_[3 * startTri + j].startVert].z > height && + vertPos_[halfedge_[3 * startTri + Next3(j)].startVert].z <= height) { + k = Next3(j); + break; + } + } + + int tri = startTri; + do { + tris.erase(tris.find(tri)); + if (vertPos_[halfedge_[3 * tri + k].endVert].z <= height) { + k = Next3(k); + } + + Halfedge up = halfedge_[3 * tri + k]; + const vec3 below = vertPos_[up.startVert]; + const vec3 above = vertPos_[up.endVert]; + const double a = (height - below.z) / (above.z - below.z); + poly.push_back(vec2(la::lerp(below, above, a))); + + const int pair = up.pairedHalfedge; + tri = pair / 3; + k = Next3(pair % 3); + } while (tri != startTri); + + polys.push_back(poly); + } + + return polys; +} + +Polygons Manifold::Impl::Project() const { + const mat2x3 projection = GetAxisAlignedProjection({0, 0, 1}); + Vec cusps(NumEdge()); + cusps.resize( + copy_if( + halfedge_.cbegin(), halfedge_.cend(), cusps.begin(), + [&](Halfedge edge) { + return faceNormal_[halfedge_[edge.pairedHalfedge].pairedHalfedge / + 3] + .z >= 0 && + faceNormal_[edge.pairedHalfedge / 3].z < 0; + }) - + cusps.begin()); + + PolygonsIdx polysIndexed = + Face2Polygons(cusps.cbegin(), cusps.cend(), projection); + + Polygons polys; + for (const auto& poly : polysIndexed) { + SimplePolygon simple; + for (const PolyVert& polyVert : poly) { + simple.push_back(polyVert.pos); + } + polys.push_back(simple); + } + + return polys; +} +} // namespace manifold diff --git a/thirdparty/manifold/src/hashtable.h b/thirdparty/manifold/src/hashtable.h new file mode 100644 index 000000000000..052c34e09cf5 --- /dev/null +++ b/thirdparty/manifold/src/hashtable.h @@ -0,0 +1,168 @@ +// Copyright 2022 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once +#include + +#include + +#include "./utils.h" +#include "./vec.h" + +namespace { +typedef unsigned long long int Uint64; +typedef Uint64 (*hash_fun_t)(Uint64); +inline constexpr Uint64 kOpen = std::numeric_limits::max(); + +template +T AtomicCAS(T& target, T compare, T val) { + std::atomic& tar = reinterpret_cast&>(target); + tar.compare_exchange_strong(compare, val, std::memory_order_acq_rel); + return compare; +} + +template +void AtomicStore(T& target, T val) { + std::atomic& tar = reinterpret_cast&>(target); + // release is good enough, although not really something general + tar.store(val, std::memory_order_release); +} + +template +T AtomicLoad(const T& target) { + const std::atomic& tar = reinterpret_cast&>(target); + // acquire is good enough, although not general + return tar.load(std::memory_order_acquire); +} + +// https://stackoverflow.com/questions/664014/what-integer-hash-function-are-good-that-accepts-an-integer-hash-key +inline Uint64 hash64bit(Uint64 x) { + x = (x ^ (x >> 30)) * 0xbf58476d1ce4e5b9ull; + x = (x ^ (x >> 27)) * 0x94d049bb133111ebull; + x = x ^ (x >> 31); + return x; +} +} // namespace + +namespace manifold { + +template +class HashTableD { + public: + HashTableD(Vec& keys, Vec& values, std::atomic& used, + uint32_t step = 1) + : step_{step}, keys_{keys}, values_{values}, used_{used} {} + + int Size() const { return keys_.size(); } + + bool Full() const { + return used_.load(std::memory_order_relaxed) * 2 > + static_cast(Size()); + } + + void Insert(Uint64 key, const V& val) { + uint32_t idx = H(key) & (Size() - 1); + while (1) { + if (Full()) return; + Uint64& k = keys_[idx]; + const Uint64 found = AtomicCAS(k, kOpen, key); + if (found == kOpen) { + used_.fetch_add(1, std::memory_order_relaxed); + values_[idx] = val; + return; + } + if (found == key) return; + idx = (idx + step_) & (Size() - 1); + } + } + + V& operator[](Uint64 key) { + uint32_t idx = H(key) & (Size() - 1); + while (1) { + const Uint64 k = AtomicLoad(keys_[idx]); + if (k == key || k == kOpen) { + return values_[idx]; + } + idx = (idx + step_) & (Size() - 1); + } + } + + const V& operator[](Uint64 key) const { + uint32_t idx = H(key) & (Size() - 1); + while (1) { + const Uint64 k = AtomicLoad(keys_[idx]); + if (k == key || k == kOpen) { + return values_[idx]; + } + idx = (idx + step_) & (Size() - 1); + } + } + + Uint64 KeyAt(int idx) const { return AtomicLoad(keys_[idx]); } + V& At(int idx) { return values_[idx]; } + const V& At(int idx) const { return values_[idx]; } + + private: + uint32_t step_; + VecView keys_; + VecView values_; + std::atomic& used_; +}; + +template +class HashTable { + public: + HashTable(size_t size, uint32_t step = 1) + : keys_{size == 0 ? 0 : 1_uz << (int)ceil(log2(size)), kOpen}, + values_{size == 0 ? 0 : 1_uz << (int)ceil(log2(size)), {}}, + step_(step) {} + + HashTable(const HashTable& other) + : keys_(other.keys_), values_(other.values_), step_(other.step_) { + used_.store(other.used_.load()); + } + + HashTable& operator=(const HashTable& other) { + if (this == &other) return *this; + keys_ = other.keys_; + values_ = other.values_; + used_.store(other.used_.load()); + step_ = other.step_; + return *this; + } + + HashTableD D() { return {keys_, values_, used_, step_}; } + + int Entries() const { return used_.load(std::memory_order_relaxed); } + + size_t Size() const { return keys_.size(); } + + bool Full() const { + return used_.load(std::memory_order_relaxed) * 2 > Size(); + } + + double FilledFraction() const { + return static_cast(used_.load(std::memory_order_relaxed)) / Size(); + } + + Vec& GetValueStore() { return values_; } + + static Uint64 Open() { return kOpen; } + + private: + Vec keys_; + Vec values_; + std::atomic used_ = 0; + uint32_t step_; +}; +} // namespace manifold diff --git a/thirdparty/manifold/src/impl.cpp b/thirdparty/manifold/src/impl.cpp new file mode 100644 index 000000000000..59cc0293d6da --- /dev/null +++ b/thirdparty/manifold/src/impl.cpp @@ -0,0 +1,689 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./impl.h" + +#include +#include +#include + +#include "./hashtable.h" +#include "./mesh_fixes.h" +#include "./parallel.h" +#include "./svd.h" + +namespace { +using namespace manifold; + +constexpr uint64_t kRemove = std::numeric_limits::max(); + +void AtomicAddVec3(vec3& target, const vec3& add) { + for (int i : {0, 1, 2}) { + std::atomic& tar = + reinterpret_cast&>(target[i]); + double old_val = tar.load(std::memory_order_relaxed); + while (!tar.compare_exchange_weak(old_val, old_val + add[i], + std::memory_order_relaxed)) { + } + } +} + +struct Transform4x3 { + const mat3x4 transform; + + vec3 operator()(vec3 position) { return transform * vec4(position, 1.0); } +}; + +template +struct AssignNormals { + VecView faceNormal; + VecView vertNormal; + VecView vertPos; + VecView halfedges; + + void operator()(const int face) { + vec3& triNormal = faceNormal[face]; + + ivec3 triVerts; + for (int i : {0, 1, 2}) triVerts[i] = halfedges[3 * face + i].startVert; + + vec3 edge[3]; + for (int i : {0, 1, 2}) { + const int j = (i + 1) % 3; + edge[i] = la::normalize(vertPos[triVerts[j]] - vertPos[triVerts[i]]); + } + + if (calculateTriNormal) { + triNormal = la::normalize(la::cross(edge[0], edge[1])); + if (std::isnan(triNormal.x)) triNormal = vec3(0, 0, 1); + } + + // corner angles + vec3 phi; + double dot = -la::dot(edge[2], edge[0]); + phi[0] = dot >= 1 ? 0 : (dot <= -1 ? kPi : std::acos(dot)); + dot = -la::dot(edge[0], edge[1]); + phi[1] = dot >= 1 ? 0 : (dot <= -1 ? kPi : std::acos(dot)); + phi[2] = kPi - phi[0] - phi[1]; + + // assign weighted sum + for (int i : {0, 1, 2}) { + AtomicAddVec3(vertNormal[triVerts[i]], phi[i] * triNormal); + } + } +}; + +struct UpdateMeshID { + const HashTableD meshIDold2new; + + void operator()(TriRef& ref) { ref.meshID = meshIDold2new[ref.meshID]; } +}; + +struct CoplanarEdge { + VecView> face2face; + VecView triArea; + VecView halfedge; + VecView vertPos; + VecView triRef; + VecView triProp; + const int numProp; + const double epsilon; + const double tolerance; + + void operator()(const int edgeIdx) { + const Halfedge edge = halfedge[edgeIdx]; + const Halfedge pair = halfedge[edge.pairedHalfedge]; + const int edgeFace = edgeIdx / 3; + const int pairFace = edge.pairedHalfedge / 3; + + if (triRef[edgeFace].meshID != triRef[pairFace].meshID) return; + + const vec3 base = vertPos[edge.startVert]; + const int baseNum = edgeIdx - 3 * edgeFace; + const int jointNum = edge.pairedHalfedge - 3 * pairFace; + + if (numProp > 0) { + if (triProp[edgeFace][baseNum] != triProp[pairFace][Next3(jointNum)] || + triProp[edgeFace][Next3(baseNum)] != triProp[pairFace][jointNum]) + return; + } + + if (!edge.IsForward()) return; + + const int edgeNum = baseNum == 0 ? 2 : baseNum - 1; + const int pairNum = jointNum == 0 ? 2 : jointNum - 1; + const vec3 jointVec = vertPos[pair.startVert] - base; + const vec3 edgeVec = + vertPos[halfedge[3 * edgeFace + edgeNum].startVert] - base; + const vec3 pairVec = + vertPos[halfedge[3 * pairFace + pairNum].startVert] - base; + + const double length = std::max(la::length(jointVec), la::length(edgeVec)); + const double lengthPair = + std::max(la::length(jointVec), la::length(pairVec)); + vec3 normal = la::cross(jointVec, edgeVec); + const double area = la::length(normal); + const double areaPair = la::length(la::cross(pairVec, jointVec)); + + // make sure we only write this once + if (edgeIdx % 3 == 0) triArea[edgeFace] = area; + // Don't link degenerate triangles + if (area < length * epsilon || areaPair < lengthPair * epsilon) return; + + const double volume = std::abs(la::dot(normal, pairVec)); + // Only operate on coplanar triangles + if (volume > std::max(area, areaPair) * tolerance) return; + + face2face[edgeIdx] = std::make_pair(edgeFace, pairFace); + } +}; + +struct CheckCoplanarity { + VecView comp2tri; + VecView halfedge; + VecView vertPos; + std::vector* components; + const double tolerance; + + void operator()(int tri) { + const int component = (*components)[tri]; + const int referenceTri = + reinterpret_cast*>(&comp2tri[component]) + ->load(std::memory_order_relaxed); + if (referenceTri < 0 || referenceTri == tri) return; + + const vec3 origin = vertPos[halfedge[3 * referenceTri].startVert]; + const vec3 normal = la::normalize( + la::cross(vertPos[halfedge[3 * referenceTri + 1].startVert] - origin, + vertPos[halfedge[3 * referenceTri + 2].startVert] - origin)); + + for (const int i : {0, 1, 2}) { + const vec3 vert = vertPos[halfedge[3 * tri + i].startVert]; + // If any component vertex is not coplanar with the component's reference + // triangle, unmark the entire component so that none of its triangles are + // marked coplanar. + if (std::abs(la::dot(normal, vert - origin)) > tolerance) { + reinterpret_cast*>(&comp2tri[component]) + ->store(-1, std::memory_order_relaxed); + break; + } + } + } +}; + +int GetLabels(std::vector& components, + const Vec>& edges, int numNodes) { + UnionFind<> uf(numNodes); + for (auto edge : edges) { + if (edge.first == -1 || edge.second == -1) continue; + uf.unionXY(edge.first, edge.second); + } + + return uf.connectedComponents(components); +} + +void DedupePropVerts(manifold::Vec& triProp, + const Vec>& vert2vert, + size_t numPropVert) { + ZoneScoped; + std::vector vertLabels; + const int numLabels = GetLabels(vertLabels, vert2vert, numPropVert); + + std::vector label2vert(numLabels); + for (size_t v = 0; v < numPropVert; ++v) label2vert[vertLabels[v]] = v; + for (auto& prop : triProp) + for (int i : {0, 1, 2}) prop[i] = label2vert[vertLabels[prop[i]]]; +} +} // namespace + +namespace manifold { + +std::atomic Manifold::Impl::meshIDCounter_(1); + +uint32_t Manifold::Impl::ReserveIDs(uint32_t n) { + return Manifold::Impl::meshIDCounter_.fetch_add(n, std::memory_order_relaxed); +} + +/** + * Create either a unit tetrahedron, cube or octahedron. The cube is in the + * first octant, while the others are symmetric about the origin. + */ +Manifold::Impl::Impl(Shape shape, const mat3x4 m) { + std::vector vertPos; + std::vector triVerts; + switch (shape) { + case Shape::Tetrahedron: + vertPos = {{-1.0, -1.0, 1.0}, + {-1.0, 1.0, -1.0}, + {1.0, -1.0, -1.0}, + {1.0, 1.0, 1.0}}; + triVerts = {{2, 0, 1}, {0, 3, 1}, {2, 3, 0}, {3, 2, 1}}; + break; + case Shape::Cube: + vertPos = {{0.0, 0.0, 0.0}, // + {0.0, 0.0, 1.0}, // + {0.0, 1.0, 0.0}, // + {0.0, 1.0, 1.0}, // + {1.0, 0.0, 0.0}, // + {1.0, 0.0, 1.0}, // + {1.0, 1.0, 0.0}, // + {1.0, 1.0, 1.0}}; + triVerts = {{1, 0, 4}, {2, 4, 0}, // + {1, 3, 0}, {3, 1, 5}, // + {3, 2, 0}, {3, 7, 2}, // + {5, 4, 6}, {5, 1, 4}, // + {6, 4, 2}, {7, 6, 2}, // + {7, 3, 5}, {7, 5, 6}}; + break; + case Shape::Octahedron: + vertPos = {{1.0, 0.0, 0.0}, // + {-1.0, 0.0, 0.0}, // + {0.0, 1.0, 0.0}, // + {0.0, -1.0, 0.0}, // + {0.0, 0.0, 1.0}, // + {0.0, 0.0, -1.0}}; + triVerts = {{0, 2, 4}, {1, 5, 3}, // + {2, 1, 4}, {3, 5, 0}, // + {1, 3, 4}, {0, 5, 2}, // + {3, 0, 4}, {2, 5, 1}}; + break; + } + vertPos_ = vertPos; + for (auto& v : vertPos_) v = m * vec4(v, 1.0); + CreateHalfedges(triVerts); + Finish(); + InitializeOriginal(); + CreateFaces(); +} + +void Manifold::Impl::RemoveUnreferencedVerts() { + ZoneScoped; + Vec vertOld2New(NumVert(), 0); + auto policy = autoPolicy(NumVert(), 1e5); + for_each(policy, halfedge_.cbegin(), halfedge_.cend(), + [&vertOld2New](Halfedge h) { + reinterpret_cast*>(&vertOld2New[h.startVert]) + ->store(1, std::memory_order_relaxed); + }); + + const Vec oldVertPos = vertPos_; + + Vec tmpBuffer(oldVertPos.size()); + auto vertIdIter = TransformIterator(countAt(0_uz), [&vertOld2New](size_t i) { + if (vertOld2New[i] > 0) return i; + return std::numeric_limits::max(); + }); + + auto next = + copy_if(vertIdIter, vertIdIter + tmpBuffer.size(), tmpBuffer.begin(), + [](size_t v) { return v != std::numeric_limits::max(); }); + if (next == tmpBuffer.end()) return; + + gather(tmpBuffer.begin(), next, oldVertPos.begin(), vertPos_.begin()); + + vertPos_.resize(std::distance(tmpBuffer.begin(), next)); + + exclusive_scan(vertOld2New.begin(), vertOld2New.end(), vertOld2New.begin()); + + for_each(policy, halfedge_.begin(), halfedge_.end(), + [&vertOld2New](Halfedge& h) { + h.startVert = vertOld2New[h.startVert]; + h.endVert = vertOld2New[h.endVert]; + }); +} + +void Manifold::Impl::InitializeOriginal(bool keepFaceID) { + const int meshID = ReserveIDs(1); + meshRelation_.originalID = meshID; + auto& triRef = meshRelation_.triRef; + triRef.resize(NumTri()); + for_each_n(autoPolicy(NumTri(), 1e5), countAt(0), NumTri(), + [meshID, keepFaceID, &triRef](const int tri) { + triRef[tri] = {meshID, meshID, tri, + keepFaceID ? triRef[tri].faceID : tri}; + }); + meshRelation_.meshIDtransform.clear(); + meshRelation_.meshIDtransform[meshID] = {meshID}; +} + +void Manifold::Impl::CreateFaces() { + ZoneScoped; + Vec> face2face(halfedge_.size(), {-1, -1}); + Vec> vert2vert(halfedge_.size(), {-1, -1}); + Vec triArea(NumTri()); + + const size_t numProp = NumProp(); + if (numProp > 0) { + for_each_n( + autoPolicy(halfedge_.size(), 1e4), countAt(0), halfedge_.size(), + [&vert2vert, numProp, this](const int edgeIdx) { + const Halfedge edge = halfedge_[edgeIdx]; + const Halfedge pair = halfedge_[edge.pairedHalfedge]; + const int edgeFace = edgeIdx / 3; + const int pairFace = edge.pairedHalfedge / 3; + + if (meshRelation_.triRef[edgeFace].meshID != + meshRelation_.triRef[pairFace].meshID) + return; + + const int baseNum = edgeIdx - 3 * edgeFace; + const int jointNum = edge.pairedHalfedge - 3 * pairFace; + + const int prop0 = meshRelation_.triProperties[edgeFace][baseNum]; + const int prop1 = + meshRelation_ + .triProperties[pairFace][jointNum == 2 ? 0 : jointNum + 1]; + if (prop0 == prop1) return; + + bool propEqual = true; + for (size_t p = 0; p < numProp; ++p) { + if (meshRelation_.properties[numProp * prop0 + p] != + meshRelation_.properties[numProp * prop1 + p]) { + propEqual = false; + break; + } + } + if (propEqual) { + vert2vert[edgeIdx] = std::make_pair(prop0, prop1); + } + }); + DedupePropVerts(meshRelation_.triProperties, vert2vert, NumPropVert()); + } + + for_each_n(autoPolicy(halfedge_.size(), 1e4), countAt(0), halfedge_.size(), + CoplanarEdge({face2face, triArea, halfedge_, vertPos_, + meshRelation_.triRef, meshRelation_.triProperties, + meshRelation_.numProp, epsilon_, tolerance_})); + + std::vector components; + const int numComponent = GetLabels(components, face2face, NumTri()); + + Vec comp2tri(numComponent, -1); + for (size_t tri = 0; tri < NumTri(); ++tri) { + const int comp = components[tri]; + const int current = comp2tri[comp]; + if (current < 0 || triArea[tri] > triArea[current]) { + comp2tri[comp] = tri; + triArea[comp] = triArea[tri]; + } + } + + for_each_n(autoPolicy(halfedge_.size(), 1e4), countAt(0), NumTri(), + CheckCoplanarity( + {comp2tri, halfedge_, vertPos_, &components, tolerance_})); + + Vec& triRef = meshRelation_.triRef; + for (size_t tri = 0; tri < NumTri(); ++tri) { + const int referenceTri = comp2tri[components[tri]]; + if (referenceTri >= 0) { + triRef[tri].faceID = referenceTri; + } + } +} + +/** + * Create the halfedge_ data structure from an input triVerts array like Mesh. + */ +void Manifold::Impl::CreateHalfedges(const Vec& triVerts) { + ZoneScoped; + const size_t numTri = triVerts.size(); + const int numHalfedge = 3 * numTri; + // drop the old value first to avoid copy + halfedge_.resize(0); + halfedge_.resize(numHalfedge); + Vec edge(numHalfedge); + Vec ids(numHalfedge); + auto policy = autoPolicy(numTri, 1e5); + sequence(ids.begin(), ids.end()); + for_each_n(policy, countAt(0), numTri, + [this, &edge, &triVerts](const int tri) { + const ivec3& verts = triVerts[tri]; + for (const int i : {0, 1, 2}) { + const int j = (i + 1) % 3; + const int e = 3 * tri + i; + halfedge_[e] = {verts[i], verts[j], -1}; + // Sort the forward halfedges in front of the backward ones + // by setting the highest-order bit. + edge[e] = uint64_t(verts[i] < verts[j] ? 1 : 0) << 63 | + ((uint64_t)std::min(verts[i], verts[j])) << 32 | + std::max(verts[i], verts[j]); + } + }); + // Stable sort is required here so that halfedges from the same face are + // paired together (the triangles were created in face order). In some + // degenerate situations the triangulator can add the same internal edge in + // two different faces, causing this edge to not be 2-manifold. These are + // fixed by duplicating verts in SimplifyTopology. + stable_sort(ids.begin(), ids.end(), [&edge](const int& a, const int& b) { + return edge[a] < edge[b]; + }); + + // Mark opposed triangles for removal + const int numEdge = numHalfedge / 2; + for (int i = 0; i < numEdge; ++i) { + const int pair0 = ids[i]; + Halfedge h0 = halfedge_[pair0]; + int k = i + numEdge; + while (1) { + const int pair1 = ids[k]; + Halfedge h1 = halfedge_[pair1]; + if (h0.startVert != h1.endVert || h0.endVert != h1.startVert) break; + if (halfedge_[NextHalfedge(pair0)].endVert == + halfedge_[NextHalfedge(pair1)].endVert) { + // Reorder so that remaining edges pair up + if (k != i + numEdge) std::swap(ids[i + numEdge], ids[k]); + break; + } + ++k; + if (k >= numHalfedge) break; + } + } + + // Once sorted, the first half of the range is the forward halfedges, which + // correspond to their backward pair at the same offset in the second half + // of the range. + for_each_n(policy, countAt(0), numEdge, [this, &ids, numEdge](int i) { + const int pair0 = ids[i]; + const int pair1 = ids[i + numEdge]; + halfedge_[pair0].pairedHalfedge = pair1; + halfedge_[pair1].pairedHalfedge = pair0; + }); + + // When opposed triangles are removed, they may strand unreferenced verts. + RemoveUnreferencedVerts(); +} + +/** + * Does a full recalculation of the face bounding boxes, including updating + * the collider, but does not resort the faces. + */ +void Manifold::Impl::Update() { + CalculateBBox(); + Vec faceBox; + Vec faceMorton; + GetFaceBoxMorton(faceBox, faceMorton); + collider_.UpdateBoxes(faceBox); +} + +void Manifold::Impl::MarkFailure(Error status) { + bBox_ = Box(); + vertPos_.resize(0); + halfedge_.resize(0); + vertNormal_.resize(0); + faceNormal_.resize(0); + halfedgeTangent_.resize(0); + meshRelation_ = MeshRelationD(); + status_ = status; +} + +void Manifold::Impl::Warp(std::function warpFunc) { + WarpBatch([&warpFunc](VecView vecs) { + for_each(ExecutionPolicy::Seq, vecs.begin(), vecs.end(), warpFunc); + }); +} + +void Manifold::Impl::WarpBatch(std::function)> warpFunc) { + warpFunc(vertPos_.view()); + CalculateBBox(); + if (!IsFinite()) { + MarkFailure(Error::NonFiniteVertex); + return; + } + Update(); + faceNormal_.resize(0); // force recalculation of triNormal + CalculateNormals(); + SetEpsilon(); + Finish(); + CreateFaces(); + meshRelation_.originalID = -1; +} + +Manifold::Impl Manifold::Impl::Transform(const mat3x4& transform_) const { + ZoneScoped; + if (transform_ == mat3x4(la::identity)) return *this; + auto policy = autoPolicy(NumVert()); + Impl result; + if (status_ != Manifold::Error::NoError) { + result.status_ = status_; + return result; + } + if (!all(la::isfinite(transform_))) { + result.MarkFailure(Error::NonFiniteVertex); + return result; + } + result.collider_ = collider_; + result.meshRelation_ = meshRelation_; + result.epsilon_ = epsilon_; + result.tolerance_ = tolerance_; + result.bBox_ = bBox_; + result.halfedge_ = halfedge_; + result.halfedgeTangent_.resize(halfedgeTangent_.size()); + + result.meshRelation_.originalID = -1; + for (auto& m : result.meshRelation_.meshIDtransform) { + m.second.transform = transform_ * Mat4(m.second.transform); + } + + result.vertPos_.resize(NumVert()); + result.faceNormal_.resize(faceNormal_.size()); + result.vertNormal_.resize(vertNormal_.size()); + transform(vertPos_.begin(), vertPos_.end(), result.vertPos_.begin(), + Transform4x3({transform_})); + + mat3 normalTransform = NormalTransform(transform_); + transform(faceNormal_.begin(), faceNormal_.end(), result.faceNormal_.begin(), + TransformNormals({normalTransform})); + transform(vertNormal_.begin(), vertNormal_.end(), result.vertNormal_.begin(), + TransformNormals({normalTransform})); + + const bool invert = la::determinant(mat3(transform_)) < 0; + + if (halfedgeTangent_.size() > 0) { + for_each_n(policy, countAt(0), halfedgeTangent_.size(), + TransformTangents({result.halfedgeTangent_, 0, mat3(transform_), + invert, halfedgeTangent_, halfedge_})); + } + + if (invert) { + for_each_n(policy, countAt(0), result.NumTri(), + FlipTris({result.halfedge_})); + } + + // This optimization does a cheap collider update if the transform is + // axis-aligned. + if (!result.collider_.Transform(transform_)) result.Update(); + + result.CalculateBBox(); + // Scale epsilon by the norm of the 3x3 portion of the transform. + result.epsilon_ *= SpectralNorm(mat3(transform_)); + // Maximum of inherited epsilon loss and translational epsilon loss. + result.SetEpsilon(result.epsilon_); + return result; +} + +/** + * Sets epsilon based on the bounding box, and limits its minimum value + * by the optional input. + */ +void Manifold::Impl::SetEpsilon(double minEpsilon, bool useSingle) { + epsilon_ = MaxEpsilon(minEpsilon, bBox_); + double minTol = epsilon_; + if (useSingle) + minTol = + std::max(minTol, std::numeric_limits::epsilon() * bBox_.Scale()); + tolerance_ = std::max(tolerance_, minTol); +} + +/** + * If face normals are already present, this function uses them to compute + * vertex normals (angle-weighted pseudo-normals); otherwise it also computes + * the face normals. Face normals are only calculated when needed because + * nearly degenerate faces will accrue rounding error, while the Boolean can + * retain their original normal, which is more accurate and can help with + * merging coplanar faces. + * + * If the face normals have been invalidated by an operation like Warp(), + * ensure you do faceNormal_.resize(0) before calling this function to force + * recalculation. + */ +void Manifold::Impl::CalculateNormals() { + ZoneScoped; + vertNormal_.resize(NumVert()); + auto policy = autoPolicy(NumTri(), 1e4); + fill(vertNormal_.begin(), vertNormal_.end(), vec3(0.0)); + bool calculateTriNormal = false; + if (faceNormal_.size() != NumTri()) { + faceNormal_.resize(NumTri()); + calculateTriNormal = true; + } + if (calculateTriNormal) + for_each_n( + policy, countAt(0), NumTri(), + AssignNormals({faceNormal_, vertNormal_, vertPos_, halfedge_})); + else + for_each_n( + policy, countAt(0), NumTri(), + AssignNormals({faceNormal_, vertNormal_, vertPos_, halfedge_})); + for_each(policy, vertNormal_.begin(), vertNormal_.end(), + [](vec3& v) { v = SafeNormalize(v); }); +} + +/** + * Remaps all the contained meshIDs to new unique values to represent new + * instances of these meshes. + */ +void Manifold::Impl::IncrementMeshIDs() { + HashTable meshIDold2new(meshRelation_.meshIDtransform.size() * 2); + // Update keys of the transform map + std::map oldTransforms; + std::swap(meshRelation_.meshIDtransform, oldTransforms); + const int numMeshIDs = oldTransforms.size(); + int nextMeshID = ReserveIDs(numMeshIDs); + for (const auto& pair : oldTransforms) { + meshIDold2new.D().Insert(pair.first, nextMeshID); + meshRelation_.meshIDtransform[nextMeshID++] = pair.second; + } + + const size_t numTri = NumTri(); + for_each_n(autoPolicy(numTri, 1e5), meshRelation_.triRef.begin(), numTri, + UpdateMeshID({meshIDold2new.D()})); +} + +/** + * Returns a sparse array of the bounding box overlaps between the edges of + * the input manifold, Q and the faces of this manifold. Returned indices only + * point to forward halfedges. + */ +SparseIndices Manifold::Impl::EdgeCollisions(const Impl& Q, + bool inverted) const { + ZoneScoped; + Vec edges = CreateTmpEdges(Q.halfedge_); + const size_t numEdge = edges.size(); + Vec QedgeBB(numEdge); + const auto& vertPos = Q.vertPos_; + auto policy = autoPolicy(numEdge, 1e5); + for_each_n( + policy, countAt(0), numEdge, [&QedgeBB, &edges, &vertPos](const int e) { + QedgeBB[e] = Box(vertPos[edges[e].first], vertPos[edges[e].second]); + }); + + SparseIndices q1p2(0); + if (inverted) + q1p2 = collider_.Collisions(QedgeBB.cview()); + else + q1p2 = collider_.Collisions(QedgeBB.cview()); + + if (inverted) + for_each(policy, countAt(0_uz), countAt(q1p2.size()), + ReindexEdge({edges, q1p2})); + else + for_each(policy, countAt(0_uz), countAt(q1p2.size()), + ReindexEdge({edges, q1p2})); + return q1p2; +} + +/** + * Returns a sparse array of the input vertices that project inside the XY + * bounding boxes of the faces of this manifold. + */ +SparseIndices Manifold::Impl::VertexCollisionsZ(VecView vertsIn, + bool inverted) const { + ZoneScoped; + if (inverted) + return collider_.Collisions(vertsIn); + else + return collider_.Collisions(vertsIn); +} + +} // namespace manifold diff --git a/thirdparty/manifold/src/impl.h b/thirdparty/manifold/src/impl.h new file mode 100644 index 000000000000..8f7d700098f1 --- /dev/null +++ b/thirdparty/manifold/src/impl.h @@ -0,0 +1,352 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include + +#include "./collider.h" +#include "./shared.h" +#include "./sparse.h" +#include "./vec.h" +#include "manifold/manifold.h" +#include "manifold/polygon.h" + +namespace manifold { + +/** @ingroup Private */ +struct Manifold::Impl { + struct Relation { + int originalID = -1; + mat3x4 transform = la::identity; + bool backSide = false; + }; + struct MeshRelationD { + /// The originalID of this Manifold if it is an original; -1 otherwise. + int originalID = -1; + int numProp = 0; + Vec properties; + std::map meshIDtransform; + Vec triRef; + Vec triProperties; + }; + struct BaryIndices { + int tri, start4, end4; + }; + + Box bBox_; + double epsilon_ = -1; + double tolerance_ = -1; + Error status_ = Error::NoError; + Vec vertPos_; + Vec halfedge_; + Vec vertNormal_; + Vec faceNormal_; + Vec halfedgeTangent_; + MeshRelationD meshRelation_; + Collider collider_; + + static std::atomic meshIDCounter_; + static uint32_t ReserveIDs(uint32_t); + + Impl() {} + enum class Shape { Tetrahedron, Cube, Octahedron }; + Impl(Shape, const mat3x4 = la::identity); + + template + Impl(const MeshGLP& meshGL) { + const uint32_t numVert = meshGL.NumVert(); + const uint32_t numTri = meshGL.NumTri(); + + if (meshGL.numProp < 3) { + MarkFailure(Error::MissingPositionProperties); + return; + } + + if (meshGL.mergeFromVert.size() != meshGL.mergeToVert.size()) { + MarkFailure(Error::MergeVectorsDifferentLengths); + return; + } + + if (!meshGL.runTransform.empty() && + 12 * meshGL.runOriginalID.size() != meshGL.runTransform.size()) { + MarkFailure(Error::TransformWrongLength); + return; + } + + if (!meshGL.runOriginalID.empty() && !meshGL.runIndex.empty() && + meshGL.runOriginalID.size() + 1 != meshGL.runIndex.size() && + meshGL.runOriginalID.size() != meshGL.runIndex.size()) { + MarkFailure(Error::RunIndexWrongLength); + return; + } + + if (!meshGL.faceID.empty() && meshGL.faceID.size() != meshGL.NumTri()) { + MarkFailure(Error::FaceIDWrongLength); + return; + } + + std::vector prop2vert(numVert); + std::iota(prop2vert.begin(), prop2vert.end(), 0); + for (size_t i = 0; i < meshGL.mergeFromVert.size(); ++i) { + const uint32_t from = meshGL.mergeFromVert[i]; + const uint32_t to = meshGL.mergeToVert[i]; + if (from >= numVert || to >= numVert) { + MarkFailure(Error::MergeIndexOutOfBounds); + return; + } + prop2vert[from] = to; + } + + const auto numProp = meshGL.numProp - 3; + meshRelation_.numProp = numProp; + meshRelation_.properties.resize(meshGL.NumVert() * numProp); + tolerance_ = meshGL.tolerance; + // This will have unreferenced duplicate positions that will be removed by + // Impl::RemoveUnreferencedVerts(). + vertPos_.resize(meshGL.NumVert()); + + for (size_t i = 0; i < meshGL.NumVert(); ++i) { + for (const int j : {0, 1, 2}) + vertPos_[i][j] = meshGL.vertProperties[meshGL.numProp * i + j]; + for (size_t j = 0; j < numProp; ++j) + meshRelation_.properties[i * numProp + j] = + meshGL.vertProperties[meshGL.numProp * i + 3 + j]; + } + + halfedgeTangent_.resize(meshGL.halfedgeTangent.size() / 4); + for (size_t i = 0; i < halfedgeTangent_.size(); ++i) { + for (const int j : {0, 1, 2, 3}) + halfedgeTangent_[i][j] = meshGL.halfedgeTangent[4 * i + j]; + } + + Vec triRef; + if (!meshGL.runOriginalID.empty()) { + auto runIndex = meshGL.runIndex; + const auto runEnd = meshGL.triVerts.size(); + if (runIndex.empty()) { + runIndex = {0, static_cast(runEnd)}; + } else if (runIndex.size() == meshGL.runOriginalID.size()) { + runIndex.push_back(runEnd); + } + triRef.resize(meshGL.NumTri()); + const auto startID = Impl::ReserveIDs(meshGL.runOriginalID.size()); + for (size_t i = 0; i < meshGL.runOriginalID.size(); ++i) { + const int meshID = startID + i; + const int originalID = meshGL.runOriginalID[i]; + for (size_t tri = runIndex[i] / 3; tri < runIndex[i + 1] / 3; ++tri) { + TriRef& ref = triRef[tri]; + ref.meshID = meshID; + ref.originalID = originalID; + ref.tri = meshGL.faceID.empty() ? tri : meshGL.faceID[tri]; + ref.faceID = tri; + } + + if (meshGL.runTransform.empty()) { + meshRelation_.meshIDtransform[meshID] = {originalID}; + } else { + const Precision* m = meshGL.runTransform.data() + 12 * i; + meshRelation_.meshIDtransform[meshID] = {originalID, + {{m[0], m[1], m[2]}, + {m[3], m[4], m[5]}, + {m[6], m[7], m[8]}, + {m[9], m[10], m[11]}}}; + } + } + } + + Vec triVerts; + triVerts.reserve(numTri); + for (size_t i = 0; i < numTri; ++i) { + ivec3 tri; + for (const size_t j : {0, 1, 2}) { + uint32_t vert = (uint32_t)meshGL.triVerts[3 * i + j]; + if (vert >= numVert) { + MarkFailure(Error::VertexOutOfBounds); + return; + } + tri[j] = prop2vert[vert]; + } + if (tri[0] != tri[1] && tri[1] != tri[2] && tri[2] != tri[0]) { + triVerts.push_back(tri); + if (triRef.size() > 0) { + meshRelation_.triRef.push_back(triRef[i]); + } + if (numProp > 0) { + meshRelation_.triProperties.push_back( + ivec3(static_cast(meshGL.triVerts[3 * i]), + static_cast(meshGL.triVerts[3 * i + 1]), + static_cast(meshGL.triVerts[3 * i + 2]))); + } + } + } + + CreateHalfedges(triVerts); + if (!IsManifold()) { + MarkFailure(Error::NotManifold); + return; + } + + CalculateBBox(); + if (!IsFinite()) { + MarkFailure(Error::NonFiniteVertex); + return; + } + SetEpsilon(-1, std::is_same::value); + + SplitPinchedVerts(); + + CalculateNormals(); + + if (meshGL.runOriginalID.empty()) { + InitializeOriginal(); + } + + CreateFaces(); + + SimplifyTopology(); + Finish(); + + // A Manifold created from an input mesh is never an original - the input is + // the original. + meshRelation_.originalID = -1; + } + + inline void ForVert(int halfedge, std::function func) { + int current = halfedge; + do { + current = NextHalfedge(halfedge_[current].pairedHalfedge); + func(current); + } while (current != halfedge); + } + + template + void ForVert( + int halfedge, std::function transform, + std::function binaryOp) { + T here = transform(halfedge); + int current = halfedge; + do { + const int nextHalfedge = NextHalfedge(halfedge_[current].pairedHalfedge); + T next = transform(nextHalfedge); + binaryOp(current, here, next); + here = next; + current = nextHalfedge; + } while (current != halfedge); + } + + void CreateFaces(); + void RemoveUnreferencedVerts(); + void InitializeOriginal(bool keepFaceID = false); + void CreateHalfedges(const Vec& triVerts); + void CalculateNormals(); + void IncrementMeshIDs(); + + void Update(); + void MarkFailure(Error status); + void Warp(std::function warpFunc); + void WarpBatch(std::function)> warpFunc); + Impl Transform(const mat3x4& transform) const; + SparseIndices EdgeCollisions(const Impl& B, bool inverted = false) const; + SparseIndices VertexCollisionsZ(VecView vertsIn, + bool inverted = false) const; + + bool IsEmpty() const { return NumTri() == 0; } + size_t NumVert() const { return vertPos_.size(); } + size_t NumEdge() const { return halfedge_.size() / 2; } + size_t NumTri() const { return halfedge_.size() / 3; } + size_t NumProp() const { return meshRelation_.numProp; } + size_t NumPropVert() const { + return NumProp() == 0 ? NumVert() + : meshRelation_.properties.size() / NumProp(); + } + + // properties.cu + enum class Property { Volume, SurfaceArea }; + double GetProperty(Property prop) const; + void CalculateCurvature(int gaussianIdx, int meanIdx); + void CalculateBBox(); + bool IsFinite() const; + bool IsIndexInBounds(VecView triVerts) const; + void SetEpsilon(double minEpsilon = -1, bool useSingle = false); + bool IsManifold() const; + bool Is2Manifold() const; + bool MatchesTriNormals() const; + int NumDegenerateTris() const; + double MinGap(const Impl& other, double searchLength) const; + + // sort.cu + void Finish(); + void SortVerts(); + void ReindexVerts(const Vec& vertNew2Old, size_t numOldVert); + void CompactProps(); + void GetFaceBoxMorton(Vec& faceBox, Vec& faceMorton) const; + void SortFaces(Vec& faceBox, Vec& faceMorton); + void GatherFaces(const Vec& faceNew2Old); + void GatherFaces(const Impl& old, const Vec& faceNew2Old); + + // face_op.cu + void Face2Tri(const Vec& faceEdge, const Vec& halfedgeRef); + PolygonsIdx Face2Polygons(VecView::IterC start, + VecView::IterC end, + mat2x3 projection) const; + Polygons Slice(double height) const; + Polygons Project() const; + + // edge_op.cu + void CleanupTopology(); + void SimplifyTopology(); + void DedupeEdge(int edge); + void CollapseEdge(int edge, std::vector& edges); + void RecursiveEdgeSwap(int edge, int& tag, std::vector& visited, + std::vector& edgeSwapStack, + std::vector& edges); + void RemoveIfFolded(int edge); + void PairUp(int edge0, int edge1); + void UpdateVert(int vert, int startEdge, int endEdge); + void FormLoop(int current, int end); + void CollapseTri(const ivec3& triEdge); + void SplitPinchedVerts(); + + // subdivision.cpp + int GetNeighbor(int tri) const; + ivec4 GetHalfedges(int tri) const; + BaryIndices GetIndices(int halfedge) const; + void FillRetainedVerts(Vec& vertBary) const; + Vec Subdivide(std::function, + bool = false); + + // smoothing.cpp + bool IsInsideQuad(int halfedge) const; + bool IsMarkedInsideQuad(int halfedge) const; + vec3 GetNormal(int halfedge, int normalIdx) const; + vec4 TangentFromNormal(const vec3& normal, int halfedge) const; + std::vector UpdateSharpenedEdges( + const std::vector&) const; + Vec FlatFaces() const; + Vec VertFlatFace(const Vec&) const; + Vec VertHalfedge() const; + std::vector SharpenEdges(double minSharpAngle, + double minSmoothness) const; + void SharpenTangent(int halfedge, double smoothness); + void SetNormals(int normalIdx, double minSharpAngle); + void LinearizeFlatTangents(); + void DistributeTangents(const Vec& fixedHalfedges); + void CreateTangents(int normalIdx); + void CreateTangents(std::vector); + void Refine(std::function, bool = false); + + // quickhull.cpp + void Hull(VecView vertPos); +}; +} // namespace manifold diff --git a/thirdparty/manifold/src/iters.h b/thirdparty/manifold/src/iters.h new file mode 100644 index 000000000000..8b5110ae1413 --- /dev/null +++ b/thirdparty/manifold/src/iters.h @@ -0,0 +1,314 @@ +// Copyright 2024 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include + +namespace manifold { + +template +struct TransformIterator { + private: + Iter iter; + F f; + + public: + using pointer = void; + using reference = std::invoke_result_t< + F, typename std::iterator_traits>::value_type>; + using difference_type = + typename std::iterator_traits>::difference_type; + using value_type = reference; + using iterator_category = typename std::iterator_traits< + std::remove_const_t>::iterator_category; + + constexpr TransformIterator(Iter iter, F f) : iter(iter), f(f) {} + + TransformIterator& operator=(const TransformIterator& other) { + if (this == &other) return *this; + // don't copy function, should be the same + iter = other.iter; + return *this; + } + + constexpr reference operator*() const { return f(*iter); } + + constexpr reference operator[](size_t i) const { return f(iter[i]); } + + // prefix increment + TransformIterator& operator++() { + iter += 1; + return *this; + } + + // postfix + TransformIterator operator++(int) { + auto old = *this; + operator++(); + return old; + } + + // prefix increment + TransformIterator& operator--() { + iter -= 1; + return *this; + } + + // postfix + TransformIterator operator--(int) { + auto old = *this; + operator--(); + return old; + } + + constexpr TransformIterator operator+(size_t n) const { + return TransformIterator(iter + n, f); + } + + TransformIterator& operator+=(size_t n) { + iter += n; + return *this; + } + + constexpr TransformIterator operator-(size_t n) const { + return TransformIterator(iter - n, f); + } + + TransformIterator& operator-=(size_t n) { + iter -= n; + return *this; + } + + constexpr bool operator==(TransformIterator other) const { + return iter == other.iter; + } + + constexpr bool operator!=(TransformIterator other) const { + return !(iter == other.iter); + } + + constexpr bool operator<(TransformIterator other) const { + return iter < other.iter; + } + + constexpr difference_type operator-(TransformIterator other) const { + return iter - other.iter; + } + + constexpr operator TransformIterator() const { + return TransformIterator(f, iter); + } +}; + +template +struct CountingIterator { + private: + T counter; + + public: + using pointer = void; + using reference = T; + using difference_type = std::make_signed_t; + using value_type = T; + using iterator_category = std::random_access_iterator_tag; + + constexpr CountingIterator(T counter) : counter(counter) {} + + constexpr value_type operator*() const { return counter; } + constexpr value_type operator[](T i) const { return counter + i; } + + // prefix increment + CountingIterator& operator++() { + counter += 1; + return *this; + } + + // postfix + CountingIterator operator++(int) { + auto old = *this; + operator++(); + return old; + } + + // prefix increment + CountingIterator& operator--() { + counter -= 1; + return *this; + } + + // postfix + CountingIterator operator--(int) { + auto old = *this; + operator--(); + return old; + } + + constexpr CountingIterator operator+(T n) const { + return CountingIterator(counter + n); + } + + CountingIterator& operator+=(T n) { + counter += n; + return *this; + } + + constexpr CountingIterator operator-(T n) const { + return CountingIterator(counter - n); + } + + CountingIterator& operator-=(T n) { + counter -= n; + return *this; + } + + constexpr friend bool operator==(CountingIterator a, CountingIterator b) { + return a.counter == b.counter; + } + + constexpr friend bool operator!=(CountingIterator a, CountingIterator b) { + return a.counter != b.counter; + } + + constexpr friend bool operator<(CountingIterator a, CountingIterator b) { + return a.counter < b.counter; + } + + constexpr friend difference_type operator-(CountingIterator a, + CountingIterator b) { + return a.counter - b.counter; + } + + constexpr operator CountingIterator() const { + return CountingIterator(counter); + } +}; + +constexpr CountingIterator countAt(size_t i) { + return CountingIterator(i); +} + +template +struct StridedRange { + private: + struct StridedRangeIter { + private: + Iter iter; + size_t stride; + + public: + using pointer = + typename std::iterator_traits>::pointer; + using reference = + typename std::iterator_traits>::reference; + using difference_type = typename std::iterator_traits< + std::remove_const_t>::difference_type; + using value_type = + typename std::iterator_traits>::value_type; + using iterator_category = typename std::iterator_traits< + std::remove_const_t>::iterator_category; + + constexpr StridedRangeIter(Iter iter, int stride) + : iter(iter), stride(stride) {} + + constexpr reference operator*() { return *iter; } + + constexpr std::add_const_t operator*() const { return *iter; } + + constexpr reference operator[](size_t i) { return iter[i * stride]; } + + constexpr std::add_const_t operator[](size_t i) const { + return iter[i * stride]; + } + + // prefix increment + StridedRangeIter& operator++() { + iter += stride; + return *this; + } + + // postfix + StridedRangeIter operator++(int) { + auto old = *this; + operator++(); + return old; + } + + // prefix increment + StridedRangeIter& operator--() { + iter -= stride; + return *this; + } + + // postfix + StridedRangeIter operator--(int) { + auto old = *this; + operator--(); + return old; + } + + constexpr StridedRangeIter operator+(size_t n) const { + return StridedRangeIter(iter + n * stride, stride); + } + + StridedRangeIter& operator+=(size_t n) { + iter += n * stride; + return *this; + } + + constexpr StridedRangeIter operator-(size_t n) const { + return StridedRangeIter(iter - n * stride, stride); + } + + StridedRangeIter& operator-=(size_t n) { + iter -= n * stride; + return *this; + } + + constexpr friend bool operator==(StridedRangeIter a, StridedRangeIter b) { + return a.iter == b.iter; + } + + constexpr friend bool operator!=(StridedRangeIter a, StridedRangeIter b) { + return !(a.iter == b.iter); + } + + constexpr friend bool operator<(StridedRangeIter a, StridedRangeIter b) { + return a.iter < b.iter; + } + + constexpr friend difference_type operator-(StridedRangeIter a, + StridedRangeIter b) { + // note that this is not well-defined if a.stride != b.stride... + return (a.iter - b.iter) / a.stride; + } + }; + Iter _start, _end; + const size_t stride; + + public: + constexpr StridedRange(Iter start, Iter end, size_t stride) + : _start(start), _end(end), stride(stride) {} + + constexpr StridedRangeIter begin() const { + return StridedRangeIter(_start, stride); + } + + constexpr StridedRangeIter end() const { + return StridedRangeIter(_start, stride) + + ((std::distance(_start, _end) + (stride - 1)) / stride); + } +}; + +} // namespace manifold diff --git a/thirdparty/manifold/src/manifold.cpp b/thirdparty/manifold/src/manifold.cpp new file mode 100644 index 000000000000..a36dd460b9ee --- /dev/null +++ b/thirdparty/manifold/src/manifold.cpp @@ -0,0 +1,1038 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "./boolean3.h" +#include "./csg_tree.h" +#include "./impl.h" +#include "./parallel.h" + +namespace { +using namespace manifold; + +ExecutionParams manifoldParams; + +struct UpdateProperties { + double* properties; + const int numProp; + const double* oldProperties; + const int numOldProp; + const vec3* vertPos; + const ivec3* triProperties; + const Halfedge* halfedges; + std::function propFunc; + + void operator()(int tri) { + for (int i : {0, 1, 2}) { + const int vert = halfedges[3 * tri + i].startVert; + const int propVert = triProperties[tri][i]; + propFunc(properties + numProp * propVert, vertPos[vert], + oldProperties + numOldProp * propVert); + } + } +}; + +Manifold Halfspace(Box bBox, vec3 normal, double originOffset) { + normal = la::normalize(normal); + Manifold cutter = Manifold::Cube(vec3(2.0), true).Translate({1.0, 0.0, 0.0}); + double size = la::length(bBox.Center() - normal * originOffset) + + 0.5 * la::length(bBox.Size()); + cutter = cutter.Scale(vec3(size)).Translate({originOffset, 0.0, 0.0}); + double yDeg = degrees(-std::asin(normal.z)); + double zDeg = degrees(std::atan2(normal.y, normal.x)); + return cutter.Rotate(0.0, yDeg, zDeg); +} + +template +MeshGLP GetMeshGLImpl(const manifold::Manifold::Impl& impl, + int normalIdx) { + ZoneScoped; + const int numProp = impl.NumProp(); + const int numVert = impl.NumPropVert(); + const int numTri = impl.NumTri(); + + const bool isOriginal = impl.meshRelation_.originalID >= 0; + const bool updateNormals = !isOriginal && normalIdx >= 0; + + MeshGLP out; + out.numProp = 3 + numProp; + out.tolerance = impl.tolerance_; + if (std::is_same::value) + out.tolerance = + std::max(out.tolerance, + static_cast(std::numeric_limits::epsilon() * + impl.bBox_.Scale())); + out.triVerts.resize(3 * numTri); + + const int numHalfedge = impl.halfedgeTangent_.size(); + out.halfedgeTangent.resize(4 * numHalfedge); + for (int i = 0; i < numHalfedge; ++i) { + const vec4 t = impl.halfedgeTangent_[i]; + out.halfedgeTangent[4 * i] = t.x; + out.halfedgeTangent[4 * i + 1] = t.y; + out.halfedgeTangent[4 * i + 2] = t.z; + out.halfedgeTangent[4 * i + 3] = t.w; + } + // Sort the triangles into runs + out.faceID.resize(numTri); + std::vector triNew2Old(numTri); + std::iota(triNew2Old.begin(), triNew2Old.end(), 0); + VecView triRef = impl.meshRelation_.triRef; + // Don't sort originals - keep them in order + if (!isOriginal) { + std::sort(triNew2Old.begin(), triNew2Old.end(), [triRef](int a, int b) { + return triRef[a].originalID == triRef[b].originalID + ? triRef[a].meshID < triRef[b].meshID + : triRef[a].originalID < triRef[b].originalID; + }); + } + + std::vector runNormalTransform; + auto addRun = [updateNormals, isOriginal]( + MeshGLP& out, + std::vector& runNormalTransform, int tri, + const manifold::Manifold::Impl::Relation& rel) { + out.runIndex.push_back(3 * tri); + out.runOriginalID.push_back(rel.originalID); + if (updateNormals) { + runNormalTransform.push_back(NormalTransform(rel.transform) * + (rel.backSide ? -1.0 : 1.0)); + } + if (!isOriginal) { + for (const int col : {0, 1, 2, 3}) { + for (const int row : {0, 1, 2}) { + out.runTransform.push_back(rel.transform[col][row]); + } + } + } + }; + + auto meshIDtransform = impl.meshRelation_.meshIDtransform; + int lastID = -1; + for (int tri = 0; tri < numTri; ++tri) { + const int oldTri = triNew2Old[tri]; + const auto ref = triRef[oldTri]; + const int meshID = ref.meshID; + + out.faceID[tri] = ref.tri; + for (const int i : {0, 1, 2}) + out.triVerts[3 * tri + i] = impl.halfedge_[3 * oldTri + i].startVert; + + if (meshID != lastID) { + manifold::Manifold::Impl::Relation rel; + auto it = meshIDtransform.find(meshID); + if (it != meshIDtransform.end()) rel = it->second; + addRun(out, runNormalTransform, tri, rel); + meshIDtransform.erase(meshID); + lastID = meshID; + } + } + // Add runs for originals that did not contribute any faces to the output + for (const auto& pair : meshIDtransform) { + addRun(out, runNormalTransform, numTri, pair.second); + } + out.runIndex.push_back(3 * numTri); + + // Early return for no props + if (numProp == 0) { + out.vertProperties.resize(3 * numVert); + for (int i = 0; i < numVert; ++i) { + const vec3 v = impl.vertPos_[i]; + out.vertProperties[3 * i] = v.x; + out.vertProperties[3 * i + 1] = v.y; + out.vertProperties[3 * i + 2] = v.z; + } + return out; + } + // Duplicate verts with different props + std::vector vert2idx(impl.NumVert(), -1); + std::vector> vertPropPair(impl.NumVert()); + out.vertProperties.reserve(numVert * static_cast(out.numProp)); + + for (size_t run = 0; run < out.runOriginalID.size(); ++run) { + for (size_t tri = out.runIndex[run] / 3; tri < out.runIndex[run + 1] / 3; + ++tri) { + const ivec3 triProp = impl.meshRelation_.triProperties[triNew2Old[tri]]; + for (const int i : {0, 1, 2}) { + const int prop = triProp[i]; + const int vert = out.triVerts[3 * tri + i]; + + auto& bin = vertPropPair[vert]; + bool bFound = false; + for (const auto& b : bin) { + if (b.x == prop) { + bFound = true; + out.triVerts[3 * tri + i] = b.y; + break; + } + } + if (bFound) continue; + const int idx = out.vertProperties.size() / out.numProp; + out.triVerts[3 * tri + i] = idx; + bin.push_back({prop, idx}); + + for (int p : {0, 1, 2}) { + out.vertProperties.push_back(impl.vertPos_[vert][p]); + } + for (int p = 0; p < numProp; ++p) { + out.vertProperties.push_back( + impl.meshRelation_.properties[prop * numProp + p]); + } + + if (updateNormals) { + vec3 normal; + const int start = out.vertProperties.size() - out.numProp; + for (int i : {0, 1, 2}) { + normal[i] = out.vertProperties[start + 3 + normalIdx + i]; + } + normal = la::normalize(runNormalTransform[run] * normal); + for (int i : {0, 1, 2}) { + out.vertProperties[start + 3 + normalIdx + i] = normal[i]; + } + } + + if (vert2idx[vert] == -1) { + vert2idx[vert] = idx; + } else { + out.mergeFromVert.push_back(idx); + out.mergeToVert.push_back(vert2idx[vert]); + } + } + } + } + return out; +} +} // namespace + +namespace manifold { + +/** + * Construct an empty Manifold. + * + */ +Manifold::Manifold() : pNode_{std::make_shared()} {} +Manifold::~Manifold() = default; +Manifold::Manifold(Manifold&&) noexcept = default; +Manifold& Manifold::operator=(Manifold&&) noexcept = default; + +Manifold::Manifold(const Manifold& other) : pNode_(other.pNode_) {} + +Manifold::Manifold(std::shared_ptr pNode) : pNode_(pNode) {} + +Manifold::Manifold(std::shared_ptr pImpl_) + : pNode_(std::make_shared(pImpl_)) {} + +Manifold Manifold::Invalid() { + auto pImpl_ = std::make_shared(); + pImpl_->status_ = Error::InvalidConstruction; + return Manifold(pImpl_); +} + +Manifold& Manifold::operator=(const Manifold& other) { + if (this != &other) { + pNode_ = other.pNode_; + } + return *this; +} + +CsgLeafNode& Manifold::GetCsgLeafNode() const { + if (pNode_->GetNodeType() != CsgNodeType::Leaf) { + pNode_ = pNode_->ToLeafNode(); + } + return *std::static_pointer_cast(pNode_); +} + +/** + * Convert a MeshGL into a Manifold, retaining its properties and merging only + * the positions according to the merge vectors. Will return an empty Manifold + * and set an Error Status if the result is not an oriented 2-manifold. Will + * collapse degenerate triangles and unnecessary vertices. + * + * All fields are read, making this structure suitable for a lossless round-trip + * of data from GetMeshGL. For multi-material input, use ReserveIDs to set a + * unique originalID for each material, and sort the materials into triangle + * runs. + * + * @param meshGL The input MeshGL. + */ +Manifold::Manifold(const MeshGL& meshGL) + : pNode_(std::make_shared(std::make_shared(meshGL))) {} + +/** + * Convert a MeshGL into a Manifold, retaining its properties and merging only + * the positions according to the merge vectors. Will return an empty Manifold + * and set an Error Status if the result is not an oriented 2-manifold. Will + * collapse degenerate triangles and unnecessary vertices. + * + * All fields are read, making this structure suitable for a lossless round-trip + * of data from GetMeshGL. For multi-material input, use ReserveIDs to set a + * unique originalID for each material, and sort the materials into triangle + * runs. + * + * @param meshGL64 The input MeshGL64. + */ +Manifold::Manifold(const MeshGL64& meshGL64) + : pNode_(std::make_shared(std::make_shared(meshGL64))) {} + +/** + * The most complete output of this library, returning a MeshGL that is designed + * to easily push into a renderer, including all interleaved vertex properties + * that may have been input. It also includes relations to all the input meshes + * that form a part of this result and the transforms applied to each. + * + * @param normalIdx If the original MeshGL inputs that formed this manifold had + * properties corresponding to normal vectors, you can specify the first of the + * three consecutive property channels forming the (x, y, z) normals, which will + * cause this output MeshGL to automatically update these normals according to + * the applied transforms and front/back side. normalIdx + 3 must be <= + * numProp, and all original MeshGLs must use the same channels for their + * normals. + */ +MeshGL Manifold::GetMeshGL(int normalIdx) const { + const Impl& impl = *GetCsgLeafNode().GetImpl(); + return GetMeshGLImpl(impl, normalIdx); +} + +/** + * The most complete output of this library, returning a MeshGL that is designed + * to easily push into a renderer, including all interleaved vertex properties + * that may have been input. It also includes relations to all the input meshes + * that form a part of this result and the transforms applied to each. + * + * @param normalIdx If the original MeshGL inputs that formed this manifold had + * properties corresponding to normal vectors, you can specify the first of the + * three consecutive property channels forming the (x, y, z) normals, which will + * cause this output MeshGL to automatically update these normals according to + * the applied transforms and front/back side. normalIdx + 3 must be <= + * numProp, and all original MeshGLs must use the same channels for their + * normals. + */ +MeshGL64 Manifold::GetMeshGL64(int normalIdx) const { + const Impl& impl = *GetCsgLeafNode().GetImpl(); + return GetMeshGLImpl(impl, normalIdx); +} + +/** + * Does the Manifold have any triangles? + */ +bool Manifold::IsEmpty() const { return GetCsgLeafNode().GetImpl()->IsEmpty(); } +/** + * Returns the reason for an input Mesh producing an empty Manifold. This Status + * only applies to Manifolds newly-created from an input Mesh - once they are + * combined into a new Manifold via operations, the status reverts to NoError, + * simply processing the problem mesh as empty. Likewise, empty meshes may still + * show NoError, for instance if they are small enough relative to their + * tolerance to be collapsed to nothing. + */ +Manifold::Error Manifold::Status() const { + return GetCsgLeafNode().GetImpl()->status_; +} +/** + * The number of vertices in the Manifold. + */ +size_t Manifold::NumVert() const { + return GetCsgLeafNode().GetImpl()->NumVert(); +} +/** + * The number of edges in the Manifold. + */ +size_t Manifold::NumEdge() const { + return GetCsgLeafNode().GetImpl()->NumEdge(); +} +/** + * The number of triangles in the Manifold. + */ +size_t Manifold::NumTri() const { return GetCsgLeafNode().GetImpl()->NumTri(); } +/** + * The number of properties per vertex in the Manifold. + */ +size_t Manifold::NumProp() const { + return GetCsgLeafNode().GetImpl()->NumProp(); +} +/** + * The number of property vertices in the Manifold. This will always be >= + * NumVert, as some physical vertices may be duplicated to account for different + * properties on different neighboring triangles. + */ +size_t Manifold::NumPropVert() const { + return GetCsgLeafNode().GetImpl()->NumPropVert(); +} + +/** + * Returns the axis-aligned bounding box of all the Manifold's vertices. + */ +Box Manifold::BoundingBox() const { return GetCsgLeafNode().GetImpl()->bBox_; } + +/** + * Returns the epsilon value of this Manifold's vertices, which tracks the + * approximate rounding error over all the transforms and operations that have + * led to this state. This is the value of ε defining + * [ε-valid](https://github.com/elalish/manifold/wiki/Manifold-Library#definition-of-%CE%B5-valid). + */ +double Manifold::GetEpsilon() const { + return GetCsgLeafNode().GetImpl()->epsilon_; +} + +/** + * Returns the tolerance value of this Manifold. Triangles that are coplanar + * within tolerance tend to be merged and edges shorter than tolerance tend to + * be collapsed. + */ +double Manifold::GetTolerance() const { + return GetCsgLeafNode().GetImpl()->tolerance_; +} + +/** + * Return a copy of the manifold with the set tolerance value. + * This performs mesh simplification when the tolerance value is increased. + */ +Manifold Manifold::SetTolerance(double tolerance) const { + auto impl = std::make_shared(*GetCsgLeafNode().GetImpl()); + if (tolerance > impl->tolerance_) { + impl->tolerance_ = tolerance; + impl->CreateFaces(); + impl->SimplifyTopology(); + impl->Finish(); + } else { + // for reducing tolerance, we need to make sure it is still at least + // equal to epsilon. + impl->tolerance_ = std::max(impl->epsilon_, tolerance); + } + return Manifold(impl); +} + +/** + * The genus is a topological property of the manifold, representing the number + * of "handles". A sphere is 0, torus 1, etc. It is only meaningful for a single + * mesh, so it is best to call Decompose() first. + */ +int Manifold::Genus() const { + int chi = NumVert() - NumEdge() + NumTri(); + return 1 - chi / 2; +} + +/** + * Returns the surface area of the manifold. + */ +double Manifold::SurfaceArea() const { + return GetCsgLeafNode().GetImpl()->GetProperty(Impl::Property::SurfaceArea); +} + +/** + * Returns the volume of the manifold. + */ +double Manifold::Volume() const { + return GetCsgLeafNode().GetImpl()->GetProperty(Impl::Property::Volume); +} + +/** + * If this mesh is an original, this returns its meshID that can be referenced + * by product manifolds' MeshRelation. If this manifold is a product, this + * returns -1. + */ +int Manifold::OriginalID() const { + return GetCsgLeafNode().GetImpl()->meshRelation_.originalID; +} + +/** + * This removes all relations (originalID, faceID, transform) to ancestor meshes + * and this new Manifold is marked an original. It also collapses colinear edges + * - these don't get collapsed at boundaries where originalID changes, so the + * reset may allow flat faces to be further simplified. + */ +Manifold Manifold::AsOriginal() const { + auto oldImpl = GetCsgLeafNode().GetImpl(); + if (oldImpl->status_ != Error::NoError) { + auto newImpl = std::make_shared(); + newImpl->status_ = oldImpl->status_; + return Manifold(std::make_shared(newImpl)); + } + auto newImpl = std::make_shared(*oldImpl); + newImpl->InitializeOriginal(); + newImpl->CreateFaces(); + newImpl->SimplifyTopology(); + newImpl->Finish(); + newImpl->InitializeOriginal(true); + return Manifold(std::make_shared(newImpl)); +} + +/** + * Returns the first of n sequential new unique mesh IDs for marking sets of + * triangles that can be looked up after further operations. Assign to + * MeshGL.runOriginalID vector. + */ +uint32_t Manifold::ReserveIDs(uint32_t n) { + return Manifold::Impl::ReserveIDs(n); +} + +/** + * The triangle normal vectors are saved over the course of operations rather + * than recalculated to avoid rounding error. This checks that triangles still + * match their normal vectors within Precision(). + */ +bool Manifold::MatchesTriNormals() const { + return GetCsgLeafNode().GetImpl()->MatchesTriNormals(); +} + +/** + * The number of triangles that are colinear within Precision(). This library + * attempts to remove all of these, but it cannot always remove all of them + * without changing the mesh by too much. + */ +size_t Manifold::NumDegenerateTris() const { + return GetCsgLeafNode().GetImpl()->NumDegenerateTris(); +} + +/** + * This is a checksum-style verification of the collider, simply returning the + * total number of edge-face bounding box overlaps between this and other. + * + * @param other A Manifold to overlap with. + */ +size_t Manifold::NumOverlaps(const Manifold& other) const { + SparseIndices overlaps = GetCsgLeafNode().GetImpl()->EdgeCollisions( + *other.GetCsgLeafNode().GetImpl()); + int num_overlaps = overlaps.size(); + + overlaps = other.GetCsgLeafNode().GetImpl()->EdgeCollisions( + *GetCsgLeafNode().GetImpl()); + return num_overlaps + overlaps.size(); +} + +/** + * Move this Manifold in space. This operation can be chained. Transforms are + * combined and applied lazily. + * + * @param v The vector to add to every vertex. + */ +Manifold Manifold::Translate(vec3 v) const { + return Manifold(pNode_->Translate(v)); +} + +/** + * Scale this Manifold in space. This operation can be chained. Transforms are + * combined and applied lazily. + * + * @param v The vector to multiply every vertex by per component. + */ +Manifold Manifold::Scale(vec3 v) const { return Manifold(pNode_->Scale(v)); } + +/** + * Applies an Euler angle rotation to the manifold, first about the X axis, then + * Y, then Z, in degrees. We use degrees so that we can minimize rounding error, + * and eliminate it completely for any multiples of 90 degrees. Additionally, + * more efficient code paths are used to update the manifold when the transforms + * only rotate by multiples of 90 degrees. This operation can be chained. + * Transforms are combined and applied lazily. + * + * @param xDegrees First rotation, degrees about the X-axis. + * @param yDegrees Second rotation, degrees about the Y-axis. + * @param zDegrees Third rotation, degrees about the Z-axis. + */ +Manifold Manifold::Rotate(double xDegrees, double yDegrees, + double zDegrees) const { + return Manifold(pNode_->Rotate(xDegrees, yDegrees, zDegrees)); +} + +/** + * Transform this Manifold in space. The first three columns form a 3x3 matrix + * transform and the last is a translation vector. This operation can be + * chained. Transforms are combined and applied lazily. + * + * @param m The affine transform matrix to apply to all the vertices. + */ +Manifold Manifold::Transform(const mat3x4& m) const { + return Manifold(pNode_->Transform(m)); +} + +/** + * Mirror this Manifold over the plane described by the unit form of the given + * normal vector. If the length of the normal is zero, an empty Manifold is + * returned. This operation can be chained. Transforms are combined and applied + * lazily. + * + * @param normal The normal vector of the plane to be mirrored over + */ +Manifold Manifold::Mirror(vec3 normal) const { + if (la::length(normal) == 0.) { + return Manifold(); + } + auto n = la::normalize(normal); + auto m = mat3x4(mat3(la::identity) - 2.0 * la::outerprod(n, n), vec3()); + return Manifold(pNode_->Transform(m)); +} + +/** + * This function does not change the topology, but allows the vertices to be + * moved according to any arbitrary input function. It is easy to create a + * function that warps a geometrically valid object into one which overlaps, but + * that is not checked here, so it is up to the user to choose their function + * with discretion. + * + * @param warpFunc A function that modifies a given vertex position. + */ +Manifold Manifold::Warp(std::function warpFunc) const { + auto oldImpl = GetCsgLeafNode().GetImpl(); + if (oldImpl->status_ != Error::NoError) { + auto pImpl = std::make_shared(); + pImpl->status_ = oldImpl->status_; + return Manifold(std::make_shared(pImpl)); + } + auto pImpl = std::make_shared(*oldImpl); + pImpl->Warp(warpFunc); + return Manifold(std::make_shared(pImpl)); +} + +/** + * Same as Manifold::Warp but calls warpFunc with with + * a VecView which is roughly equivalent to std::span + * pointing to all vec3 elements to be modified in-place + * + * @param warpFunc A function that modifies multiple vertex positions. + */ +Manifold Manifold::WarpBatch( + std::function)> warpFunc) const { + auto oldImpl = GetCsgLeafNode().GetImpl(); + if (oldImpl->status_ != Error::NoError) { + auto pImpl = std::make_shared(); + pImpl->status_ = oldImpl->status_; + return Manifold(std::make_shared(pImpl)); + } + auto pImpl = std::make_shared(*oldImpl); + pImpl->WarpBatch(warpFunc); + return Manifold(std::make_shared(pImpl)); +} + +/** + * Create a new copy of this manifold with updated vertex properties by + * supplying a function that takes the existing position and properties as + * input. You may specify any number of output properties, allowing creation and + * removal of channels. Note: undefined behavior will result if you read past + * the number of input properties or write past the number of output properties. + * + * If propFunc is a nullptr, this function will just set the channel to zeroes. + * + * @param numProp The new number of properties per vertex. + * @param propFunc A function that modifies the properties of a given vertex. + */ +Manifold Manifold::SetProperties( + int numProp, + std::function + propFunc) const { + auto pImpl = std::make_shared(*GetCsgLeafNode().GetImpl()); + const int oldNumProp = NumProp(); + const Vec oldProperties = pImpl->meshRelation_.properties; + + auto& triProperties = pImpl->meshRelation_.triProperties; + if (numProp == 0) { + triProperties.resize(0); + pImpl->meshRelation_.properties.resize(0); + } else { + if (triProperties.size() == 0) { + const int numTri = NumTri(); + triProperties.resize(numTri); + for (int i = 0; i < numTri; ++i) { + for (const int j : {0, 1, 2}) { + triProperties[i][j] = pImpl->halfedge_[3 * i + j].startVert; + } + } + pImpl->meshRelation_.properties = Vec(numProp * NumVert(), 0); + } else { + pImpl->meshRelation_.properties = Vec(numProp * NumPropVert(), 0); + } + for_each_n( + propFunc == nullptr ? ExecutionPolicy::Par : ExecutionPolicy::Seq, + countAt(0), NumTri(), + UpdateProperties( + {pImpl->meshRelation_.properties.data(), numProp, + oldProperties.data(), oldNumProp, pImpl->vertPos_.data(), + triProperties.data(), pImpl->halfedge_.data(), + propFunc == nullptr ? [](double* newProp, vec3 position, + const double* oldProp) { *newProp = 0; } + : propFunc})); + } + + pImpl->meshRelation_.numProp = numProp; + return Manifold(std::make_shared(pImpl)); +} + +/** + * Curvature is the inverse of the radius of curvature, and signed such that + * positive is convex and negative is concave. There are two orthogonal + * principal curvatures at any point on a manifold, with one maximum and the + * other minimum. Gaussian curvature is their product, while mean + * curvature is their sum. This approximates them for every vertex and assigns + * them as vertex properties on the given channels. + * + * @param gaussianIdx The property channel index in which to store the Gaussian + * curvature. An index < 0 will be ignored (stores nothing). The property set + * will be automatically expanded to include the channel index specified. + * + * @param meanIdx The property channel index in which to store the mean + * curvature. An index < 0 will be ignored (stores nothing). The property set + * will be automatically expanded to include the channel index specified. + */ +Manifold Manifold::CalculateCurvature(int gaussianIdx, int meanIdx) const { + auto pImpl = std::make_shared(*GetCsgLeafNode().GetImpl()); + pImpl->CalculateCurvature(gaussianIdx, meanIdx); + return Manifold(std::make_shared(pImpl)); +} + +/** + * Fills in vertex properties for normal vectors, calculated from the mesh + * geometry. Flat faces composed of three or more triangles will remain flat. + * + * @param normalIdx The property channel in which to store the X + * values of the normals. The X, Y, and Z channels will be sequential. The + * property set will be automatically expanded such that NumProp will be at + * least normalIdx + 3. + * + * @param minSharpAngle Any edges with angles greater than this value will + * remain sharp, getting different normal vector properties on each side of the + * edge. By default, no edges are sharp and all normals are shared. With a value + * of zero, the model is faceted and all normals match their triangle normals, + * but in this case it would be better not to calculate normals at all. + */ +Manifold Manifold::CalculateNormals(int normalIdx, double minSharpAngle) const { + auto pImpl = std::make_shared(*GetCsgLeafNode().GetImpl()); + pImpl->SetNormals(normalIdx, minSharpAngle); + return Manifold(std::make_shared(pImpl)); +} + +/** + * Smooths out the Manifold by filling in the halfedgeTangent vectors. The + * geometry will remain unchanged until Refine or RefineToLength is called to + * interpolate the surface. This version uses the supplied vertex normal + * properties to define the tangent vectors. Faces of two coplanar triangles + * will be marked as quads, while faces with three or more will be flat. + * + * @param normalIdx The first property channel of the normals. NumProp must be + * at least normalIdx + 3. Any vertex where multiple normals exist and don't + * agree will result in a sharp edge. + */ +Manifold Manifold::SmoothByNormals(int normalIdx) const { + auto pImpl = std::make_shared(*GetCsgLeafNode().GetImpl()); + if (!IsEmpty()) { + pImpl->CreateTangents(normalIdx); + } + return Manifold(std::make_shared(pImpl)); +} + +/** + * Smooths out the Manifold by filling in the halfedgeTangent vectors. The + * geometry will remain unchanged until Refine or RefineToLength is called to + * interpolate the surface. This version uses the geometry of the triangles and + * pseudo-normals to define the tangent vectors. Faces of two coplanar triangles + * will be marked as quads. + * + * @param minSharpAngle degrees, default 60. Any edges with angles greater than + * this value will remain sharp. The rest will be smoothed to G1 continuity, + * with the caveat that flat faces of three or more triangles will always remain + * flat. With a value of zero, the model is faceted, but in this case there is + * no point in smoothing. + * + * @param minSmoothness range: 0 - 1, default 0. The smoothness applied to sharp + * angles. The default gives a hard edge, while values > 0 will give a small + * fillet on these sharp edges. A value of 1 is equivalent to a minSharpAngle of + * 180 - all edges will be smooth. + */ +Manifold Manifold::SmoothOut(double minSharpAngle, double minSmoothness) const { + auto pImpl = std::make_shared(*GetCsgLeafNode().GetImpl()); + if (!IsEmpty()) { + if (minSmoothness == 0) { + const int numProp = pImpl->meshRelation_.numProp; + Vec properties = pImpl->meshRelation_.properties; + Vec triProperties = pImpl->meshRelation_.triProperties; + pImpl->SetNormals(0, minSharpAngle); + pImpl->CreateTangents(0); + pImpl->meshRelation_.numProp = numProp; + pImpl->meshRelation_.properties.swap(properties); + pImpl->meshRelation_.triProperties.swap(triProperties); + } else { + pImpl->CreateTangents(pImpl->SharpenEdges(minSharpAngle, minSmoothness)); + } + } + return Manifold(std::make_shared(pImpl)); +} + +/** + * Increase the density of the mesh by splitting every edge into n pieces. For + * instance, with n = 2, each triangle will be split into 4 triangles. Quads + * will ignore their interior triangle bisector. These will all be coplanar (and + * will not be immediately collapsed) unless the Mesh/Manifold has + * halfedgeTangents specified (e.g. from the Smooth() constructor), in which + * case the new vertices will be moved to the interpolated surface according to + * their barycentric coordinates. + * + * @param n The number of pieces to split every edge into. Must be > 1. + */ +Manifold Manifold::Refine(int n) const { + auto pImpl = std::make_shared(*GetCsgLeafNode().GetImpl()); + if (n > 1) { + pImpl->Refine( + [n](vec3 edge, vec4 tangentStart, vec4 tangentEnd) { return n - 1; }); + } + return Manifold(std::make_shared(pImpl)); +} + +/** + * Increase the density of the mesh by splitting each edge into pieces of + * roughly the input length. Interior verts are added to keep the rest of the + * triangulation edges also of roughly the same length. If halfedgeTangents are + * present (e.g. from the Smooth() constructor), the new vertices will be moved + * to the interpolated surface according to their barycentric coordinates. Quads + * will ignore their interior triangle bisector. + * + * @param length The length that edges will be broken down to. + */ +Manifold Manifold::RefineToLength(double length) const { + length = std::abs(length); + auto pImpl = std::make_shared(*GetCsgLeafNode().GetImpl()); + pImpl->Refine([length](vec3 edge, vec4 tangentStart, vec4 tangentEnd) { + return static_cast(la::length(edge) / length); + }); + return Manifold(std::make_shared(pImpl)); +} + +/** + * Increase the density of the mesh by splitting each edge into pieces such that + * any point on the resulting triangles is roughly within tolerance of the + * smoothly curved surface defined by the tangent vectors. This means tightly + * curving regions will be divided more finely than smoother regions. If + * halfedgeTangents are not present, the result will simply be a copy of the + * original. Quads will ignore their interior triangle bisector. + * + * @param tolerance The desired maximum distance between the faceted mesh + * produced and the exact smoothly curving surface. All vertices are exactly on + * the surface, within rounding error. + */ +Manifold Manifold::RefineToTolerance(double tolerance) const { + tolerance = std::abs(tolerance); + auto pImpl = std::make_shared(*GetCsgLeafNode().GetImpl()); + if (!pImpl->halfedgeTangent_.empty()) { + pImpl->Refine( + [tolerance](vec3 edge, vec4 tangentStart, vec4 tangentEnd) { + const vec3 edgeNorm = la::normalize(edge); + // Weight heuristic + const vec3 tStart = vec3(tangentStart); + const vec3 tEnd = vec3(tangentEnd); + // Perpendicular to edge + const vec3 start = tStart - edgeNorm * la::dot(edgeNorm, tStart); + const vec3 end = tEnd - edgeNorm * la::dot(edgeNorm, tEnd); + // Circular arc result plus heuristic term for non-circular curves + const double d = 0.5 * (la::length(start) + la::length(end)) + + la::length(start - end); + return static_cast(std::sqrt(3 * d / (4 * tolerance))); + }, + true); + } + return Manifold(std::make_shared(pImpl)); +} + +/** + * The central operation of this library: the Boolean combines two manifolds + * into another by calculating their intersections and removing the unused + * portions. + * [ε-valid](https://github.com/elalish/manifold/wiki/Manifold-Library#definition-of-%CE%B5-valid) + * inputs will produce ε-valid output. ε-invalid input may fail + * triangulation. + * + * These operations are optimized to produce nearly-instant results if either + * input is empty or their bounding boxes do not overlap. + * + * @param second The other Manifold. + * @param op The type of operation to perform. + */ +Manifold Manifold::Boolean(const Manifold& second, OpType op) const { + return Manifold(pNode_->Boolean(second.pNode_, op)); +} + +/** + * Perform the given boolean operation on a list of Manifolds. In case of + * Subtract, all Manifolds in the tail are differenced from the head. + */ +Manifold Manifold::BatchBoolean(const std::vector& manifolds, + OpType op) { + if (manifolds.size() == 0) + return Manifold(); + else if (manifolds.size() == 1) + return manifolds[0]; + std::vector> children; + children.reserve(manifolds.size()); + for (const auto& m : manifolds) children.push_back(m.pNode_); + return Manifold(std::make_shared(children, op)); +} + +/** + * Shorthand for Boolean Union. + */ +Manifold Manifold::operator+(const Manifold& Q) const { + return Boolean(Q, OpType::Add); +} + +/** + * Shorthand for Boolean Union assignment. + */ +Manifold& Manifold::operator+=(const Manifold& Q) { + *this = *this + Q; + return *this; +} + +/** + * Shorthand for Boolean Difference. + */ +Manifold Manifold::operator-(const Manifold& Q) const { + return Boolean(Q, OpType::Subtract); +} + +/** + * Shorthand for Boolean Difference assignment. + */ +Manifold& Manifold::operator-=(const Manifold& Q) { + *this = *this - Q; + return *this; +} + +/** + * Shorthand for Boolean Intersection. + */ +Manifold Manifold::operator^(const Manifold& Q) const { + return Boolean(Q, OpType::Intersect); +} + +/** + * Shorthand for Boolean Intersection assignment. + */ +Manifold& Manifold::operator^=(const Manifold& Q) { + *this = *this ^ Q; + return *this; +} + +/** + * Split cuts this manifold in two using the cutter manifold. The first result + * is the intersection, second is the difference. This is more efficient than + * doing them separately. + * + * @param cutter + */ +std::pair Manifold::Split(const Manifold& cutter) const { + auto impl1 = GetCsgLeafNode().GetImpl(); + auto impl2 = cutter.GetCsgLeafNode().GetImpl(); + + Boolean3 boolean(*impl1, *impl2, OpType::Subtract); + auto result1 = std::make_shared( + std::make_unique(boolean.Result(OpType::Intersect))); + auto result2 = std::make_shared( + std::make_unique(boolean.Result(OpType::Subtract))); + return std::make_pair(Manifold(result1), Manifold(result2)); +} + +/** + * Convenient version of Split() for a half-space. + * + * @param normal This vector is normal to the cutting plane and its length does + * not matter. The first result is in the direction of this vector, the second + * result is on the opposite side. + * @param originOffset The distance of the plane from the origin in the + * direction of the normal vector. + */ +std::pair Manifold::SplitByPlane( + vec3 normal, double originOffset) const { + return Split(Halfspace(BoundingBox(), normal, originOffset)); +} + +/** + * Identical to SplitByPlane(), but calculating and returning only the first + * result. + * + * @param normal This vector is normal to the cutting plane and its length does + * not matter. The result is in the direction of this vector from the plane. + * @param originOffset The distance of the plane from the origin in the + * direction of the normal vector. + */ +Manifold Manifold::TrimByPlane(vec3 normal, double originOffset) const { + return *this ^ Halfspace(BoundingBox(), normal, originOffset); +} + +/** + * Returns the cross section of this object parallel to the X-Y plane at the + * specified Z height, defaulting to zero. Using a height equal to the bottom of + * the bounding box will return the bottom faces, while using a height equal to + * the top of the bounding box will return empty. + */ +Polygons Manifold::Slice(double height) const { + return GetCsgLeafNode().GetImpl()->Slice(height); +} + +/** + * Returns polygons representing the projected outline of this object + * onto the X-Y plane. These polygons will often self-intersect, so it is + * recommended to run them through the positive fill rule of CrossSection to get + * a sensible result before using them. + */ +Polygons Manifold::Project() const { + return GetCsgLeafNode().GetImpl()->Project(); +} + +ExecutionParams& ManifoldParams() { return manifoldParams; } + +/** + * Compute the convex hull of a set of points. If the given points are fewer + * than 4, or they are all coplanar, an empty Manifold will be returned. + * + * @param pts A vector of 3-dimensional points over which to compute a convex + * hull. + */ +Manifold Manifold::Hull(const std::vector& pts) { + std::shared_ptr impl = std::make_shared(); + impl->Hull(Vec(pts)); + return Manifold(std::make_shared(impl)); +} + +/** + * Compute the convex hull of this manifold. + */ +Manifold Manifold::Hull() const { + std::shared_ptr impl = std::make_shared(); + impl->Hull(GetCsgLeafNode().GetImpl()->vertPos_); + return Manifold(std::make_shared(impl)); +} + +/** + * Compute the convex hull enveloping a set of manifolds. + * + * @param manifolds A vector of manifolds over which to compute a convex hull. + */ +Manifold Manifold::Hull(const std::vector& manifolds) { + return Compose(manifolds).Hull(); +} + +/** + * Returns the minimum gap between two manifolds. Returns a double between + * 0 and searchLength. + * + * @param other The other manifold to compute the minimum gap to. + * @param searchLength The maximum distance to search for a minimum gap. + */ +double Manifold::MinGap(const Manifold& other, double searchLength) const { + auto intersect = *this ^ other; + if (!intersect.IsEmpty()) return 0.0; + + return GetCsgLeafNode().GetImpl()->MinGap(*other.GetCsgLeafNode().GetImpl(), + searchLength); +} +} // namespace manifold diff --git a/thirdparty/manifold/src/mesh_fixes.h b/thirdparty/manifold/src/mesh_fixes.h new file mode 100644 index 000000000000..d4aa800b4a7e --- /dev/null +++ b/thirdparty/manifold/src/mesh_fixes.h @@ -0,0 +1,65 @@ +// Copyright 2024 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once +#include "./shared.h" + +namespace { +using namespace manifold; + +inline int FlipHalfedge(int halfedge) { + const int tri = halfedge / 3; + const int vert = 2 - (halfedge - 3 * tri); + return 3 * tri + vert; +} + +struct TransformNormals { + mat3 transform; + + vec3 operator()(vec3 normal) const { + normal = la::normalize(transform * normal); + if (std::isnan(normal.x)) normal = vec3(0.0); + return normal; + } +}; + +struct TransformTangents { + VecView tangent; + const int edgeOffset; + const mat3 transform; + const bool invert; + VecView oldTangents; + VecView halfedge; + + void operator()(const int edgeOut) { + const int edgeIn = + invert ? halfedge[FlipHalfedge(edgeOut)].pairedHalfedge : edgeOut; + tangent[edgeOut + edgeOffset] = + vec4(transform * vec3(oldTangents[edgeIn]), oldTangents[edgeIn].w); + } +}; + +struct FlipTris { + VecView halfedge; + + void operator()(const int tri) { + std::swap(halfedge[3 * tri], halfedge[3 * tri + 2]); + + for (const int i : {0, 1, 2}) { + std::swap(halfedge[3 * tri + i].startVert, halfedge[3 * tri + i].endVert); + halfedge[3 * tri + i].pairedHalfedge = + FlipHalfedge(halfedge[3 * tri + i].pairedHalfedge); + } + } +}; +} // namespace diff --git a/thirdparty/manifold/src/parallel.h b/thirdparty/manifold/src/parallel.h new file mode 100644 index 000000000000..a434939494c6 --- /dev/null +++ b/thirdparty/manifold/src/parallel.h @@ -0,0 +1,1125 @@ +// Copyright 2022 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Simple implementation of selected functions in PSTL. +// Iterators must be RandomAccessIterator. + +#pragma once + +#include "./iters.h" +#if (MANIFOLD_PAR == 1) +#include +#include +#include +#include +#include +#endif +#include +#include + +namespace manifold { + +enum class ExecutionPolicy { + Par, + Seq, +}; + +constexpr size_t kSeqThreshold = 1e4; +// ExecutionPolicy: +// - Sequential for small workload, +// - Parallel (CPU) for medium workload, +inline constexpr ExecutionPolicy autoPolicy(size_t size, + size_t threshold = kSeqThreshold) { + if (size <= threshold) { + return ExecutionPolicy::Seq; + } + return ExecutionPolicy::Par; +} + +template >> +inline constexpr ExecutionPolicy autoPolicy(Iter first, Iter last, + size_t threshold = kSeqThreshold) { + if (static_cast(std::distance(first, last)) <= threshold) { + return ExecutionPolicy::Seq; + } + return ExecutionPolicy::Par; +} + +template +void copy(ExecutionPolicy policy, InputIter first, InputIter last, + OutputIter d_first); +template +void copy(InputIter first, InputIter last, OutputIter d_first); + +#if (MANIFOLD_PAR == 1) +namespace details { +using manifold::kSeqThreshold; +// implementation from +// https://duvanenko.tech.blog/2018/01/14/parallel-merge/ +// https://github.com/DragonSpit/ParallelAlgorithms +// note that the ranges are now [p, r) to fit our convention. +template +void mergeRec(SrcIter src, DestIter dest, size_t p1, size_t r1, size_t p2, + size_t r2, size_t p3, Comp comp) { + size_t length1 = r1 - p1; + size_t length2 = r2 - p2; + if (length1 < length2) { + std::swap(p1, p2); + std::swap(r1, r2); + std::swap(length1, length2); + } + if (length1 == 0) return; + if (length1 + length2 <= kSeqThreshold) { + std::merge(src + p1, src + r1, src + p2, src + r2, dest + p3, comp); + } else { + size_t q1 = p1 + length1 / 2; + size_t q2 = + std::distance(src, std::lower_bound(src + p2, src + r2, src[q1], comp)); + size_t q3 = p3 + (q1 - p1) + (q2 - p2); + dest[q3] = src[q1]; + tbb::parallel_invoke( + [=] { mergeRec(src, dest, p1, q1, p2, q2, p3, comp); }, + [=] { mergeRec(src, dest, q1 + 1, r1, q2, r2, q3 + 1, comp); }); + } +} + +template +void mergeSortRec(SrcIter src, DestIter dest, size_t begin, size_t end, + Comp comp) { + size_t numElements = end - begin; + if (numElements <= kSeqThreshold) { + std::copy(src + begin, src + end, dest + begin); + std::stable_sort(dest + begin, dest + end, comp); + } else { + size_t middle = begin + numElements / 2; + tbb::parallel_invoke([=] { mergeSortRec(dest, src, begin, middle, comp); }, + [=] { mergeSortRec(dest, src, middle, end, comp); }); + mergeRec(src, dest, begin, middle, middle, end, begin, comp); + } +} + +template +struct ScanBody { + T sum; + T identity; + BinOp &f; + InputIter input; + OutputIter output; + + ScanBody(T sum, T identity, BinOp &f, InputIter input, OutputIter output) + : sum(sum), identity(identity), f(f), input(input), output(output) {} + ScanBody(ScanBody &b, tbb::split) + : sum(b.identity), + identity(b.identity), + f(b.f), + input(b.input), + output(b.output) {} + template + void operator()(const tbb::blocked_range &r, Tag) { + T temp = sum; + for (size_t i = r.begin(); i < r.end(); ++i) { + T inputTmp = input[i]; + if (Tag::is_final_scan()) output[i] = temp; + temp = f(temp, inputTmp); + } + sum = temp; + } + T get_sum() const { return sum; } + void reverse_join(ScanBody &a) { sum = f(a.sum, sum); } + void assign(ScanBody &b) { sum = b.sum; } +}; + +template +struct CopyIfScanBody { + size_t sum; + P &pred; + InputIter input; + OutputIter output; + + CopyIfScanBody(P &pred, InputIter input, OutputIter output) + : sum(0), pred(pred), input(input), output(output) {} + CopyIfScanBody(CopyIfScanBody &b, tbb::split) + : sum(0), pred(b.pred), input(b.input), output(b.output) {} + template + void operator()(const tbb::blocked_range &r, Tag) { + size_t temp = sum; + for (size_t i = r.begin(); i < r.end(); ++i) { + if (pred(i)) { + temp += 1; + if (Tag::is_final_scan()) output[temp - 1] = input[i]; + } + } + sum = temp; + } + size_t get_sum() const { return sum; } + void reverse_join(CopyIfScanBody &a) { sum = a.sum + sum; } + void assign(CopyIfScanBody &b) { sum = b.sum; } +}; + +template +struct Hist { + using SizeType = N; + static constexpr int k = K; + N hist[k][256] = {{0}}; + void merge(const Hist &other) { + for (int i = 0; i < k; ++i) + for (int j = 0; j < 256; ++j) hist[i][j] += other.hist[i][j]; + } + void prefixSum(N total, bool *canSkip) { + for (int i = 0; i < k; ++i) { + size_t count = 0; + for (int j = 0; j < 256; ++j) { + N tmp = hist[i][j]; + hist[i][j] = count; + count += tmp; + if (tmp == total) canSkip[i] = true; + } + } + } +}; + +template +void histogram(T *ptr, typename H::SizeType n, H &hist) { + auto worker = [](T *ptr, typename H::SizeType n, H &hist) { + for (typename H::SizeType i = 0; i < n; ++i) + for (int k = 0; k < hist.k; ++k) + ++hist.hist[k][(ptr[i] >> (8 * k)) & 0xFF]; + }; + if (n < kSeqThreshold) { + worker(ptr, n, hist); + } else { + tbb::combinable store; + tbb::parallel_for( + tbb::blocked_range(0, n, kSeqThreshold), + [&worker, &store, ptr](const auto &r) { + worker(ptr + r.begin(), r.end() - r.begin(), store.local()); + }); + store.combine_each([&hist](const H &h) { hist.merge(h); }); + } +} + +template +void shuffle(T *src, T *target, typename H::SizeType n, H &hist, int k) { + for (typename H::SizeType i = 0; i < n; ++i) + target[hist.hist[k][(src[i] >> (8 * k)) & 0xFF]++] = src[i]; +} + +template +bool LSB_radix_sort(T *input, T *tmp, SizeType n) { + Hist hist; + if (std::is_sorted(input, input + n)) return false; + histogram(input, n, hist); + bool canSkip[hist.k] = {0}; + hist.prefixSum(n, canSkip); + T *a = input, *b = tmp; + for (int k = 0; k < hist.k; ++k) { + if (!canSkip[k]) { + shuffle(a, b, n, hist, k); + std::swap(a, b); + } + } + return a == tmp; +} + +// LSB radix sort with merge +template +struct SortedRange { + T *input, *tmp; + SizeType offset = 0, length = 0; + bool inTmp = false; + + SortedRange(T *input, T *tmp, SizeType offset = 0, SizeType length = 0) + : input(input), tmp(tmp), offset(offset), length(length) {} + SortedRange(SortedRange &r, tbb::split) + : input(r.input), tmp(r.tmp) {} + // FIXME: no idea why thread sanitizer reports data race here +#if defined(__has_feature) +#if __has_feature(thread_sanitizer) + __attribute__((no_sanitize("thread"))) +#endif +#endif + void + operator()(const tbb::blocked_range &range) { + SortedRange rhs(input, tmp, range.begin(), + range.end() - range.begin()); + rhs.inTmp = + LSB_radix_sort(input + rhs.offset, tmp + rhs.offset, rhs.length); + if (length == 0) + *this = rhs; + else + join(rhs); + } + bool swapBuffer() const { + T *src = input, *target = tmp; + if (inTmp) std::swap(src, target); + copy(src + offset, src + offset + length, target + offset); + return !inTmp; + } + void join(const SortedRange &rhs) { + if (inTmp != rhs.inTmp) { + if (length < rhs.length) + inTmp = swapBuffer(); + else + rhs.swapBuffer(); + } + T *src = input, *target = tmp; + if (inTmp) std::swap(src, target); + if (src[offset + length - 1] > src[rhs.offset]) { + mergeRec(src, target, offset, offset + length, rhs.offset, + rhs.offset + rhs.length, offset, std::less()); + inTmp = !inTmp; + } + length += rhs.length; + } +}; + +template +void radix_sort(T *input, SizeTy n) { + T *aux = new T[n]; + SizeTy blockSize = std::max(n / tbb::this_task_arena::max_concurrency() / 4, + static_cast(kSeqThreshold / sizeof(T))); + SortedRange result(input, aux); + tbb::parallel_reduce(tbb::blocked_range(0, n, blockSize), result); + if (result.inTmp) copy(aux, aux + n, input); + delete[] aux; +} + +template ::value_type, + typename Comp = decltype(std::less())> +void mergeSort(ExecutionPolicy policy, Iterator first, Iterator last, + Comp comp) { +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + // apparently this prioritizes threads inside here? + tbb::this_task_arena::isolate([&] { + size_t length = std::distance(first, last); + T *tmp = new T[length]; + copy(policy, first, last, tmp); + details::mergeSortRec(tmp, first, 0, length, comp); + delete[] tmp; + }); + return; + } +#endif + std::stable_sort(first, last, comp); +} + +// stable_sort using merge sort. +// +// For simpler implementation, we do not support types that are not trivially +// destructable. +template ::value_type, + typename Dummy = void> +struct SortFunctor { + void operator()(ExecutionPolicy policy, Iterator first, Iterator last) { + static_assert( + std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert(std::is_trivially_destructible_v, + "Our simple implementation does not support types that are " + "not trivially destructable."); + return mergeSort(policy, first, last, std::less()); + } +}; + +// stable_sort specialized with radix sort for integral types. +// Typically faster than merge sort. +template +struct SortFunctor< + Iterator, T, + std::enable_if_t< + std::is_integral_v && + std::is_pointer_v::pointer>>> { + void operator()(ExecutionPolicy policy, Iterator first, Iterator last) { + static_assert( + std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert(std::is_trivially_destructible_v, + "Our simple implementation does not support types that are " + "not trivially destructable."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + radix_sort(&*first, static_cast(std::distance(first, last))); + return; + } +#endif + stable_sort(policy, first, last, std::less()); + } +}; + +} // namespace details + +#endif + +// Applies the function `f` to each element in the range `[first, last)` +template +void for_each(ExecutionPolicy policy, Iter first, Iter last, F f) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + tbb::parallel_for(tbb::blocked_range(first, last), + [&f](const tbb::blocked_range &range) { + for (Iter i = range.begin(); i != range.end(); i++) + f(*i); + }); + return; + } +#endif + std::for_each(first, last, f); +} + +// Applies the function `f` to each element in the range `[first, last)` +template +void for_each_n(ExecutionPolicy policy, Iter first, size_t n, F f) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + for_each(policy, first, first + n, f); +} + +// Reduce the range `[first, last)` using a binary operation `f` with an initial +// value `init`. +// +// The binary operation should be commutative and associative. Otherwise, the +// result is non-deterministic. +template ::value_type> +T reduce(ExecutionPolicy policy, InputIter first, InputIter last, T init, + BinaryOp f) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + // should we use deterministic reduce here? + return tbb::parallel_reduce( + tbb::blocked_range(first, last, details::kSeqThreshold), + init, + [&f](const tbb::blocked_range &range, T value) { + return std::reduce(range.begin(), range.end(), value, f); + }, + f); + } +#endif + return std::reduce(first, last, init, f); +} + +// Reduce the range `[first, last)` using a binary operation `f` with an initial +// value `init`. +// +// The binary operation should be commutative and associative. Otherwise, the +// result is non-deterministic. +template ::value_type> +T reduce(InputIter first, InputIter last, T init, BinaryOp f) { + return reduce(autoPolicy(first, last, 1e5), first, last, init, f); +} + +// Transform and reduce the range `[first, last)` by first applying a unary +// function `g`, and then combining the results using a binary operation `f` +// with an initial value `init`. +// +// The binary operation should be commutative and associative. Otherwise, the +// result is non-deterministic. +template ::value_type>> +T transform_reduce(ExecutionPolicy policy, InputIter first, InputIter last, + T init, BinaryOp f, UnaryOp g) { + return reduce(policy, TransformIterator(first, g), TransformIterator(last, g), + init, f); +} + +// Transform and reduce the range `[first, last)` by first applying a unary +// function `g`, and then combining the results using a binary operation `f` +// with an initial value `init`. +// +// The binary operation should be commutative and associative. Otherwise, the +// result is non-deterministic. +template ::value_type>> +T transform_reduce(InputIter first, InputIter last, T init, BinaryOp f, + UnaryOp g) { + return manifold::reduce(TransformIterator(first, g), + TransformIterator(last, g), init, f); +} + +// Compute the inclusive prefix sum for the range `[first, last)` +// using the summation operator, and store the result in the range +// starting from `d_first`. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must be equal or non-overlapping. +template ::value_type> +void inclusive_scan(ExecutionPolicy policy, InputIter first, InputIter last, + OutputIter d_first) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert( + std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + tbb::parallel_scan( + tbb::blocked_range(0, std::distance(first, last)), + static_cast(0), + [&](const tbb::blocked_range &range, T sum, + bool is_final_scan) { + T temp = sum; + for (size_t i = range.begin(); i < range.end(); ++i) { + temp = temp + first[i]; + if (is_final_scan) d_first[i] = temp; + } + return temp; + }, + std::plus()); + return; + } +#endif + std::inclusive_scan(first, last, d_first); +} + +// Compute the inclusive prefix sum for the range `[first, last)` using the +// summation operator, and store the result in the range +// starting from `d_first`. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must be equal or non-overlapping. +template ::value_type> +void inclusive_scan(InputIter first, InputIter last, OutputIter d_first) { + return inclusive_scan(autoPolicy(first, last, 1e5), first, last, d_first); +} + +// Compute the inclusive prefix sum for the range `[first, last)` using the +// binary operator `f`, with initial value `init` and +// identity element `identity`, and store the result in the range +// starting from `d_first`. +// +// This is different from `exclusive_scan` in the sequential algorithm by +// requiring an identity element. This is needed so that each block can be +// scanned in parallel and combined later. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must be equal or non-overlapping. +template ::value_type>()), + typename T = typename std::iterator_traits::value_type> +void exclusive_scan(ExecutionPolicy policy, InputIter first, InputIter last, + OutputIter d_first, T init = static_cast(0), + BinOp f = std::plus(), T identity = static_cast(0)) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert( + std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + details::ScanBody body(init, identity, f, + first, d_first); + tbb::parallel_scan( + tbb::blocked_range(0, std::distance(first, last)), body); + return; + } +#endif + std::exclusive_scan(first, last, d_first, init, f); +} + +// Compute the inclusive prefix sum for the range `[first, last)` using the +// binary operator `f`, with initial value `init` and +// identity element `identity`, and store the result in the range +// starting from `d_first`. +// +// This is different from `exclusive_scan` in the sequential algorithm by +// requiring an identity element. This is needed so that each block can be +// scanned in parallel and combined later. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must be equal or non-overlapping. +template ::value_type>()), + typename T = typename std::iterator_traits::value_type> +void exclusive_scan(InputIter first, InputIter last, OutputIter d_first, + T init = static_cast(0), BinOp f = std::plus(), + T identity = static_cast(0)) { + exclusive_scan(autoPolicy(first, last, 1e5), first, last, d_first, init, f, + identity); +} + +// Apply function `f` on the input range `[first, last)` and store the result in +// the range starting from `d_first`. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must be equal or non-overlapping. +template +void transform(ExecutionPolicy policy, InputIter first, InputIter last, + OutputIter d_first, F f) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert( + std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + tbb::parallel_for(tbb::blocked_range( + 0, static_cast(std::distance(first, last))), + [&](const tbb::blocked_range &range) { + std::transform(first + range.begin(), + first + range.end(), + d_first + range.begin(), f); + }); + return; + } +#endif + std::transform(first, last, d_first, f); +} + +// Apply function `f` on the input range `[first, last)` and store the result in +// the range starting from `d_first`. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must be equal or non-overlapping. +template +void transform(InputIter first, InputIter last, OutputIter d_first, F f) { + transform(autoPolicy(first, last, 1e5), first, last, d_first, f); +} + +// Copy the input range `[first, last)` to the output range +// starting from `d_first`. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must not overlap. +template +void copy(ExecutionPolicy policy, InputIter first, InputIter last, + OutputIter d_first) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert( + std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + tbb::parallel_for(tbb::blocked_range( + 0, static_cast(std::distance(first, last)), + details::kSeqThreshold), + [&](const tbb::blocked_range &range) { + std::copy(first + range.begin(), first + range.end(), + d_first + range.begin()); + }); + return; + } +#endif + std::copy(first, last, d_first); +} + +// Copy the input range `[first, last)` to the output range +// starting from `d_first`. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must not overlap. +template +void copy(InputIter first, InputIter last, OutputIter d_first) { + copy(autoPolicy(first, last, 1e6), first, last, d_first); +} + +// Copy the input range `[first, first + n)` to the output range +// starting from `d_first`. +// +// The input range `[first, first + n)` and +// the output range `[d_first, d_first + n)` +// must not overlap. +template +void copy_n(ExecutionPolicy policy, InputIter first, size_t n, + OutputIter d_first) { + copy(policy, first, first + n, d_first); +} + +// Copy the input range `[first, first + n)` to the output range +// starting from `d_first`. +// +// The input range `[first, first + n)` and +// the output range `[d_first, d_first + n)` +// must not overlap. +template +void copy_n(InputIter first, size_t n, OutputIter d_first) { + copy(autoPolicy(n, 1e6), first, first + n, d_first); +} + +// Fill the range `[first, last)` with `value`. +template +void fill(ExecutionPolicy policy, OutputIter first, OutputIter last, T value) { + static_assert( + std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + tbb::parallel_for(tbb::blocked_range(first, last), + [&](const tbb::blocked_range &range) { + std::fill(range.begin(), range.end(), value); + }); + return; + } +#endif + std::fill(first, last, value); +} + +// Fill the range `[first, last)` with `value`. +template +void fill(OutputIter first, OutputIter last, T value) { + fill(autoPolicy(first, last, 5e5), first, last, value); +} + +// Count the number of elements in the input range `[first, last)` satisfying +// predicate `pred`, i.e. `pred(x) == true`. +template +size_t count_if(ExecutionPolicy policy, InputIter first, InputIter last, + P pred) { +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + return reduce(policy, TransformIterator(first, pred), + TransformIterator(last, pred), 0, std::plus()); + } +#endif + return std::count_if(first, last, pred); +} + +// Count the number of elements in the input range `[first, last)` satisfying +// predicate `pred`, i.e. `pred(x) == true`. +template +size_t count_if(InputIter first, InputIter last, P pred) { + return count_if(autoPolicy(first, last, 1e4), first, last, pred); +} + +// Check if all elements in the input range `[first, last)` satisfy +// predicate `pred`, i.e. `pred(x) == true`. +template +bool all_of(ExecutionPolicy policy, InputIter first, InputIter last, P pred) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + // should we use deterministic reduce here? + return tbb::parallel_reduce( + tbb::blocked_range(first, last), true, + [&](const tbb::blocked_range &range, bool value) { + if (!value) return false; + for (InputIter i = range.begin(); i != range.end(); i++) + if (!pred(*i)) return false; + return true; + }, + [](bool a, bool b) { return a && b; }); + } +#endif + return std::all_of(first, last, pred); +} + +// Check if all elements in the input range `[first, last)` satisfy +// predicate `pred`, i.e. `pred(x) == true`. +template +bool all_of(InputIter first, InputIter last, P pred) { + return all_of(autoPolicy(first, last, 1e5), first, last, pred); +} + +// Copy values in the input range `[first, last)` to the output range +// starting from `d_first` that satisfies the predicate `pred`, +// i.e. `pred(x) == true`, and returns `d_first + n` where `n` is the number of +// times the predicate is evaluated to true. +// +// This function is stable, meaning that the relative order of elements in the +// output range remains unchanged. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must not overlap. +template +OutputIter copy_if(ExecutionPolicy policy, InputIter first, InputIter last, + OutputIter d_first, P pred) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert( + std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + auto pred2 = [&](size_t i) { return pred(first[i]); }; + details::CopyIfScanBody body(pred2, first, d_first); + tbb::parallel_scan( + tbb::blocked_range(0, std::distance(first, last)), body); + return d_first + body.get_sum(); + } +#endif + return std::copy_if(first, last, d_first, pred); +} + +// Copy values in the input range `[first, last)` to the output range +// starting from `d_first` that satisfies the predicate `pred`, i.e. `pred(x) == +// true`, and returns `d_first + n` where `n` is the number of times the +// predicate is evaluated to true. +// +// This function is stable, meaning that the relative order of elements in the +// output range remains unchanged. +// +// The input range `[first, last)` and +// the output range `[d_first, d_first + last - first)` +// must not overlap. +template +OutputIter copy_if(InputIter first, InputIter last, OutputIter d_first, + P pred) { + return copy_if(autoPolicy(first, last, 1e5), first, last, d_first, pred); +} + +// Remove values in the input range `[first, last)` that satisfies +// the predicate `pred`, i.e. `pred(x) == true`, and returns `first + n` +// where `n` is the number of times the predicate is evaluated to false. +// +// This function is stable, meaning that the relative order of elements that +// remained are unchanged. +// +// Only trivially destructable types are supported. +template ::value_type> +Iter remove_if(ExecutionPolicy policy, Iter first, Iter last, P pred) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert(std::is_trivially_destructible_v, + "Our simple implementation does not support types that are " + "not trivially destructable."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + T *tmp = new T[std::distance(first, last)]; + auto back = + copy_if(policy, first, last, tmp, [&](T v) { return !pred(v); }); + copy(policy, tmp, back, first); + auto d = std::distance(tmp, back); + delete[] tmp; + return first + d; + } +#endif + return std::remove_if(first, last, pred); +} + +// Remove values in the input range `[first, last)` that satisfies +// the predicate `pred`, i.e. `pred(x) == true`, and +// returns `first + n` where `n` is the number of times the predicate is +// evaluated to false. +// +// This function is stable, meaning that the relative order of elements that +// remained are unchanged. +// +// Only trivially destructable types are supported. +template ::value_type> +Iter remove_if(Iter first, Iter last, P pred) { + return remove_if(autoPolicy(first, last, 1e4), first, last, pred); +} + +// Remove values in the input range `[first, last)` that are equal to `value`. +// Returns `first + n` where `n` is the number of values +// that are not equal to `value`. +// +// This function is stable, meaning that the relative order of elements that +// remained are unchanged. +// +// Only trivially destructable types are supported. +template ::value_type> +Iter remove(ExecutionPolicy policy, Iter first, Iter last, T value) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert(std::is_trivially_destructible_v, + "Our simple implementation does not support types that are " + "not trivially destructable."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par) { + T *tmp = new T[std::distance(first, last)]; + auto back = + copy_if(policy, first, last, tmp, [&](T v) { return v != value; }); + copy(policy, tmp, back, first); + auto d = std::distance(tmp, back); + delete[] tmp; + return first + d; + } +#endif + return std::remove(first, last, value); +} + +// Remove values in the input range `[first, last)` that are equal to `value`. +// Returns `first + n` where `n` is the number of values +// that are not equal to `value`. +// +// This function is stable, meaning that the relative order of elements that +// remained are unchanged. +// +// Only trivially destructable types are supported. +template ::value_type> +Iter remove(Iter first, Iter last, T value) { + return remove(autoPolicy(first, last, 1e4), first, last, value); +} + +// For each group of consecutive elements in the range `[first, last)` with the +// same value, unique removes all but the first element of the group. The return +// value is an iterator `new_last` such that no two consecutive elements in the +// range `[first, new_last)` are equal. +// +// This function is stable, meaning that the relative order of elements that +// remained are unchanged. +// +// Only trivially destructable types are supported. +template ::value_type> +Iter unique(ExecutionPolicy policy, Iter first, Iter last) { + static_assert(std::is_convertible_v< + typename std::iterator_traits::iterator_category, + std::random_access_iterator_tag>, + "You can only parallelize RandomAccessIterator."); + static_assert(std::is_trivially_destructible_v, + "Our simple implementation does not support types that are " + "not trivially destructable."); +#if (MANIFOLD_PAR == 1) + if (policy == ExecutionPolicy::Par && first != last) { + Iter newSrcStart = first; + // cap the maximum buffer size, proved to be beneficial for unique with huge + // array size + constexpr size_t MAX_BUFFER_SIZE = 1 << 16; + T *tmp = new T[std::min(MAX_BUFFER_SIZE, + static_cast(std::distance(first, last)))]; + auto pred = [&](size_t i) { return tmp[i] != tmp[i + 1]; }; + do { + size_t length = + std::min(MAX_BUFFER_SIZE, + static_cast(std::distance(newSrcStart, last))); + copy(policy, newSrcStart, newSrcStart + length, tmp); + *first = *newSrcStart; + // this is not a typo, the index i is offset by 1, so to compare an + // element with its predecessor we need to compare i and i + 1. + details::CopyIfScanBody body(pred, tmp + 1, first + 1); + tbb::parallel_scan(tbb::blocked_range(0, length - 1), body); + first += body.get_sum() + 1; + newSrcStart += length; + } while (newSrcStart != last); + delete[] tmp; + return first; + } +#endif + return std::unique(first, last); +} + +// For each group of consecutive elements in the range `[first, last)` with the +// same value, unique removes all but the first element of the group. The return +// value is an iterator `new_last` such that no two consecutive elements in the +// range `[first, new_last)` are equal. +// +// This function is stable, meaning that the relative order of elements that +// remained are unchanged. +// +// Only trivially destructable types are supported. +template ::value_type> +Iter unique(Iter first, Iter last) { + return unique(autoPolicy(first, last, 1e4), first, last); +} + +// Sort the input range `[first, last)` in ascending order. +// +// This function is stable, meaning that the relative order of elements that are +// incomparable remains unchanged. +// +// Only trivially destructable types are supported. +template ::value_type> +void stable_sort(ExecutionPolicy policy, Iterator first, Iterator last) { +#if (MANIFOLD_PAR == 1) + details::SortFunctor()(policy, first, last); +#else + std::stable_sort(first, last); +#endif +} + +// Sort the input range `[first, last)` in ascending order. +// +// This function is stable, meaning that the relative order of elements that are +// incomparable remains unchanged. +// +// Only trivially destructable types are supported. +template ::value_type> +void stable_sort(Iterator first, Iterator last) { + stable_sort(autoPolicy(first, last, 1e4), first, last); +} + +// Sort the input range `[first, last)` in ascending order using the comparison +// function `comp`. +// +// This function is stable, meaning that the relative order of elements that are +// incomparable remains unchanged. +// +// Only trivially destructable types are supported. +template ::value_type, + typename Comp = decltype(std::less())> +void stable_sort(ExecutionPolicy policy, Iterator first, Iterator last, + Comp comp) { +#if (MANIFOLD_PAR == 1) + details::mergeSort(policy, first, last, comp); +#else + std::stable_sort(first, last, comp); +#endif +} + +// Sort the input range `[first, last)` in ascending order using the comparison +// function `comp`. +// +// This function is stable, meaning that the relative order of elements that are +// incomparable remains unchanged. +// +// Only trivially destructable types are supported. +template ::value_type, + typename Comp = decltype(std::less())> +void stable_sort(Iterator first, Iterator last, Comp comp) { + stable_sort(autoPolicy(first, last, 1e4), first, last, comp); +} + +// `scatter` copies elements from a source range into an output array according +// to a map. For each iterator `i` in the range `[first, last)`, the value `*i` +// is assigned to `outputFirst[mapFirst[i - first]]`. If the same index appears +// more than once in the range `[mapFirst, mapFirst + (last - first))`, the +// result is undefined. +// +// The map range, input range and the output range must not overlap. +template +void scatter(ExecutionPolicy policy, InputIterator1 first, InputIterator1 last, + InputIterator2 mapFirst, OutputIterator outputFirst) { + for_each(policy, countAt(0), + countAt(static_cast(std::distance(first, last))), + [first, mapFirst, outputFirst](size_t i) { + outputFirst[mapFirst[i]] = first[i]; + }); +} + +// `scatter` copies elements from a source range into an output array according +// to a map. For each iterator `i` in the range `[first, last)`, the value `*i` +// is assigned to `outputFirst[mapFirst[i - first]]`. If the same index appears +// more than once in the range `[mapFirst, mapFirst + (last - first))`, +// the result is undefined. +// +// The map range, input range and the output range must not overlap. +template +void scatter(InputIterator1 first, InputIterator1 last, InputIterator2 mapFirst, + OutputIterator outputFirst) { + scatter(autoPolicy(first, last, 1e5), first, last, mapFirst, outputFirst); +} + +// `gather` copies elements from a source array into a destination range +// according to a map. For each input iterator `i` +// in the range `[mapFirst, mapLast)`, the value `inputFirst[*i]` +// is assigned to `outputFirst[i - map_first]`. +// +// The map range, input range and the output range must not overlap. +template +void gather(ExecutionPolicy policy, InputIterator mapFirst, + InputIterator mapLast, RandomAccessIterator inputFirst, + OutputIterator outputFirst) { + for_each(policy, countAt(0), + countAt(static_cast(std::distance(mapFirst, mapLast))), + [mapFirst, inputFirst, outputFirst](size_t i) { + outputFirst[i] = inputFirst[mapFirst[i]]; + }); +} + +// `gather` copies elements from a source array into a destination range +// according to a map. For each input iterator `i` +// in the range `[mapFirst, mapLast)`, the value `inputFirst[*i]` +// is assigned to `outputFirst[i - map_first]`. +// +// The map range, input range and the output range must not overlap. +template +void gather(InputIterator mapFirst, InputIterator mapLast, + RandomAccessIterator inputFirst, OutputIterator outputFirst) { + gather(autoPolicy(std::distance(mapFirst, mapLast), 1e5), mapFirst, mapLast, + inputFirst, outputFirst); +} + +// Write `[0, last - first)` to the range `[first, last)`. +template +void sequence(ExecutionPolicy policy, Iterator first, Iterator last) { + for_each(policy, countAt(0), + countAt(static_cast(std::distance(first, last))), + [first](size_t i) { first[i] = i; }); +} + +// Write `[0, last - first)` to the range `[first, last)`. +template +void sequence(Iterator first, Iterator last) { + sequence(autoPolicy(first, last, 1e5), first, last); +} + +} // namespace manifold diff --git a/thirdparty/manifold/src/polygon.cpp b/thirdparty/manifold/src/polygon.cpp new file mode 100644 index 000000000000..60f510a98b22 --- /dev/null +++ b/thirdparty/manifold/src/polygon.cpp @@ -0,0 +1,1010 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manifold/polygon.h" + +#include +#include +#include + +#include "./collider.h" +#include "./parallel.h" +#include "./utils.h" +#include "manifold/optional_assert.h" + +namespace { +using namespace manifold; + +static ExecutionParams params; + +constexpr double kBest = -std::numeric_limits::infinity(); + +// it seems that MSVC cannot optimize la::determinant(mat2(a, b)) +constexpr double determinant2x2(vec2 a, vec2 b) { + return a.x * b.y - a.y * b.x; +} + +#ifdef MANIFOLD_DEBUG +struct PolyEdge { + int startVert, endVert; +}; + +std::vector Polygons2Edges(const PolygonsIdx &polys) { + std::vector halfedges; + for (const auto &poly : polys) { + for (size_t i = 1; i < poly.size(); ++i) { + halfedges.push_back({poly[i - 1].idx, poly[i].idx}); + } + halfedges.push_back({poly.back().idx, poly[0].idx}); + } + return halfedges; +} + +std::vector Triangles2Edges(const std::vector &triangles) { + std::vector halfedges; + halfedges.reserve(triangles.size() * 3); + for (const ivec3 &tri : triangles) { + halfedges.push_back({tri[0], tri[1]}); + halfedges.push_back({tri[1], tri[2]}); + halfedges.push_back({tri[2], tri[0]}); + } + return halfedges; +} + +void CheckTopology(const std::vector &halfedges) { + DEBUG_ASSERT(halfedges.size() % 2 == 0, topologyErr, + "Odd number of halfedges."); + size_t n_edges = halfedges.size() / 2; + std::vector forward(halfedges.size()), backward(halfedges.size()); + + auto end = std::copy_if(halfedges.begin(), halfedges.end(), forward.begin(), + [](PolyEdge e) { return e.endVert > e.startVert; }); + DEBUG_ASSERT( + static_cast(std::distance(forward.begin(), end)) == n_edges, + topologyErr, "Half of halfedges should be forward."); + forward.resize(n_edges); + + end = std::copy_if(halfedges.begin(), halfedges.end(), backward.begin(), + [](PolyEdge e) { return e.endVert < e.startVert; }); + DEBUG_ASSERT( + static_cast(std::distance(backward.begin(), end)) == n_edges, + topologyErr, "Half of halfedges should be backward."); + backward.resize(n_edges); + + std::for_each(backward.begin(), backward.end(), + [](PolyEdge &e) { std::swap(e.startVert, e.endVert); }); + auto cmp = [](const PolyEdge &a, const PolyEdge &b) { + return a.startVert < b.startVert || + (a.startVert == b.startVert && a.endVert < b.endVert); + }; + std::stable_sort(forward.begin(), forward.end(), cmp); + std::stable_sort(backward.begin(), backward.end(), cmp); + for (size_t i = 0; i < n_edges; ++i) { + DEBUG_ASSERT(forward[i].startVert == backward[i].startVert && + forward[i].endVert == backward[i].endVert, + topologyErr, "Not manifold."); + } +} + +void CheckTopology(const std::vector &triangles, + const PolygonsIdx &polys) { + std::vector halfedges = Triangles2Edges(triangles); + std::vector openEdges = Polygons2Edges(polys); + for (PolyEdge e : openEdges) { + halfedges.push_back({e.endVert, e.startVert}); + } + CheckTopology(halfedges); +} + +void CheckGeometry(const std::vector &triangles, + const PolygonsIdx &polys, double epsilon) { + std::unordered_map vertPos; + for (const auto &poly : polys) { + for (size_t i = 0; i < poly.size(); ++i) { + vertPos[poly[i].idx] = poly[i].pos; + } + } + DEBUG_ASSERT(std::all_of(triangles.begin(), triangles.end(), + [&vertPos, epsilon](const ivec3 &tri) { + return CCW(vertPos[tri[0]], vertPos[tri[1]], + vertPos[tri[2]], epsilon) >= 0; + }), + geometryErr, "triangulation is not entirely CCW!"); +} + +void Dump(const PolygonsIdx &polys, double epsilon) { + std::cout << "Polygon 0 " << epsilon << " " << polys.size() << std::endl; + for (auto poly : polys) { + std::cout << poly.size() << std::endl; + for (auto v : poly) { + std::cout << v.pos.x << " " << v.pos.y << std::endl; + } + } + std::cout << "# ... " << std::endl; + for (auto poly : polys) { + std::cout << "show(array([" << std::endl; + for (auto v : poly) { + std::cout << " [" << v.pos.x << ", " << v.pos.y << "]," << std::endl; + } + std::cout << "]))" << std::endl; + } +} + +void PrintFailure(const std::exception &e, const PolygonsIdx &polys, + std::vector &triangles, double epsilon) { + std::cout << "-----------------------------------" << std::endl; + std::cout << "Triangulation failed! Precision = " << epsilon << std::endl; + std::cout << e.what() << std::endl; + if (triangles.size() > 1000 && !PolygonParams().verbose) { + std::cout << "Output truncated due to producing " << triangles.size() + << " triangles." << std::endl; + return; + } + Dump(polys, epsilon); + std::cout << "produced this triangulation:" << std::endl; + for (size_t j = 0; j < triangles.size(); ++j) { + std::cout << triangles[j][0] << ", " << triangles[j][1] << ", " + << triangles[j][2] << std::endl; + } +} + +#define PRINT(msg) \ + if (params.verbose) std::cout << msg << std::endl; +#else +#define PRINT(msg) +#endif + +/** + * Tests if the input polygons are convex by searching for any reflex vertices. + * Exactly colinear edges and zero-length edges are treated conservatively as + * reflex. Does not check for overlaps. + */ +bool IsConvex(const PolygonsIdx &polys, double epsilon) { + for (const SimplePolygonIdx &poly : polys) { + const vec2 firstEdge = poly[0].pos - poly[poly.size() - 1].pos; + // Zero-length edges comes out NaN, which won't trip the early return, but + // it's okay because that zero-length edge will also get tested + // non-normalized and will trip det == 0. + vec2 lastEdge = la::normalize(firstEdge); + for (size_t v = 0; v < poly.size(); ++v) { + const vec2 edge = + v + 1 < poly.size() ? poly[v + 1].pos - poly[v].pos : firstEdge; + const double det = determinant2x2(lastEdge, edge); + if (det <= 0 || (std::abs(det) < epsilon && la::dot(lastEdge, edge) < 0)) + return false; + lastEdge = la::normalize(edge); + } + } + return true; +} + +/** + * Triangulates a set of convex polygons by alternating instead of a fan, to + * avoid creating high-degree vertices. + */ +std::vector TriangulateConvex(const PolygonsIdx &polys) { + const size_t numTri = manifold::transform_reduce( + polys.begin(), polys.end(), 0_uz, + [](size_t a, size_t b) { return a + b; }, + [](const SimplePolygonIdx &poly) { return poly.size() - 2; }); + std::vector triangles; + triangles.reserve(numTri); + for (const SimplePolygonIdx &poly : polys) { + size_t i = 0; + size_t k = poly.size() - 1; + bool right = true; + while (i + 1 < k) { + const size_t j = right ? i + 1 : k - 1; + triangles.push_back({poly[i].idx, poly[j].idx, poly[k].idx}); + if (right) { + i = j; + } else { + k = j; + } + right = !right; + } + } + return triangles; +} + +/** + * Ear-clipping triangulator based on David Eberly's approach from Geometric + * Tools, but adjusted to handle epsilon-valid polygons, and including a + * fallback that ensures a manifold triangulation even for overlapping polygons. + * This is an O(n^2) algorithm, but hopefully this is not a big problem as the + * number of edges in a given polygon is generally much less than the number of + * triangles in a mesh, and relatively few faces even need triangulation. + * + * The main adjustments for robustness involve clipping the sharpest ears first + * (a known technique to get higher triangle quality), and doing an exhaustive + * search to determine ear convexity exactly if the first geometric result is + * within epsilon. + */ + +class EarClip { + public: + EarClip(const PolygonsIdx &polys, double epsilon) : epsilon_(epsilon) { + ZoneScoped; + + size_t numVert = 0; + for (const SimplePolygonIdx &poly : polys) { + numVert += poly.size(); + } + polygon_.reserve(numVert + 2 * polys.size()); + + std::vector starts = Initialize(polys); + + for (VertItr v = polygon_.begin(); v != polygon_.end(); ++v) { + ClipIfDegenerate(v); + } + + for (const VertItr first : starts) { + FindStart(first); + } + } + + std::vector Triangulate() { + ZoneScoped; + + for (const VertItr start : holes_) { + CutKeyhole(start); + } + + for (const VertItr start : simples_) { + TriangulatePoly(start); + } + + return triangles_; + } + + double GetPrecision() const { return epsilon_; } + + private: + struct Vert; + typedef std::vector::iterator VertItr; + typedef std::vector::const_iterator VertItrC; + struct MaxX { + bool operator()(const VertItr &a, const VertItr &b) const { + return a->pos.x > b->pos.x; + } + }; + struct MinCost { + bool operator()(const VertItr &a, const VertItr &b) const { + return a->cost < b->cost; + } + }; + typedef std::set::iterator qItr; + + // The flat list where all the Verts are stored. Not used much for traversal. + std::vector polygon_; + // The set of right-most starting points, one for each negative-area contour. + std::multiset holes_; + // The set of starting points, one for each positive-area contour. + std::vector outers_; + // The set of starting points, one for each simple polygon. + std::vector simples_; + // Maps each hole (by way of starting point) to its bounding box. + std::map hole2BBox_; + // A priority queue of valid ears - the multiset allows them to be updated. + std::multiset earsQueue_; + // The output triangulation. + std::vector triangles_; + // Bounding box of the entire set of polygons + Rect bBox_; + // Working epsilon: max of float error and input value. + double epsilon_; + + struct IdxCollider { + Collider collider; + std::vector itr; + SparseIndices ind; + }; + + // A circularly-linked list representing the polygon(s) that still need to be + // triangulated. This gets smaller as ears are clipped until it degenerates to + // two points and terminates. + struct Vert { + int mesh_idx; + double cost; + qItr ear; + vec2 pos, rightDir; + VertItr left, right; + + // Shorter than half of epsilon, to be conservative so that it doesn't + // cause CW triangles that exceed epsilon due to rounding error. + bool IsShort(double epsilon) const { + const vec2 edge = right->pos - pos; + return la::dot(edge, edge) * 4 < epsilon * epsilon; + } + + // Like CCW, returns 1 if v is on the inside of the angle formed at this + // vert, -1 on the outside, and 0 if it's within epsilon of the boundary. + // Ensure v is more than epsilon from pos, as case this will not return 0. + int Interior(vec2 v, double epsilon) const { + const vec2 diff = v - pos; + if (la::dot(diff, diff) < epsilon * epsilon) { + return 0; + } + return CCW(pos, left->pos, right->pos, epsilon) + + CCW(pos, right->pos, v, epsilon) + CCW(pos, v, left->pos, epsilon); + } + + // Returns true if Vert is on the inside of the edge that goes from tail to + // tail->right. This will walk the edges if necessary until a clear answer + // is found (beyond epsilon). If toLeft is true, this Vert will walk its + // edges to the left. This should be chosen so that the edges walk in the + // same general direction - tail always walks to the right. + bool InsideEdge(VertItr tail, double epsilon, bool toLeft) const { + const double p2 = epsilon * epsilon; + VertItr nextL = left->right; + VertItr nextR = tail->right; + VertItr center = tail; + VertItr last = center; + + while (nextL != nextR && tail != nextR && + nextL != (toLeft ? right : left)) { + const vec2 edgeL = nextL->pos - center->pos; + const double l2 = la::dot(edgeL, edgeL); + if (l2 <= p2) { + nextL = toLeft ? nextL->left : nextL->right; + continue; + } + + const vec2 edgeR = nextR->pos - center->pos; + const double r2 = la::dot(edgeR, edgeR); + if (r2 <= p2) { + nextR = nextR->right; + continue; + } + + const vec2 vecLR = nextR->pos - nextL->pos; + const double lr2 = la::dot(vecLR, vecLR); + if (lr2 <= p2) { + last = center; + center = nextL; + nextL = toLeft ? nextL->left : nextL->right; + if (nextL == nextR) break; + nextR = nextR->right; + continue; + } + + int convexity = CCW(nextL->pos, center->pos, nextR->pos, epsilon); + if (center != last) { + convexity += CCW(last->pos, center->pos, nextL->pos, epsilon) + + CCW(nextR->pos, center->pos, last->pos, epsilon); + } + if (convexity != 0) return convexity > 0; + + if (l2 < r2) { + center = nextL; + nextL = toLeft ? nextL->left : nextL->right; + } else { + center = nextR; + nextR = nextR->right; + } + last = center; + } + // The whole polygon is degenerate - consider this to be convex. + return true; + } + + // A major key to robustness is to only clip convex ears, but this is + // difficult to determine when an edge is folded back on itself. This + // function walks down the kinks in a degenerate portion of a polygon until + // it finds a clear geometric result. In the vast majority of cases the loop + // will only need one or two iterations. + bool IsConvex(double epsilon) const { + const int convexity = CCW(left->pos, pos, right->pos, epsilon); + if (convexity != 0) { + return convexity > 0; + } + if (la::dot(left->pos - pos, right->pos - pos) <= 0) { + return true; + } + return left->InsideEdge(left->right, epsilon, true); + } + + // Subtly different from !IsConvex because IsConvex will return true for + // colinear non-folded verts, while IsReflex will always check until actual + // certainty is determined. + bool IsReflex(double epsilon) const { + return !left->InsideEdge(left->right, epsilon, true); + } + + // Returns the x-value on this edge corresponding to the start.y value, + // returning NAN if the edge does not cross the value from below to above, + // right of start - all within a epsilon tolerance. If onTop != 0, this + // restricts which end is allowed to terminate within the epsilon band. + double InterpY2X(vec2 start, int onTop, double epsilon) const { + if (la::abs(pos.y - start.y) <= epsilon) { + if (right->pos.y <= start.y + epsilon || onTop == 1) { + return NAN; + } else { + return pos.x; + } + } else if (pos.y < start.y - epsilon) { + if (right->pos.y > start.y + epsilon) { + return pos.x + (start.y - pos.y) * (right->pos.x - pos.x) / + (right->pos.y - pos.y); + } else if (right->pos.y < start.y - epsilon || onTop == -1) { + return NAN; + } else { + return right->pos.x; + } + } else { + return NAN; + } + } + + // This finds the cost of this vert relative to one of the two closed sides + // of the ear. Points are valid even when they touch, so long as their edge + // goes to the outside. No need to check the other side, since all verts are + // processed in the EarCost loop. + double SignedDist(VertItr v, vec2 unit, double epsilon) const { + double d = determinant2x2(unit, v->pos - pos); + if (std::abs(d) < epsilon) { + double dR = determinant2x2(unit, v->right->pos - pos); + if (std::abs(dR) > epsilon) return dR; + double dL = determinant2x2(unit, v->left->pos - pos); + if (std::abs(dL) > epsilon) return dL; + } + return d; + } + + // Find the cost of Vert v within this ear, where openSide is the unit + // vector from Verts right to left - passed in for reuse. + double Cost(VertItr v, vec2 openSide, double epsilon) const { + double cost = std::min(SignedDist(v, rightDir, epsilon), + SignedDist(v, left->rightDir, epsilon)); + + const double openCost = determinant2x2(openSide, v->pos - right->pos); + return std::min(cost, openCost); + } + + // For verts outside the ear, apply a cost based on the Delaunay condition + // to aid in prioritization and produce cleaner triangulations. This doesn't + // affect robustness, but may be adjusted to improve output. + static double DelaunayCost(vec2 diff, double scale, double epsilon) { + return -epsilon - scale * la::dot(diff, diff); + } + + // This is the expensive part of the algorithm, checking this ear against + // every Vert to ensure none are inside. The Collider brings the total + // triangulator cost down from O(n^2) to O(nlogn) for most large polygons. + // + // Think of a cost as vaguely a distance metric - 0 is right on the edge of + // being invalid. cost > epsilon is definitely invalid. Cost < -epsilon + // is definitely valid, so all improvement costs are designed to always give + // values < -epsilon so they will never affect validity. The first + // totalCost is designed to give priority to sharper angles. Any cost < (-1 + // - epsilon) has satisfied the Delaunay condition. + double EarCost(double epsilon, IdxCollider &collider) const { + vec2 openSide = left->pos - right->pos; + const vec2 center = 0.5 * (left->pos + right->pos); + const double scale = 4 / la::dot(openSide, openSide); + const double radius = la::length(openSide) / 2; + openSide = la::normalize(openSide); + + double totalCost = la::dot(left->rightDir, rightDir) - 1 - epsilon; + if (CCW(pos, left->pos, right->pos, epsilon) == 0) { + // Clip folded ears first + return totalCost; + } + + Box earBox = Box{vec3(center.x - radius, center.y - radius, 0), + vec3(center.x + radius, center.y + radius, 0)}; + earBox.Union(vec3(pos, 0)); + collider.collider.Collisions(VecView(&earBox, 1), + collider.ind); + + const int lid = left->mesh_idx; + const int rid = right->mesh_idx; + + totalCost = transform_reduce( + countAt(0), countAt(collider.ind.size()), totalCost, + [](double a, double b) { return std::max(a, b); }, + [&](size_t i) { + const VertItr test = collider.itr[collider.ind.Get(i, true)]; + if (!Clipped(test) && test->mesh_idx != mesh_idx && + test->mesh_idx != lid && + test->mesh_idx != rid) { // Skip duplicated verts + double cost = Cost(test, openSide, epsilon); + if (cost < -epsilon) { + cost = DelaunayCost(test->pos - center, scale, epsilon); + } + return cost; + } + return std::numeric_limits::lowest(); + }); + collider.ind.Clear(); + return totalCost; + } + + void PrintVert() const { +#ifdef MANIFOLD_DEBUG + if (!params.verbose) return; + std::cout << "vert: " << mesh_idx << ", left: " << left->mesh_idx + << ", right: " << right->mesh_idx << ", cost: " << cost + << std::endl; +#endif + } + }; + + static vec2 SafeNormalize(vec2 v) { + vec2 n = la::normalize(v); + return std::isfinite(n.x) ? n : vec2(0, 0); + } + + // This function and JoinPolygons are the only functions that affect the + // circular list data structure. This helps ensure it remains circular. + static void Link(VertItr left, VertItr right) { + left->right = right; + right->left = left; + left->rightDir = SafeNormalize(right->pos - left->pos); + } + + // When an ear vert is clipped, its neighbors get linked, so they get unlinked + // from it, but it is still linked to them. + static bool Clipped(VertItr v) { return v->right->left != v; } + + // Apply func to each un-clipped vert in a polygon and return an un-clipped + // vert. + VertItrC Loop(VertItr first, std::function func) const { + VertItr v = first; + do { + if (Clipped(v)) { + // Update first to an un-clipped vert so we will return to it instead + // of infinite-looping. + first = v->right->left; + if (!Clipped(first)) { + v = first; + if (v->right == v->left) { + return polygon_.end(); + } + func(v); + } + } else { + if (v->right == v->left) { + return polygon_.end(); + } + func(v); + } + v = v->right; + } while (v != first); + return v; + } + + // Remove this vert from the circular list and output a corresponding + // triangle. + void ClipEar(VertItrC ear) { + Link(ear->left, ear->right); + if (ear->left->mesh_idx != ear->mesh_idx && + ear->mesh_idx != ear->right->mesh_idx && + ear->right->mesh_idx != ear->left->mesh_idx) { + // Filter out topological degenerates, which can form in bad + // triangulations of polygons with holes, due to vert duplication. + triangles_.push_back( + {ear->left->mesh_idx, ear->mesh_idx, ear->right->mesh_idx}); + } else { + PRINT("Topological degenerate!"); + } + } + + // If an ear will make a degenerate triangle, clip it early to avoid + // difficulty in key-holing. This function is recursive, as the process of + // clipping may cause the neighbors to degenerate. Reflex degenerates *must + // not* be clipped, unless they have a short edge. + void ClipIfDegenerate(VertItr ear) { + if (Clipped(ear)) { + return; + } + if (ear->left == ear->right) { + return; + } + if (ear->IsShort(epsilon_) || + (CCW(ear->left->pos, ear->pos, ear->right->pos, epsilon_) == 0 && + la::dot(ear->left->pos - ear->pos, ear->right->pos - ear->pos) > 0 && + ear->IsConvex(epsilon_))) { + ClipEar(ear); + ClipIfDegenerate(ear->left); + ClipIfDegenerate(ear->right); + } + } + + // Build the circular list polygon structures. + std::vector Initialize(const PolygonsIdx &polys) { + std::vector starts; + for (const SimplePolygonIdx &poly : polys) { + auto vert = poly.begin(); + polygon_.push_back({vert->idx, 0.0, earsQueue_.end(), vert->pos}); + const VertItr first = std::prev(polygon_.end()); + + bBox_.Union(first->pos); + VertItr last = first; + // This is not the real rightmost start, but just an arbitrary vert for + // now to identify each polygon. + starts.push_back(first); + + for (++vert; vert != poly.end(); ++vert) { + bBox_.Union(vert->pos); + + polygon_.push_back({vert->idx, 0.0, earsQueue_.end(), vert->pos}); + VertItr next = std::prev(polygon_.end()); + + Link(last, next); + last = next; + } + Link(last, first); + } + + if (epsilon_ < 0) epsilon_ = bBox_.Scale() * kPrecision; + + // Slightly more than enough, since each hole can cause two extra triangles. + triangles_.reserve(polygon_.size() + 2 * starts.size()); + return starts; + } + + // Find the actual rightmost starts after degenerate removal. Also calculate + // the polygon bounding boxes. + void FindStart(VertItr first) { + const vec2 origin = first->pos; + + VertItr start = first; + double maxX = -std::numeric_limits::infinity(); + Rect bBox; + // Kahan summation + double area = 0; + double areaCompensation = 0; + + auto AddPoint = [&](VertItr v) { + bBox.Union(v->pos); + const double area1 = + determinant2x2(v->pos - origin, v->right->pos - origin); + const double t1 = area + area1; + areaCompensation += (area - t1) + area1; + area = t1; + + if (v->pos.x > maxX) { + maxX = v->pos.x; + start = v; + } + }; + + if (Loop(first, AddPoint) == polygon_.end()) { + // No polygon left if all ears were degenerate and already clipped. + return; + } + + area += areaCompensation; + const vec2 size = bBox.Size(); + const double minArea = epsilon_ * std::max(size.x, size.y); + + if (std::isfinite(maxX) && area < -minArea) { + holes_.insert(start); + hole2BBox_.insert({start, bBox}); + } else { + simples_.push_back(start); + if (area > minArea) { + outers_.push_back(start); + } + } + } + + // All holes must be key-holed (attached to an outer polygon) before ear + // clipping can commence. Instead of relying on sorting, which may be + // incorrect due to epsilon, we check for polygon edges both ahead and + // behind to ensure all valid options are found. + void CutKeyhole(const VertItr start) { + const Rect bBox = hole2BBox_[start]; + const int onTop = start->pos.y >= bBox.max.y - epsilon_ ? 1 + : start->pos.y <= bBox.min.y + epsilon_ ? -1 + : 0; + VertItr connector = polygon_.end(); + + auto CheckEdge = [&](VertItr edge) { + const double x = edge->InterpY2X(start->pos, onTop, epsilon_); + if (std::isfinite(x) && start->InsideEdge(edge, epsilon_, true) && + (connector == polygon_.end() || + CCW({x, start->pos.y}, connector->pos, connector->right->pos, + epsilon_) == 1 || + (connector->pos.y < edge->pos.y + ? edge->InsideEdge(connector, epsilon_, false) + : !connector->InsideEdge(edge, epsilon_, false)))) { + connector = edge; + } + }; + + for (const VertItr first : outers_) { + Loop(first, CheckEdge); + } + + if (connector == polygon_.end()) { + PRINT("hole did not find an outer contour!"); + simples_.push_back(start); + return; + } + + connector = FindCloserBridge(start, connector); + + JoinPolygons(start, connector); + +#ifdef MANIFOLD_DEBUG + if (params.verbose) { + std::cout << "connected " << start->mesh_idx << " to " + << connector->mesh_idx << std::endl; + } +#endif + } + + // This converts the initial guess for the keyhole location into the final one + // and returns it. It does so by finding any reflex verts inside the triangle + // containing the best connection and the initial horizontal line. + VertItr FindCloserBridge(VertItr start, VertItr edge) { + VertItr connector = + edge->pos.x < start->pos.x ? edge->right + : edge->right->pos.x < start->pos.x ? edge + : edge->right->pos.y - start->pos.y > start->pos.y - edge->pos.y + ? edge + : edge->right; + if (la::abs(connector->pos.y - start->pos.y) <= epsilon_) { + return connector; + } + const double above = connector->pos.y > start->pos.y ? 1 : -1; + + auto CheckVert = [&](VertItr vert) { + const double inside = + above * CCW(start->pos, vert->pos, connector->pos, epsilon_); + if (vert->pos.x > start->pos.x - epsilon_ && + vert->pos.y * above > start->pos.y * above - epsilon_ && + (inside > 0 || (inside == 0 && vert->pos.x < connector->pos.x)) && + vert->InsideEdge(edge, epsilon_, true) && vert->IsReflex(epsilon_)) { + connector = vert; + } + }; + + for (const VertItr first : outers_) { + Loop(first, CheckVert); + } + + return connector; + } + + // Creates a keyhole between the start vert of a hole and the connector vert + // of an outer polygon. To do this, both verts are duplicated and reattached. + // This process may create degenerate ears, so these are clipped if necessary + // to keep from confusing subsequent key-holing operations. + void JoinPolygons(VertItr start, VertItr connector) { + polygon_.push_back(*start); + const VertItr newStart = std::prev(polygon_.end()); + polygon_.push_back(*connector); + const VertItr newConnector = std::prev(polygon_.end()); + + start->right->left = newStart; + connector->left->right = newConnector; + Link(start, connector); + Link(newConnector, newStart); + + ClipIfDegenerate(start); + ClipIfDegenerate(newStart); + ClipIfDegenerate(connector); + ClipIfDegenerate(newConnector); + } + + // Recalculate the cost of the Vert v ear, updating it in the queue by + // removing and reinserting it. + void ProcessEar(VertItr v, IdxCollider &collider) { + if (v->ear != earsQueue_.end()) { + earsQueue_.erase(v->ear); + v->ear = earsQueue_.end(); + } + if (v->IsShort(epsilon_)) { + v->cost = kBest; + v->ear = earsQueue_.insert(v); + } else if (v->IsConvex(2 * epsilon_)) { + v->cost = v->EarCost(epsilon_, collider); + v->ear = earsQueue_.insert(v); + } else { + v->cost = 1; // not used, but marks reflex verts for debug + } + } + + // Create a collider of all vertices in this polygon, each expanded by + // epsilon_. Each ear uses this BVH to quickly find a subset of vertices to + // check for cost. + IdxCollider VertCollider(VertItr start) const { + Vec vertBox; + Vec vertMorton; + std::vector itr; + const Box box(vec3(bBox_.min, 0), vec3(bBox_.max, 0)); + + Loop(start, [&vertBox, &vertMorton, &itr, &box, this](VertItr v) { + itr.push_back(v); + const vec3 pos(v->pos, 0); + vertBox.push_back({pos - epsilon_, pos + epsilon_}); + vertMorton.push_back(Collider::MortonCode(pos, box)); + }); + + if (itr.empty()) { + return {Collider(), itr}; + } + + const int numVert = itr.size(); + Vec vertNew2Old(numVert); + sequence(vertNew2Old.begin(), vertNew2Old.end()); + + stable_sort(vertNew2Old.begin(), vertNew2Old.end(), + [&vertMorton](const int a, const int b) { + return vertMorton[a] < vertMorton[b]; + }); + Permute(vertMorton, vertNew2Old); + Permute(vertBox, vertNew2Old); + Permute(itr, vertNew2Old); + + return {Collider(vertBox, vertMorton), itr}; + } + + // The main ear-clipping loop. This is called once for each simple polygon - + // all holes have already been key-holed and joined to an outer polygon. + void TriangulatePoly(VertItr start) { + ZoneScoped; + + IdxCollider vertCollider = VertCollider(start); + + if (vertCollider.itr.empty()) { + PRINT("Empty poly"); + return; + } + + // A simple polygon always creates two fewer triangles than it has verts. + int numTri = -2; + earsQueue_.clear(); + + auto QueueVert = [&](VertItr v) { + ProcessEar(v, vertCollider); + ++numTri; + v->PrintVert(); + }; + + VertItrC v = Loop(start, QueueVert); + if (v == polygon_.end()) return; + Dump(v); + + while (numTri > 0) { + const qItr ear = earsQueue_.begin(); + if (ear != earsQueue_.end()) { + v = *ear; + // Cost should always be negative, generally < -epsilon. + v->PrintVert(); + earsQueue_.erase(ear); + } else { + PRINT("No ear found!"); + } + + ClipEar(v); + --numTri; + + ProcessEar(v->left, vertCollider); + ProcessEar(v->right, vertCollider); + // This is a backup vert that is used if the queue is empty (geometrically + // invalid polygon), to ensure manifoldness. + v = v->right; + } + + DEBUG_ASSERT(v->right == v->left, logicErr, "Triangulator error!"); + PRINT("Finished poly"); + } + + void Dump(VertItrC start) const { +#ifdef MANIFOLD_DEBUG + if (!params.verbose) return; + VertItrC v = start; + std::cout << "show(array([" << std::setprecision(15) << std::endl; + do { + std::cout << " [" << v->pos.x << ", " << v->pos.y << "],# " + << v->mesh_idx << ", cost: " << v->cost << std::endl; + v = v->right; + } while (v != start); + std::cout << " [" << v->pos.x << ", " << v->pos.y << "],# " << v->mesh_idx + << std::endl; + std::cout << "]))" << std::endl; + + v = start; + std::cout << "polys.push_back({" << std::setprecision(15) << std::endl; + do { + std::cout << " {" << v->pos.x << ", " << v->pos.y << "}, //" + << std::endl; + v = v->right; + } while (v != start); + std::cout << "});" << std::endl; +#endif + } +}; +} // namespace + +namespace manifold { + +/** + * @brief Triangulates a set of ε-valid polygons. If the input is not + * ε-valid, the triangulation may overlap, but will always return a + * manifold result that matches the input edge directions. + * + * @param polys The set of polygons, wound CCW and representing multiple + * polygons and/or holes. These have 2D-projected positions as well as + * references back to the original vertices. + * @param epsilon The value of ε, bounding the uncertainty of the + * input. + * @return std::vector The triangles, referencing the original + * vertex indicies. + */ +std::vector TriangulateIdx(const PolygonsIdx &polys, double epsilon) { + std::vector triangles; + double updatedEpsilon = epsilon; +#ifdef MANIFOLD_DEBUG + try { +#endif + if (IsConvex(polys, epsilon)) { // fast path + triangles = TriangulateConvex(polys); + } else { + EarClip triangulator(polys, epsilon); + triangles = triangulator.Triangulate(); + updatedEpsilon = triangulator.GetPrecision(); + } +#ifdef MANIFOLD_DEBUG + if (params.intermediateChecks) { + CheckTopology(triangles, polys); + if (!params.processOverlaps) { + CheckGeometry(triangles, polys, 2 * updatedEpsilon); + } + } + } catch (const geometryErr &e) { + if (!params.suppressErrors) { + PrintFailure(e, polys, triangles, updatedEpsilon); + } + throw; + } catch (const std::exception &e) { + PrintFailure(e, polys, triangles, updatedEpsilon); + throw; + } +#endif + return triangles; +} + +/** + * @brief Triangulates a set of ε-valid polygons. If the input is not + * ε-valid, the triangulation may overlap, but will always return a + * manifold result that matches the input edge directions. + * + * @param polygons The set of polygons, wound CCW and representing multiple + * polygons and/or holes. + * @param epsilon The value of ε, bounding the uncertainty of the + * input. + * @return std::vector The triangles, referencing the original + * polygon points in order. + */ +std::vector Triangulate(const Polygons &polygons, double epsilon) { + int idx = 0; + PolygonsIdx polygonsIndexed; + for (const auto &poly : polygons) { + SimplePolygonIdx simpleIndexed; + for (const vec2 &polyVert : poly) { + simpleIndexed.push_back({polyVert, idx++}); + } + polygonsIndexed.push_back(simpleIndexed); + } + return TriangulateIdx(polygonsIndexed, epsilon); +} + +ExecutionParams &PolygonParams() { return params; } + +} // namespace manifold diff --git a/thirdparty/manifold/src/properties.cpp b/thirdparty/manifold/src/properties.cpp new file mode 100644 index 000000000000..911133776ab3 --- /dev/null +++ b/thirdparty/manifold/src/properties.cpp @@ -0,0 +1,388 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "./impl.h" +#include "./parallel.h" +#include "./tri_dist.h" + +namespace { +using namespace manifold; + +struct CurvatureAngles { + VecView meanCurvature; + VecView gaussianCurvature; + VecView area; + VecView degree; + VecView halfedge; + VecView vertPos; + VecView triNormal; + + void operator()(size_t tri) { + vec3 edge[3]; + vec3 edgeLength(0.0); + for (int i : {0, 1, 2}) { + const int startVert = halfedge[3 * tri + i].startVert; + const int endVert = halfedge[3 * tri + i].endVert; + edge[i] = vertPos[endVert] - vertPos[startVert]; + edgeLength[i] = la::length(edge[i]); + edge[i] /= edgeLength[i]; + const int neighborTri = halfedge[3 * tri + i].pairedHalfedge / 3; + const double dihedral = + 0.25 * edgeLength[i] * + std::asin(la::dot(la::cross(triNormal[tri], triNormal[neighborTri]), + edge[i])); + AtomicAdd(meanCurvature[startVert], dihedral); + AtomicAdd(meanCurvature[endVert], dihedral); + AtomicAdd(degree[startVert], 1.0); + } + + vec3 phi; + phi[0] = std::acos(-la::dot(edge[2], edge[0])); + phi[1] = std::acos(-la::dot(edge[0], edge[1])); + phi[2] = kPi - phi[0] - phi[1]; + const double area3 = edgeLength[0] * edgeLength[1] * + la::length(la::cross(edge[0], edge[1])) / 6; + + for (int i : {0, 1, 2}) { + const int vert = halfedge[3 * tri + i].startVert; + AtomicAdd(gaussianCurvature[vert], -phi[i]); + AtomicAdd(area[vert], area3); + } + } +}; + +struct UpdateProperties { + VecView triProp; + VecView properties; + VecView counters; + + VecView oldProperties; + VecView halfedge; + VecView meanCurvature; + VecView gaussianCurvature; + const int oldNumProp; + const int numProp; + const int gaussianIdx; + const int meanIdx; + + void operator()(const size_t tri) { + for (const int i : {0, 1, 2}) { + const int vert = halfedge[3 * tri + i].startVert; + if (oldNumProp == 0) { + triProp[tri][i] = vert; + } + const int propVert = triProp[tri][i]; + + auto old = std::atomic_exchange( + reinterpret_cast*>(&counters[propVert]), + static_cast(1)); + if (old == 1) continue; + + for (int p = 0; p < oldNumProp; ++p) { + properties[numProp * propVert + p] = + oldProperties[oldNumProp * propVert + p]; + } + + if (gaussianIdx >= 0) { + properties[numProp * propVert + gaussianIdx] = gaussianCurvature[vert]; + } + if (meanIdx >= 0) { + properties[numProp * propVert + meanIdx] = meanCurvature[vert]; + } + } + } +}; + +struct CheckHalfedges { + VecView halfedges; + + bool operator()(size_t edge) const { + const Halfedge halfedge = halfedges[edge]; + if (halfedge.startVert == -1 || halfedge.endVert == -1) return true; + if (halfedge.pairedHalfedge == -1) return false; + + const Halfedge paired = halfedges[halfedge.pairedHalfedge]; + bool good = true; + good &= paired.pairedHalfedge == static_cast(edge); + good &= halfedge.startVert != halfedge.endVert; + good &= halfedge.startVert == paired.endVert; + good &= halfedge.endVert == paired.startVert; + return good; + } +}; + +struct CheckCCW { + VecView halfedges; + VecView vertPos; + VecView triNormal; + const double tol; + + bool operator()(size_t face) const { + if (halfedges[3 * face].pairedHalfedge < 0) return true; + + const mat2x3 projection = GetAxisAlignedProjection(triNormal[face]); + vec2 v[3]; + for (int i : {0, 1, 2}) + v[i] = projection * vertPos[halfedges[3 * face + i].startVert]; + + int ccw = CCW(v[0], v[1], v[2], std::abs(tol)); + bool check = tol > 0 ? ccw >= 0 : ccw == 0; + +#ifdef MANIFOLD_DEBUG + if (tol > 0 && !check) { + vec2 v1 = v[1] - v[0]; + vec2 v2 = v[2] - v[0]; + double area = v1.x * v2.y - v1.y * v2.x; + double base2 = std::max(la::dot(v1, v1), la::dot(v2, v2)); + double base = std::sqrt(base2); + vec3 V0 = vertPos[halfedges[3 * face].startVert]; + vec3 V1 = vertPos[halfedges[3 * face + 1].startVert]; + vec3 V2 = vertPos[halfedges[3 * face + 2].startVert]; + vec3 norm = la::cross(V1 - V0, V2 - V0); + printf( + "Tri %ld does not match normal, approx height = %g, base = %g\n" + "tol = %g, area2 = %g, base2*tol2 = %g\n" + "normal = %g, %g, %g\n" + "norm = %g, %g, %g\nverts: %d, %d, %d\n", + static_cast(face), area / base, base, tol, area * area, + base2 * tol * tol, triNormal[face].x, triNormal[face].y, + triNormal[face].z, norm.x, norm.y, norm.z, + halfedges[3 * face].startVert, halfedges[3 * face + 1].startVert, + halfedges[3 * face + 2].startVert); + } +#endif + return check; + } +}; +} // namespace + +namespace manifold { + +/** + * Returns true if this manifold is in fact an oriented even manifold and all of + * the data structures are consistent. + */ +bool Manifold::Impl::IsManifold() const { + if (halfedge_.size() == 0) return true; + return all_of(countAt(0_uz), countAt(halfedge_.size()), + CheckHalfedges({halfedge_})); +} + +/** + * Returns true if this manifold is in fact an oriented 2-manifold and all of + * the data structures are consistent. + */ +bool Manifold::Impl::Is2Manifold() const { + if (halfedge_.size() == 0) return true; + if (!IsManifold()) return false; + + Vec halfedge(halfedge_); + stable_sort(halfedge.begin(), halfedge.end()); + + return all_of( + countAt(0_uz), countAt(2 * NumEdge() - 1), [&halfedge](size_t edge) { + const Halfedge h = halfedge[edge]; + if (h.startVert == -1 && h.endVert == -1 && h.pairedHalfedge == -1) + return true; + return h.startVert != halfedge[edge + 1].startVert || + h.endVert != halfedge[edge + 1].endVert; + }); +} + +/** + * Returns true if all triangles are CCW relative to their triNormals_. + */ +bool Manifold::Impl::MatchesTriNormals() const { + if (halfedge_.size() == 0 || faceNormal_.size() != NumTri()) return true; + return all_of(countAt(0_uz), countAt(NumTri()), + CheckCCW({halfedge_, vertPos_, faceNormal_, 2 * epsilon_})); +} + +/** + * Returns the number of triangles that are colinear within epsilon_. + */ +int Manifold::Impl::NumDegenerateTris() const { + if (halfedge_.size() == 0 || faceNormal_.size() != NumTri()) return true; + return count_if( + countAt(0_uz), countAt(NumTri()), + CheckCCW({halfedge_, vertPos_, faceNormal_, -1 * epsilon_ / 2})); +} + +double Manifold::Impl::GetProperty(Property prop) const { + ZoneScoped; + if (IsEmpty()) return 0; + + auto Volume = [this](size_t tri) { + const vec3 v = vertPos_[halfedge_[3 * tri].startVert]; + vec3 crossP = la::cross(vertPos_[halfedge_[3 * tri + 1].startVert] - v, + vertPos_[halfedge_[3 * tri + 2].startVert] - v); + return la::dot(crossP, v) / 6.0; + }; + + auto Area = [this](size_t tri) { + const vec3 v = vertPos_[halfedge_[3 * tri].startVert]; + return la::length( + la::cross(vertPos_[halfedge_[3 * tri + 1].startVert] - v, + vertPos_[halfedge_[3 * tri + 2].startVert] - v)) / + 2.0; + }; + + // Kahan summation + double value = 0; + double valueCompensation = 0; + for (size_t i = 0; i < NumTri(); ++i) { + const double value1 = prop == Property::SurfaceArea ? Area(i) : Volume(i); + const double t = value + value1; + valueCompensation += (value - t) + value1; + value = t; + } + value += valueCompensation; + return value; +} + +void Manifold::Impl::CalculateCurvature(int gaussianIdx, int meanIdx) { + ZoneScoped; + if (IsEmpty()) return; + if (gaussianIdx < 0 && meanIdx < 0) return; + Vec vertMeanCurvature(NumVert(), 0); + Vec vertGaussianCurvature(NumVert(), kTwoPi); + Vec vertArea(NumVert(), 0); + Vec degree(NumVert(), 0); + auto policy = autoPolicy(NumTri(), 1e4); + for_each(policy, countAt(0_uz), countAt(NumTri()), + CurvatureAngles({vertMeanCurvature, vertGaussianCurvature, vertArea, + degree, halfedge_, vertPos_, faceNormal_})); + for_each_n(policy, countAt(0), NumVert(), + [&vertMeanCurvature, &vertGaussianCurvature, &vertArea, + °ree](const int vert) { + const double factor = degree[vert] / (6 * vertArea[vert]); + vertMeanCurvature[vert] *= factor; + vertGaussianCurvature[vert] *= factor; + }); + + const int oldNumProp = NumProp(); + const int numProp = std::max(oldNumProp, std::max(gaussianIdx, meanIdx) + 1); + const Vec oldProperties = meshRelation_.properties; + meshRelation_.properties = Vec(numProp * NumPropVert(), 0); + meshRelation_.numProp = numProp; + if (meshRelation_.triProperties.size() == 0) { + meshRelation_.triProperties.resize(NumTri()); + } + + const Vec counters(NumPropVert(), 0); + for_each_n( + policy, countAt(0_uz), NumTri(), + UpdateProperties({meshRelation_.triProperties, meshRelation_.properties, + counters, oldProperties, halfedge_, vertMeanCurvature, + vertGaussianCurvature, oldNumProp, numProp, gaussianIdx, + meanIdx})); +} + +/** + * Calculates the bounding box of the entire manifold, which is stored + * internally to short-cut Boolean operations. Ignores NaNs. + */ +void Manifold::Impl::CalculateBBox() { + bBox_.min = + reduce(vertPos_.begin(), vertPos_.end(), + vec3(std::numeric_limits::infinity()), [](auto a, auto b) { + if (std::isnan(a.x)) return b; + if (std::isnan(b.x)) return a; + return la::min(a, b); + }); + bBox_.max = reduce(vertPos_.begin(), vertPos_.end(), + vec3(-std::numeric_limits::infinity()), + [](auto a, auto b) { + if (std::isnan(a.x)) return b; + if (std::isnan(b.x)) return a; + return la::max(a, b); + }); +} + +/** + * Determines if all verts are finite. Checking just the bounding box dimensions + * is insufficient as it ignores NaNs. + */ +bool Manifold::Impl::IsFinite() const { + return transform_reduce( + vertPos_.begin(), vertPos_.end(), true, + [](bool a, bool b) { return a && b; }, + [](auto v) { return la::all(la::isfinite(v)); }); +} + +/** + * Checks that the input triVerts array has all indices inside bounds of the + * vertPos_ array. + */ +bool Manifold::Impl::IsIndexInBounds(VecView triVerts) const { + ivec2 minmax = transform_reduce( + triVerts.begin(), triVerts.end(), + ivec2(std::numeric_limits::max(), std::numeric_limits::min()), + [](auto a, auto b) { + a[0] = std::min(a[0], b[0]); + a[1] = std::max(a[1], b[1]); + return a; + }, + [](auto tri) { + return ivec2(std::min(tri[0], std::min(tri[1], tri[2])), + std::max(tri[0], std::max(tri[1], tri[2]))); + }); + + return minmax[0] >= 0 && minmax[1] < static_cast(NumVert()); +} + +/* + * Returns the minimum gap between two manifolds. Returns a double between + * 0 and searchLength. + */ +double Manifold::Impl::MinGap(const Manifold::Impl& other, + double searchLength) const { + ZoneScoped; + Vec faceBoxOther; + Vec faceMortonOther; + + other.GetFaceBoxMorton(faceBoxOther, faceMortonOther); + + transform(faceBoxOther.begin(), faceBoxOther.end(), faceBoxOther.begin(), + [searchLength](const Box& box) { + return Box(box.min - vec3(searchLength), + box.max + vec3(searchLength)); + }); + + SparseIndices collisions = collider_.Collisions(faceBoxOther.cview()); + + double minDistanceSquared = transform_reduce( + countAt(0_uz), countAt(collisions.size()), searchLength * searchLength, + [](double a, double b) { return std::min(a, b); }, + [&collisions, this, &other](int i) { + const int tri = collisions.Get(i, 1); + const int triOther = collisions.Get(i, 0); + + std::array p; + std::array q; + + for (const int j : {0, 1, 2}) { + p[j] = vertPos_[halfedge_[3 * tri + j].startVert]; + q[j] = other.vertPos_[other.halfedge_[3 * triOther + j].startVert]; + } + + return DistanceTriangleTriangleSquared(p, q); + }); + + return sqrt(minDistanceSquared); +}; + +} // namespace manifold diff --git a/thirdparty/manifold/src/quickhull.cpp b/thirdparty/manifold/src/quickhull.cpp new file mode 100644 index 000000000000..be2b89e1cad7 --- /dev/null +++ b/thirdparty/manifold/src/quickhull.cpp @@ -0,0 +1,860 @@ +// Copyright 2024 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Derived from the public domain work of Antti Kuukka at +// https://github.com/akuukka/quickhull + +#include "quickhull.h" + +#include +#include + +#include "./impl.h" + +namespace manifold { + +double defaultEps() { return 0.0000001; } + +inline double getSquaredDistanceBetweenPointAndRay(const vec3& p, + const Ray& r) { + const vec3 s = p - r.S; + double t = la::dot(s, r.V); + return la::dot(s, s) - t * t * r.VInvLengthSquared; +} + +inline double getSquaredDistance(const vec3& p1, const vec3& p2) { + return la::dot(p1 - p2, p1 - p2); +} +// Note that the unit of distance returned is relative to plane's normal's +// length (divide by N.getNormalized() if needed to get the "real" distance). +inline double getSignedDistanceToPlane(const vec3& v, const Plane& p) { + return la::dot(p.N, v) + p.D; +} + +inline vec3 getTriangleNormal(const vec3& a, const vec3& b, const vec3& c) { + // We want to get (a-c).crossProduct(b-c) without constructing temp vectors + double x = a.x - c.x; + double y = a.y - c.y; + double z = a.z - c.z; + double rhsx = b.x - c.x; + double rhsy = b.y - c.y; + double rhsz = b.z - c.z; + double px = y * rhsz - z * rhsy; + double py = z * rhsx - x * rhsz; + double pz = x * rhsy - y * rhsx; + return la::normalize(vec3(px, py, pz)); +} + +size_t MeshBuilder::addFace() { + if (disabledFaces.size()) { + size_t index = disabledFaces.back(); + auto& f = faces[index]; + DEBUG_ASSERT(f.isDisabled(), logicErr, "f should be disabled"); + DEBUG_ASSERT(!f.pointsOnPositiveSide, logicErr, + "f should not be on the positive side"); + f.mostDistantPointDist = 0; + disabledFaces.pop_back(); + return index; + } + faces.emplace_back(); + return faces.size() - 1; +} + +size_t MeshBuilder::addHalfedge() { + if (disabledHalfedges.size()) { + const size_t index = disabledHalfedges.back(); + disabledHalfedges.pop_back(); + return index; + } + halfedges.push_back({}); + halfedgeToFace.push_back(0); + halfedgeNext.push_back(0); + return halfedges.size() - 1; +} + +void MeshBuilder::setup(int a, int b, int c, int d) { + faces.clear(); + halfedges.clear(); + halfedgeToFace.clear(); + halfedgeNext.clear(); + disabledFaces.clear(); + disabledHalfedges.clear(); + + faces.reserve(4); + halfedges.reserve(12); + + // Create halfedges + // AB + halfedges.push_back({0, b, 6}); + halfedgeToFace.push_back(0); + halfedgeNext.push_back(1); + // BC + halfedges.push_back({0, c, 9}); + halfedgeToFace.push_back(0); + halfedgeNext.push_back(2); + // CA + halfedges.push_back({0, a, 3}); + halfedgeToFace.push_back(0); + halfedgeNext.push_back(0); + // AC + halfedges.push_back({0, c, 2}); + halfedgeToFace.push_back(1); + halfedgeNext.push_back(4); + // CD + halfedges.push_back({0, d, 11}); + halfedgeToFace.push_back(1); + halfedgeNext.push_back(5); + // DA + halfedges.push_back({0, a, 7}); + halfedgeToFace.push_back(1); + halfedgeNext.push_back(3); + // BA + halfedges.push_back({0, a, 0}); + halfedgeToFace.push_back(2); + halfedgeNext.push_back(7); + // AD + halfedges.push_back({0, d, 5}); + halfedgeToFace.push_back(2); + halfedgeNext.push_back(8); + // DB + halfedges.push_back({0, b, 10}); + halfedgeToFace.push_back(2); + halfedgeNext.push_back(6); + // CB + halfedges.push_back({0, b, 1}); + halfedgeToFace.push_back(3); + halfedgeNext.push_back(10); + // BD + halfedges.push_back({0, d, 8}); + halfedgeToFace.push_back(3); + halfedgeNext.push_back(11); + // DC + halfedges.push_back({0, c, 4}); + halfedgeToFace.push_back(3); + halfedgeNext.push_back(9); + + // Create faces + faces.emplace_back(0); + faces.emplace_back(3); + faces.emplace_back(6); + faces.emplace_back(9); +} + +std::array MeshBuilder::getVertexIndicesOfFace(const Face& f) const { + std::array v; + size_t index = f.he; + auto* he = &halfedges[index]; + v[0] = he->endVert; + + index = halfedgeNext[index]; + he = &halfedges[index]; + v[1] = he->endVert; + + index = halfedgeNext[index]; + he = &halfedges[index]; + v[2] = he->endVert; + return v; +} + +HalfEdgeMesh::HalfEdgeMesh(const MeshBuilder& builderObject, + const VecView& vertexData) { + std::unordered_map faceMapping; + std::unordered_map halfEdgeMapping; + std::unordered_map vertexMapping; + + size_t i = 0; + for (const auto& face : builderObject.faces) { + if (!face.isDisabled()) { + halfEdgeIndexFaces.emplace_back(static_cast(face.he)); + faceMapping[i] = halfEdgeIndexFaces.size() - 1; + + const auto heIndices = builderObject.getHalfEdgeIndicesOfFace(face); + for (const auto heIndex : heIndices) { + const auto vertexIndex = builderObject.halfedges[heIndex].endVert; + if (vertexMapping.count(vertexIndex) == 0) { + vertices.push_back(vertexData[vertexIndex]); + vertexMapping[vertexIndex] = vertices.size() - 1; + } + } + } + i++; + } + + i = 0; + for (const auto& halfEdge : builderObject.halfedges) { + if (halfEdge.pairedHalfedge != -1) { + halfedges.push_back({halfEdge.endVert, halfEdge.pairedHalfedge, + builderObject.halfedgeToFace[i]}); + halfedgeToFace.push_back(builderObject.halfedgeToFace[i]); + halfedgeNext.push_back(builderObject.halfedgeNext[i]); + halfEdgeMapping[i] = halfedges.size() - 1; + } + i++; + } + + for (auto& halfEdgeIndexFace : halfEdgeIndexFaces) { + DEBUG_ASSERT(halfEdgeMapping.count(halfEdgeIndexFace) == 1, logicErr, + "invalid halfedge mapping"); + halfEdgeIndexFace = halfEdgeMapping[halfEdgeIndexFace]; + } + + for (size_t i = 0; i < halfedges.size(); i++) { + auto& he = halfedges[i]; + halfedgeToFace[i] = faceMapping[halfedgeToFace[i]]; + he.pairedHalfedge = halfEdgeMapping[he.pairedHalfedge]; + halfedgeNext[i] = halfEdgeMapping[halfedgeNext[i]]; + he.endVert = vertexMapping[he.endVert]; + } +} + +/* + * Implementation of the algorithm + */ +std::pair, Vec> QuickHull::buildMesh(double epsilon) { + if (originalVertexData.size() == 0) { + return {Vec(), Vec()}; + } + + // Very first: find extreme values and use them to compute the scale of the + // point cloud. + extremeValues = getExtremeValues(); + scale = getScale(extremeValues); + + // Epsilon we use depends on the scale + m_epsilon = epsilon * scale; + epsilonSquared = m_epsilon * m_epsilon; + + // The planar case happens when all the points appear to lie on a two + // dimensional subspace of R^3. + planar = false; + createConvexHalfedgeMesh(); + if (planar) { + const int extraPointIndex = planarPointCloudTemp.size() - 1; + for (auto& he : mesh.halfedges) { + if (he.endVert == extraPointIndex) { + he.endVert = 0; + } + } + planarPointCloudTemp.clear(); + } + + // reorder halfedges + Vec halfedges(mesh.halfedges.size()); + Vec halfedgeToFace(mesh.halfedges.size()); + Vec counts(mesh.halfedges.size(), 0); + Vec mapping(mesh.halfedges.size()); + Vec faceMap(mesh.faces.size()); + + // Some faces are disabled and should not go into the halfedge vector, we can + // update the face indices of the halfedges at the end using index/3 + int j = 0; + for_each( + autoPolicy(mesh.halfedges.size()), countAt(0_uz), + countAt(mesh.halfedges.size()), [&](size_t i) { + if (mesh.halfedges[i].pairedHalfedge < 0) return; + if (mesh.faces[mesh.halfedgeToFace[i]].isDisabled()) return; + if (AtomicAdd(counts[mesh.halfedgeToFace[i]], 1) > 0) return; + int currIndex = AtomicAdd(j, 3); + mapping[i] = currIndex; + halfedges[currIndex + 0] = mesh.halfedges[i]; + halfedgeToFace[currIndex + 0] = mesh.halfedgeToFace[i]; + + size_t k = mesh.halfedgeNext[i]; + mapping[k] = currIndex + 1; + halfedges[currIndex + 1] = mesh.halfedges[k]; + halfedgeToFace[currIndex + 1] = mesh.halfedgeToFace[k]; + + k = mesh.halfedgeNext[k]; + mapping[k] = currIndex + 2; + halfedges[currIndex + 2] = mesh.halfedges[k]; + halfedgeToFace[currIndex + 2] = mesh.halfedgeToFace[k]; + halfedges[currIndex + 0].startVert = halfedges[currIndex + 2].endVert; + halfedges[currIndex + 1].startVert = halfedges[currIndex + 0].endVert; + halfedges[currIndex + 2].startVert = halfedges[currIndex + 1].endVert; + }); + halfedges.resize(j); + halfedgeToFace.resize(j); + // fix pairedHalfedge id + for_each( + autoPolicy(halfedges.size()), halfedges.begin(), halfedges.end(), + [&](Halfedge& he) { he.pairedHalfedge = mapping[he.pairedHalfedge]; }); + counts.resize(originalVertexData.size() + 1); + fill(counts.begin(), counts.end(), 0); + + // remove unused vertices + for_each(autoPolicy(halfedges.size() / 3), countAt(0_uz), + countAt(halfedges.size() / 3), [&](size_t i) { + AtomicAdd(counts[halfedges[3 * i].startVert], 1); + AtomicAdd(counts[halfedges[3 * i + 1].startVert], 1); + AtomicAdd(counts[halfedges[3 * i + 2].startVert], 1); + }); + auto saturate = [](int c) { return c > 0 ? 1 : 0; }; + exclusive_scan(TransformIterator(counts.begin(), saturate), + TransformIterator(counts.end(), saturate), counts.begin(), 0); + Vec vertices(counts.back()); + for_each(autoPolicy(originalVertexData.size()), countAt(0_uz), + countAt(originalVertexData.size()), [&](size_t i) { + if (counts[i + 1] - counts[i] > 0) { + vertices[counts[i]] = originalVertexData[i]; + } + }); + for_each(autoPolicy(halfedges.size()), halfedges.begin(), halfedges.end(), + [&](Halfedge& he) { + he.startVert = counts[he.startVert]; + he.endVert = counts[he.endVert]; + }); + return {std::move(halfedges), std::move(vertices)}; +} + +void QuickHull::createConvexHalfedgeMesh() { + visibleFaces.clear(); + horizonEdgesData.clear(); + possiblyVisibleFaces.clear(); + + // Compute base tetrahedron + setupInitialTetrahedron(); + DEBUG_ASSERT(mesh.faces.size() == 4, logicErr, "not a tetrahedron"); + + // Init face stack with those faces that have points assigned to them + faceList.clear(); + for (size_t i = 0; i < 4; i++) { + auto& f = mesh.faces[i]; + if (f.pointsOnPositiveSide && f.pointsOnPositiveSide->size() > 0) { + faceList.push_back(i); + f.inFaceStack = 1; + } + } + + // Process faces until the face list is empty. + size_t iter = 0; + while (!faceList.empty()) { + iter++; + if (iter == std::numeric_limits::max()) { + // Visible face traversal marks visited faces with iteration counter (to + // mark that the face has been visited on this iteration) and the max + // value represents unvisited faces. At this point we have to reset + // iteration counter. This shouldn't be an issue on 64 bit machines. + iter = 0; + } + + const auto topFaceIndex = faceList.front(); + faceList.pop_front(); + + auto& tf = mesh.faces[topFaceIndex]; + tf.inFaceStack = 0; + + DEBUG_ASSERT( + !tf.pointsOnPositiveSide || tf.pointsOnPositiveSide->size() > 0, + logicErr, "there should be points on the positive side"); + if (!tf.pointsOnPositiveSide || tf.isDisabled()) { + continue; + } + + // Pick the most distant point to this triangle plane as the point to which + // we extrude + const vec3& activePoint = originalVertexData[tf.mostDistantPoint]; + const size_t activePointIndex = tf.mostDistantPoint; + + // Find out the faces that have our active point on their positive side + // (these are the "visible faces"). The face on top of the stack of course + // is one of them. At the same time, we create a list of horizon edges. + horizonEdgesData.clear(); + possiblyVisibleFaces.clear(); + visibleFaces.clear(); + possiblyVisibleFaces.push_back({topFaceIndex, -1}); + while (possiblyVisibleFaces.size()) { + const auto faceData = possiblyVisibleFaces.back(); + possiblyVisibleFaces.pop_back(); + auto& pvf = mesh.faces[faceData.faceIndex]; + DEBUG_ASSERT(!pvf.isDisabled(), logicErr, "pvf should not be disabled"); + + if (pvf.visibilityCheckedOnIteration == iter) { + if (pvf.isVisibleFaceOnCurrentIteration) { + continue; + } + } else { + const Plane& P = pvf.P; + pvf.visibilityCheckedOnIteration = iter; + const double d = la::dot(P.N, activePoint) + P.D; + if (d > 0) { + pvf.isVisibleFaceOnCurrentIteration = 1; + pvf.horizonEdgesOnCurrentIteration = 0; + visibleFaces.push_back(faceData.faceIndex); + for (auto heIndex : mesh.getHalfEdgeIndicesOfFace(pvf)) { + if (mesh.halfedges[heIndex].pairedHalfedge != + faceData.enteredFromHalfedge) { + possiblyVisibleFaces.push_back( + {mesh.halfedgeToFace[mesh.halfedges[heIndex].pairedHalfedge], + heIndex}); + } + } + continue; + } + DEBUG_ASSERT(faceData.faceIndex != topFaceIndex, logicErr, + "face index invalid"); + } + + // The face is not visible. Therefore, the halfedge we came from is part + // of the horizon edge. + pvf.isVisibleFaceOnCurrentIteration = 0; + horizonEdgesData.push_back(faceData.enteredFromHalfedge); + // Store which half edge is the horizon edge. The other half edges of the + // face will not be part of the final mesh so their data slots can by + // recycled. + const auto halfEdgesMesh = mesh.getHalfEdgeIndicesOfFace( + mesh.faces[mesh.halfedgeToFace[faceData.enteredFromHalfedge]]); + const std::int8_t ind = + (halfEdgesMesh[0] == faceData.enteredFromHalfedge) + ? 0 + : (halfEdgesMesh[1] == faceData.enteredFromHalfedge ? 1 : 2); + mesh.faces[mesh.halfedgeToFace[faceData.enteredFromHalfedge]] + .horizonEdgesOnCurrentIteration |= (1 << ind); + } + const size_t horizonEdgeCount = horizonEdgesData.size(); + + // Order horizon edges so that they form a loop. This may fail due to + // numerical instability in which case we give up trying to solve horizon + // edge for this point and accept a minor degeneration in the convex hull. + if (!reorderHorizonEdges(horizonEdgesData)) { + failedHorizonEdges++; + int change_flag = 0; + for (size_t index = 0; index < tf.pointsOnPositiveSide->size(); index++) { + if ((*tf.pointsOnPositiveSide)[index] == activePointIndex) { + change_flag = 1; + } else if (change_flag == 1) { + change_flag = 2; + (*tf.pointsOnPositiveSide)[index - 1] = + (*tf.pointsOnPositiveSide)[index]; + } + } + if (change_flag == 1) + tf.pointsOnPositiveSide->resize(tf.pointsOnPositiveSide->size() - 1); + + if (tf.pointsOnPositiveSide->size() == 0) { + reclaimToIndexVectorPool(tf.pointsOnPositiveSide); + } + continue; + } + + // Except for the horizon edges, all half edges of the visible faces can be + // marked as disabled. Their data slots will be reused. The faces will be + // disabled as well, but we need to remember the points that were on the + // positive side of them - therefore we save pointers to them. + newFaceIndices.clear(); + newHalfedgeIndices.clear(); + disabledFacePointVectors.clear(); + size_t disableCounter = 0; + for (auto faceIndex : visibleFaces) { + auto& disabledFace = mesh.faces[faceIndex]; + auto halfEdgesMesh = mesh.getHalfEdgeIndicesOfFace(disabledFace); + for (size_t j = 0; j < 3; j++) { + if ((disabledFace.horizonEdgesOnCurrentIteration & (1 << j)) == 0) { + if (disableCounter < horizonEdgeCount * 2) { + // Use on this iteration + newHalfedgeIndices.push_back(halfEdgesMesh[j]); + disableCounter++; + } else { + // Mark for reusal on later iteration step + mesh.disableHalfedge(halfEdgesMesh[j]); + } + } + } + // Disable the face, but retain pointer to the points that were on the + // positive side of it. We need to assign those points to the new faces we + // create shortly. + auto t = mesh.disableFace(faceIndex); + if (t) { + // Because we should not assign point vectors to faces unless needed... + DEBUG_ASSERT(t->size(), logicErr, "t should not be empty"); + disabledFacePointVectors.push_back(std::move(t)); + } + } + if (disableCounter < horizonEdgeCount * 2) { + const size_t newHalfEdgesNeeded = horizonEdgeCount * 2 - disableCounter; + for (size_t i = 0; i < newHalfEdgesNeeded; i++) { + newHalfedgeIndices.push_back(mesh.addHalfedge()); + } + } + + // Create new faces using the edgeloop + for (size_t i = 0; i < horizonEdgeCount; i++) { + const size_t AB = horizonEdgesData[i]; + + auto horizonEdgeVertexIndices = + mesh.getVertexIndicesOfHalfEdge(mesh.halfedges[AB]); + size_t A, B, C; + A = horizonEdgeVertexIndices[0]; + B = horizonEdgeVertexIndices[1]; + C = activePointIndex; + + const size_t newFaceIndex = mesh.addFace(); + newFaceIndices.push_back(newFaceIndex); + + const size_t CA = newHalfedgeIndices[2 * i + 0]; + const size_t BC = newHalfedgeIndices[2 * i + 1]; + + mesh.halfedgeNext[AB] = BC; + mesh.halfedgeNext[BC] = CA; + mesh.halfedgeNext[CA] = AB; + + mesh.halfedgeToFace[BC] = newFaceIndex; + mesh.halfedgeToFace[CA] = newFaceIndex; + mesh.halfedgeToFace[AB] = newFaceIndex; + + mesh.halfedges[CA].endVert = A; + mesh.halfedges[BC].endVert = C; + + auto& newFace = mesh.faces[newFaceIndex]; + + const vec3 planeNormal = getTriangleNormal( + originalVertexData[A], originalVertexData[B], activePoint); + newFace.P = Plane(planeNormal, activePoint); + newFace.he = AB; + + mesh.halfedges[CA].pairedHalfedge = + newHalfedgeIndices[i > 0 ? i * 2 - 1 : 2 * horizonEdgeCount - 1]; + mesh.halfedges[BC].pairedHalfedge = + newHalfedgeIndices[((i + 1) * 2) % (horizonEdgeCount * 2)]; + } + + // Assign points that were on the positive side of the disabled faces to the + // new faces. + for (auto& disabledPoints : disabledFacePointVectors) { + DEBUG_ASSERT(disabledPoints != nullptr, logicErr, + "disabledPoints should not be null"); + for (const auto& point : *(disabledPoints)) { + if (point == activePointIndex) { + continue; + } + for (size_t j = 0; j < horizonEdgeCount; j++) { + if (addPointToFace(mesh.faces[newFaceIndices[j]], point)) { + break; + } + } + } + // The points are no longer needed: we can move them to the vector pool + // for reuse. + reclaimToIndexVectorPool(disabledPoints); + } + + // Increase face stack size if needed + for (const auto newFaceIndex : newFaceIndices) { + auto& newFace = mesh.faces[newFaceIndex]; + if (newFace.pointsOnPositiveSide) { + DEBUG_ASSERT(newFace.pointsOnPositiveSide->size() > 0, logicErr, + "there should be points on the positive side"); + if (!newFace.inFaceStack) { + faceList.push_back(newFaceIndex); + newFace.inFaceStack = 1; + } + } + } + } + + // Cleanup + indexVectorPool.clear(); +} + +/* + * Private helper functions + */ + +std::array QuickHull::getExtremeValues() { + std::array outIndices{0, 0, 0, 0, 0, 0}; + double extremeVals[6] = {originalVertexData[0].x, originalVertexData[0].x, + originalVertexData[0].y, originalVertexData[0].y, + originalVertexData[0].z, originalVertexData[0].z}; + const size_t vCount = originalVertexData.size(); + for (size_t i = 1; i < vCount; i++) { + const vec3& pos = originalVertexData[i]; + if (pos.x > extremeVals[0]) { + extremeVals[0] = pos.x; + outIndices[0] = i; + } else if (pos.x < extremeVals[1]) { + extremeVals[1] = pos.x; + outIndices[1] = i; + } + if (pos.y > extremeVals[2]) { + extremeVals[2] = pos.y; + outIndices[2] = i; + } else if (pos.y < extremeVals[3]) { + extremeVals[3] = pos.y; + outIndices[3] = i; + } + if (pos.z > extremeVals[4]) { + extremeVals[4] = pos.z; + outIndices[4] = i; + } else if (pos.z < extremeVals[5]) { + extremeVals[5] = pos.z; + outIndices[5] = i; + } + } + return outIndices; +} + +bool QuickHull::reorderHorizonEdges(VecView& horizonEdges) { + const size_t horizonEdgeCount = horizonEdges.size(); + for (size_t i = 0; i + 1 < horizonEdgeCount; i++) { + const size_t endVertexCheck = mesh.halfedges[horizonEdges[i]].endVert; + bool foundNext = false; + for (size_t j = i + 1; j < horizonEdgeCount; j++) { + const size_t beginVertex = + mesh.halfedges[mesh.halfedges[horizonEdges[j]].pairedHalfedge] + .endVert; + if (beginVertex == endVertexCheck) { + std::swap(horizonEdges[i + 1], horizonEdges[j]); + foundNext = true; + break; + } + } + if (!foundNext) { + return false; + } + } + DEBUG_ASSERT( + mesh.halfedges[horizonEdges[horizonEdges.size() - 1]].endVert == + mesh.halfedges[mesh.halfedges[horizonEdges[0]].pairedHalfedge] + .endVert, + logicErr, "invalid halfedge"); + return true; +} + +double QuickHull::getScale(const std::array& extremeValuesInput) { + double s = 0; + for (size_t i = 0; i < 6; i++) { + const double* v = + (const double*)(&originalVertexData[extremeValuesInput[i]]); + v += i / 2; + auto a = std::abs(*v); + if (a > s) { + s = a; + } + } + return s; +} + +void QuickHull::setupInitialTetrahedron() { + const size_t vertexCount = originalVertexData.size(); + + // If we have at most 4 points, just return a degenerate tetrahedron: + if (vertexCount <= 4) { + size_t v[4] = {0, std::min((size_t)1, vertexCount - 1), + std::min((size_t)2, vertexCount - 1), + std::min((size_t)3, vertexCount - 1)}; + const vec3 N = + getTriangleNormal(originalVertexData[v[0]], originalVertexData[v[1]], + originalVertexData[v[2]]); + const Plane trianglePlane(N, originalVertexData[v[0]]); + if (trianglePlane.isPointOnPositiveSide(originalVertexData[v[3]])) { + std::swap(v[0], v[1]); + } + return mesh.setup(v[0], v[1], v[2], v[3]); + } + + // Find two most distant extreme points. + double maxD = epsilonSquared; + std::pair selectedPoints; + for (size_t i = 0; i < 6; i++) { + for (size_t j = i + 1; j < 6; j++) { + // I found a function for squaredDistance but i can't seem to include it + // like this for some reason + const double d = getSquaredDistance(originalVertexData[extremeValues[i]], + originalVertexData[extremeValues[j]]); + if (d > maxD) { + maxD = d; + selectedPoints = {extremeValues[i], extremeValues[j]}; + } + } + } + if (maxD == epsilonSquared) { + // A degenerate case: the point cloud seems to consists of a single point + return mesh.setup(0, std::min((size_t)1, vertexCount - 1), + std::min((size_t)2, vertexCount - 1), + std::min((size_t)3, vertexCount - 1)); + } + DEBUG_ASSERT(selectedPoints.first != selectedPoints.second, logicErr, + "degenerate selectedPoints"); + + // Find the most distant point to the line between the two chosen extreme + // points. + const Ray r(originalVertexData[selectedPoints.first], + (originalVertexData[selectedPoints.second] - + originalVertexData[selectedPoints.first])); + maxD = epsilonSquared; + size_t maxI = std::numeric_limits::max(); + const size_t vCount = originalVertexData.size(); + for (size_t i = 0; i < vCount; i++) { + const double distToRay = + getSquaredDistanceBetweenPointAndRay(originalVertexData[i], r); + if (distToRay > maxD) { + maxD = distToRay; + maxI = i; + } + } + if (maxD == epsilonSquared) { + // It appears that the point cloud belongs to a 1 dimensional subspace of + // R^3: convex hull has no volume => return a thin triangle Pick any point + // other than selectedPoints.first and selectedPoints.second as the third + // point of the triangle + auto it = + std::find_if(originalVertexData.begin(), originalVertexData.end(), + [&](const vec3& ve) { + return ve != originalVertexData[selectedPoints.first] && + ve != originalVertexData[selectedPoints.second]; + }); + const size_t thirdPoint = + (it == originalVertexData.end()) + ? selectedPoints.first + : std::distance(originalVertexData.begin(), it); + it = + std::find_if(originalVertexData.begin(), originalVertexData.end(), + [&](const vec3& ve) { + return ve != originalVertexData[selectedPoints.first] && + ve != originalVertexData[selectedPoints.second] && + ve != originalVertexData[thirdPoint]; + }); + const size_t fourthPoint = + (it == originalVertexData.end()) + ? selectedPoints.first + : std::distance(originalVertexData.begin(), it); + return mesh.setup(selectedPoints.first, selectedPoints.second, thirdPoint, + fourthPoint); + } + + // These three points form the base triangle for our tetrahedron. + DEBUG_ASSERT(selectedPoints.first != maxI && selectedPoints.second != maxI, + logicErr, "degenerate selectedPoints"); + std::array baseTriangle{selectedPoints.first, + selectedPoints.second, maxI}; + const vec3 baseTriangleVertices[] = {originalVertexData[baseTriangle[0]], + originalVertexData[baseTriangle[1]], + originalVertexData[baseTriangle[2]]}; + + // Next step is to find the 4th vertex of the tetrahedron. We naturally choose + // the point farthest away from the triangle plane. + maxD = m_epsilon; + maxI = 0; + const vec3 N = + getTriangleNormal(baseTriangleVertices[0], baseTriangleVertices[1], + baseTriangleVertices[2]); + Plane trianglePlane(N, baseTriangleVertices[0]); + for (size_t i = 0; i < vCount; i++) { + const double d = std::abs( + getSignedDistanceToPlane(originalVertexData[i], trianglePlane)); + if (d > maxD) { + maxD = d; + maxI = i; + } + } + if (maxD == m_epsilon) { + // All the points seem to lie on a 2D subspace of R^3. How to handle this? + // Well, let's add one extra point to the point cloud so that the convex + // hull will have volume. + planar = true; + const vec3 N1 = + getTriangleNormal(baseTriangleVertices[1], baseTriangleVertices[2], + baseTriangleVertices[0]); + planarPointCloudTemp = Vec(originalVertexData); + const vec3 extraPoint = N1 + originalVertexData[0]; + planarPointCloudTemp.push_back(extraPoint); + maxI = planarPointCloudTemp.size() - 1; + originalVertexData = planarPointCloudTemp; + } + + // Enforce CCW orientation (if user prefers clockwise orientation, swap two + // vertices in each triangle when final mesh is created) + const Plane triPlane(N, baseTriangleVertices[0]); + if (triPlane.isPointOnPositiveSide(originalVertexData[maxI])) { + std::swap(baseTriangle[0], baseTriangle[1]); + } + + // Create a tetrahedron half edge mesh and compute planes defined by each + // triangle + mesh.setup(baseTriangle[0], baseTriangle[1], baseTriangle[2], maxI); + for (auto& f : mesh.faces) { + auto v = mesh.getVertexIndicesOfFace(f); + const vec3 N1 = + getTriangleNormal(originalVertexData[v[0]], originalVertexData[v[1]], + originalVertexData[v[2]]); + const Plane plane(N1, originalVertexData[v[0]]); + f.P = plane; + } + + // Finally we assign a face for each vertex outside the tetrahedron (vertices + // inside the tetrahedron have no role anymore) + for (size_t i = 0; i < vCount; i++) { + for (auto& face : mesh.faces) { + if (addPointToFace(face, i)) { + break; + } + } + } +} + +std::unique_ptr> QuickHull::getIndexVectorFromPool() { + auto r = indexVectorPool.get(); + r->resize(0); + return r; +} + +void QuickHull::reclaimToIndexVectorPool(std::unique_ptr>& ptr) { + const size_t oldSize = ptr->size(); + if ((oldSize + 1) * 128 < ptr->capacity()) { + // Reduce memory usage! Huge vectors are needed at the beginning of + // iteration when faces have many points on their positive side. Later on, + // smaller vectors will suffice. + ptr.reset(nullptr); + return; + } + indexVectorPool.reclaim(ptr); +} + +bool QuickHull::addPointToFace(typename MeshBuilder::Face& f, + size_t pointIndex) { + const double D = + getSignedDistanceToPlane(originalVertexData[pointIndex], f.P); + if (D > 0 && D * D > epsilonSquared * f.P.sqrNLength) { + if (!f.pointsOnPositiveSide) { + f.pointsOnPositiveSide = getIndexVectorFromPool(); + } + f.pointsOnPositiveSide->push_back(pointIndex); + if (D > f.mostDistantPointDist) { + f.mostDistantPointDist = D; + f.mostDistantPoint = pointIndex; + } + return true; + } + return false; +} + +// Wrapper to call the QuickHull algorithm with the given vertex data to build +// the Impl +void Manifold::Impl::Hull(VecView vertPos) { + size_t numVert = vertPos.size(); + if (numVert < 4) { + status_ = Error::InvalidConstruction; + return; + } + + QuickHull qh(vertPos); + std::tie(halfedge_, vertPos_) = qh.buildMesh(); + CalculateBBox(); + SetEpsilon(); + CalculateNormals(); + InitializeOriginal(); + Finish(); + CreateFaces(); +} + +} // namespace manifold diff --git a/thirdparty/manifold/src/quickhull.h b/thirdparty/manifold/src/quickhull.h new file mode 100644 index 000000000000..3f90575097d6 --- /dev/null +++ b/thirdparty/manifold/src/quickhull.h @@ -0,0 +1,288 @@ +// Copyright 2024 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Derived from the public domain work of Antti Kuukka at +// https://github.com/akuukka/quickhull + +/* + * INPUT: a list of points in 3D space (for example, vertices of a 3D mesh) + * + * OUTPUT: a ConvexHull object which provides vertex and index buffers of the + *generated convex hull as a triangle mesh. + * + * + * + * The implementation is thread-safe if each thread is using its own QuickHull + *object. + * + * + * SUMMARY OF THE ALGORITHM: + * - Create initial simplex (tetrahedron) using extreme points. We have + *four faces now and they form a convex mesh M. + * - For each point, assign them to the first face for which they are on + *the positive side of (so each point is assigned to at most one face). Points + *inside the initial tetrahedron are left behind now and no longer affect the + *calculations. + * - Add all faces that have points assigned to them to Face Stack. + * - Iterate until Face Stack is empty: + * - Pop topmost face F from the stack + * - From the points assigned to F, pick the point P that is + *farthest away from the plane defined by F. + * - Find all faces of M that have P on their positive side. Let us + *call these the "visible faces". + * - Because of the way M is constructed, these faces are + *connected. Solve their horizon edge loop. + * - "Extrude to P": Create new faces by connecting + *P with the points belonging to the horizon edge. Add the new faces to M and + *remove the visible faces from M. + * - Each point that was assigned to visible faces is now assigned + *to at most one of the newly created faces. + * - Those new faces that have points assigned to them are added to + *the top of Face Stack. + * - M is now the convex hull. + * + * */ +#pragma once +#include +#include +#include + +#include "./shared.h" +#include "./vec.h" + +namespace manifold { + +class Pool { + std::vector>> data; + + public: + void clear() { data.clear(); } + + void reclaim(std::unique_ptr>& ptr) { + data.push_back(std::move(ptr)); + } + + std::unique_ptr> get() { + if (data.size() == 0) { + return std::make_unique>(); + } + auto it = data.end() - 1; + std::unique_ptr> r = std::move(*it); + data.erase(it); + return r; + } +}; + +class Plane { + public: + vec3 N; + + // Signed distance (if normal is of length 1) to the plane from origin + double D; + + // Normal length squared + double sqrNLength; + + bool isPointOnPositiveSide(const vec3& Q) const { + double d = la::dot(N, Q) + D; + if (d >= 0) return true; + return false; + } + + Plane() = default; + + // Construct a plane using normal N and any point P on the plane + Plane(const vec3& N, const vec3& P) + : N(N), D(la::dot(-N, P)), sqrNLength(la::dot(N, N)) {} +}; + +struct Ray { + const vec3 S; + const vec3 V; + const double VInvLengthSquared; + + Ray(const vec3& S, const vec3& V) + : S(S), V(V), VInvLengthSquared(1 / (la::dot(V, V))) {} +}; + +class MeshBuilder { + public: + struct Face { + int he; + Plane P{}; + double mostDistantPointDist = 0.0; + size_t mostDistantPoint = 0; + size_t visibilityCheckedOnIteration = 0; + std::uint8_t isVisibleFaceOnCurrentIteration : 1; + std::uint8_t inFaceStack : 1; + // Bit for each half edge assigned to this face, each being 0 or 1 depending + // on whether the edge belongs to horizon edge + std::uint8_t horizonEdgesOnCurrentIteration : 3; + std::unique_ptr> pointsOnPositiveSide; + + Face(size_t he) + : he(he), + isVisibleFaceOnCurrentIteration(0), + inFaceStack(0), + horizonEdgesOnCurrentIteration(0) {} + + Face() + : he(-1), + isVisibleFaceOnCurrentIteration(0), + inFaceStack(0), + horizonEdgesOnCurrentIteration(0) {} + + void disable() { he = -1; } + + bool isDisabled() const { return he == -1; } + }; + + // Mesh data + std::vector faces; + Vec halfedges; + Vec halfedgeToFace; + Vec halfedgeNext; + + // When the mesh is modified and faces and half edges are removed from it, we + // do not actually remove them from the container vectors. Insted, they are + // marked as disabled which means that the indices can be reused when we need + // to add new faces and half edges to the mesh. We store the free indices in + // the following vectors. + Vec disabledFaces, disabledHalfedges; + + size_t addFace(); + + size_t addHalfedge(); + + // Mark a face as disabled and return a pointer to the points that were on the + // positive of it. + std::unique_ptr> disableFace(size_t faceIndex) { + auto& f = faces[faceIndex]; + f.disable(); + disabledFaces.push_back(faceIndex); + return std::move(f.pointsOnPositiveSide); + } + + void disableHalfedge(size_t heIndex) { + auto& he = halfedges[heIndex]; + he.pairedHalfedge = -1; + disabledHalfedges.push_back(heIndex); + } + + MeshBuilder() = default; + + // Create a mesh with initial tetrahedron ABCD. Dot product of AB with the + // normal of triangle ABC should be negative. + void setup(int a, int b, int c, int d); + + std::array getVertexIndicesOfFace(const Face& f) const; + + std::array getVertexIndicesOfHalfEdge(const Halfedge& he) const { + return {halfedges[he.pairedHalfedge].endVert, he.endVert}; + } + + std::array getHalfEdgeIndicesOfFace(const Face& f) const { + return {f.he, halfedgeNext[f.he], halfedgeNext[halfedgeNext[f.he]]}; + } +}; + +class HalfEdgeMesh { + public: + Vec vertices; + // Index of one of the half edges of the faces + std::vector halfEdgeIndexFaces; + Vec halfedges; + Vec halfedgeToFace; + Vec halfedgeNext; + + HalfEdgeMesh(const MeshBuilder& builderObject, + const VecView& vertexData); +}; + +double defaultEps(); + +class QuickHull { + struct FaceData { + int faceIndex; + // If the face turns out not to be visible, this half edge will be marked as + // horizon edge + int enteredFromHalfedge; + }; + + double m_epsilon, epsilonSquared, scale; + bool planar; + Vec planarPointCloudTemp; + VecView originalVertexData; + MeshBuilder mesh; + std::array extremeValues; + size_t failedHorizonEdges = 0; + + // Temporary variables used during iteration process + Vec newFaceIndices; + Vec newHalfedgeIndices; + Vec visibleFaces; + Vec horizonEdgesData; + Vec possiblyVisibleFaces; + std::vector>> disabledFacePointVectors; + std::deque faceList; + + // Create a half edge mesh representing the base tetrahedron from which the + // QuickHull iteration proceeds. extremeValues must be properly set up when + // this is called. + void setupInitialTetrahedron(); + + // Given a list of half edges, try to rearrange them so that they form a loop. + // Return true on success. + bool reorderHorizonEdges(VecView& horizonEdges); + + // Find indices of extreme values (max x, min x, max y, min y, max z, min z) + // for the given point cloud + std::array getExtremeValues(); + + // Compute scale of the vertex data. + double getScale(const std::array& extremeValuesInput); + + // Each face contains a unique pointer to a vector of indices. However, many - + // often most - faces do not have any points on the positive side of them + // especially at the the end of the iteration. When a face is removed from the + // mesh, its associated point vector, if such exists, is moved to the index + // vector pool, and when we need to add new faces with points on the positive + // side to the mesh, we reuse these vectors. This reduces the amount of + // std::vectors we have to deal with, and impact on performance is remarkable. + Pool indexVectorPool; + inline std::unique_ptr> getIndexVectorFromPool(); + inline void reclaimToIndexVectorPool(std::unique_ptr>& ptr); + + // Associates a point with a face if the point resides on the positive side of + // the plane. Returns true if the points was on the positive side. + inline bool addPointToFace(typename MeshBuilder::Face& f, size_t pointIndex); + + // This will create HalfedgeMesh from which we create the ConvexHull object + // that buildMesh function returns + void createConvexHalfedgeMesh(); + + public: + // This function assumes that the pointCloudVec data resides in memory in the + // following format: x_0,y_0,z_0,x_1,y_1,z_1,... + QuickHull(VecView pointCloudVec) + : originalVertexData(VecView(pointCloudVec)) {} + + // Computes convex hull for a given point cloud. Params: eps: minimum distance + // to a plane to consider a point being on positive side of it (for a point + // cloud with scale 1) Returns: Convex hull of the point cloud as halfEdge + // vector and vertex vector + std::pair, Vec> buildMesh(double eps = defaultEps()); +}; + +} // namespace manifold diff --git a/thirdparty/manifold/src/sdf.cpp b/thirdparty/manifold/src/sdf.cpp new file mode 100644 index 000000000000..64dd7094d7f4 --- /dev/null +++ b/thirdparty/manifold/src/sdf.cpp @@ -0,0 +1,533 @@ +// Copyright 2023 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./hashtable.h" +#include "./impl.h" +#include "./parallel.h" +#include "./utils.h" +#include "./vec.h" +#include "manifold/manifold.h" + +namespace { +using namespace manifold; + +constexpr int kCrossing = -2; +constexpr int kNone = -1; +constexpr ivec4 kVoxelOffset(1, 1, 1, 0); +// Maximum fraction of spacing that a vert can move. +constexpr double kS = 0.25; +// Corresponding approximate distance ratio bound. +constexpr double kD = 1 / kS - 1; +// Maximum number of opposed verts (of 7) to allow collapse. +constexpr int kMaxOpposed = 3; + +ivec3 TetTri0(int i) { + constexpr ivec3 tetTri0[16] = {{-1, -1, -1}, // + {0, 3, 4}, // + {0, 1, 5}, // + {1, 5, 3}, // + {1, 4, 2}, // + {1, 0, 3}, // + {2, 5, 0}, // + {5, 3, 2}, // + {2, 3, 5}, // + {0, 5, 2}, // + {3, 0, 1}, // + {2, 4, 1}, // + {3, 5, 1}, // + {5, 1, 0}, // + {4, 3, 0}, // + {-1, -1, -1}}; + return tetTri0[i]; +} + +ivec3 TetTri1(int i) { + constexpr ivec3 tetTri1[16] = {{-1, -1, -1}, // + {-1, -1, -1}, // + {-1, -1, -1}, // + {3, 4, 1}, // + {-1, -1, -1}, // + {3, 2, 1}, // + {0, 4, 2}, // + {-1, -1, -1}, // + {-1, -1, -1}, // + {2, 4, 0}, // + {1, 2, 3}, // + {-1, -1, -1}, // + {1, 4, 3}, // + {-1, -1, -1}, // + {-1, -1, -1}, // + {-1, -1, -1}}; + return tetTri1[i]; +} + +ivec4 Neighbor(ivec4 base, int i) { + constexpr ivec4 neighbors[14] = {{0, 0, 0, 1}, // + {1, 0, 0, 0}, // + {0, 1, 0, 0}, // + {0, 0, 1, 0}, // + {-1, 0, 0, 1}, // + {0, -1, 0, 1}, // + {0, 0, -1, 1}, // + {-1, -1, -1, 1}, // + {-1, 0, 0, 0}, // + {0, -1, 0, 0}, // + {0, 0, -1, 0}, // + {0, -1, -1, 1}, // + {-1, 0, -1, 1}, // + {-1, -1, 0, 1}}; + ivec4 neighborIndex = base + neighbors[i]; + if (neighborIndex.w == 2) { + neighborIndex += 1; + neighborIndex.w = 0; + } + return neighborIndex; +} + +Uint64 EncodeIndex(ivec4 gridPos, ivec3 gridPow) { + return static_cast(gridPos.w) | static_cast(gridPos.z) << 1 | + static_cast(gridPos.y) << (1 + gridPow.z) | + static_cast(gridPos.x) << (1 + gridPow.z + gridPow.y); +} + +ivec4 DecodeIndex(Uint64 idx, ivec3 gridPow) { + ivec4 gridPos; + gridPos.w = idx & 1; + idx = idx >> 1; + gridPos.z = idx & ((1 << gridPow.z) - 1); + idx = idx >> gridPow.z; + gridPos.y = idx & ((1 << gridPow.y) - 1); + idx = idx >> gridPow.y; + gridPos.x = idx & ((1 << gridPow.x) - 1); + return gridPos; +} + +vec3 Position(ivec4 gridIndex, vec3 origin, vec3 spacing) { + return origin + spacing * (vec3(gridIndex) + (gridIndex.w == 1 ? 0.0 : -0.5)); +} + +vec3 Bound(vec3 pos, vec3 origin, vec3 spacing, ivec3 gridSize) { + return min(max(pos, origin), origin + spacing * (vec3(gridSize) - 1)); +} + +double BoundedSDF(ivec4 gridIndex, vec3 origin, vec3 spacing, ivec3 gridSize, + double level, std::function sdf) { + const ivec3 xyz(gridIndex); + const int lowerBoundDist = minelem(xyz); + const int upperBoundDist = minelem(gridSize - xyz); + const int boundDist = std::min(lowerBoundDist, upperBoundDist - gridIndex.w); + + if (boundDist < 0) { + return 0.0; + } + const double d = sdf(Position(gridIndex, origin, spacing)) - level; + return boundDist == 0 ? std::min(d, 0.0) : d; +} + +// Simplified ITP root finding algorithm - same worst-case performance as +// bisection, better average performance. +inline vec3 FindSurface(vec3 pos0, double d0, vec3 pos1, double d1, double tol, + double level, std::function sdf) { + if (d0 == 0) { + return pos0; + } else if (d1 == 0) { + return pos1; + } + + // Sole tuning parameter, k: (0, 1) - smaller value gets better median + // performance, but also hits the worst case more often. + const double k = 0.1; + const double check = 2 * tol / la::length(pos0 - pos1); + double frac = 1; + double biFrac = 1; + while (frac > check) { + const double t = la::lerp(d0 / (d0 - d1), 0.5, k); + const double r = biFrac / frac - 0.5; + const double x = la::abs(t - 0.5) < r ? t : 0.5 - r * (t < 0.5 ? 1 : -1); + + const vec3 mid = la::lerp(pos0, pos1, x); + const double d = sdf(mid) - level; + + if ((d > 0) == (d0 > 0)) { + d0 = d; + pos0 = mid; + frac *= 1 - x; + } else { + d1 = d; + pos1 = mid; + frac *= x; + } + biFrac /= 2; + } + + return la::lerp(pos0, pos1, d0 / (d0 - d1)); +} + +/** + * Each GridVert is connected to 14 others, and in charge of 7 of these edges + * (see Neighbor() above). Each edge that changes sign contributes one vert, + * unless the GridVert is close enough to the surface, in which case it + * contributes only a single movedVert and all crossing edgeVerts refer to that. + */ +struct GridVert { + double distance = NAN; + int movedVert = kNone; + int edgeVerts[7] = {kNone, kNone, kNone, kNone, kNone, kNone, kNone}; + + inline bool HasMoved() const { return movedVert >= 0; } + + inline bool SameSide(double dist) const { + return (dist > 0) == (distance > 0); + } + + inline int Inside() const { return distance > 0 ? 1 : -1; } + + inline int NeighborInside(int i) const { + return Inside() * (edgeVerts[i] == kNone ? 1 : -1); + } +}; + +struct NearSurface { + VecView vertPos; + VecView vertIndex; + HashTableD gridVerts; + VecView voxels; + const std::function sdf; + const vec3 origin; + const ivec3 gridSize; + const ivec3 gridPow; + const vec3 spacing; + const double level; + const double tol; + + inline void operator()(Uint64 index) { + ZoneScoped; + if (gridVerts.Full()) return; + + const ivec4 gridIndex = DecodeIndex(index, gridPow); + + if (la::any(la::greater(ivec3(gridIndex), gridSize))) return; + + GridVert gridVert; + gridVert.distance = voxels[EncodeIndex(gridIndex + kVoxelOffset, gridPow)]; + + bool keep = false; + double vMax = 0; + int closestNeighbor = -1; + int opposedVerts = 0; + for (int i = 0; i < 7; ++i) { + const double val = + voxels[EncodeIndex(Neighbor(gridIndex, i) + kVoxelOffset, gridPow)]; + const double valOp = voxels[EncodeIndex( + Neighbor(gridIndex, i + 7) + kVoxelOffset, gridPow)]; + + if (!gridVert.SameSide(val)) { + gridVert.edgeVerts[i] = kCrossing; + keep = true; + if (!gridVert.SameSide(valOp)) { + ++opposedVerts; + } + // Approximate bound on vert movement. + if (la::abs(val) > kD * la::abs(gridVert.distance) && + la::abs(val) > la::abs(vMax)) { + vMax = val; + closestNeighbor = i; + } + } else if (!gridVert.SameSide(valOp) && + la::abs(valOp) > kD * la::abs(gridVert.distance) && + la::abs(valOp) > la::abs(vMax)) { + vMax = valOp; + closestNeighbor = i + 7; + } + } + + // This is where we collapse all the crossing edge verts into this GridVert, + // speeding up the algorithm and avoiding poor quality triangles. Without + // this step the result is guaranteed 2-manifold, but with this step it can + // become an even-manifold with kissing verts. These must be removed in a + // post-process: CleanupTopology(). + if (closestNeighbor >= 0 && opposedVerts <= kMaxOpposed) { + const vec3 gridPos = Position(gridIndex, origin, spacing); + const ivec4 neighborIndex = Neighbor(gridIndex, closestNeighbor); + const vec3 pos = FindSurface(gridPos, gridVert.distance, + Position(neighborIndex, origin, spacing), + vMax, tol, level, sdf); + // Bound the delta of each vert to ensure the tetrahedron cannot invert. + if (la::all(la::less(la::abs(pos - gridPos), kS * spacing))) { + const int idx = AtomicAdd(vertIndex[0], 1); + vertPos[idx] = Bound(pos, origin, spacing, gridSize); + gridVert.movedVert = idx; + for (int j = 0; j < 7; ++j) { + if (gridVert.edgeVerts[j] == kCrossing) gridVert.edgeVerts[j] = idx; + } + keep = true; + } + } else { + for (int j = 0; j < 7; ++j) gridVert.edgeVerts[j] = kNone; + } + + if (keep) gridVerts.Insert(index, gridVert); + } +}; + +struct ComputeVerts { + VecView vertPos; + VecView vertIndex; + HashTableD gridVerts; + VecView voxels; + const std::function sdf; + const vec3 origin; + const ivec3 gridSize; + const ivec3 gridPow; + const vec3 spacing; + const double level; + const double tol; + + void operator()(int idx) { + ZoneScoped; + Uint64 baseKey = gridVerts.KeyAt(idx); + if (baseKey == kOpen) return; + + GridVert& gridVert = gridVerts.At(idx); + + if (gridVert.HasMoved()) return; + + const ivec4 gridIndex = DecodeIndex(baseKey, gridPow); + + const vec3 position = Position(gridIndex, origin, spacing); + + // These seven edges are uniquely owned by this gridVert; any of them + // which intersect the surface create a vert. + for (int i = 0; i < 7; ++i) { + const ivec4 neighborIndex = Neighbor(gridIndex, i); + const GridVert& neighbor = gridVerts[EncodeIndex(neighborIndex, gridPow)]; + + const double val = + std::isfinite(neighbor.distance) + ? neighbor.distance + : voxels[EncodeIndex(neighborIndex + kVoxelOffset, gridPow)]; + if (gridVert.SameSide(val)) continue; + + if (neighbor.HasMoved()) { + gridVert.edgeVerts[i] = neighbor.movedVert; + continue; + } + + const int idx = AtomicAdd(vertIndex[0], 1); + const vec3 pos = FindSurface(position, gridVert.distance, + Position(neighborIndex, origin, spacing), + val, tol, level, sdf); + vertPos[idx] = Bound(pos, origin, spacing, gridSize); + gridVert.edgeVerts[i] = idx; + } + } +}; + +struct BuildTris { + VecView triVerts; + VecView triIndex; + const HashTableD gridVerts; + const ivec3 gridPow; + + void CreateTri(const ivec3& tri, const int edges[6]) { + if (tri[0] < 0) return; + const ivec3 verts(edges[tri[0]], edges[tri[1]], edges[tri[2]]); + if (verts[0] == verts[1] || verts[1] == verts[2] || verts[2] == verts[0]) + return; + int idx = AtomicAdd(triIndex[0], 1); + triVerts[idx] = verts; + } + + void CreateTris(const ivec4& tet, const int edges[6]) { + const int i = (tet[0] > 0 ? 1 : 0) + (tet[1] > 0 ? 2 : 0) + + (tet[2] > 0 ? 4 : 0) + (tet[3] > 0 ? 8 : 0); + CreateTri(TetTri0(i), edges); + CreateTri(TetTri1(i), edges); + } + + void operator()(int idx) { + ZoneScoped; + Uint64 baseKey = gridVerts.KeyAt(idx); + if (baseKey == kOpen) return; + + const GridVert& base = gridVerts.At(idx); + const ivec4 baseIndex = DecodeIndex(baseKey, gridPow); + + ivec4 leadIndex = baseIndex; + if (leadIndex.w == 0) + leadIndex.w = 1; + else { + leadIndex += 1; + leadIndex.w = 0; + } + + // This GridVert is in charge of the 6 tetrahedra surrounding its edge in + // the (1,1,1) direction (edge 0). + ivec4 tet(base.NeighborInside(0), base.Inside(), -2, -2); + ivec4 thisIndex = baseIndex; + thisIndex.x += 1; + + GridVert thisVert = gridVerts[EncodeIndex(thisIndex, gridPow)]; + + tet[2] = base.NeighborInside(1); + for (const int i : {0, 1, 2}) { + thisIndex = leadIndex; + --thisIndex[Prev3(i)]; + // Indices take unsigned input, so check for negatives, given the + // decrement. If negative, the vert is outside and only connected to other + // outside verts - no edgeVerts. + GridVert nextVert = thisIndex[Prev3(i)] < 0 + ? GridVert() + : gridVerts[EncodeIndex(thisIndex, gridPow)]; + tet[3] = base.NeighborInside(Prev3(i) + 4); + + const int edges1[6] = {base.edgeVerts[0], + base.edgeVerts[i + 1], + nextVert.edgeVerts[Next3(i) + 4], + nextVert.edgeVerts[Prev3(i) + 1], + thisVert.edgeVerts[i + 4], + base.edgeVerts[Prev3(i) + 4]}; + thisVert = nextVert; + CreateTris(tet, edges1); + + thisIndex = baseIndex; + ++thisIndex[Next3(i)]; + nextVert = gridVerts[EncodeIndex(thisIndex, gridPow)]; + tet[2] = tet[3]; + tet[3] = base.NeighborInside(Next3(i) + 1); + + const int edges2[6] = {base.edgeVerts[0], + edges1[5], + thisVert.edgeVerts[i + 4], + nextVert.edgeVerts[Next3(i) + 4], + edges1[3], + base.edgeVerts[Next3(i) + 1]}; + thisVert = nextVert; + CreateTris(tet, edges2); + + tet[2] = tet[3]; + } + } +}; +} // namespace + +namespace manifold { + +/** + * Constructs a level-set manifold from the input Signed-Distance Function + * (SDF). This uses a form of Marching Tetrahedra (akin to Marching + * Cubes, but better for manifoldness). Instead of using a cubic grid, it uses a + * body-centered cubic grid (two shifted cubic grids). These grid points are + * snapped to the surface where possible to keep short edges from forming. + * + * @param sdf The signed-distance functor, containing this function signature: + * `double operator()(vec3 point)`, which returns the + * signed distance of a given point in R^3. Positive values are inside, + * negative outside. There is no requirement that the function be a true + * distance, or even continuous. + * @param bounds An axis-aligned box that defines the extent of the grid. + * @param edgeLength Approximate maximum edge length of the triangles in the + * final result. This affects grid spacing, and hence has a strong effect on + * performance. + * @param level Extract the surface at this value of your sdf; defaults to + * zero. You can inset your mesh by using a positive value, or outset it with a + * negative value. + * @param tolerance Ensure each vertex is within this distance of the true + * surface. Defaults to -1, which will return the interpolated + * crossing-point based on the two nearest grid points. Small positive values + * will require more sdf evaluations per output vertex. + * @param canParallel Parallel policies violate will crash language runtimes + * with runtime locks that expect to not be called back by unregistered threads. + * This allows bindings use LevelSet despite being compiled with MANIFOLD_PAR + * active. + */ +Manifold Manifold::LevelSet(std::function sdf, Box bounds, + double edgeLength, double level, double tolerance, + bool canParallel) { + if (tolerance <= 0) { + tolerance = std::numeric_limits::infinity(); + } + + auto pImpl_ = std::make_shared(); + auto& vertPos = pImpl_->vertPos_; + + const vec3 dim = bounds.Size(); + const ivec3 gridSize(dim / edgeLength + 1.0); + const vec3 spacing = dim / (vec3(gridSize - 1)); + + const ivec3 gridPow(la::log2(gridSize + 2) + 1); + const Uint64 maxIndex = EncodeIndex(ivec4(gridSize + 2, 1), gridPow); + + // Parallel policies violate will crash language runtimes with runtime locks + // that expect to not be called back by unregistered threads. This allows + // bindings use LevelSet despite being compiled with MANIFOLD_PAR + // active. + const auto pol = canParallel ? autoPolicy(maxIndex) : ExecutionPolicy::Seq; + + const vec3 origin = bounds.min; + Vec voxels(maxIndex); + for_each_n( + pol, countAt(0_uz), maxIndex, + [&voxels, sdf, level, origin, spacing, gridSize, gridPow](Uint64 idx) { + voxels[idx] = BoundedSDF(DecodeIndex(idx, gridPow) - kVoxelOffset, + origin, spacing, gridSize, level, sdf); + }); + + size_t tableSize = std::min( + 2 * maxIndex, static_cast(10 * la::pow(maxIndex, 0.667))); + HashTable gridVerts(tableSize); + vertPos.resize(gridVerts.Size() * 7); + + while (1) { + Vec index(1, 0); + for_each_n(pol, countAt(0_uz), EncodeIndex(ivec4(gridSize, 1), gridPow), + NearSurface({vertPos, index, gridVerts.D(), voxels, sdf, origin, + gridSize, gridPow, spacing, level, tolerance})); + + if (gridVerts.Full()) { // Resize HashTable + const vec3 lastVert = vertPos[index[0] - 1]; + const Uint64 lastIndex = + EncodeIndex(ivec4(ivec3((lastVert - origin) / spacing), 1), gridPow); + const double ratio = static_cast(maxIndex) / lastIndex; + + if (ratio > 1000) // do not trust the ratio if it is too large + tableSize *= 2; + else + tableSize *= ratio; + gridVerts = HashTable(tableSize); + vertPos = Vec(gridVerts.Size() * 7); + } else { // Success + for_each_n( + pol, countAt(0), gridVerts.Size(), + ComputeVerts({vertPos, index, gridVerts.D(), voxels, sdf, origin, + gridSize, gridPow, spacing, level, tolerance})); + vertPos.resize(index[0]); + break; + } + } + + Vec triVerts(gridVerts.Entries() * 12); // worst case + + Vec index(1, 0); + for_each_n(pol, countAt(0), gridVerts.Size(), + BuildTris({triVerts, index, gridVerts.D(), gridPow})); + triVerts.resize(index[0]); + + pImpl_->CreateHalfedges(triVerts); + pImpl_->CleanupTopology(); + pImpl_->Finish(); + pImpl_->InitializeOriginal(); + return Manifold(pImpl_); +} +} // namespace manifold diff --git a/thirdparty/manifold/src/shared.h b/thirdparty/manifold/src/shared.h new file mode 100644 index 000000000000..3f3336141d29 --- /dev/null +++ b/thirdparty/manifold/src/shared.h @@ -0,0 +1,219 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "./parallel.h" +#include "./sparse.h" +#include "./utils.h" +#include "./vec.h" + +namespace manifold { + +inline vec3 SafeNormalize(vec3 v) { + v = la::normalize(v); + return std::isfinite(v.x) ? v : vec3(0.0); +} + +inline double MaxEpsilon(double minEpsilon, const Box& bBox) { + double epsilon = std::max(minEpsilon, kPrecision * bBox.Scale()); + return std::isfinite(epsilon) ? epsilon : -1; +} + +inline int NextHalfedge(int current) { + ++current; + if (current % 3 == 0) current -= 3; + return current; +} + +inline mat3 NormalTransform(const mat3x4& transform) { + return la::inverse(la::transpose(mat3(transform))); +} + +/** + * By using the closest axis-aligned projection to the normal instead of a + * projection along the normal, we avoid introducing any rounding error. + */ +inline mat2x3 GetAxisAlignedProjection(vec3 normal) { + vec3 absNormal = la::abs(normal); + double xyzMax; + mat3x2 projection; + if (absNormal.z > absNormal.x && absNormal.z > absNormal.y) { + projection = mat3x2({1.0, 0.0, 0.0}, // + {0.0, 1.0, 0.0}); + xyzMax = normal.z; + } else if (absNormal.y > absNormal.x) { + projection = mat3x2({0.0, 0.0, 1.0}, // + {1.0, 0.0, 0.0}); + xyzMax = normal.y; + } else { + projection = mat3x2({0.0, 1.0, 0.0}, // + {0.0, 0.0, 1.0}); + xyzMax = normal.x; + } + if (xyzMax < 0) projection[0] *= -1.0; + return la::transpose(projection); +} + +inline vec3 GetBarycentric(const vec3& v, const mat3& triPos, + double tolerance) { + const mat3 edges(triPos[2] - triPos[1], triPos[0] - triPos[2], + triPos[1] - triPos[0]); + const vec3 d2(la::dot(edges[0], edges[0]), la::dot(edges[1], edges[1]), + la::dot(edges[2], edges[2])); + const int longSide = d2[0] > d2[1] && d2[0] > d2[2] ? 0 + : d2[1] > d2[2] ? 1 + : 2; + const vec3 crossP = la::cross(edges[0], edges[1]); + const double area2 = la::dot(crossP, crossP); + const double tol2 = tolerance * tolerance; + + vec3 uvw(0.0); + for (const int i : {0, 1, 2}) { + const vec3 dv = v - triPos[i]; + if (la::dot(dv, dv) < tol2) { + // Return exactly equal if within tolerance of vert. + uvw[i] = 1; + return uvw; + } + } + + if (d2[longSide] < tol2) { // point + return vec3(1, 0, 0); + } else if (area2 > d2[longSide] * tol2) { // triangle + for (const int i : {0, 1, 2}) { + const int j = Next3(i); + const vec3 crossPv = la::cross(edges[i], v - triPos[j]); + const double area2v = la::dot(crossPv, crossPv); + // Return exactly equal if within tolerance of edge. + uvw[i] = area2v < d2[i] * tol2 ? 0 : la::dot(crossPv, crossP); + } + uvw /= (uvw[0] + uvw[1] + uvw[2]); + return uvw; + } else { // line + const int nextV = Next3(longSide); + const double alpha = + la::dot(v - triPos[nextV], edges[longSide]) / d2[longSide]; + uvw[longSide] = 0; + uvw[nextV] = 1 - alpha; + const int lastV = Next3(nextV); + uvw[lastV] = alpha; + return uvw; + } +} + +/** + * The fundamental component of the halfedge data structure used for storing and + * operating on the Manifold. + */ +struct Halfedge { + int startVert, endVert; + int pairedHalfedge; + bool IsForward() const { return startVert < endVert; } + bool operator<(const Halfedge& other) const { + return startVert == other.startVert ? endVert < other.endVert + : startVert < other.startVert; + } +}; + +struct Barycentric { + int tri; + vec4 uvw; +}; + +struct TriRef { + /// The unique ID of the mesh instance of this triangle. If .meshID and .tri + /// match for two triangles, then they are coplanar and came from the same + /// face. + int meshID; + /// The OriginalID of the mesh this triangle came from. This ID is ideal for + /// reapplying properties like UV coordinates to the output mesh. + int originalID; + /// Probably the triangle index of the original triangle this was part of: + /// Mesh.triVerts[tri], but it's an input, so just pass it along unchanged. + int tri; + /// Triangles with the same face ID are coplanar. + int faceID; + + bool SameFace(const TriRef& other) const { + return meshID == other.meshID && faceID == other.faceID; + } +}; + +/** + * This is a temporary edge structure which only stores edges forward and + * references the halfedge it was created from. + */ +struct TmpEdge { + int first, second, halfedgeIdx; + + TmpEdge() {} + TmpEdge(int start, int end, int idx) { + first = std::min(start, end); + second = std::max(start, end); + halfedgeIdx = idx; + } + + bool operator<(const TmpEdge& other) const { + return first == other.first ? second < other.second : first < other.first; + } +}; + +Vec inline CreateTmpEdges(const Vec& halfedge) { + Vec edges(halfedge.size()); + for_each_n(autoPolicy(edges.size()), countAt(0), edges.size(), + [&edges, &halfedge](const int idx) { + const Halfedge& half = halfedge[idx]; + edges[idx] = TmpEdge(half.startVert, half.endVert, + half.IsForward() ? idx : -1); + }); + + size_t numEdge = + remove_if(edges.begin(), edges.end(), + [](const TmpEdge& edge) { return edge.halfedgeIdx < 0; }) - + edges.begin(); + DEBUG_ASSERT(numEdge == halfedge.size() / 2, topologyErr, "Not oriented!"); + edges.resize(numEdge); + return edges; +} + +template +struct ReindexEdge { + VecView edges; + SparseIndices& indices; + + void operator()(size_t i) { + int& edge = indices.Get(i, inverted); + edge = edges[edge].halfedgeIdx; + } +}; + +#ifdef MANIFOLD_DEBUG +inline std::ostream& operator<<(std::ostream& stream, const Halfedge& edge) { + return stream << "startVert = " << edge.startVert + << ", endVert = " << edge.endVert + << ", pairedHalfedge = " << edge.pairedHalfedge; +} + +inline std::ostream& operator<<(std::ostream& stream, const Barycentric& bary) { + return stream << "tri = " << bary.tri << ", uvw = " << bary.uvw; +} + +inline std::ostream& operator<<(std::ostream& stream, const TriRef& ref) { + return stream << "meshID: " << ref.meshID + << ", originalID: " << ref.originalID << ", tri: " << ref.tri + << ", faceID: " << ref.faceID; +} +#endif +} // namespace manifold diff --git a/thirdparty/manifold/src/smoothing.cpp b/thirdparty/manifold/src/smoothing.cpp new file mode 100644 index 000000000000..57d81b4623fb --- /dev/null +++ b/thirdparty/manifold/src/smoothing.cpp @@ -0,0 +1,1003 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./impl.h" +#include "./parallel.h" + +namespace { +using namespace manifold; + +// Returns a normalized vector orthogonal to ref, in the plane of ref and in, +// unless in and ref are colinear, in which case it falls back to the plane of +// ref and altIn. +vec3 OrthogonalTo(vec3 in, vec3 altIn, vec3 ref) { + vec3 out = in - la::dot(in, ref) * ref; + if (la::dot(out, out) < kPrecision * la::dot(in, in)) { + out = altIn - la::dot(altIn, ref) * ref; + } + return SafeNormalize(out); +} + +double Wrap(double radians) { + return radians < -kPi ? radians + kTwoPi + : radians > kPi ? radians - kTwoPi + : radians; +} + +// Get the angle between two unit-vectors. +double AngleBetween(vec3 a, vec3 b) { + const double dot = la::dot(a, b); + return dot >= 1 ? 0 : (dot <= -1 ? kPi : la::acos(dot)); +} + +// Calculate a tangent vector in the form of a weighted cubic Bezier taking as +// input the desired tangent direction (length doesn't matter) and the edge +// vector to the neighboring vertex. In a symmetric situation where the tangents +// at each end are mirror images of each other, this will result in a circular +// arc. +vec4 CircularTangent(const vec3& tangent, const vec3& edgeVec) { + const vec3 dir = SafeNormalize(tangent); + + double weight = std::max(0.5, la::dot(dir, SafeNormalize(edgeVec))); + // Quadratic weighted bezier for circular interpolation + const vec4 bz2 = vec4(dir * 0.5 * la::length(edgeVec), weight); + // Equivalent cubic weighted bezier + const vec4 bz3 = la::lerp(vec4(0, 0, 0, 1), bz2, 2 / 3.0); + // Convert from homogeneous form to geometric form + return vec4(vec3(bz3) / bz3.w, bz3.w); +} + +struct InterpTri { + VecView vertPos; + VecView vertBary; + const Manifold::Impl* impl; + + static vec4 Homogeneous(vec4 v) { + v.x *= v.w; + v.y *= v.w; + v.z *= v.w; + return v; + } + + static vec4 Homogeneous(vec3 v) { return vec4(v, 1.0); } + + static vec3 HNormalize(vec4 v) { + return v.w == 0 ? vec3(v) : (vec3(v) / v.w); + } + + static vec4 Scale(vec4 v, double scale) { return vec4(scale * vec3(v), v.w); } + + static vec4 Bezier(vec3 point, vec4 tangent) { + return Homogeneous(vec4(point, 0) + tangent); + } + + static mat4x2 CubicBezier2Linear(vec4 p0, vec4 p1, vec4 p2, vec4 p3, + double x) { + mat4x2 out; + vec4 p12 = la::lerp(p1, p2, x); + out[0] = la::lerp(la::lerp(p0, p1, x), p12, x); + out[1] = la::lerp(p12, la::lerp(p2, p3, x), x); + return out; + } + + static vec3 BezierPoint(mat4x2 points, double x) { + return HNormalize(la::lerp(points[0], points[1], x)); + } + + static vec3 BezierTangent(mat4x2 points) { + return SafeNormalize(HNormalize(points[1]) - HNormalize(points[0])); + } + + static vec3 RotateFromTo(vec3 v, quat start, quat end) { + return la::qrot(end, la::qrot(la::qconj(start), v)); + } + + static quat Slerp(const quat& x, const quat& y, double a, bool longWay) { + quat z = y; + double cosTheta = la::dot(x, y); + + // Take the long way around the sphere only when requested + if ((cosTheta < 0) != longWay) { + z = -y; + cosTheta = -cosTheta; + } + + if (cosTheta > 1.0 - std::numeric_limits::epsilon()) { + return la::lerp(x, z, a); // for numerical stability + } else { + double angle = std::acos(cosTheta); + return (std::sin((1.0 - a) * angle) * x + std::sin(a * angle) * z) / + std::sin(angle); + } + } + + static mat4x2 Bezier2Bezier(const mat3x2& corners, const mat4x2& tangentsX, + const mat4x2& tangentsY, double x, + const vec3& anchor) { + const mat4x2 bez = CubicBezier2Linear( + Homogeneous(corners[0]), Bezier(corners[0], tangentsX[0]), + Bezier(corners[1], tangentsX[1]), Homogeneous(corners[1]), x); + const vec3 end = BezierPoint(bez, x); + const vec3 tangent = BezierTangent(bez); + + const mat3x2 nTangentsX(SafeNormalize(vec3(tangentsX[0])), + -SafeNormalize(vec3(tangentsX[1]))); + const mat3x2 biTangents = { + OrthogonalTo(vec3(tangentsY[0]), (anchor - corners[0]), nTangentsX[0]), + OrthogonalTo(vec3(tangentsY[1]), (anchor - corners[1]), nTangentsX[1])}; + + const quat q0 = la::rotation_quat(mat3( + nTangentsX[0], biTangents[0], la::cross(nTangentsX[0], biTangents[0]))); + const quat q1 = la::rotation_quat(mat3( + nTangentsX[1], biTangents[1], la::cross(nTangentsX[1], biTangents[1]))); + const vec3 edge = corners[1] - corners[0]; + const bool longWay = + la::dot(nTangentsX[0], edge) + la::dot(nTangentsX[1], edge) < 0; + const quat qTmp = Slerp(q0, q1, x, longWay); + const quat q = la::qmul(la::rotation_quat(la::qxdir(qTmp), tangent), qTmp); + + const vec3 delta = la::lerp(RotateFromTo(vec3(tangentsY[0]), q0, q), + RotateFromTo(vec3(tangentsY[1]), q1, q), x); + const double deltaW = la::lerp(tangentsY[0].w, tangentsY[1].w, x); + + return {Homogeneous(end), vec4(delta, deltaW)}; + } + + static vec3 Bezier2D(const mat3x4& corners, const mat4& tangentsX, + const mat4& tangentsY, double x, double y, + const vec3& centroid) { + mat4x2 bez0 = + Bezier2Bezier({corners[0], corners[1]}, {tangentsX[0], tangentsX[1]}, + {tangentsY[0], tangentsY[1]}, x, centroid); + mat4x2 bez1 = + Bezier2Bezier({corners[2], corners[3]}, {tangentsX[2], tangentsX[3]}, + {tangentsY[2], tangentsY[3]}, 1 - x, centroid); + + const mat4x2 bez = + CubicBezier2Linear(bez0[0], Bezier(vec3(bez0[0]), bez0[1]), + Bezier(vec3(bez1[0]), bez1[1]), bez1[0], y); + return BezierPoint(bez, y); + } + + void operator()(const int vert) { + vec3& pos = vertPos[vert]; + const int tri = vertBary[vert].tri; + const vec4 uvw = vertBary[vert].uvw; + + const ivec4 halfedges = impl->GetHalfedges(tri); + const mat3x4 corners = { + impl->vertPos_[impl->halfedge_[halfedges[0]].startVert], + impl->vertPos_[impl->halfedge_[halfedges[1]].startVert], + impl->vertPos_[impl->halfedge_[halfedges[2]].startVert], + halfedges[3] < 0 + ? vec3(0.0) + : impl->vertPos_[impl->halfedge_[halfedges[3]].startVert]}; + + for (const int i : {0, 1, 2, 3}) { + if (uvw[i] == 1) { + pos = corners[i]; + return; + } + } + + vec4 posH(0.0); + + if (halfedges[3] < 0) { // tri + const mat4x3 tangentR = {impl->halfedgeTangent_[halfedges[0]], + impl->halfedgeTangent_[halfedges[1]], + impl->halfedgeTangent_[halfedges[2]]}; + const mat4x3 tangentL = { + impl->halfedgeTangent_[impl->halfedge_[halfedges[2]].pairedHalfedge], + impl->halfedgeTangent_[impl->halfedge_[halfedges[0]].pairedHalfedge], + impl->halfedgeTangent_[impl->halfedge_[halfedges[1]].pairedHalfedge]}; + const vec3 centroid = mat3(corners) * vec3(1.0 / 3); + + for (const int i : {0, 1, 2}) { + const int j = Next3(i); + const int k = Prev3(i); + const double x = uvw[k] / (1 - uvw[i]); + + const mat4x2 bez = + Bezier2Bezier({corners[j], corners[k]}, {tangentR[j], tangentL[k]}, + {tangentL[j], tangentR[k]}, x, centroid); + + const mat4x2 bez1 = CubicBezier2Linear( + bez[0], Bezier(vec3(bez[0]), bez[1]), + Bezier(corners[i], la::lerp(tangentR[i], tangentL[i], x)), + Homogeneous(corners[i]), uvw[i]); + const vec3 p = BezierPoint(bez1, uvw[i]); + posH += Homogeneous(vec4(p, uvw[j] * uvw[k])); + } + } else { // quad + const mat4 tangentsX = { + impl->halfedgeTangent_[halfedges[0]], + impl->halfedgeTangent_[impl->halfedge_[halfedges[0]].pairedHalfedge], + impl->halfedgeTangent_[halfedges[2]], + impl->halfedgeTangent_[impl->halfedge_[halfedges[2]].pairedHalfedge]}; + const mat4 tangentsY = { + impl->halfedgeTangent_[impl->halfedge_[halfedges[3]].pairedHalfedge], + impl->halfedgeTangent_[halfedges[1]], + impl->halfedgeTangent_[impl->halfedge_[halfedges[1]].pairedHalfedge], + impl->halfedgeTangent_[halfedges[3]]}; + const vec3 centroid = corners * vec4(0.25); + const double x = uvw[1] + uvw[2]; + const double y = uvw[2] + uvw[3]; + const vec3 pX = Bezier2D(corners, tangentsX, tangentsY, x, y, centroid); + const vec3 pY = + Bezier2D({corners[1], corners[2], corners[3], corners[0]}, + {tangentsY[1], tangentsY[2], tangentsY[3], tangentsY[0]}, + {tangentsX[1], tangentsX[2], tangentsX[3], tangentsX[0]}, y, + 1 - x, centroid); + posH += Homogeneous(vec4(pX, x * (1 - x))); + posH += Homogeneous(vec4(pY, y * (1 - y))); + } + pos = HNormalize(posH); + } +}; +} // namespace + +namespace manifold { + +/** + * Get the property normal associated with the startVert of this halfedge, where + * normalIdx shows the beginning of where normals are stored in the properties. + */ +vec3 Manifold::Impl::GetNormal(int halfedge, int normalIdx) const { + const int tri = halfedge / 3; + const int j = halfedge % 3; + const int prop = meshRelation_.triProperties[tri][j]; + vec3 normal; + for (const int i : {0, 1, 2}) { + normal[i] = + meshRelation_.properties[prop * meshRelation_.numProp + normalIdx + i]; + } + return normal; +} + +/** + * Returns a circular tangent for the requested halfedge, orthogonal to the + * given normal vector, and avoiding folding. + */ +vec4 Manifold::Impl::TangentFromNormal(const vec3& normal, int halfedge) const { + const Halfedge edge = halfedge_[halfedge]; + const vec3 edgeVec = vertPos_[edge.endVert] - vertPos_[edge.startVert]; + const vec3 edgeNormal = + faceNormal_[halfedge / 3] + faceNormal_[edge.pairedHalfedge / 3]; + vec3 dir = la::cross(la::cross(edgeNormal, edgeVec), normal); + return CircularTangent(dir, edgeVec); +} + +/** + * Returns true if this halfedge should be marked as the interior of a quad, as + * defined by its two triangles referring to the same face, and those triangles + * having no further face neighbors beyond. + */ +bool Manifold::Impl::IsInsideQuad(int halfedge) const { + if (halfedgeTangent_.size() > 0) { + return halfedgeTangent_[halfedge].w < 0; + } + const int tri = halfedge / 3; + const TriRef ref = meshRelation_.triRef[tri]; + const int pair = halfedge_[halfedge].pairedHalfedge; + const int pairTri = pair / 3; + const TriRef pairRef = meshRelation_.triRef[pairTri]; + if (!ref.SameFace(pairRef)) return false; + + auto SameFace = [this](int halfedge, const TriRef& ref) { + return ref.SameFace( + meshRelation_.triRef[halfedge_[halfedge].pairedHalfedge / 3]); + }; + + int neighbor = NextHalfedge(halfedge); + if (SameFace(neighbor, ref)) return false; + neighbor = NextHalfedge(neighbor); + if (SameFace(neighbor, ref)) return false; + neighbor = NextHalfedge(pair); + if (SameFace(neighbor, pairRef)) return false; + neighbor = NextHalfedge(neighbor); + if (SameFace(neighbor, pairRef)) return false; + return true; +} + +/** + * Returns true if this halfedge is an interior of a quad, as defined by its + * halfedge tangent having negative weight. + */ +bool Manifold::Impl::IsMarkedInsideQuad(int halfedge) const { + return halfedgeTangent_.size() > 0 && halfedgeTangent_[halfedge].w < 0; +} + +// sharpenedEdges are referenced to the input Mesh, but the triangles have +// been sorted in creating the Manifold, so the indices are converted using +// meshRelation_. +std::vector Manifold::Impl::UpdateSharpenedEdges( + const std::vector& sharpenedEdges) const { + std::unordered_map oldHalfedge2New; + for (size_t tri = 0; tri < NumTri(); ++tri) { + int oldTri = meshRelation_.triRef[tri].tri; + for (int i : {0, 1, 2}) oldHalfedge2New[3 * oldTri + i] = 3 * tri + i; + } + std::vector newSharp = sharpenedEdges; + for (Smoothness& edge : newSharp) { + edge.halfedge = oldHalfedge2New[edge.halfedge]; + } + return newSharp; +} + +// Find faces containing at least 3 triangles - these will not have +// interpolated normals - all their vert normals must match their face normal. +Vec Manifold::Impl::FlatFaces() const { + const int numTri = NumTri(); + Vec triIsFlatFace(numTri, false); + for_each_n(autoPolicy(numTri, 1e5), countAt(0), numTri, + [this, &triIsFlatFace](const int tri) { + const TriRef& ref = meshRelation_.triRef[tri]; + int faceNeighbors = 0; + ivec3 faceTris = {-1, -1, -1}; + for (const int j : {0, 1, 2}) { + const int neighborTri = + halfedge_[3 * tri + j].pairedHalfedge / 3; + const TriRef& jRef = meshRelation_.triRef[neighborTri]; + if (jRef.SameFace(ref)) { + ++faceNeighbors; + faceTris[j] = neighborTri; + } + } + if (faceNeighbors > 1) { + triIsFlatFace[tri] = true; + for (const int j : {0, 1, 2}) { + if (faceTris[j] >= 0) { + triIsFlatFace[faceTris[j]] = true; + } + } + } + }); + return triIsFlatFace; +} + +// Returns a vector of length numVert that has a tri that is part of a +// neighboring flat face if there is only one flat face. If there are none it +// gets -1, and if there are more than one it gets -2. +Vec Manifold::Impl::VertFlatFace(const Vec& flatFaces) const { + Vec vertFlatFace(NumVert(), -1); + Vec vertRef(NumVert(), {-1, -1, -1}); + for (size_t tri = 0; tri < NumTri(); ++tri) { + if (flatFaces[tri]) { + for (const int j : {0, 1, 2}) { + const int vert = halfedge_[3 * tri + j].startVert; + if (vertRef[vert].SameFace(meshRelation_.triRef[tri])) continue; + vertRef[vert] = meshRelation_.triRef[tri]; + vertFlatFace[vert] = vertFlatFace[vert] == -1 ? tri : -2; + } + } + } + return vertFlatFace; +} + +Vec Manifold::Impl::VertHalfedge() const { + Vec vertHalfedge(NumVert()); + Vec counters(NumVert(), 0); + for_each_n(autoPolicy(halfedge_.size(), 1e5), countAt(0), halfedge_.size(), + [&vertHalfedge, &counters, this](const int idx) { + auto old = std::atomic_exchange( + reinterpret_cast*>( + &counters[halfedge_[idx].startVert]), + static_cast(1)); + if (old == 1) return; + // arbitrary, last one wins. + vertHalfedge[halfedge_[idx].startVert] = idx; + }); + return vertHalfedge; +} + +std::vector Manifold::Impl::SharpenEdges( + double minSharpAngle, double minSmoothness) const { + std::vector sharpenedEdges; + const double minRadians = radians(minSharpAngle); + for (size_t e = 0; e < halfedge_.size(); ++e) { + if (!halfedge_[e].IsForward()) continue; + const size_t pair = halfedge_[e].pairedHalfedge; + const double dihedral = + std::acos(la::dot(faceNormal_[e / 3], faceNormal_[pair / 3])); + if (dihedral > minRadians) { + sharpenedEdges.push_back({e, minSmoothness}); + sharpenedEdges.push_back({pair, minSmoothness}); + } + } + return sharpenedEdges; +} + +/** + * Sharpen tangents that intersect an edge to sharpen that edge. The weight is + * unchanged, as this has a squared effect on radius of curvature, except + * in the case of zero radius, which is marked with weight = 0. + */ +void Manifold::Impl::SharpenTangent(int halfedge, double smoothness) { + halfedgeTangent_[halfedge] = + vec4(smoothness * vec3(halfedgeTangent_[halfedge]), + smoothness == 0 ? 0 : halfedgeTangent_[halfedge].w); +} + +/** + * Instead of calculating the internal shared normals like CalculateNormals + * does, this method fills in vertex properties, unshared across edges that + * are bent more than minSharpAngle. + */ +void Manifold::Impl::SetNormals(int normalIdx, double minSharpAngle) { + if (IsEmpty()) return; + if (normalIdx < 0) return; + + const int oldNumProp = NumProp(); + const int numTri = NumTri(); + + Vec triIsFlatFace = FlatFaces(); + Vec vertFlatFace = VertFlatFace(triIsFlatFace); + Vec vertNumSharp(NumVert(), 0); + for (size_t e = 0; e < halfedge_.size(); ++e) { + if (!halfedge_[e].IsForward()) continue; + const int pair = halfedge_[e].pairedHalfedge; + const int tri1 = e / 3; + const int tri2 = pair / 3; + const double dihedral = + degrees(std::acos(la::dot(faceNormal_[tri1], faceNormal_[tri2]))); + if (dihedral > minSharpAngle) { + ++vertNumSharp[halfedge_[e].startVert]; + ++vertNumSharp[halfedge_[e].endVert]; + } else { + const bool faceSplit = + triIsFlatFace[tri1] != triIsFlatFace[tri2] || + (triIsFlatFace[tri1] && triIsFlatFace[tri2] && + !meshRelation_.triRef[tri1].SameFace(meshRelation_.triRef[tri2])); + if (vertFlatFace[halfedge_[e].startVert] == -2 && faceSplit) { + ++vertNumSharp[halfedge_[e].startVert]; + } + if (vertFlatFace[halfedge_[e].endVert] == -2 && faceSplit) { + ++vertNumSharp[halfedge_[e].endVert]; + } + } + } + + const int numProp = std::max(oldNumProp, normalIdx + 3); + Vec oldProperties(numProp * NumPropVert(), 0); + meshRelation_.properties.swap(oldProperties); + meshRelation_.numProp = numProp; + if (meshRelation_.triProperties.size() == 0) { + meshRelation_.triProperties.resize(numTri); + for_each_n(autoPolicy(numTri, 1e5), countAt(0), numTri, [this](int tri) { + for (const int j : {0, 1, 2}) + meshRelation_.triProperties[tri][j] = halfedge_[3 * tri + j].startVert; + }); + } + Vec oldTriProp(numTri, {-1, -1, -1}); + meshRelation_.triProperties.swap(oldTriProp); + + for (int tri = 0; tri < numTri; ++tri) { + for (const int i : {0, 1, 2}) { + if (meshRelation_.triProperties[tri][i] >= 0) continue; + int startEdge = 3 * tri + i; + const int vert = halfedge_[startEdge].startVert; + + if (vertNumSharp[vert] < 2) { // vertex has single normal + const vec3 normal = vertFlatFace[vert] >= 0 + ? faceNormal_[vertFlatFace[vert]] + : vertNormal_[vert]; + int lastProp = -1; + ForVert(startEdge, [&](int current) { + const int thisTri = current / 3; + const int j = current - 3 * thisTri; + const int prop = oldTriProp[thisTri][j]; + meshRelation_.triProperties[thisTri][j] = prop; + if (prop == lastProp) return; + lastProp = prop; + // update property vertex + auto start = oldProperties.begin() + prop * oldNumProp; + std::copy(start, start + oldNumProp, + meshRelation_.properties.begin() + prop * numProp); + for (const int i : {0, 1, 2}) + meshRelation_.properties[prop * numProp + normalIdx + i] = + normal[i]; + }); + } else { // vertex has multiple normals + const vec3 centerPos = vertPos_[vert]; + // Length degree + std::vector group; + // Length number of normals + std::vector normals; + int current = startEdge; + int prevFace = current / 3; + + do { // find a sharp edge to start on + int next = NextHalfedge(halfedge_[current].pairedHalfedge); + const int face = next / 3; + + const double dihedral = degrees( + std::acos(la::dot(faceNormal_[face], faceNormal_[prevFace]))); + if (dihedral > minSharpAngle || + triIsFlatFace[face] != triIsFlatFace[prevFace] || + (triIsFlatFace[face] && triIsFlatFace[prevFace] && + !meshRelation_.triRef[face].SameFace( + meshRelation_.triRef[prevFace]))) { + break; + } + current = next; + prevFace = face; + } while (current != startEdge); + + const int endEdge = current; + + struct FaceEdge { + int face; + vec3 edgeVec; + }; + + // calculate pseudo-normals between each sharp edge + ForVert( + endEdge, + [this, centerPos, &vertNumSharp, &vertFlatFace](int current) { + if (IsInsideQuad(current)) { + return FaceEdge({current / 3, vec3(NAN)}); + } + const int vert = halfedge_[current].endVert; + vec3 pos = vertPos_[vert]; + const vec3 edgeVec = centerPos - pos; + if (vertNumSharp[vert] < 2) { + // opposite vert has fixed normal + const vec3 normal = vertFlatFace[vert] >= 0 + ? faceNormal_[vertFlatFace[vert]] + : vertNormal_[vert]; + // Flair out the normal we're calculating to give the edge a + // more constant curvature to meet the opposite normal. Achieve + // this by pointing the tangent toward the opposite bezier + // control point instead of the vert itself. + pos += vec3(TangentFromNormal( + normal, halfedge_[current].pairedHalfedge)); + } + return FaceEdge({current / 3, SafeNormalize(pos - centerPos)}); + }, + [this, &triIsFlatFace, &normals, &group, minSharpAngle]( + int current, const FaceEdge& here, FaceEdge& next) { + const double dihedral = degrees(std::acos( + la::dot(faceNormal_[here.face], faceNormal_[next.face]))); + if (dihedral > minSharpAngle || + triIsFlatFace[here.face] != triIsFlatFace[next.face] || + (triIsFlatFace[here.face] && triIsFlatFace[next.face] && + !meshRelation_.triRef[here.face].SameFace( + meshRelation_.triRef[next.face]))) { + normals.push_back(vec3(0.0)); + } + group.push_back(normals.size() - 1); + if (std::isfinite(next.edgeVec.x)) { + normals.back() += + SafeNormalize(la::cross(next.edgeVec, here.edgeVec)) * + AngleBetween(here.edgeVec, next.edgeVec); + } else { + next.edgeVec = here.edgeVec; + } + }); + + for (auto& normal : normals) { + normal = SafeNormalize(normal); + } + + int lastGroup = 0; + int lastProp = -1; + int newProp = -1; + int idx = 0; + ForVert(endEdge, [&](int current1) { + const int thisTri = current1 / 3; + const int j = current1 - 3 * thisTri; + const int prop = oldTriProp[thisTri][j]; + auto start = oldProperties.begin() + prop * oldNumProp; + + if (group[idx] != lastGroup && group[idx] != 0 && prop == lastProp) { + // split property vertex, duplicating but with an updated normal + lastGroup = group[idx]; + newProp = NumPropVert(); + meshRelation_.properties.resize(meshRelation_.properties.size() + + numProp); + std::copy(start, start + oldNumProp, + meshRelation_.properties.begin() + newProp * numProp); + for (const int i : {0, 1, 2}) { + meshRelation_.properties[newProp * numProp + normalIdx + i] = + normals[group[idx]][i]; + } + } else if (prop != lastProp) { + // update property vertex + lastProp = prop; + newProp = prop; + std::copy(start, start + oldNumProp, + meshRelation_.properties.begin() + prop * numProp); + for (const int i : {0, 1, 2}) + meshRelation_.properties[prop * numProp + normalIdx + i] = + normals[group[idx]][i]; + } + + // point to updated property vertex + meshRelation_.triProperties[thisTri][j] = newProp; + ++idx; + }); + } + } + } +} + +/** + * Tangents get flattened to create sharp edges by setting their weight to zero. + * This is the natural limit of reducing the weight to increase the sharpness + * smoothly. This limit gives a decent shape, but it causes the parameterization + * to be stretched and compresses it near the edges, which is good for resolving + * tight curvature, but bad for property interpolation. This function fixes the + * parameter stretch at the limit for sharp edges, since there is no curvature + * to resolve. Note this also changes the overall shape - making it more evenly + * curved. + */ +void Manifold::Impl::LinearizeFlatTangents() { + const int n = halfedgeTangent_.size(); + for_each_n(autoPolicy(n, 1e4), countAt(0), n, [this](const int halfedge) { + vec4& tangent = halfedgeTangent_[halfedge]; + vec4& otherTangent = halfedgeTangent_[halfedge_[halfedge].pairedHalfedge]; + + const bool flat[2] = {tangent.w == 0, otherTangent.w == 0}; + if (!halfedge_[halfedge].IsForward() || (!flat[0] && !flat[1])) { + return; + } + + const vec3 edgeVec = vertPos_[halfedge_[halfedge].endVert] - + vertPos_[halfedge_[halfedge].startVert]; + + if (flat[0] && flat[1]) { + tangent = vec4(edgeVec / 3.0, 1); + otherTangent = vec4(-edgeVec / 3.0, 1); + } else if (flat[0]) { + tangent = vec4((edgeVec + vec3(otherTangent)) / 2.0, 1); + } else { + otherTangent = vec4((-edgeVec + vec3(tangent)) / 2.0, 1); + } + }); +} + +/** + * Redistribute the tangents around each vertex so that the angles between them + * have the same ratios as the angles of the triangles between the corresponding + * edges. This avoids folding the output shape and gives smoother results. There + * must be at least one fixed halfedge on a vertex for that vertex to be + * operated on. If there is only one, then that halfedge is not treated as + * fixed, but the whole circle is turned to an average orientation. + */ +void Manifold::Impl::DistributeTangents(const Vec& fixedHalfedges) { + const int numHalfedge = fixedHalfedges.size(); + for_each_n( + autoPolicy(numHalfedge, 1e4), countAt(0), numHalfedge, + [this, &fixedHalfedges](int halfedge) { + if (!fixedHalfedges[halfedge]) return; + + if (IsMarkedInsideQuad(halfedge)) { + halfedge = NextHalfedge(halfedge_[halfedge].pairedHalfedge); + } + + vec3 normal(0.0); + Vec currentAngle; + Vec desiredAngle; + + const vec3 approxNormal = vertNormal_[halfedge_[halfedge].startVert]; + const vec3 center = vertPos_[halfedge_[halfedge].startVert]; + vec3 lastEdgeVec = + SafeNormalize(vertPos_[halfedge_[halfedge].endVert] - center); + const vec3 firstTangent = + SafeNormalize(vec3(halfedgeTangent_[halfedge])); + vec3 lastTangent = firstTangent; + int current = halfedge; + do { + current = NextHalfedge(halfedge_[current].pairedHalfedge); + if (IsMarkedInsideQuad(current)) continue; + const vec3 thisEdgeVec = + SafeNormalize(vertPos_[halfedge_[current].endVert] - center); + const vec3 thisTangent = + SafeNormalize(vec3(halfedgeTangent_[current])); + normal += la::cross(thisTangent, lastTangent); + // cumulative sum + desiredAngle.push_back( + AngleBetween(thisEdgeVec, lastEdgeVec) + + (desiredAngle.size() > 0 ? desiredAngle.back() : 0)); + if (current == halfedge) { + currentAngle.push_back(kTwoPi); + } else { + currentAngle.push_back(AngleBetween(thisTangent, firstTangent)); + if (la::dot(approxNormal, la::cross(thisTangent, firstTangent)) < + 0) { + currentAngle.back() = kTwoPi - currentAngle.back(); + } + } + lastEdgeVec = thisEdgeVec; + lastTangent = thisTangent; + } while (!fixedHalfedges[current]); + + if (currentAngle.size() == 1 || la::dot(normal, normal) == 0) return; + + const double scale = currentAngle.back() / desiredAngle.back(); + double offset = 0; + if (current == halfedge) { // only one - find average offset + for (size_t i = 0; i < currentAngle.size(); ++i) { + offset += Wrap(currentAngle[i] - scale * desiredAngle[i]); + } + offset /= currentAngle.size(); + } + + current = halfedge; + size_t i = 0; + do { + current = NextHalfedge(halfedge_[current].pairedHalfedge); + if (IsMarkedInsideQuad(current)) continue; + desiredAngle[i] *= scale; + const double lastAngle = i > 0 ? desiredAngle[i - 1] : 0; + // shrink obtuse angles + if (desiredAngle[i] - lastAngle > kPi) { + desiredAngle[i] = lastAngle + kPi; + } else if (i + 1 < desiredAngle.size() && + scale * desiredAngle[i + 1] - desiredAngle[i] > kPi) { + desiredAngle[i] = scale * desiredAngle[i + 1] - kPi; + } + const double angle = currentAngle[i] - desiredAngle[i] - offset; + vec3 tangent(halfedgeTangent_[current]); + const quat q = la::rotation_quat(la::normalize(normal), angle); + halfedgeTangent_[current] = + vec4(la::qrot(q, tangent), halfedgeTangent_[current].w); + ++i; + } while (!fixedHalfedges[current]); + }); +} + +/** + * Calculates halfedgeTangent_, allowing the manifold to be refined and + * smoothed. The tangents form weighted cubic Beziers along each edge. This + * function creates circular arcs where possible (minimizing maximum curvature), + * constrained to the indicated property normals. Across edges that form + * discontinuities in the normals, the tangent vectors are zero-length, allowing + * the shape to form a sharp corner with minimal oscillation. + */ +void Manifold::Impl::CreateTangents(int normalIdx) { + ZoneScoped; + const int numVert = NumVert(); + const int numHalfedge = halfedge_.size(); + halfedgeTangent_.resize(0); + Vec tangent(numHalfedge); + Vec fixedHalfedge(numHalfedge, false); + + Vec vertHalfedge = VertHalfedge(); + for_each_n( + autoPolicy(numVert, 1e4), vertHalfedge.begin(), numVert, + [this, &tangent, &fixedHalfedge, normalIdx](int e) { + struct FlatNormal { + bool isFlatFace; + vec3 normal; + }; + + ivec2 faceEdges(-1, -1); + + ForVert( + e, + [normalIdx, this](int halfedge) { + const vec3 normal = GetNormal(halfedge, normalIdx); + const vec3 diff = faceNormal_[halfedge / 3] - normal; + return FlatNormal( + {la::dot(diff, diff) < kPrecision * kPrecision, normal}); + }, + [&faceEdges, &tangent, &fixedHalfedge, this]( + int halfedge, const FlatNormal& here, const FlatNormal& next) { + if (IsInsideQuad(halfedge)) { + tangent[halfedge] = {0, 0, 0, -1}; + return; + } + // mark special edges + const vec3 diff = next.normal - here.normal; + const bool differentNormals = + la::dot(diff, diff) > kPrecision * kPrecision; + if (differentNormals || here.isFlatFace != next.isFlatFace) { + fixedHalfedge[halfedge] = true; + if (faceEdges[0] == -1) { + faceEdges[0] = halfedge; + } else if (faceEdges[1] == -1) { + faceEdges[1] = halfedge; + } else { + faceEdges[0] = -2; + } + } + // calculate tangents + if (differentNormals) { + const vec3 edgeVec = vertPos_[halfedge_[halfedge].endVert] - + vertPos_[halfedge_[halfedge].startVert]; + const vec3 dir = la::cross(here.normal, next.normal); + tangent[halfedge] = CircularTangent( + (la::dot(dir, edgeVec) < 0 ? -1.0 : 1.0) * dir, edgeVec); + } else { + tangent[halfedge] = TangentFromNormal(here.normal, halfedge); + } + }); + + if (faceEdges[0] >= 0 && faceEdges[1] >= 0) { + const vec3 edge0 = vertPos_[halfedge_[faceEdges[0]].endVert] - + vertPos_[halfedge_[faceEdges[0]].startVert]; + const vec3 edge1 = vertPos_[halfedge_[faceEdges[1]].endVert] - + vertPos_[halfedge_[faceEdges[1]].startVert]; + const vec3 newTangent = la::normalize(edge0) - la::normalize(edge1); + tangent[faceEdges[0]] = CircularTangent(newTangent, edge0); + tangent[faceEdges[1]] = CircularTangent(-newTangent, edge1); + } else if (faceEdges[0] == -1 && faceEdges[0] == -1) { + fixedHalfedge[e] = true; + } + }); + + halfedgeTangent_.swap(tangent); + DistributeTangents(fixedHalfedge); +} + +/** + * Calculates halfedgeTangent_, allowing the manifold to be refined and + * smoothed. The tangents form weighted cubic Beziers along each edge. This + * function creates circular arcs where possible (minimizing maximum curvature), + * constrained to the vertex normals. Where sharpenedEdges are specified, the + * tangents are shortened that intersect the sharpened edge, concentrating the + * curvature there, while the tangents of the sharp edges themselves are aligned + * for continuity. + */ +void Manifold::Impl::CreateTangents(std::vector sharpenedEdges) { + ZoneScoped; + const int numHalfedge = halfedge_.size(); + halfedgeTangent_.resize(0); + Vec tangent(numHalfedge); + Vec fixedHalfedge(numHalfedge, false); + + Vec vertHalfedge = VertHalfedge(); + Vec triIsFlatFace = FlatFaces(); + Vec vertFlatFace = VertFlatFace(triIsFlatFace); + Vec vertNormal = vertNormal_; + for (size_t v = 0; v < NumVert(); ++v) { + if (vertFlatFace[v] >= 0) { + vertNormal[v] = faceNormal_[vertFlatFace[v]]; + } + } + + for_each_n(autoPolicy(numHalfedge, 1e4), countAt(0), numHalfedge, + [&tangent, &vertNormal, this](const int edgeIdx) { + tangent[edgeIdx] = + IsInsideQuad(edgeIdx) + ? vec4(0, 0, 0, -1) + : TangentFromNormal( + vertNormal[halfedge_[edgeIdx].startVert], edgeIdx); + }); + + halfedgeTangent_.swap(tangent); + + // Add sharpened edges around faces, just on the face side. + for (size_t tri = 0; tri < NumTri(); ++tri) { + if (!triIsFlatFace[tri]) continue; + for (const int j : {0, 1, 2}) { + const int tri2 = halfedge_[3 * tri + j].pairedHalfedge / 3; + if (!triIsFlatFace[tri2] || + !meshRelation_.triRef[tri].SameFace(meshRelation_.triRef[tri2])) { + sharpenedEdges.push_back({3 * tri + j, 0}); + } + } + } + + using Pair = std::pair; + // Fill in missing pairs with default smoothness = 1. + std::map edges; + for (Smoothness edge : sharpenedEdges) { + if (edge.smoothness >= 1) continue; + const bool forward = halfedge_[edge.halfedge].IsForward(); + const int pair = halfedge_[edge.halfedge].pairedHalfedge; + const int idx = forward ? edge.halfedge : pair; + if (edges.find(idx) == edges.end()) { + edges[idx] = {edge, {static_cast(pair), 1}}; + if (!forward) std::swap(edges[idx].first, edges[idx].second); + } else { + Smoothness& e = forward ? edges[idx].first : edges[idx].second; + e.smoothness = std::min(edge.smoothness, e.smoothness); + } + } + + std::map> vertTangents; + for (const auto& value : edges) { + const Pair edge = value.second; + vertTangents[halfedge_[edge.first.halfedge].startVert].push_back(edge); + vertTangents[halfedge_[edge.second.halfedge].startVert].push_back( + {edge.second, edge.first}); + } + + const int numVert = NumVert(); + for_each_n( + autoPolicy(numVert, 1e4), countAt(0), numVert, + [this, &vertTangents, &fixedHalfedge, &vertHalfedge, + &triIsFlatFace](int v) { + auto it = vertTangents.find(v); + if (it == vertTangents.end()) { + fixedHalfedge[vertHalfedge[v]] = true; + return; + } + const std::vector& vert = it->second; + // Sharp edges that end are smooth at their terminal vert. + if (vert.size() == 1) return; + if (vert.size() == 2) { // Make continuous edge + const int first = vert[0].first.halfedge; + const int second = vert[1].first.halfedge; + fixedHalfedge[first] = true; + fixedHalfedge[second] = true; + const vec3 newTangent = la::normalize(vec3(halfedgeTangent_[first]) - + vec3(halfedgeTangent_[second])); + + const vec3 pos = vertPos_[halfedge_[first].startVert]; + halfedgeTangent_[first] = CircularTangent( + newTangent, vertPos_[halfedge_[first].endVert] - pos); + halfedgeTangent_[second] = CircularTangent( + -newTangent, vertPos_[halfedge_[second].endVert] - pos); + + double smoothness = + (vert[0].second.smoothness + vert[1].first.smoothness) / 2; + ForVert(first, [this, &smoothness, &vert, first, + second](int current) { + if (current == second) { + smoothness = + (vert[1].second.smoothness + vert[0].first.smoothness) / 2; + } else if (current != first && !IsMarkedInsideQuad(current)) { + SharpenTangent(current, smoothness); + } + }); + } else { // Sharpen vertex uniformly + double smoothness = 0; + double denom = 0; + for (const Pair& pair : vert) { + smoothness += pair.first.smoothness; + smoothness += pair.second.smoothness; + denom += pair.first.smoothness == 0 ? 0 : 1; + denom += pair.second.smoothness == 0 ? 0 : 1; + } + smoothness /= denom; + + ForVert(vert[0].first.halfedge, + [this, &triIsFlatFace, smoothness](int current) { + if (!IsMarkedInsideQuad(current)) { + const int pair = halfedge_[current].pairedHalfedge; + SharpenTangent(current, triIsFlatFace[current / 3] || + triIsFlatFace[pair / 3] + ? 0 + : smoothness); + } + }); + } + }); + + LinearizeFlatTangents(); + DistributeTangents(fixedHalfedge); +} + +void Manifold::Impl::Refine(std::function edgeDivisions, + bool keepInterior) { + if (IsEmpty()) return; + Manifold::Impl old = *this; + Vec vertBary = Subdivide(edgeDivisions, keepInterior); + if (vertBary.size() == 0) return; + + if (old.halfedgeTangent_.size() == old.halfedge_.size()) { + for_each_n(autoPolicy(NumTri(), 1e4), countAt(0), NumVert(), + InterpTri({vertPos_, vertBary, &old})); + } + + halfedgeTangent_.resize(0); + Finish(); + CreateFaces(); + meshRelation_.originalID = -1; +} + +} // namespace manifold diff --git a/thirdparty/manifold/src/sort.cpp b/thirdparty/manifold/src/sort.cpp new file mode 100644 index 000000000000..f394aa526632 --- /dev/null +++ b/thirdparty/manifold/src/sort.cpp @@ -0,0 +1,517 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "./impl.h" +#include "./parallel.h" + +namespace { +using namespace manifold; + +constexpr uint32_t kNoCode = 0xFFFFFFFFu; + +uint32_t MortonCode(vec3 position, Box bBox) { + // Unreferenced vertices are marked NaN, and this will sort them to the end + // (the Morton code only uses the first 30 of 32 bits). + if (std::isnan(position.x)) return kNoCode; + + return Collider::MortonCode(position, bBox); +} + +struct Reindex { + VecView indexInv; + + void operator()(Halfedge& edge) { + if (edge.startVert < 0) return; + edge.startVert = indexInv[edge.startVert]; + edge.endVert = indexInv[edge.endVert]; + } +}; + +struct MarkProp { + VecView keep; + + void operator()(ivec3 triProp) { + for (const int i : {0, 1, 2}) { + reinterpret_cast*>(&keep[triProp[i]]) + ->store(1, std::memory_order_relaxed); + } + } +}; + +struct ReindexProps { + VecView old2new; + + void operator()(ivec3& triProp) { + for (const int i : {0, 1, 2}) { + triProp[i] = old2new[triProp[i]]; + } + } +}; + +struct ReindexFace { + VecView halfedge; + VecView halfedgeTangent; + VecView oldHalfedge; + VecView oldHalfedgeTangent; + VecView faceNew2Old; + VecView faceOld2New; + + void operator()(int newFace) { + const int oldFace = faceNew2Old[newFace]; + for (const int i : {0, 1, 2}) { + const int oldEdge = 3 * oldFace + i; + Halfedge edge = oldHalfedge[oldEdge]; + const int pairedFace = edge.pairedHalfedge / 3; + const int offset = edge.pairedHalfedge - 3 * pairedFace; + edge.pairedHalfedge = 3 * faceOld2New[pairedFace] + offset; + const int newEdge = 3 * newFace + i; + halfedge[newEdge] = edge; + if (!oldHalfedgeTangent.empty()) { + halfedgeTangent[newEdge] = oldHalfedgeTangent[oldEdge]; + } + } + } +}; + +template +bool MergeMeshGLP(MeshGLP& mesh) { + ZoneScoped; + std::multiset> openEdges; + + std::vector merge(mesh.NumVert()); + std::iota(merge.begin(), merge.end(), 0); + for (size_t i = 0; i < mesh.mergeFromVert.size(); ++i) { + merge[mesh.mergeFromVert[i]] = mesh.mergeToVert[i]; + } + + const auto numVert = mesh.NumVert(); + const auto numTri = mesh.NumTri(); + const int next[3] = {1, 2, 0}; + for (size_t tri = 0; tri < numTri; ++tri) { + for (int i : {0, 1, 2}) { + auto edge = std::make_pair(merge[mesh.triVerts[3 * tri + next[i]]], + merge[mesh.triVerts[3 * tri + i]]); + auto it = openEdges.find(edge); + if (it == openEdges.end()) { + std::swap(edge.first, edge.second); + openEdges.insert(edge); + } else { + openEdges.erase(it); + } + } + } + if (openEdges.empty()) { + return false; + } + + const auto numOpenVert = openEdges.size(); + Vec openVerts(numOpenVert); + int i = 0; + for (const auto& edge : openEdges) { + const int vert = edge.first; + openVerts[i++] = vert; + } + + Vec vertPropD(mesh.vertProperties); + Box bBox; + for (const int i : {0, 1, 2}) { + auto iPos = + StridedRange(vertPropD.begin() + i, vertPropD.end(), mesh.numProp); + auto minMax = manifold::transform_reduce( + iPos.begin(), iPos.end(), + std::make_pair(std::numeric_limits::infinity(), + -std::numeric_limits::infinity()), + [](auto a, auto b) { + return std::make_pair(std::min(a.first, b.first), + std::max(a.second, b.second)); + }, + [](double f) { return std::make_pair(f, f); }); + bBox.min[i] = minMax.first; + bBox.max[i] = minMax.second; + } + + const double tolerance = std::max(static_cast(mesh.tolerance), + (std::is_same::value + ? std::numeric_limits::epsilon() + : kPrecision) * + bBox.Scale()); + + auto policy = autoPolicy(numOpenVert, 1e5); + Vec vertBox(numOpenVert); + Vec vertMorton(numOpenVert); + + for_each_n(policy, countAt(0), numOpenVert, + [&vertMorton, &vertBox, &openVerts, &bBox, &mesh, + tolerance](const int i) { + int vert = openVerts[i]; + + const vec3 center(mesh.vertProperties[mesh.numProp * vert], + mesh.vertProperties[mesh.numProp * vert + 1], + mesh.vertProperties[mesh.numProp * vert + 2]); + + vertBox[i].min = center - tolerance / 2.0; + vertBox[i].max = center + tolerance / 2.0; + + vertMorton[i] = MortonCode(center, bBox); + }); + + Vec vertNew2Old(numOpenVert); + sequence(vertNew2Old.begin(), vertNew2Old.end()); + + stable_sort(vertNew2Old.begin(), vertNew2Old.end(), + [&vertMorton](const int& a, const int& b) { + return vertMorton[a] < vertMorton[b]; + }); + + Permute(vertMorton, vertNew2Old); + Permute(vertBox, vertNew2Old); + Permute(openVerts, vertNew2Old); + Collider collider(vertBox, vertMorton); + SparseIndices toMerge = collider.Collisions(vertBox.cview()); + + UnionFind<> uf(numVert); + for (size_t i = 0; i < mesh.mergeFromVert.size(); ++i) { + uf.unionXY(static_cast(mesh.mergeFromVert[i]), + static_cast(mesh.mergeToVert[i])); + } + for (size_t i = 0; i < toMerge.size(); ++i) { + uf.unionXY(openVerts[toMerge.Get(i, false)], + openVerts[toMerge.Get(i, true)]); + } + + mesh.mergeToVert.clear(); + mesh.mergeFromVert.clear(); + for (size_t v = 0; v < numVert; ++v) { + const size_t mergeTo = uf.find(v); + if (mergeTo != v) { + mesh.mergeFromVert.push_back(v); + mesh.mergeToVert.push_back(mergeTo); + } + } + + return true; +} +} // namespace + +namespace manifold { + +/** + * Once halfedge_ has been filled in, this function can be called to create the + * rest of the internal data structures. This function also removes the verts + * and halfedges flagged for removal (NaN verts and -1 halfedges). + */ +void Manifold::Impl::Finish() { + if (halfedge_.size() == 0) return; + + CalculateBBox(); + SetEpsilon(epsilon_); + if (!bBox_.IsFinite()) { + // Decimated out of existence - early out. + MarkFailure(Error::NoError); + return; + } + + SortVerts(); + Vec faceBox; + Vec faceMorton; + GetFaceBoxMorton(faceBox, faceMorton); + SortFaces(faceBox, faceMorton); + if (halfedge_.size() == 0) return; + CompactProps(); + + DEBUG_ASSERT(halfedge_.size() % 6 == 0, topologyErr, + "Not an even number of faces after sorting faces!"); + +#ifdef MANIFOLD_DEBUG + auto MaxOrMinus = [](int a, int b) { + return std::min(a, b) < 0 ? -1 : std::max(a, b); + }; + int face = 0; + Halfedge extrema = {0, 0, 0}; + for (size_t i = 0; i < halfedge_.size(); i++) { + Halfedge e = halfedge_[i]; + if (!e.IsForward()) std::swap(e.startVert, e.endVert); + extrema.startVert = std::min(extrema.startVert, e.startVert); + extrema.endVert = std::min(extrema.endVert, e.endVert); + extrema.pairedHalfedge = + MaxOrMinus(extrema.pairedHalfedge, e.pairedHalfedge); + face = MaxOrMinus(face, i / 3); + } + DEBUG_ASSERT(extrema.startVert >= 0, topologyErr, + "Vertex index is negative!"); + DEBUG_ASSERT(extrema.endVert < static_cast(NumVert()), topologyErr, + "Vertex index exceeds number of verts!"); + DEBUG_ASSERT(extrema.pairedHalfedge >= 0, topologyErr, + "Halfedge index is negative!"); + DEBUG_ASSERT(extrema.pairedHalfedge < 2 * static_cast(NumEdge()), + topologyErr, "Halfedge index exceeds number of halfedges!"); + DEBUG_ASSERT(face >= 0, topologyErr, "Face index is negative!"); + DEBUG_ASSERT(face < static_cast(NumTri()), topologyErr, + "Face index exceeds number of faces!"); +#endif + + DEBUG_ASSERT(meshRelation_.triRef.size() == NumTri() || + meshRelation_.triRef.size() == 0, + logicErr, "Mesh Relation doesn't fit!"); + DEBUG_ASSERT(faceNormal_.size() == NumTri() || faceNormal_.size() == 0, + logicErr, + "faceNormal size = " + std::to_string(faceNormal_.size()) + + ", NumTri = " + std::to_string(NumTri())); + CalculateNormals(); + collider_ = Collider(faceBox, faceMorton); + + DEBUG_ASSERT(Is2Manifold(), logicErr, "mesh is not 2-manifold!"); +} + +/** + * Sorts the vertices according to their Morton code. + */ +void Manifold::Impl::SortVerts() { + ZoneScoped; + const auto numVert = NumVert(); + Vec vertMorton(numVert); + auto policy = autoPolicy(numVert, 1e5); + for_each_n(policy, countAt(0), numVert, [this, &vertMorton](const int vert) { + vertMorton[vert] = MortonCode(vertPos_[vert], bBox_); + }); + + Vec vertNew2Old(numVert); + sequence(vertNew2Old.begin(), vertNew2Old.end()); + + stable_sort(vertNew2Old.begin(), vertNew2Old.end(), + [&vertMorton](const int& a, const int& b) { + return vertMorton[a] < vertMorton[b]; + }); + + ReindexVerts(vertNew2Old, numVert); + + // Verts were flagged for removal with NaNs and assigned kNoCode to sort + // them to the end, which allows them to be removed. + const auto newNumVert = std::find_if(vertNew2Old.begin(), vertNew2Old.end(), + [&vertMorton](const int vert) { + return vertMorton[vert] == kNoCode; + }) - + vertNew2Old.begin(); + + vertNew2Old.resize(newNumVert); + Permute(vertPos_, vertNew2Old); + + if (vertNormal_.size() == numVert) { + Permute(vertNormal_, vertNew2Old); + } +} + +/** + * Updates the halfedges to point to new vert indices based on a mapping, + * vertNew2Old. This may be a subset, so the total number of original verts is + * also given. + */ +void Manifold::Impl::ReindexVerts(const Vec& vertNew2Old, + size_t oldNumVert) { + ZoneScoped; + Vec vertOld2New(oldNumVert); + scatter(countAt(0), countAt(static_cast(NumVert())), vertNew2Old.begin(), + vertOld2New.begin()); + for_each(autoPolicy(oldNumVert, 1e5), halfedge_.begin(), halfedge_.end(), + Reindex({vertOld2New})); +} + +/** + * Removes unreferenced property verts and reindexes triProperties. + */ +void Manifold::Impl::CompactProps() { + ZoneScoped; + if (meshRelation_.numProp == 0) return; + + const auto numVerts = meshRelation_.properties.size() / meshRelation_.numProp; + Vec keep(numVerts, 0); + auto policy = autoPolicy(numVerts, 1e5); + + for_each(policy, meshRelation_.triProperties.cbegin(), + meshRelation_.triProperties.cend(), MarkProp({keep})); + Vec propOld2New(numVerts + 1, 0); + inclusive_scan(keep.begin(), keep.end(), propOld2New.begin() + 1); + + Vec oldProp = meshRelation_.properties; + const int numVertsNew = propOld2New[numVerts]; + const int numProp = meshRelation_.numProp; + auto& properties = meshRelation_.properties; + properties.resize(numProp * numVertsNew); + for_each_n( + policy, countAt(0), numVerts, + [&properties, &oldProp, &propOld2New, &keep, &numProp](const int oldIdx) { + if (keep[oldIdx] == 0) return; + for (int p = 0; p < numProp; ++p) { + properties[propOld2New[oldIdx] * numProp + p] = + oldProp[oldIdx * numProp + p]; + } + }); + for_each_n(policy, meshRelation_.triProperties.begin(), NumTri(), + ReindexProps({propOld2New})); +} + +/** + * Fills the faceBox and faceMorton input with the bounding boxes and Morton + * codes of the faces, respectively. The Morton code is based on the center of + * the bounding box. + */ +void Manifold::Impl::GetFaceBoxMorton(Vec& faceBox, + Vec& faceMorton) const { + ZoneScoped; + faceBox.resize(NumTri()); + faceMorton.resize(NumTri()); + for_each_n(autoPolicy(NumTri(), 1e5), countAt(0), NumTri(), + [this, &faceBox, &faceMorton](const int face) { + // Removed tris are marked by all halfedges having pairedHalfedge + // = -1, and this will sort them to the end (the Morton code only + // uses the first 30 of 32 bits). + if (halfedge_[3 * face].pairedHalfedge < 0) { + faceMorton[face] = kNoCode; + return; + } + + vec3 center(0.0); + + for (const int i : {0, 1, 2}) { + const vec3 pos = vertPos_[halfedge_[3 * face + i].startVert]; + center += pos; + faceBox[face].Union(pos); + } + center /= 3; + + faceMorton[face] = MortonCode(center, bBox_); + }); +} + +/** + * Sorts the faces of this manifold according to their input Morton code. The + * bounding box and Morton code arrays are also sorted accordingly. + */ +void Manifold::Impl::SortFaces(Vec& faceBox, Vec& faceMorton) { + ZoneScoped; + Vec faceNew2Old(NumTri()); + sequence(faceNew2Old.begin(), faceNew2Old.end()); + + stable_sort(faceNew2Old.begin(), faceNew2Old.end(), + [&faceMorton](const int& a, const int& b) { + return faceMorton[a] < faceMorton[b]; + }); + + // Tris were flagged for removal with pairedHalfedge = -1 and assigned kNoCode + // to sort them to the end, which allows them to be removed. + const int newNumTri = std::find_if(faceNew2Old.begin(), faceNew2Old.end(), + [&faceMorton](const int face) { + return faceMorton[face] == kNoCode; + }) - + faceNew2Old.begin(); + faceNew2Old.resize(newNumTri); + + Permute(faceMorton, faceNew2Old); + Permute(faceBox, faceNew2Old); + GatherFaces(faceNew2Old); +} + +/** + * Creates the halfedge_ vector for this manifold by copying a set of faces from + * another manifold, given by oldHalfedge. Input faceNew2Old defines the old + * faces to gather into this. + */ +void Manifold::Impl::GatherFaces(const Vec& faceNew2Old) { + ZoneScoped; + const auto numTri = faceNew2Old.size(); + if (meshRelation_.triRef.size() == NumTri()) + Permute(meshRelation_.triRef, faceNew2Old); + if (meshRelation_.triProperties.size() == NumTri()) + Permute(meshRelation_.triProperties, faceNew2Old); + if (faceNormal_.size() == NumTri()) Permute(faceNormal_, faceNew2Old); + + Vec oldHalfedge(std::move(halfedge_)); + Vec oldHalfedgeTangent(std::move(halfedgeTangent_)); + Vec faceOld2New(oldHalfedge.size() / 3); + auto policy = autoPolicy(numTri, 1e5); + scatter(countAt(0_uz), countAt(numTri), faceNew2Old.begin(), + faceOld2New.begin()); + + halfedge_.resize(3 * numTri); + if (oldHalfedgeTangent.size() != 0) halfedgeTangent_.resize(3 * numTri); + for_each_n(policy, countAt(0), numTri, + ReindexFace({halfedge_, halfedgeTangent_, oldHalfedge, + oldHalfedgeTangent, faceNew2Old, faceOld2New})); +} + +void Manifold::Impl::GatherFaces(const Impl& old, const Vec& faceNew2Old) { + ZoneScoped; + const auto numTri = faceNew2Old.size(); + + meshRelation_.triRef.resize(numTri); + gather(faceNew2Old.begin(), faceNew2Old.end(), + old.meshRelation_.triRef.begin(), meshRelation_.triRef.begin()); + + for (const auto& pair : old.meshRelation_.meshIDtransform) { + meshRelation_.meshIDtransform[pair.first] = pair.second; + } + + if (old.meshRelation_.triProperties.size() > 0) { + meshRelation_.triProperties.resize(numTri); + gather(faceNew2Old.begin(), faceNew2Old.end(), + old.meshRelation_.triProperties.begin(), + meshRelation_.triProperties.begin()); + meshRelation_.numProp = old.meshRelation_.numProp; + meshRelation_.properties = old.meshRelation_.properties; + } + + if (old.faceNormal_.size() == old.NumTri()) { + faceNormal_.resize(numTri); + gather(faceNew2Old.begin(), faceNew2Old.end(), old.faceNormal_.begin(), + faceNormal_.begin()); + } + + Vec faceOld2New(old.NumTri()); + scatter(countAt(0_uz), countAt(numTri), faceNew2Old.begin(), + faceOld2New.begin()); + + halfedge_.resize(3 * numTri); + if (old.halfedgeTangent_.size() != 0) halfedgeTangent_.resize(3 * numTri); + for_each_n(autoPolicy(numTri, 1e5), countAt(0), numTri, + ReindexFace({halfedge_, halfedgeTangent_, old.halfedge_, + old.halfedgeTangent_, faceNew2Old, faceOld2New})); +} + +/** + * Updates the mergeFromVert and mergeToVert vectors in order to create a + * manifold solid. If the MeshGL is already manifold, no change will occur and + * the function will return false. Otherwise, this will merge verts along open + * edges within tolerance (the maximum of the MeshGL tolerance and the + * baseline bounding-box tolerance), keeping any from the existing merge + * vectors, and return true. + * + * There is no guarantee the result will be manifold - this is a best-effort + * helper function designed primarily to aid in the case where a manifold + * multi-material MeshGL was produced, but its merge vectors were lost due to + * a round-trip through a file format. Constructing a Manifold from the result + * will report an error status if it is not manifold. + */ +template <> +bool MeshGL::Merge() { + return MergeMeshGLP(*this); +} + +template <> +bool MeshGL64::Merge() { + return MergeMeshGLP(*this); +} +} // namespace manifold diff --git a/thirdparty/manifold/src/sparse.h b/thirdparty/manifold/src/sparse.h new file mode 100644 index 000000000000..a25ea611412b --- /dev/null +++ b/thirdparty/manifold/src/sparse.h @@ -0,0 +1,225 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include "./parallel.h" +#include "./utils.h" +#include "./vec.h" +#include "manifold/common.h" +#include "manifold/optional_assert.h" + +namespace { +template +inline bool FirstFinite(T v) { + return std::isfinite(v[0]); +} + +template <> +inline bool FirstFinite(double v) { + return std::isfinite(v); +} +} // namespace + +namespace manifold { + +/** @ingroup Private */ +class SparseIndices { + // sparse indices where {p1: q1, p2: q2, ...} are laid out as + // p1 q1 p2 q2 or q1 p1 q2 p2, depending on endianness + // such that the indices are sorted by (p << 32) | q + public: +#if defined(__BYTE_ORDER) && __BYTE_ORDER == __BIG_ENDIAN || \ + defined(__BIG_ENDIAN__) || defined(__ARMEB__) || defined(__THUMBEB__) || \ + defined(__AARCH64EB__) || defined(_MIBSEB) || defined(__MIBSEB) || \ + defined(__MIBSEB__) + static constexpr size_t pOffset = 0; +#elif defined(__BYTE_ORDER) && __BYTE_ORDER == __LITTLE_ENDIAN || \ + defined(__LITTLE_ENDIAN__) || defined(__ARMEL__) || \ + defined(__THUMBEL__) || defined(__AARCH64EL__) || defined(_MIPSEL) || \ + defined(__MIPSEL) || defined(__MIPSEL__) || defined(__EMSCRIPTEN__) || \ + defined(_WIN32) + static constexpr size_t pOffset = 1; +#else +#error "unknown architecture" +#endif + static constexpr int64_t EncodePQ(int p, int q) { + return (int64_t(p) << 32) | q; + } + + SparseIndices() = default; + SparseIndices(size_t size) { data_ = Vec(size * sizeof(int64_t)); } + + void Clear() { data_.clear(false); } + + void FromIndices(const std::vector& indices) { + std::vector sizes; + size_t total_size = 0; + for (const auto& ind : indices) { + sizes.push_back(total_size); + total_size += ind.data_.size(); + } + data_ = Vec(total_size); + for_each_n(ExecutionPolicy::Par, countAt(0), indices.size(), [&](size_t i) { + std::copy(indices[i].data_.begin(), indices[i].data_.end(), + data_.begin() + sizes[i]); + }); + } + + size_t size() const { return data_.size() / sizeof(int64_t); } + + Vec Copy(bool use_q) const { + Vec out(size()); + size_t offset = pOffset; + if (use_q) offset = 1 - offset; + const int* p = ptr(); + for_each(autoPolicy(out.size()), countAt(0_uz), countAt(out.size()), + [&](size_t i) { out[i] = p[i * 2 + offset]; }); + return out; + } + + void Sort() { + VecView view = AsVec64(); + stable_sort(view.begin(), view.end()); + } + + void Resize(size_t size) { data_.resize(size * sizeof(int64_t), -1); } + + inline int& Get(size_t i, bool use_q) { + if (use_q) + return ptr()[2 * i + 1 - pOffset]; + else + return ptr()[2 * i + pOffset]; + } + + inline int Get(size_t i, bool use_q) const { + if (use_q) + return ptr()[2 * i + 1 - pOffset]; + else + return ptr()[2 * i + pOffset]; + } + + inline int64_t GetPQ(size_t i) const { + VecView view = AsVec64(); + return view[i]; + } + + inline void Set(size_t i, int p, int q) { + VecView view = AsVec64(); + view[i] = EncodePQ(p, q); + } + + inline void SetPQ(size_t i, int64_t pq) { + VecView view = AsVec64(); + view[i] = pq; + } + + VecView AsVec64() { + return VecView(reinterpret_cast(data_.data()), + data_.size() / sizeof(int64_t)); + } + + VecView AsVec64() const { + return VecView( + reinterpret_cast(data_.data()), + data_.size() / sizeof(int64_t)); + } + + VecView AsVec32() { + return VecView(reinterpret_cast(data_.data()), + data_.size() / sizeof(int32_t)); + } + + VecView AsVec32() const { + return VecView( + reinterpret_cast(data_.data()), + data_.size() / sizeof(int32_t)); + } + + inline void Add(int p, int q, bool seq = false) { + data_.extend(sizeof(int64_t), seq); + Set(size() - 1, p, q); + } + + void Unique() { + Sort(); + VecView view = AsVec64(); + size_t newSize = unique(view.begin(), view.end()) - view.begin(); + Resize(newSize); + } + + size_t RemoveZeros(Vec& S) { + DEBUG_ASSERT(S.size() == size(), userErr, + "Different number of values than indicies!"); + + Vec new2Old(S.size()); + sequence(new2Old.begin(), new2Old.end()); + + size_t size = copy_if(countAt(0_uz), countAt(S.size()), new2Old.begin(), + [&S](const size_t i) { return S[i] != 0; }) - + new2Old.begin(); + new2Old.resize(size); + + Permute(S, new2Old); + Vec tmp(std::move(data_)); + Resize(size); + gather(new2Old.begin(), new2Old.end(), + reinterpret_cast(tmp.data()), + reinterpret_cast(data_.data())); + + return size; + } + + template + size_t KeepFinite(Vec& v, Vec& x) { + DEBUG_ASSERT(x.size() == size(), userErr, + "Different number of values than indicies!"); + + Vec new2Old(v.size()); + size_t size = copy_if(countAt(0_uz), countAt(v.size()), new2Old.begin(), + [&v](size_t i) { return FirstFinite(v[i]); }) - + new2Old.begin(); + new2Old.resize(size); + + Permute(v, new2Old); + Permute(x, new2Old); + Vec tmp(std::move(data_)); + Resize(size); + gather(new2Old.begin(), new2Old.end(), + reinterpret_cast(tmp.data()), + reinterpret_cast(data_.data())); + + return size; + } + +#ifdef MANIFOLD_DEBUG + void Dump() const { + std::cout << "SparseIndices = " << std::endl; + const int* p = ptr(); + for (size_t i = 0; i < size(); ++i) { + std::cout << i << ", p = " << Get(i, false) << ", q = " << Get(i, true) + << std::endl; + } + std::cout << std::endl; + } +#endif + + private: + Vec data_; + inline int* ptr() { return reinterpret_cast(data_.data()); } + inline const int* ptr() const { + return reinterpret_cast(data_.data()); + } +}; + +} // namespace manifold diff --git a/thirdparty/manifold/src/subdivision.cpp b/thirdparty/manifold/src/subdivision.cpp new file mode 100644 index 000000000000..c8631d79c806 --- /dev/null +++ b/thirdparty/manifold/src/subdivision.cpp @@ -0,0 +1,809 @@ +// Copyright 2024 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./impl.h" +#include "./parallel.h" + +template <> +struct std::hash { + size_t operator()(const manifold::ivec4& p) const { + return std::hash()(p.x) ^ std::hash()(p.y) ^ + std::hash()(p.z) ^ std::hash()(p.w); + } +}; + +namespace { +using namespace manifold; + +class Partition { + public: + // The cached partitions don't have idx - it's added to the copy returned + // from GetPartition that contains the mapping of the input divisions into the + // sorted divisions that are uniquely cached. + ivec4 idx; + ivec4 sortedDivisions; + Vec vertBary; + Vec triVert; + + int InteriorOffset() const { + return sortedDivisions[0] + sortedDivisions[1] + sortedDivisions[2] + + sortedDivisions[3]; + } + + int NumInterior() const { return vertBary.size() - InteriorOffset(); } + + static Partition GetPartition(ivec4 divisions) { + if (divisions[0] == 0) return Partition(); // skip wrong side of quad + + ivec4 sortedDiv = divisions; + ivec4 triIdx = {0, 1, 2, 3}; + if (divisions[3] == 0) { // triangle + if (sortedDiv[2] > sortedDiv[1]) { + std::swap(sortedDiv[2], sortedDiv[1]); + std::swap(triIdx[2], triIdx[1]); + } + if (sortedDiv[1] > sortedDiv[0]) { + std::swap(sortedDiv[1], sortedDiv[0]); + std::swap(triIdx[1], triIdx[0]); + if (sortedDiv[2] > sortedDiv[1]) { + std::swap(sortedDiv[2], sortedDiv[1]); + std::swap(triIdx[2], triIdx[1]); + } + } + } else { // quad + int minIdx = 0; + int min = divisions[minIdx]; + int next = divisions[1]; + for (const int i : {1, 2, 3}) { + const int n = divisions[(i + 1) % 4]; + if (divisions[i] < min || (divisions[i] == min && n < next)) { + minIdx = i; + min = divisions[i]; + next = n; + } + } + // Backwards (mirrored) quads get a separate cache key for now for + // simplicity, so there is no reversal necessary for quads when + // re-indexing. + ivec4 tmp = sortedDiv; + for (const int i : {0, 1, 2, 3}) { + triIdx[i] = (i + minIdx) % 4; + sortedDiv[i] = tmp[triIdx[i]]; + } + } + + Partition partition = GetCachedPartition(sortedDiv); + partition.idx = triIdx; + + return partition; + } + + Vec Reindex(ivec4 triVerts, ivec4 edgeOffsets, bvec4 edgeFwd, + int interiorOffset) const { + Vec newVerts; + newVerts.reserve(vertBary.size()); + ivec4 triIdx = idx; + ivec4 outTri = {0, 1, 2, 3}; + if (triVerts[3] < 0 && idx[1] != Next3(idx[0])) { + triIdx = {idx[2], idx[0], idx[1], idx[3]}; + edgeFwd = !edgeFwd; + std::swap(outTri[0], outTri[1]); + } + for (const int i : {0, 1, 2, 3}) { + if (triVerts[triIdx[i]] >= 0) newVerts.push_back(triVerts[triIdx[i]]); + } + for (const int i : {0, 1, 2, 3}) { + const int n = sortedDivisions[i] - 1; + int offset = edgeOffsets[idx[i]] + (edgeFwd[idx[i]] ? 0 : n - 1); + for (int j = 0; j < n; ++j) { + newVerts.push_back(offset); + offset += edgeFwd[idx[i]] ? 1 : -1; + } + } + const int offset = interiorOffset - newVerts.size(); + size_t old = newVerts.size(); + newVerts.resize(vertBary.size()); + std::iota(newVerts.begin() + old, newVerts.end(), old + offset); + + const int numTri = triVert.size(); + Vec newTriVert(numTri); + for_each_n(autoPolicy(numTri), countAt(0), numTri, + [&newTriVert, &outTri, &newVerts, this](const int tri) { + for (const int j : {0, 1, 2}) { + newTriVert[tri][outTri[j]] = newVerts[triVert[tri][j]]; + } + }); + return newTriVert; + } + + private: + static inline auto cacheLock = std::mutex(); + static inline auto cache = + std::unordered_map>(); + + // This triangulation is purely topological - it depends only on the number of + // divisions of the three sides of the triangle. This allows them to be cached + // and reused for similar triangles. The shape of the final surface is defined + // by the tangents and the barycentric coordinates of the new verts. For + // triangles, the input must be sorted: n[0] >= n[1] >= n[2] > 0. + static Partition GetCachedPartition(ivec4 n) { + { + auto lockGuard = std::lock_guard(cacheLock); + auto cached = cache.find(n); + if (cached != cache.end()) { + return *cached->second; + } + } + Partition partition; + partition.sortedDivisions = n; + if (n[3] > 0) { // quad + partition.vertBary.push_back({1, 0, 0, 0}); + partition.vertBary.push_back({0, 1, 0, 0}); + partition.vertBary.push_back({0, 0, 1, 0}); + partition.vertBary.push_back({0, 0, 0, 1}); + ivec4 edgeOffsets; + edgeOffsets[0] = 4; + for (const int i : {0, 1, 2, 3}) { + if (i > 0) { + edgeOffsets[i] = edgeOffsets[i - 1] + n[i - 1] - 1; + } + const vec4 nextBary = partition.vertBary[(i + 1) % 4]; + for (int j = 1; j < n[i]; ++j) { + partition.vertBary.push_back( + la::lerp(partition.vertBary[i], nextBary, (double)j / n[i])); + } + } + PartitionQuad(partition.triVert, partition.vertBary, {0, 1, 2, 3}, + edgeOffsets, n - 1, {true, true, true, true}); + } else { // tri + partition.vertBary.push_back({1, 0, 0, 0}); + partition.vertBary.push_back({0, 1, 0, 0}); + partition.vertBary.push_back({0, 0, 1, 0}); + for (const int i : {0, 1, 2}) { + const vec4 nextBary = partition.vertBary[(i + 1) % 3]; + for (int j = 1; j < n[i]; ++j) { + partition.vertBary.push_back( + la::lerp(partition.vertBary[i], nextBary, (double)j / n[i])); + } + } + const ivec3 edgeOffsets = {3, 3 + n[0] - 1, 3 + n[0] - 1 + n[1] - 1}; + + const double f = n[2] * n[2] + n[0] * n[0]; + if (n[1] == 1) { + if (n[0] == 1) { + partition.triVert.push_back({0, 1, 2}); + } else { + PartitionFan(partition.triVert, {0, 1, 2}, n[0] - 1, edgeOffsets[0]); + } + } else if (n[1] * n[1] > f - std::sqrt(2.0) * n[0] * n[2]) { // acute-ish + partition.triVert.push_back({edgeOffsets[1] - 1, 1, edgeOffsets[1]}); + PartitionQuad(partition.triVert, partition.vertBary, + {edgeOffsets[1] - 1, edgeOffsets[1], 2, 0}, + {-1, edgeOffsets[1] + 1, edgeOffsets[2], edgeOffsets[0]}, + {0, n[1] - 2, n[2] - 1, n[0] - 2}, + {true, true, true, true}); + } else { // obtuse -> spit into two acute + // portion of n[0] under n[2] + const int ns = + std::min(n[0] - 2, (int)std::round((f - n[1] * n[1]) / (2 * n[0]))); + // height from n[0]: nh <= n[2] + const int nh = + std::max(1., std::round(std::sqrt(n[2] * n[2] - ns * ns))); + + const int hOffset = partition.vertBary.size(); + const vec4 middleBary = partition.vertBary[edgeOffsets[0] + ns - 1]; + for (int j = 1; j < nh; ++j) { + partition.vertBary.push_back( + la::lerp(partition.vertBary[2], middleBary, (double)j / nh)); + } + + partition.triVert.push_back({edgeOffsets[1] - 1, 1, edgeOffsets[1]}); + PartitionQuad( + partition.triVert, partition.vertBary, + {edgeOffsets[1] - 1, edgeOffsets[1], 2, edgeOffsets[0] + ns - 1}, + {-1, edgeOffsets[1] + 1, hOffset, edgeOffsets[0] + ns}, + {0, n[1] - 2, nh - 1, n[0] - ns - 2}, {true, true, true, true}); + + if (n[2] == 1) { + PartitionFan(partition.triVert, {0, edgeOffsets[0] + ns - 1, 2}, + ns - 1, edgeOffsets[0]); + } else { + if (ns == 1) { + partition.triVert.push_back({hOffset, 2, edgeOffsets[2]}); + PartitionQuad(partition.triVert, partition.vertBary, + {hOffset, edgeOffsets[2], 0, edgeOffsets[0]}, + {-1, edgeOffsets[2] + 1, -1, hOffset + nh - 2}, + {0, n[2] - 2, ns - 1, nh - 2}, + {true, true, true, false}); + } else { + partition.triVert.push_back({hOffset - 1, 0, edgeOffsets[0]}); + PartitionQuad( + partition.triVert, partition.vertBary, + {hOffset - 1, edgeOffsets[0], edgeOffsets[0] + ns - 1, 2}, + {-1, edgeOffsets[0] + 1, hOffset + nh - 2, edgeOffsets[2]}, + {0, ns - 2, nh - 1, n[2] - 2}, {true, true, false, true}); + } + } + } + } + + auto lockGuard = std::lock_guard(cacheLock); + cache.insert({n, std::make_unique(partition)}); + return partition; + } + + // Side 0 has added edges while sides 1 and 2 do not. Fan spreads from vert 2. + static void PartitionFan(Vec& triVert, ivec3 cornerVerts, int added, + int edgeOffset) { + int last = cornerVerts[0]; + for (int i = 0; i < added; ++i) { + const int next = edgeOffset + i; + triVert.push_back({last, next, cornerVerts[2]}); + last = next; + } + triVert.push_back({last, cornerVerts[1], cornerVerts[2]}); + } + + // Partitions are parallel to the first edge unless two consecutive edgeAdded + // are zero, in which case a terminal triangulation is performed. + static void PartitionQuad(Vec& triVert, Vec& vertBary, + ivec4 cornerVerts, ivec4 edgeOffsets, + ivec4 edgeAdded, bvec4 edgeFwd) { + auto GetEdgeVert = [&](int edge, int idx) { + return edgeOffsets[edge] + (edgeFwd[edge] ? 1 : -1) * idx; + }; + + DEBUG_ASSERT(la::all(la::gequal(edgeAdded, ivec4(0))), logicErr, + "negative divisions!"); + + int corner = -1; + int last = 3; + int maxEdge = -1; + for (const int i : {0, 1, 2, 3}) { + if (corner == -1 && edgeAdded[i] == 0 && edgeAdded[last] == 0) { + corner = i; + } + if (edgeAdded[i] > 0) { + maxEdge = maxEdge == -1 ? i : -2; + } + last = i; + } + if (corner >= 0) { // terminate + if (maxEdge >= 0) { + ivec4 edge = (ivec4(0, 1, 2, 3) + maxEdge) % 4; + const int middle = edgeAdded[maxEdge] / 2; + triVert.push_back({cornerVerts[edge[2]], cornerVerts[edge[3]], + GetEdgeVert(maxEdge, middle)}); + int last = cornerVerts[edge[0]]; + for (int i = 0; i <= middle; ++i) { + const int next = GetEdgeVert(maxEdge, i); + triVert.push_back({cornerVerts[edge[3]], last, next}); + last = next; + } + last = cornerVerts[edge[1]]; + for (int i = edgeAdded[maxEdge] - 1; i >= middle; --i) { + const int next = GetEdgeVert(maxEdge, i); + triVert.push_back({cornerVerts[edge[2]], next, last}); + last = next; + } + } else { + int sideVert = cornerVerts[0]; // initial value is unused + for (const int j : {1, 2}) { + const int side = (corner + j) % 4; + if (j == 2 && edgeAdded[side] > 0) { + triVert.push_back( + {cornerVerts[side], GetEdgeVert(side, 0), sideVert}); + } else { + sideVert = cornerVerts[side]; + } + for (int i = 0; i < edgeAdded[side]; ++i) { + const int nextVert = GetEdgeVert(side, i); + triVert.push_back({cornerVerts[corner], sideVert, nextVert}); + sideVert = nextVert; + } + if (j == 2 || edgeAdded[side] == 0) { + triVert.push_back({cornerVerts[corner], sideVert, + cornerVerts[(corner + j + 1) % 4]}); + } + } + } + return; + } + // recursively partition + const int partitions = 1 + std::min(edgeAdded[1], edgeAdded[3]); + ivec4 newCornerVerts = {cornerVerts[1], -1, -1, cornerVerts[0]}; + ivec4 newEdgeOffsets = {edgeOffsets[1], -1, + GetEdgeVert(3, edgeAdded[3] + 1), edgeOffsets[0]}; + ivec4 newEdgeAdded = {0, -1, 0, edgeAdded[0]}; + bvec4 newEdgeFwd = {edgeFwd[1], true, edgeFwd[3], edgeFwd[0]}; + + for (int i = 1; i < partitions; ++i) { + const int cornerOffset1 = (edgeAdded[1] * i) / partitions; + const int cornerOffset3 = + edgeAdded[3] - 1 - (edgeAdded[3] * i) / partitions; + const int nextOffset1 = GetEdgeVert(1, cornerOffset1 + 1); + const int nextOffset3 = GetEdgeVert(3, cornerOffset3 + 1); + const int added = std::round(la::lerp( + (double)edgeAdded[0], (double)edgeAdded[2], (double)i / partitions)); + + newCornerVerts[1] = GetEdgeVert(1, cornerOffset1); + newCornerVerts[2] = GetEdgeVert(3, cornerOffset3); + newEdgeAdded[0] = std::abs(nextOffset1 - newEdgeOffsets[0]) - 1; + newEdgeAdded[1] = added; + newEdgeAdded[2] = std::abs(nextOffset3 - newEdgeOffsets[2]) - 1; + newEdgeOffsets[1] = vertBary.size(); + newEdgeOffsets[2] = nextOffset3; + + for (int j = 0; j < added; ++j) { + vertBary.push_back(la::lerp(vertBary[newCornerVerts[1]], + vertBary[newCornerVerts[2]], + (j + 1.0) / (added + 1.0))); + } + + PartitionQuad(triVert, vertBary, newCornerVerts, newEdgeOffsets, + newEdgeAdded, newEdgeFwd); + + newCornerVerts[0] = newCornerVerts[1]; + newCornerVerts[3] = newCornerVerts[2]; + newEdgeAdded[3] = newEdgeAdded[1]; + newEdgeOffsets[0] = nextOffset1; + newEdgeOffsets[3] = newEdgeOffsets[1] + newEdgeAdded[1] - 1; + newEdgeFwd[3] = false; + } + + newCornerVerts[1] = cornerVerts[2]; + newCornerVerts[2] = cornerVerts[3]; + newEdgeOffsets[1] = edgeOffsets[2]; + newEdgeAdded[0] = + edgeAdded[1] - std::abs(newEdgeOffsets[0] - edgeOffsets[1]); + newEdgeAdded[1] = edgeAdded[2]; + newEdgeAdded[2] = std::abs(newEdgeOffsets[2] - edgeOffsets[3]) - 1; + newEdgeOffsets[2] = edgeOffsets[3]; + newEdgeFwd[1] = edgeFwd[2]; + + PartitionQuad(triVert, vertBary, newCornerVerts, newEdgeOffsets, + newEdgeAdded, newEdgeFwd); + } +}; +} // namespace + +namespace manifold { + +/** + * Returns the tri side index (0-2) connected to the other side of this quad if + * this tri is part of a quad, or -1 otherwise. + */ +int Manifold::Impl::GetNeighbor(int tri) const { + int neighbor = -1; + for (const int i : {0, 1, 2}) { + if (IsMarkedInsideQuad(3 * tri + i)) { + neighbor = neighbor == -1 ? i : -2; + } + } + return neighbor; +} + +/** + * For the given triangle index, returns either the three halfedge indices of + * that triangle and halfedges[3] = -1, or if the triangle is part of a quad, it + * returns those four indices. If the triangle is part of a quad and is not the + * lower of the two triangle indices, it returns all -1s. + */ +ivec4 Manifold::Impl::GetHalfedges(int tri) const { + ivec4 halfedges(-1); + for (const int i : {0, 1, 2}) { + halfedges[i] = 3 * tri + i; + } + const int neighbor = GetNeighbor(tri); + if (neighbor >= 0) { // quad + const int pair = halfedge_[3 * tri + neighbor].pairedHalfedge; + if (pair / 3 < tri) { + return ivec4(-1); // only process lower tri index + } + // The order here matters to keep small quads split the way they started, or + // else it can create a 4-manifold edge. + halfedges[2] = NextHalfedge(halfedges[neighbor]); + halfedges[3] = NextHalfedge(halfedges[2]); + halfedges[0] = NextHalfedge(pair); + halfedges[1] = NextHalfedge(halfedges[0]); + } + return halfedges; +} + +/** + * Returns the BaryIndices, which gives the tri and indices (0-3), such that + * GetHalfedges(val.tri)[val.start4] points back to this halfedge, and val.end4 + * will point to the next one. This function handles this for both triangles and + * quads. Returns {-1, -1, -1} if the edge is the interior of a quad. + */ +Manifold::Impl::BaryIndices Manifold::Impl::GetIndices(int halfedge) const { + int tri = halfedge / 3; + int idx = halfedge % 3; + const int neighbor = GetNeighbor(tri); + if (idx == neighbor) { + return {-1, -1, -1}; + } + + if (neighbor < 0) { // tri + return {tri, idx, Next3(idx)}; + } else { // quad + const int pair = halfedge_[3 * tri + neighbor].pairedHalfedge; + if (pair / 3 < tri) { + tri = pair / 3; + idx = Next3(neighbor) == idx ? 0 : 1; + } else { + idx = Next3(neighbor) == idx ? 2 : 3; + } + return {tri, idx, (idx + 1) % 4}; + } +} + +/** + * Retained verts are part of several triangles, and it doesn't matter which one + * the vertBary refers to. Here, whichever is last will win and it's done on the + * CPU for simplicity for now. Using AtomicCAS on .tri should work for a GPU + * version if desired. + */ +void Manifold::Impl::FillRetainedVerts(Vec& vertBary) const { + const int numTri = halfedge_.size() / 3; + for (int tri = 0; tri < numTri; ++tri) { + for (const int i : {0, 1, 2}) { + const BaryIndices indices = GetIndices(3 * tri + i); + if (indices.start4 < 0) continue; // skip quad interiors + vec4 uvw(0.0); + uvw[indices.start4] = 1; + vertBary[halfedge_[3 * tri + i].startVert] = {indices.tri, uvw}; + } + } +} + +/** + * Split each edge into n pieces as defined by calling the edgeDivisions + * function, and sub-triangulate each triangle accordingly. This function + * doesn't run Finish(), as that is expensive and it'll need to be run after + * the new vertices have moved, which is a likely scenario after refinement + * (smoothing). + */ +Vec Manifold::Impl::Subdivide( + std::function edgeDivisions, bool keepInterior) { + Vec edges = CreateTmpEdges(halfedge_); + const int numVert = NumVert(); + const int numEdge = edges.size(); + const int numTri = NumTri(); + Vec half2Edge(2 * numEdge); + auto policy = autoPolicy(numEdge, 1e4); + for_each_n(policy, countAt(0), numEdge, + [&half2Edge, &edges, this](const int edge) { + const int idx = edges[edge].halfedgeIdx; + half2Edge[idx] = edge; + half2Edge[halfedge_[idx].pairedHalfedge] = edge; + }); + + Vec faceHalfedges(numTri); + for_each_n(policy, countAt(0), numTri, [&faceHalfedges, this](const int tri) { + faceHalfedges[tri] = GetHalfedges(tri); + }); + + Vec edgeAdded(numEdge); + for_each_n(policy, countAt(0), numEdge, + [&edgeAdded, &edges, edgeDivisions, this](const int i) { + const TmpEdge edge = edges[i]; + const int hIdx = edge.halfedgeIdx; + if (IsMarkedInsideQuad(hIdx)) { + edgeAdded[i] = 0; + return; + } + const vec3 vec = vertPos_[edge.first] - vertPos_[edge.second]; + const vec4 tangent0 = halfedgeTangent_.empty() + ? vec4(0.0) + : halfedgeTangent_[hIdx]; + const vec4 tangent1 = + halfedgeTangent_.empty() + ? vec4(0.0) + : halfedgeTangent_[halfedge_[hIdx].pairedHalfedge]; + edgeAdded[i] = edgeDivisions(vec, tangent0, tangent1); + }); + + if (keepInterior) { + // Triangles where the greatest number of divisions exceeds the sum of the + // other two sides will be triangulated as a strip, since if the sub-edges + // were all equal length it would be degenerate. This leads to poor results + // with RefineToTolerance, so we avoid this case by adding some extra + // divisions to the short sides so that the triangulation has some thickness + // and creates more interior facets. + Vec tmp(numEdge); + for_each_n( + policy, countAt(0), numEdge, + [&tmp, &edgeAdded, &edges, &half2Edge, this](const int i) { + tmp[i] = edgeAdded[i]; + const TmpEdge edge = edges[i]; + int hIdx = edge.halfedgeIdx; + if (IsMarkedInsideQuad(hIdx)) return; + + const int thisAdded = tmp[i]; + auto Added = [&edgeAdded, &half2Edge, thisAdded, this](int hIdx) { + int longest = 0; + int total = 0; + for (int j : {0, 1, 2}) { + const int added = edgeAdded[half2Edge[hIdx]]; + longest = la::max(longest, added); + total += added; + hIdx = NextHalfedge(hIdx); + if (IsMarkedInsideQuad(hIdx)) { + // No extra on quads + longest = 0; + total = 1; + break; + } + } + const int minExtra = longest * 0.2 + 1; + const int extra = 2 * longest + minExtra - total; + return extra > 0 ? (extra * (longest - thisAdded)) / longest : 0; + }; + + tmp[i] += la::max(Added(hIdx), Added(halfedge_[hIdx].pairedHalfedge)); + }); + edgeAdded.swap(tmp); + } + + Vec edgeOffset(numEdge); + exclusive_scan(edgeAdded.begin(), edgeAdded.end(), edgeOffset.begin(), + numVert); + + Vec vertBary(edgeOffset.back() + edgeAdded.back()); + const int totalEdgeAdded = vertBary.size() - numVert; + FillRetainedVerts(vertBary); + for_each_n(policy, countAt(0), numEdge, + [&vertBary, &edges, &edgeAdded, &edgeOffset, this](const int i) { + const int n = edgeAdded[i]; + const int offset = edgeOffset[i]; + + const BaryIndices indices = GetIndices(edges[i].halfedgeIdx); + if (indices.tri < 0) { + return; // inside quad + } + const double frac = 1.0 / (n + 1); + + for (int i = 0; i < n; ++i) { + vec4 uvw(0.0); + uvw[indices.end4] = (i + 1) * frac; + uvw[indices.start4] = 1 - uvw[indices.end4]; + vertBary[offset + i].uvw = uvw; + vertBary[offset + i].tri = indices.tri; + } + }); + + std::vector subTris(numTri); + for_each_n(policy, countAt(0), numTri, + [this, &subTris, &half2Edge, &edgeAdded, &faceHalfedges](int tri) { + const ivec4 halfedges = faceHalfedges[tri]; + ivec4 divisions(0); + for (const int i : {0, 1, 2, 3}) { + if (halfedges[i] >= 0) { + divisions[i] = edgeAdded[half2Edge[halfedges[i]]] + 1; + } + } + subTris[tri] = Partition::GetPartition(divisions); + }); + + Vec triOffset(numTri); + auto numSubTris = + TransformIterator(subTris.begin(), [](const Partition& part) { + return static_cast(part.triVert.size()); + }); + manifold::exclusive_scan(numSubTris, numSubTris + numTri, triOffset.begin(), + 0); + + Vec interiorOffset(numTri); + auto numInterior = + TransformIterator(subTris.begin(), [](const Partition& part) { + return static_cast(part.NumInterior()); + }); + manifold::exclusive_scan(numInterior, numInterior + numTri, + interiorOffset.begin(), + static_cast(vertBary.size())); + + Vec triVerts(triOffset.back() + subTris.back().triVert.size()); + vertBary.resize(interiorOffset.back() + subTris.back().NumInterior()); + Vec triRef(triVerts.size()); + for_each_n( + policy, countAt(0), numTri, + [this, &triVerts, &triRef, &vertBary, &subTris, &edgeOffset, &half2Edge, + &triOffset, &interiorOffset, &faceHalfedges](int tri) { + const ivec4 halfedges = faceHalfedges[tri]; + if (halfedges[0] < 0) return; + ivec4 tri3; + ivec4 edgeOffsets; + bvec4 edgeFwd(false); + for (const int i : {0, 1, 2, 3}) { + if (halfedges[i] < 0) { + tri3[i] = -1; + continue; + } + const Halfedge& halfedge = halfedge_[halfedges[i]]; + tri3[i] = halfedge.startVert; + edgeOffsets[i] = edgeOffset[half2Edge[halfedges[i]]]; + edgeFwd[i] = halfedge.IsForward(); + } + + Vec newTris = subTris[tri].Reindex(tri3, edgeOffsets, edgeFwd, + interiorOffset[tri]); + copy(newTris.begin(), newTris.end(), triVerts.begin() + triOffset[tri]); + auto start = triRef.begin() + triOffset[tri]; + fill(start, start + newTris.size(), meshRelation_.triRef[tri]); + + const ivec4 idx = subTris[tri].idx; + const ivec4 vIdx = halfedges[3] >= 0 || idx[1] == Next3(idx[0]) + ? idx + : ivec4(idx[2], idx[0], idx[1], idx[3]); + ivec4 rIdx; + for (const int i : {0, 1, 2, 3}) { + rIdx[vIdx[i]] = i; + } + + const auto& subBary = subTris[tri].vertBary; + transform(subBary.begin() + subTris[tri].InteriorOffset(), + subBary.end(), vertBary.begin() + interiorOffset[tri], + [tri, rIdx](vec4 bary) { + return Barycentric({tri, + {bary[rIdx[0]], bary[rIdx[1]], + bary[rIdx[2]], bary[rIdx[3]]}}); + }); + }); + meshRelation_.triRef = triRef; + + Vec newVertPos(vertBary.size()); + for_each_n(policy, countAt(0), vertBary.size(), + [&newVertPos, &vertBary, &faceHalfedges, this](const int vert) { + const Barycentric bary = vertBary[vert]; + const ivec4 halfedges = faceHalfedges[bary.tri]; + if (halfedges[3] < 0) { + mat3 triPos; + for (const int i : {0, 1, 2}) { + triPos[i] = vertPos_[halfedge_[halfedges[i]].startVert]; + } + newVertPos[vert] = triPos * vec3(bary.uvw); + } else { + mat3x4 quadPos; + for (const int i : {0, 1, 2, 3}) { + quadPos[i] = vertPos_[halfedge_[halfedges[i]].startVert]; + } + newVertPos[vert] = quadPos * bary.uvw; + } + }); + vertPos_ = newVertPos; + + faceNormal_.resize(0); + + if (meshRelation_.numProp > 0) { + const int numPropVert = NumPropVert(); + const int addedVerts = NumVert() - numVert; + const int propOffset = numPropVert - numVert; + Vec prop(meshRelation_.numProp * + (numPropVert + addedVerts + totalEdgeAdded)); + + // copy retained prop verts + copy(meshRelation_.properties.begin(), meshRelation_.properties.end(), + prop.begin()); + + // copy interior prop verts and forward edge prop verts + for_each_n( + policy, countAt(0), addedVerts, + [&prop, &vertBary, &faceHalfedges, numVert, numPropVert, + this](const int i) { + const int vert = numPropVert + i; + const Barycentric bary = vertBary[numVert + i]; + const ivec4 halfedges = faceHalfedges[bary.tri]; + auto& rel = meshRelation_; + + for (int p = 0; p < rel.numProp; ++p) { + if (halfedges[3] < 0) { + vec3 triProp; + for (const int i : {0, 1, 2}) { + triProp[i] = rel.properties[rel.triProperties[bary.tri][i] * + rel.numProp + + p]; + } + prop[vert * rel.numProp + p] = la::dot(triProp, vec3(bary.uvw)); + } else { + vec4 quadProp; + for (const int i : {0, 1, 2, 3}) { + const int tri = halfedges[i] / 3; + const int j = halfedges[i] % 3; + quadProp[i] = + rel.properties[rel.triProperties[tri][j] * rel.numProp + p]; + } + prop[vert * rel.numProp + p] = la::dot(quadProp, bary.uvw); + } + } + }); + + // copy backward edge prop verts + for_each_n(policy, countAt(0), numEdge, + [this, &prop, &edges, &edgeAdded, &edgeOffset, propOffset, + addedVerts](const int i) { + const int n = edgeAdded[i]; + const int offset = edgeOffset[i] + propOffset + addedVerts; + auto& rel = meshRelation_; + + const double frac = 1.0 / (n + 1); + const int halfedgeIdx = + halfedge_[edges[i].halfedgeIdx].pairedHalfedge; + const int v0 = halfedgeIdx % 3; + const int tri = halfedgeIdx / 3; + const int prop0 = rel.triProperties[tri][v0]; + const int prop1 = rel.triProperties[tri][Next3(v0)]; + for (int i = 0; i < n; ++i) { + for (int p = 0; p < rel.numProp; ++p) { + prop[(offset + i) * rel.numProp + p] = + la::lerp(rel.properties[prop0 * rel.numProp + p], + rel.properties[prop1 * rel.numProp + p], + (i + 1) * frac); + } + } + }); + + Vec triProp(triVerts.size()); + for_each_n(policy, countAt(0), numTri, + [this, &triProp, &subTris, &edgeOffset, &half2Edge, &triOffset, + &interiorOffset, &faceHalfedges, propOffset, + addedVerts](const int tri) { + const ivec4 halfedges = faceHalfedges[tri]; + if (halfedges[0] < 0) return; + + auto& rel = meshRelation_; + ivec4 tri3; + ivec4 edgeOffsets; + bvec4 edgeFwd(true); + for (const int i : {0, 1, 2, 3}) { + if (halfedges[i] < 0) { + tri3[i] = -1; + continue; + } + const int thisTri = halfedges[i] / 3; + const int j = halfedges[i] % 3; + const Halfedge& halfedge = halfedge_[halfedges[i]]; + tri3[i] = rel.triProperties[thisTri][j]; + edgeOffsets[i] = edgeOffset[half2Edge[halfedges[i]]]; + if (!halfedge.IsForward()) { + const int pairTri = halfedge.pairedHalfedge / 3; + const int k = halfedge.pairedHalfedge % 3; + if (rel.triProperties[pairTri][k] != + rel.triProperties[thisTri][Next3(j)] || + rel.triProperties[pairTri][Next3(k)] != + rel.triProperties[thisTri][j]) { + edgeOffsets[i] += addedVerts; + } else { + edgeFwd[i] = false; + } + } + } + + Vec newTris = subTris[tri].Reindex( + tri3, edgeOffsets + propOffset, edgeFwd, + interiorOffset[tri] + propOffset); + copy(newTris.begin(), newTris.end(), + triProp.begin() + triOffset[tri]); + }); + + meshRelation_.properties = prop; + meshRelation_.triProperties = triProp; + } + + CreateHalfedges(triVerts); + + return vertBary; +} + +} // namespace manifold diff --git a/thirdparty/manifold/src/svd.h b/thirdparty/manifold/src/svd.h new file mode 100644 index 000000000000..cc1a82035f7e --- /dev/null +++ b/thirdparty/manifold/src/svd.h @@ -0,0 +1,308 @@ +// MIT License + +// Copyright (c) 2019 wi-re +// Copyright 2023 The Manifold Authors. + +// 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. + +// Modified from https://github.com/wi-re/tbtSVD, removing CUDA dependence and +// approximate inverse square roots. + +#include + +#include "manifold/common.h" + +namespace { +using manifold::mat3; +using manifold::vec3; +using manifold::vec4; + +// Constants used for calculation of Givens quaternions +inline constexpr double _gamma = 5.82842712474619; // sqrt(8)+3; +inline constexpr double _cStar = 0.9238795325112867; // cos(pi/8) +inline constexpr double _sStar = 0.3826834323650898; // sin(pi/8) +// Threshold value +inline constexpr double _SVD_EPSILON = 1e-6; +// Iteration counts for Jacobi Eigen Analysis, influences precision +inline constexpr int JACOBI_STEPS = 12; + +// Helper function used to swap X with Y and Y with X if c == true +inline void CondSwap(bool c, double& X, double& Y) { + double Z = X; + X = c ? Y : X; + Y = c ? Z : Y; +} +// Helper function used to swap X with Y and Y with -X if c == true +inline void CondNegSwap(bool c, double& X, double& Y) { + double Z = -X; + X = c ? Y : X; + Y = c ? Z : Y; +} +// A simple symmetric 3x3 Matrix class (contains no storage for (0, 1) (0, 2) +// and (1, 2) +struct Symmetric3x3 { + double m_00 = 1.0; + double m_10 = 0.0, m_11 = 1.0; + double m_20 = 0.0, m_21 = 0.0, m_22 = 1.0; + + Symmetric3x3(double a11 = 1.0, double a21 = 0.0, double a22 = 1.0, + double a31 = 0.0, double a32 = 0.0, double a33 = 1.0) + : m_00(a11), m_10(a21), m_11(a22), m_20(a31), m_21(a32), m_22(a33) {} + Symmetric3x3(mat3 o) + : m_00(o[0][0]), + m_10(o[0][1]), + m_11(o[1][1]), + m_20(o[0][2]), + m_21(o[1][2]), + m_22(o[2][2]) {} +}; +// Helper struct to store 2 doubles to avoid OUT parameters on functions +struct Givens { + double ch = _cStar; + double sh = _sStar; +}; +// Helper struct to store 2 Matrices to avoid OUT parameters on functions +struct QR { + mat3 Q, R; +}; +// Calculates the squared norm of the vector. +inline double Dist2(vec3 v) { return la::dot(v, v); } +// For an explanation of the math see +// http://pages.cs.wisc.edu/~sifakis/papers/SVD_TR1690.pdf Computing the +// Singular Value Decomposition of 3 x 3 matrices with minimal branching and +// elementary floating point operations See Algorithm 2 in reference. Given a +// matrix A this function returns the Givens quaternion (x and w component, y +// and z are 0) +inline Givens ApproximateGivensQuaternion(Symmetric3x3& A) { + Givens g{2.0 * (A.m_00 - A.m_11), A.m_10}; + bool b = _gamma * g.sh * g.sh < g.ch * g.ch; + double w = 1.0 / hypot(g.ch, g.sh); + if (!std::isfinite(w)) b = 0; + return Givens{b ? w * g.ch : _cStar, b ? w * g.sh : _sStar}; +} +// Function used to apply a Givens rotation S. Calculates the weights and +// updates the quaternion to contain the cumulative rotation +inline void JacobiConjugation(const int32_t x, const int32_t y, const int32_t z, + Symmetric3x3& S, vec4& q) { + auto g = ApproximateGivensQuaternion(S); + double scale = 1.0 / fma(g.ch, g.ch, g.sh * g.sh); + double a = fma(g.ch, g.ch, -g.sh * g.sh) * scale; + double b = 2.0 * g.sh * g.ch * scale; + Symmetric3x3 _S = S; + // perform conjugation S = Q'*S*Q + S.m_00 = + fma(a, fma(a, _S.m_00, b * _S.m_10), b * (fma(a, _S.m_10, b * _S.m_11))); + S.m_10 = fma(a, fma(-b, _S.m_00, a * _S.m_10), + b * (fma(-b, _S.m_10, a * _S.m_11))); + S.m_11 = fma(-b, fma(-b, _S.m_00, a * _S.m_10), + a * (fma(-b, _S.m_10, a * _S.m_11))); + S.m_20 = fma(a, _S.m_20, b * _S.m_21); + S.m_21 = fma(-b, _S.m_20, a * _S.m_21); + S.m_22 = _S.m_22; + // update cumulative rotation qV + vec3 tmp = g.sh * vec3(q); + g.sh *= q[3]; + // (x,y,z) corresponds to ((0,1,2),(1,2,0),(2,0,1)) for (p,q) = + // ((0,1),(1,2),(0,2)) + q[z] = fma(q[z], g.ch, g.sh); + q[3] = fma(q[3], g.ch, -tmp[z]); // w + q[x] = fma(q[x], g.ch, tmp[y]); + q[y] = fma(q[y], g.ch, -tmp[x]); + // re-arrange matrix for next iteration + _S.m_00 = S.m_11; + _S.m_10 = S.m_21; + _S.m_11 = S.m_22; + _S.m_20 = S.m_10; + _S.m_21 = S.m_20; + _S.m_22 = S.m_00; + S.m_00 = _S.m_00; + S.m_10 = _S.m_10; + S.m_11 = _S.m_11; + S.m_20 = _S.m_20; + S.m_21 = _S.m_21; + S.m_22 = _S.m_22; +} +// Function used to contain the Givens permutations and the loop of the jacobi +// steps controlled by JACOBI_STEPS Returns the quaternion q containing the +// cumulative result used to reconstruct S +inline mat3 JacobiEigenAnalysis(Symmetric3x3 S) { + vec4 q(0, 0, 0, 1); + for (int32_t i = 0; i < JACOBI_STEPS; i++) { + JacobiConjugation(0, 1, 2, S, q); + JacobiConjugation(1, 2, 0, S, q); + JacobiConjugation(2, 0, 1, S, q); + } + return mat3({1.0 - 2.0 * (fma(q.y, q.y, q.z * q.z)), // + 2.0 * fma(q.x, q.y, +q.w * q.z), // + 2.0 * fma(q.x, q.z, -q.w * q.y)}, // + {2 * fma(q.x, q.y, -q.w * q.z), // + 1 - 2 * fma(q.x, q.x, q.z * q.z), // + 2 * fma(q.y, q.z, q.w * q.x)}, // + {2 * fma(q.x, q.z, q.w * q.y), // + 2 * fma(q.y, q.z, -q.w * q.x), // + 1 - 2 * fma(q.x, q.x, q.y * q.y)}); +} +// Implementation of Algorithm 3 +inline void SortSingularValues(mat3& B, mat3& V) { + double rho1 = Dist2(B[0]); + double rho2 = Dist2(B[1]); + double rho3 = Dist2(B[2]); + bool c; + c = rho1 < rho2; + CondNegSwap(c, B[0][0], B[1][0]); + CondNegSwap(c, V[0][0], V[1][0]); + CondNegSwap(c, B[0][1], B[1][1]); + CondNegSwap(c, V[0][1], V[1][1]); + CondNegSwap(c, B[0][2], B[1][2]); + CondNegSwap(c, V[0][2], V[1][2]); + CondSwap(c, rho1, rho2); + c = rho1 < rho3; + CondNegSwap(c, B[0][0], B[2][0]); + CondNegSwap(c, V[0][0], V[2][0]); + CondNegSwap(c, B[0][1], B[2][1]); + CondNegSwap(c, V[0][1], V[2][1]); + CondNegSwap(c, B[0][2], B[2][2]); + CondNegSwap(c, V[0][2], V[2][2]); + CondSwap(c, rho1, rho3); + c = rho2 < rho3; + CondNegSwap(c, B[1][0], B[2][0]); + CondNegSwap(c, V[1][0], V[2][0]); + CondNegSwap(c, B[1][1], B[2][1]); + CondNegSwap(c, V[1][1], V[2][1]); + CondNegSwap(c, B[1][2], B[2][2]); + CondNegSwap(c, V[1][2], V[2][2]); +} +// Implementation of Algorithm 4 +inline Givens QRGivensQuaternion(double a1, double a2) { + // a1 = pivot point on diagonal + // a2 = lower triangular entry we want to annihilate + double epsilon = _SVD_EPSILON; + double rho = hypot(a1, a2); + Givens g{fabs(a1) + fmax(rho, epsilon), rho > epsilon ? a2 : 0}; + bool b = a1 < 0.0; + CondSwap(b, g.sh, g.ch); + double w = 1.0 / hypot(g.ch, g.sh); + g.ch *= w; + g.sh *= w; + return g; +} +// Implements a QR decomposition of a Matrix, see Sec 4.2 +inline QR QRDecomposition(mat3& B) { + mat3 Q, R; + // first Givens rotation (ch,0,0,sh) + auto g1 = QRGivensQuaternion(B[0][0], B[0][1]); + auto a = fma(-2.0, g1.sh * g1.sh, 1.0); + auto b = 2.0 * g1.ch * g1.sh; + // apply B = Q' * B + R[0][0] = fma(a, B[0][0], b * B[0][1]); + R[1][0] = fma(a, B[1][0], b * B[1][1]); + R[2][0] = fma(a, B[2][0], b * B[2][1]); + R[0][1] = fma(-b, B[0][0], a * B[0][1]); + R[1][1] = fma(-b, B[1][0], a * B[1][1]); + R[2][1] = fma(-b, B[2][0], a * B[2][1]); + R[0][2] = B[0][2]; + R[1][2] = B[1][2]; + R[2][2] = B[2][2]; + // second Givens rotation (ch,0,-sh,0) + auto g2 = QRGivensQuaternion(R[0][0], R[0][2]); + a = fma(-2.0, g2.sh * g2.sh, 1.0); + b = 2.0 * g2.ch * g2.sh; + // apply B = Q' * B; + B[0][0] = fma(a, R[0][0], b * R[0][2]); + B[1][0] = fma(a, R[1][0], b * R[1][2]); + B[2][0] = fma(a, R[2][0], b * R[2][2]); + B[0][1] = R[0][1]; + B[1][1] = R[1][1]; + B[2][1] = R[2][1]; + B[0][2] = fma(-b, R[0][0], a * R[0][2]); + B[1][2] = fma(-b, R[1][0], a * R[1][2]); + B[2][2] = fma(-b, R[2][0], a * R[2][2]); + // third Givens rotation (ch,sh,0,0) + auto g3 = QRGivensQuaternion(B[1][1], B[1][2]); + a = fma(-2.0, g3.sh * g3.sh, 1.0); + b = 2.0 * g3.ch * g3.sh; + // R is now set to desired value + R[0][0] = B[0][0]; + R[1][0] = B[1][0]; + R[2][0] = B[2][0]; + R[0][1] = fma(a, B[0][1], b * B[0][2]); + R[1][1] = fma(a, B[1][1], b * B[1][2]); + R[2][1] = fma(a, B[2][1], b * B[2][2]); + R[0][2] = fma(-b, B[0][1], a * B[0][2]); + R[1][2] = fma(-b, B[1][1], a * B[1][2]); + R[2][2] = fma(-b, B[2][1], a * B[2][2]); + // construct the cumulative rotation Q=Q1 * Q2 * Q3 + // the number of floating point operations for three quaternion + // multiplications is more or less comparable to the explicit form of the + // joined matrix. certainly more memory-efficient! + auto sh12 = 2.0 * fma(g1.sh, g1.sh, -0.5); + auto sh22 = 2.0 * fma(g2.sh, g2.sh, -0.5); + auto sh32 = 2.0 * fma(g3.sh, g3.sh, -0.5); + Q[0][0] = sh12 * sh22; + Q[1][0] = fma(4.0 * g2.ch * g3.ch, sh12 * g2.sh * g3.sh, + 2.0 * g1.ch * g1.sh * sh32); + Q[2][0] = fma(4.0 * g1.ch * g3.ch, g1.sh * g3.sh, + -2.0 * g2.ch * sh12 * g2.sh * sh32); + + Q[0][1] = -2.0 * g1.ch * g1.sh * sh22; + Q[1][1] = + fma(-8.0 * g1.ch * g2.ch * g3.ch, g1.sh * g2.sh * g3.sh, sh12 * sh32); + Q[2][1] = fma( + -2.0 * g3.ch, g3.sh, + 4.0 * g1.sh * fma(g3.ch * g1.sh, g3.sh, g1.ch * g2.ch * g2.sh * sh32)); + + Q[0][2] = 2.0 * g2.ch * g2.sh; + Q[1][2] = -2.0 * g3.ch * sh22 * g3.sh; + Q[2][2] = sh22 * sh32; + return QR{Q, R}; +} +} // namespace + +namespace manifold { + +/** + * The three matrices of a Singular Value Decomposition. + */ +struct SVDSet { + mat3 U, S, V; +}; + +/** + * Returns the Singular Value Decomposition of A: A = U * S * la::transpose(V). + * + * @param A The matrix to decompose. + */ +inline SVDSet SVD(mat3 A) { + mat3 V = JacobiEigenAnalysis(la::transpose(A) * A); + auto B = A * V; + SortSingularValues(B, V); + QR qr = QRDecomposition(B); + return SVDSet{qr.Q, qr.R, V}; +} + +/** + * Returns the largest singular value of A. + * + * @param A The matrix to measure. + */ +inline double SpectralNorm(mat3 A) { + SVDSet usv = SVD(A); + return usv.S[0][0]; +} +} // namespace manifold diff --git a/thirdparty/manifold/src/tri_dist.h b/thirdparty/manifold/src/tri_dist.h new file mode 100644 index 000000000000..4e83ca5253e4 --- /dev/null +++ b/thirdparty/manifold/src/tri_dist.h @@ -0,0 +1,225 @@ +// Copyright 2024 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include "manifold/common.h" + +namespace manifold { + +// From NVIDIA-Omniverse PhysX - BSD 3-Clause "New" or "Revised" License +// https://github.com/NVIDIA-Omniverse/PhysX/blob/main/LICENSE.md +// https://github.com/NVIDIA-Omniverse/PhysX/blob/main/physx/source/geomutils/src/sweep/GuSweepCapsuleCapsule.cpp +// With minor modifications + +/** + * Returns the distance between two line segments. + * + * @param[out] x Closest point on line segment pa. + * @param[out] y Closest point on line segment qb. + * @param[in] p One endpoint of the first line segment. + * @param[in] a Other endpoint of the first line segment. + * @param[in] p One endpoint of the second line segment. + * @param[in] b Other endpoint of the second line segment. + */ +inline void EdgeEdgeDist(vec3& x, vec3& y, // closest points + const vec3& p, + const vec3& a, // seg 1 origin, vector + const vec3& q, + const vec3& b) // seg 2 origin, vector +{ + const vec3 T = q - p; + const auto ADotA = la::dot(a, a); + const auto BDotB = la::dot(b, b); + const auto ADotB = la::dot(a, b); + const auto ADotT = la::dot(a, T); + const auto BDotT = la::dot(b, T); + + // t parameterizes ray (p, a) + // u parameterizes ray (q, b) + + // Compute t for the closest point on ray (p, a) to ray (q, b) + const auto Denom = ADotA * BDotB - ADotB * ADotB; + + double t; // We will clamp result so t is on the segment (p, a) + t = Denom != 0.0 + ? la::clamp((ADotT * BDotB - BDotT * ADotB) / Denom, 0.0, 1.0) + : 0.0; + + // find u for point on ray (q, b) closest to point at t + double u; + if (BDotB != 0.0) { + u = (t * ADotB - BDotT) / BDotB; + + // if u is on segment (q, b), t and u correspond to closest points, + // otherwise, clamp u, recompute and clamp t + if (u < 0.0) { + u = 0.0; + t = ADotA != 0.0 ? la::clamp(ADotT / ADotA, 0.0, 1.0) : 0.0; + } else if (u > 1.0) { + u = 1.0; + t = ADotA != 0.0 ? la::clamp((ADotB + ADotT) / ADotA, 0.0, 1.0) : 0.0; + } + } else { + u = 0.0; + t = ADotA != 0.0 ? la::clamp(ADotT / ADotA, 0.0, 1.0) : 0.0; + } + x = p + a * t; + y = q + b * u; +} + +// From NVIDIA-Omniverse PhysX - BSD 3-Clause "New" or "Revised" License +// https://github.com/NVIDIA-Omniverse/PhysX/blob/main/LICENSE.md +// https://github.com/NVIDIA-Omniverse/PhysX/blob/main/physx/source/geomutils/src/distance/GuDistanceTriangleTriangle.cpp +// With minor modifications + +/** + * Returns the minimum squared distance between two triangles. + * + * @param p First triangle. + * @param q Second triangle. + */ +inline auto DistanceTriangleTriangleSquared(const std::array& p, + const std::array& q) { + std::array Sv; + Sv[0] = p[1] - p[0]; + Sv[1] = p[2] - p[1]; + Sv[2] = p[0] - p[2]; + + std::array Tv; + Tv[0] = q[1] - q[0]; + Tv[1] = q[2] - q[1]; + Tv[2] = q[0] - q[2]; + + bool shown_disjoint = false; + + auto mindd = std::numeric_limits::max(); + + for (uint32_t i = 0; i < 3; i++) { + for (uint32_t j = 0; j < 3; j++) { + vec3 cp; + vec3 cq; + EdgeEdgeDist(cp, cq, p[i], Sv[i], q[j], Tv[j]); + const vec3 V = cq - cp; + const auto dd = la::dot(V, V); + + if (dd <= mindd) { + mindd = dd; + + uint32_t id = i + 2; + if (id >= 3) id -= 3; + vec3 Z = p[id] - cp; + auto a = la::dot(Z, V); + id = j + 2; + if (id >= 3) id -= 3; + Z = q[id] - cq; + auto b = la::dot(Z, V); + + if ((a <= 0.0) && (b >= 0.0)) { + return la::dot(V, V); + }; + + if (a <= 0.0) + a = 0.0; + else if (b > 0.0) + b = 0.0; + + if ((mindd - a + b) > 0.0) shown_disjoint = true; + } + } + } + + vec3 Sn = la::cross(Sv[0], Sv[1]); + auto Snl = la::dot(Sn, Sn); + + if (Snl > 1e-15) { + const vec3 Tp(la::dot(p[0] - q[0], Sn), la::dot(p[0] - q[1], Sn), + la::dot(p[0] - q[2], Sn)); + + int index = -1; + if ((Tp[0] > 0.0) && (Tp[1] > 0.0) && (Tp[2] > 0.0)) { + index = Tp[0] < Tp[1] ? 0 : 1; + if (Tp[2] < Tp[index]) index = 2; + } else if ((Tp[0] < 0.0) && (Tp[1] < 0.0) && (Tp[2] < 0.0)) { + index = Tp[0] > Tp[1] ? 0 : 1; + if (Tp[2] > Tp[index]) index = 2; + } + + if (index >= 0) { + shown_disjoint = true; + + const vec3& qIndex = q[index]; + + vec3 V = qIndex - p[0]; + vec3 Z = la::cross(Sn, Sv[0]); + if (la::dot(V, Z) > 0.0) { + V = qIndex - p[1]; + Z = la::cross(Sn, Sv[1]); + if (la::dot(V, Z) > 0.0) { + V = qIndex - p[2]; + Z = la::cross(Sn, Sv[2]); + if (la::dot(V, Z) > 0.0) { + vec3 cp = qIndex + Sn * Tp[index] / Snl; + vec3 cq = qIndex; + return la::dot(cp - cq, cp - cq); + } + } + } + } + } + + vec3 Tn = la::cross(Tv[0], Tv[1]); + auto Tnl = la::dot(Tn, Tn); + + if (Tnl > 1e-15) { + const vec3 Sp(la::dot(q[0] - p[0], Tn), la::dot(q[0] - p[1], Tn), + la::dot(q[0] - p[2], Tn)); + + int index = -1; + if ((Sp[0] > 0.0) && (Sp[1] > 0.0) && (Sp[2] > 0.0)) { + index = Sp[0] < Sp[1] ? 0 : 1; + if (Sp[2] < Sp[index]) index = 2; + } else if ((Sp[0] < 0.0) && (Sp[1] < 0.0) && (Sp[2] < 0.0)) { + index = Sp[0] > Sp[1] ? 0 : 1; + if (Sp[2] > Sp[index]) index = 2; + } + + if (index >= 0) { + shown_disjoint = true; + + const vec3& pIndex = p[index]; + + vec3 V = pIndex - q[0]; + vec3 Z = la::cross(Tn, Tv[0]); + if (la::dot(V, Z) > 0.0) { + V = pIndex - q[1]; + Z = la::cross(Tn, Tv[1]); + if (la::dot(V, Z) > 0.0) { + V = pIndex - q[2]; + Z = la::cross(Tn, Tv[2]); + if (la::dot(V, Z) > 0.0) { + vec3 cp = pIndex; + vec3 cq = pIndex + Tn * Sp[index] / Tnl; + return la::dot(cp - cq, cp - cq); + } + } + } + } + } + + return shown_disjoint ? mindd : 0.0; +}; +} // namespace manifold diff --git a/thirdparty/manifold/src/utils.h b/thirdparty/manifold/src/utils.h new file mode 100644 index 000000000000..12d6a584775a --- /dev/null +++ b/thirdparty/manifold/src/utils.h @@ -0,0 +1,227 @@ +// Copyright 2020 The Manifold Authors, Jared Hoberock and Nathan Bell of +// NVIDIA Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#include +#include +#include +#include + +#include "./vec.h" +#include "manifold/common.h" + +#ifndef MANIFOLD_PAR +#error "MANIFOLD_PAR must be defined to either 1 (parallel) or -1 (series)" +#else +#if (MANIFOLD_PAR != 1) && (MANIFOLD_PAR != -1) +#define XSTR(x) STR(x) +#define STR(x) #x +#pragma message "Current value of MANIFOLD_PAR is: " XSTR(MANIFOLD_PAR) +#error "MANIFOLD_PAR must be defined to either 1 (parallel) or -1 (series)" +#endif +#endif + +#include "./parallel.h" + +#if __has_include() +#include +#else +#define FrameMarkStart(x) +#define FrameMarkEnd(x) +// putting ZoneScoped in a function will instrument the function execution when +// TRACY_ENABLE is set, which allows the profiler to record more accurate +// timing. +#define ZoneScoped +#define ZoneScopedN(name) +#endif + +namespace manifold { + +/** + * Stand-in for C++23's operator""uz (P0330R8)[https://wg21.link/P0330R8]. + */ +[[nodiscard]] constexpr std::size_t operator""_uz( + unsigned long long n) noexcept { + return n; +} + +constexpr double kPrecision = 1e-12; + +inline int Next3(int i) { + constexpr ivec3 next3(1, 2, 0); + return next3[i]; +} + +inline int Prev3(int i) { + constexpr ivec3 prev3(2, 0, 1); + return prev3[i]; +} + +template +void Permute(Vec& inOut, const Vec& new2Old) { + Vec tmp(std::move(inOut)); + inOut.resize(new2Old.size()); + gather(new2Old.begin(), new2Old.end(), tmp.begin(), inOut.begin()); +} + +template +void Permute(std::vector& inOut, const Vec& new2Old) { + std::vector tmp(std::move(inOut)); + inOut.resize(new2Old.size()); + gather(new2Old.begin(), new2Old.end(), tmp.begin(), inOut.begin()); +} + +template +T AtomicAdd(T& target, T add) { + std::atomic& tar = reinterpret_cast&>(target); + T old_val = tar.load(); + while (!tar.compare_exchange_weak(old_val, old_val + add, + std::memory_order_seq_cst)) { + } + return old_val; +} + +template <> +inline int AtomicAdd(int& target, int add) { + std::atomic& tar = reinterpret_cast&>(target); + int old_val = tar.fetch_add(add, std::memory_order_seq_cst); + return old_val; +} + +template +class ConcurrentSharedPtr { + public: + ConcurrentSharedPtr(T value) : impl(std::make_shared(value)) {} + ConcurrentSharedPtr(const ConcurrentSharedPtr& other) + : impl(other.impl), mutex(other.mutex) {} + class SharedPtrGuard { + public: + SharedPtrGuard(std::recursive_mutex* mutex, T* content) + : mutex(mutex), content(content) { + mutex->lock(); + } + ~SharedPtrGuard() { mutex->unlock(); } + + T& operator*() { return *content; } + T* operator->() { return content; } + + private: + std::recursive_mutex* mutex; + T* content; + }; + SharedPtrGuard GetGuard() { return SharedPtrGuard(mutex.get(), impl.get()); }; + unsigned int UseCount() { return impl.use_count(); }; + + private: + std::shared_ptr impl; + std::shared_ptr mutex = + std::make_shared(); +}; + +template +struct UnionFind { + Vec parents; + // we do union by rank + // note that we shift rank by 1, rank 0 means it is not connected to anything + // else + Vec ranks; + + UnionFind(I numNodes) : parents(numNodes), ranks(numNodes, 0) { + sequence(parents.begin(), parents.end()); + } + + I find(I x) { + while (parents[x] != x) { + parents[x] = parents[parents[x]]; + x = parents[x]; + } + return x; + } + + void unionXY(I x, I y) { + if (x == y) return; + if (ranks[x] == 0) ranks[x] = 1; + if (ranks[y] == 0) ranks[y] = 1; + x = find(x); + y = find(y); + if (x == y) return; + if (ranks[x] < ranks[y]) std::swap(x, y); + if (ranks[x] == ranks[y]) ranks[x]++; + parents[y] = x; + } + + I connectedComponents(std::vector& components) { + components.resize(parents.size()); + I lonelyNodes = 0; + std::unordered_map toLabel; + for (size_t i = 0; i < parents.size(); ++i) { + // we optimize for connected component of size 1 + // no need to put them into the hashmap + if (ranks[i] == 0) { + components[i] = static_cast(toLabel.size()) + lonelyNodes++; + continue; + } + parents[i] = find(i); + auto iter = toLabel.find(parents[i]); + if (iter == toLabel.end()) { + I s = static_cast(toLabel.size()) + lonelyNodes; + toLabel.insert(std::make_pair(parents[i], s)); + components[i] = s; + } else { + components[i] = iter->second; + } + } + return toLabel.size() + lonelyNodes; + } +}; + +template +struct Identity { + T operator()(T v) const { return v; } +}; + +template +struct Negate { + T operator()(T v) const { return -v; } +}; + +/** + * Determines if the three points are wound counter-clockwise, clockwise, or + * colinear within the specified tolerance. + * + * @param p0 First point + * @param p1 Second point + * @param p2 Third point + * @param tol Tolerance value for colinearity + * @return int, like Signum, this returns 1 for CCW, -1 for CW, and 0 if within + * tol of colinear. + */ +inline int CCW(vec2 p0, vec2 p1, vec2 p2, double tol) { + vec2 v1 = p1 - p0; + vec2 v2 = p2 - p0; + double area = fma(v1.x, v2.y, -v1.y * v2.x); + double base2 = la::max(la::dot(v1, v1), la::dot(v2, v2)); + if (area * area * 4 <= base2 * tol * tol) + return 0; + else + return area > 0 ? 1 : -1; +} + +inline mat4 Mat4(mat3x4 a) { + return mat4({a[0], 0}, {a[1], 0}, {a[2], 0}, {a[3], 1}); +} +inline mat3 Mat3(mat2x3 a) { return mat3({a[0], 0}, {a[1], 0}, {a[2], 1}); } + +} // namespace manifold diff --git a/thirdparty/manifold/src/vec.h b/thirdparty/manifold/src/vec.h new file mode 100644 index 000000000000..b59b4b6ad54e --- /dev/null +++ b/thirdparty/manifold/src/vec.h @@ -0,0 +1,219 @@ +// Copyright 2021 The Manifold Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once +#if TRACY_ENABLE && TRACY_MEMORY_USAGE +#include "tracy/Tracy.hpp" +#else +#define TracyAllocS(ptr, size, n) (void)0 +#define TracyFreeS(ptr, n) (void)0 +#endif +#include + +#include "./parallel.h" +#include "manifold/vec_view.h" + +namespace manifold { + +template +class Vec; + +/* + * Specialized vector implementation with multithreaded fill and uninitialized + * memory optimizations. + * Note that the constructor and resize function will not perform initialization + * if the parameter val is not set. Also, this implementation is a toy + * implementation that did not consider things like non-trivial + * constructor/destructor, please keep T trivial. + */ +template +class Vec : public VecView { + public: + Vec() {} + + // Note that the vector constructed with this constructor will contain + // uninitialized memory. Please specify `val` if you need to make sure that + // the data is initialized. + Vec(size_t size) { + reserve(size); + this->size_ = size; + } + + Vec(size_t size, T val) { resize(size, val); } + + Vec(const Vec &vec) { *this = Vec(vec.view()); } + + Vec(const VecView &vec) { + this->size_ = vec.size(); + this->capacity_ = this->size_; + auto policy = autoPolicy(this->size_); + if (this->size_ != 0) { + this->ptr_ = reinterpret_cast(malloc(this->size_ * sizeof(T))); + ASSERT(this->ptr_ != nullptr, std::bad_alloc()); + TracyAllocS(this->ptr_, this->size_ * sizeof(T), 3); + copy(policy, vec.begin(), vec.end(), this->ptr_); + } + } + + Vec(const std::vector &vec) { + this->size_ = vec.size(); + this->capacity_ = this->size_; + auto policy = autoPolicy(this->size_); + if (this->size_ != 0) { + this->ptr_ = reinterpret_cast(malloc(this->size_ * sizeof(T))); + ASSERT(this->ptr_ != nullptr, std::bad_alloc()); + TracyAllocS(this->ptr_, this->size_ * sizeof(T), 3); + copy(policy, vec.begin(), vec.end(), this->ptr_); + } + } + + Vec(Vec &&vec) { + this->ptr_ = vec.ptr_; + this->size_ = vec.size_; + capacity_ = vec.capacity_; + vec.ptr_ = nullptr; + vec.size_ = 0; + vec.capacity_ = 0; + } + + operator VecView() { return {this->ptr_, this->size_}; } + operator VecView() const { return {this->ptr_, this->size_}; } + + ~Vec() { + if (this->ptr_ != nullptr) { + TracyFreeS(this->ptr_, 3); + free(this->ptr_); + } + this->ptr_ = nullptr; + this->size_ = 0; + capacity_ = 0; + } + + Vec &operator=(const Vec &other) { + if (&other == this) return *this; + if (this->ptr_ != nullptr) { + TracyFreeS(this->ptr_, 3); + free(this->ptr_); + } + this->size_ = other.size_; + capacity_ = other.size_; + if (this->size_ != 0) { + this->ptr_ = reinterpret_cast(malloc(this->size_ * sizeof(T))); + ASSERT(this->ptr_ != nullptr, std::bad_alloc()); + TracyAllocS(this->ptr_, this->size_ * sizeof(T), 3); + manifold::copy(other.begin(), other.end(), this->ptr_); + } + return *this; + } + + Vec &operator=(Vec &&other) { + if (&other == this) return *this; + if (this->ptr_ != nullptr) { + TracyFreeS(this->ptr_, 3); + free(this->ptr_); + } + this->size_ = other.size_; + capacity_ = other.capacity_; + this->ptr_ = other.ptr_; + other.ptr_ = nullptr; + other.size_ = 0; + other.capacity_ = 0; + return *this; + } + + operator VecView() const { return {this->ptr_, this->size_}; } + + void swap(Vec &other) { + std::swap(this->ptr_, other.ptr_); + std::swap(this->size_, other.size_); + std::swap(capacity_, other.capacity_); + } + + inline void push_back(const T &val, bool seq = false) { + if (this->size_ >= capacity_) { + // avoid dangling pointer in case val is a reference of our array + T val_copy = val; + reserve(capacity_ == 0 ? 128 : capacity_ * 2, seq); + this->ptr_[this->size_++] = val_copy; + return; + } + this->ptr_[this->size_++] = val; + } + + inline void extend(size_t n, bool seq = false) { + if (this->size_ + n >= capacity_) + reserve(capacity_ == 0 ? 128 : std::max(capacity_ * 2, this->size_ + n), + seq); + this->size_ += n; + } + + void reserve(size_t n, bool seq = false) { + if (n > capacity_) { + T *newBuffer = reinterpret_cast(malloc(n * sizeof(T))); + ASSERT(newBuffer != nullptr, std::bad_alloc()); + TracyAllocS(newBuffer, n * sizeof(T), 3); + if (this->size_ > 0) + manifold::copy(seq ? ExecutionPolicy::Seq : autoPolicy(this->size_), + this->ptr_, this->ptr_ + this->size_, newBuffer); + if (this->ptr_ != nullptr) { + TracyFreeS(this->ptr_, 3); + free(this->ptr_); + } + this->ptr_ = newBuffer; + capacity_ = n; + } + } + + void resize(size_t newSize, T val = T()) { + bool shrink = this->size_ > 2 * newSize; + reserve(newSize); + if (this->size_ < newSize) { + fill(autoPolicy(newSize - this->size_), this->ptr_ + this->size_, + this->ptr_ + newSize, val); + } + this->size_ = newSize; + if (shrink) shrink_to_fit(); + } + + void pop_back() { resize(this->size_ - 1); } + + void clear(bool shrink = true) { + this->size_ = 0; + if (shrink) shrink_to_fit(); + } + + void shrink_to_fit() { + T *newBuffer = nullptr; + if (this->size_ > 0) { + newBuffer = reinterpret_cast(malloc(this->size_ * sizeof(T))); + ASSERT(newBuffer != nullptr, std::bad_alloc()); + TracyAllocS(newBuffer, this->size_ * sizeof(T), 3); + manifold::copy(this->ptr_, this->ptr_ + this->size_, newBuffer); + } + if (this->ptr_ != nullptr) { + TracyFreeS(this->ptr_, 3); + free(this->ptr_); + } + this->ptr_ = newBuffer; + capacity_ = this->size_; + } + + size_t capacity() const { return capacity_; } + + private: + size_t capacity_ = 0; + + static_assert(std::is_trivially_destructible::value); +}; +} // namespace manifold diff --git a/thirdparty/mbedtls/include/mbedtls/build_info.h b/thirdparty/mbedtls/include/mbedtls/build_info.h index 8242ec68281b..d91d2964b6ae 100644 --- a/thirdparty/mbedtls/include/mbedtls/build_info.h +++ b/thirdparty/mbedtls/include/mbedtls/build_info.h @@ -26,16 +26,16 @@ */ #define MBEDTLS_VERSION_MAJOR 3 #define MBEDTLS_VERSION_MINOR 6 -#define MBEDTLS_VERSION_PATCH 1 +#define MBEDTLS_VERSION_PATCH 2 /** * The single version number has the following structure: * MMNNPP00 * Major version | Minor version | Patch version */ -#define MBEDTLS_VERSION_NUMBER 0x03060100 -#define MBEDTLS_VERSION_STRING "3.6.1" -#define MBEDTLS_VERSION_STRING_FULL "Mbed TLS 3.6.1" +#define MBEDTLS_VERSION_NUMBER 0x03060200 +#define MBEDTLS_VERSION_STRING "3.6.2" +#define MBEDTLS_VERSION_STRING_FULL "Mbed TLS 3.6.2" /* Macros for build-time platform detection */ diff --git a/thirdparty/mbedtls/library/pkwrite.c b/thirdparty/mbedtls/library/pkwrite.c index 5e009c565ea4..2a698448bee6 100644 --- a/thirdparty/mbedtls/library/pkwrite.c +++ b/thirdparty/mbedtls/library/pkwrite.c @@ -65,17 +65,21 @@ static int pk_write_rsa_der(unsigned char **p, unsigned char *buf, #if defined(MBEDTLS_USE_PSA_CRYPTO) if (mbedtls_pk_get_type(pk) == MBEDTLS_PK_OPAQUE) { uint8_t tmp[PSA_EXPORT_KEY_PAIR_MAX_SIZE]; - size_t len = 0, tmp_len = 0; + size_t tmp_len = 0; if (psa_export_key(pk->priv_id, tmp, sizeof(tmp), &tmp_len) != PSA_SUCCESS) { return MBEDTLS_ERR_PK_BAD_INPUT_DATA; } + /* Ensure there's enough space in the provided buffer before copying data into it. */ + if (tmp_len > (size_t) (*p - buf)) { + mbedtls_platform_zeroize(tmp, sizeof(tmp)); + return MBEDTLS_ERR_ASN1_BUF_TOO_SMALL; + } *p -= tmp_len; memcpy(*p, tmp, tmp_len); - len += tmp_len; mbedtls_platform_zeroize(tmp, sizeof(tmp)); - return (int) len; + return (int) tmp_len; } #endif /* MBEDTLS_USE_PSA_CRYPTO */ return mbedtls_rsa_write_key(mbedtls_pk_rsa(*pk), buf, p); @@ -125,6 +129,10 @@ static int pk_write_ec_pubkey(unsigned char **p, unsigned char *start, if (psa_export_public_key(pk->priv_id, buf, sizeof(buf), &len) != PSA_SUCCESS) { return MBEDTLS_ERR_PK_BAD_INPUT_DATA; } + /* Ensure there's enough space in the provided buffer before copying data into it. */ + if (len > (size_t) (*p - start)) { + return MBEDTLS_ERR_ASN1_BUF_TOO_SMALL; + } *p -= len; memcpy(*p, buf, len); return (int) len; diff --git a/thirdparty/miniupnpc/include/igd_desc_parse.h b/thirdparty/miniupnpc/include/miniupnpc/igd_desc_parse.h similarity index 100% rename from thirdparty/miniupnpc/include/igd_desc_parse.h rename to thirdparty/miniupnpc/include/miniupnpc/igd_desc_parse.h diff --git a/thirdparty/miniupnpc/include/miniupnpc.h b/thirdparty/miniupnpc/include/miniupnpc/miniupnpc.h similarity index 100% rename from thirdparty/miniupnpc/include/miniupnpc.h rename to thirdparty/miniupnpc/include/miniupnpc/miniupnpc.h diff --git a/thirdparty/miniupnpc/include/miniupnpc_declspec.h b/thirdparty/miniupnpc/include/miniupnpc/miniupnpc_declspec.h similarity index 100% rename from thirdparty/miniupnpc/include/miniupnpc_declspec.h rename to thirdparty/miniupnpc/include/miniupnpc/miniupnpc_declspec.h diff --git a/thirdparty/miniupnpc/include/miniupnpctypes.h b/thirdparty/miniupnpc/include/miniupnpc/miniupnpctypes.h similarity index 100% rename from thirdparty/miniupnpc/include/miniupnpctypes.h rename to thirdparty/miniupnpc/include/miniupnpc/miniupnpctypes.h diff --git a/thirdparty/miniupnpc/include/miniwget.h b/thirdparty/miniupnpc/include/miniupnpc/miniwget.h similarity index 100% rename from thirdparty/miniupnpc/include/miniwget.h rename to thirdparty/miniupnpc/include/miniupnpc/miniwget.h diff --git a/thirdparty/miniupnpc/include/portlistingparse.h b/thirdparty/miniupnpc/include/miniupnpc/portlistingparse.h similarity index 100% rename from thirdparty/miniupnpc/include/portlistingparse.h rename to thirdparty/miniupnpc/include/miniupnpc/portlistingparse.h diff --git a/thirdparty/miniupnpc/include/upnpcommands.h b/thirdparty/miniupnpc/include/miniupnpc/upnpcommands.h similarity index 100% rename from thirdparty/miniupnpc/include/upnpcommands.h rename to thirdparty/miniupnpc/include/miniupnpc/upnpcommands.h diff --git a/thirdparty/miniupnpc/include/upnpdev.h b/thirdparty/miniupnpc/include/miniupnpc/upnpdev.h similarity index 100% rename from thirdparty/miniupnpc/include/upnpdev.h rename to thirdparty/miniupnpc/include/miniupnpc/upnpdev.h diff --git a/thirdparty/miniupnpc/include/upnpreplyparse.h b/thirdparty/miniupnpc/include/miniupnpc/upnpreplyparse.h similarity index 100% rename from thirdparty/miniupnpc/include/upnpreplyparse.h rename to thirdparty/miniupnpc/include/miniupnpc/upnpreplyparse.h diff --git a/thirdparty/misc/patches/qoa-min-fix.patch b/thirdparty/misc/patches/qoa-min-fix.patch deleted file mode 100644 index 6008b5f8bcdc..000000000000 --- a/thirdparty/misc/patches/qoa-min-fix.patch +++ /dev/null @@ -1,53 +0,0 @@ -diff --git a/qoa.h b/qoa.h -index cfed266bef..23612bb0bf 100644 ---- a/qoa.h -+++ b/qoa.h -@@ -140,14 +140,14 @@ typedef struct { - #endif - } qoa_desc; - --unsigned int qoa_encode_header(qoa_desc *qoa, unsigned char *bytes); --unsigned int qoa_encode_frame(const short *sample_data, qoa_desc *qoa, unsigned int frame_len, unsigned char *bytes); --void *qoa_encode(const short *sample_data, qoa_desc *qoa, unsigned int *out_len); -+inline unsigned int qoa_encode_header(qoa_desc *qoa, unsigned char *bytes); -+inline unsigned int qoa_encode_frame(const short *sample_data, qoa_desc *qoa, unsigned int frame_len, unsigned char *bytes); -+inline void *qoa_encode(const short *sample_data, qoa_desc *qoa, unsigned int *out_len); - --unsigned int qoa_max_frame_size(qoa_desc *qoa); --unsigned int qoa_decode_header(const unsigned char *bytes, int size, qoa_desc *qoa); --unsigned int qoa_decode_frame(const unsigned char *bytes, unsigned int size, qoa_desc *qoa, short *sample_data, unsigned int *frame_len); --short *qoa_decode(const unsigned char *bytes, int size, qoa_desc *file); -+inline unsigned int qoa_max_frame_size(qoa_desc *qoa); -+inline unsigned int qoa_decode_header(const unsigned char *bytes, int size, qoa_desc *qoa); -+inline unsigned int qoa_decode_frame(const unsigned char *bytes, unsigned int size, qoa_desc *qoa, short *sample_data, unsigned int *frame_len); -+inline short *qoa_decode(const unsigned char *bytes, int size, qoa_desc *file); - - #ifndef QOA_NO_STDIO - -@@ -395,7 +395,7 @@ unsigned int qoa_encode_frame(const short *sample_data, qoa_desc *qoa, unsigned - qoa_uint64_t best_error = -1; - #endif - qoa_uint64_t best_slice = 0; -- qoa_lms_t best_lms; -+ qoa_lms_t best_lms = {}; - int best_scalefactor = 0; - - for (int sfi = 0; sfi < 16; sfi++) { -@@ -500,7 +500,7 @@ void *qoa_encode(const short *sample_data, qoa_desc *qoa, unsigned int *out_len) - num_frames * QOA_LMS_LEN * 4 * qoa->channels + /* 4 * 4 bytes lms state per channel */ - num_slices * 8 * qoa->channels; /* 8 byte slices */ - -- unsigned char *bytes = QOA_MALLOC(encoded_size); -+ unsigned char *bytes = (unsigned char *)QOA_MALLOC(encoded_size); - - for (unsigned int c = 0; c < qoa->channels; c++) { - /* Set the initial LMS weights to {0, 0, -1, 2}. This helps with the -@@ -655,7 +655,7 @@ short *qoa_decode(const unsigned char *bytes, int size, qoa_desc *qoa) { - - /* Calculate the required size of the sample buffer and allocate */ - int total_samples = qoa->samples * qoa->channels; -- short *sample_data = QOA_MALLOC(total_samples * sizeof(short)); -+ short *sample_data = (short *)QOA_MALLOC(total_samples * sizeof(short)); - - unsigned int sample_index = 0; - unsigned int frame_len; diff --git a/thirdparty/misc/qoa.c b/thirdparty/misc/qoa.c new file mode 100644 index 000000000000..7f7d366dfa1c --- /dev/null +++ b/thirdparty/misc/qoa.c @@ -0,0 +1,4 @@ +#define QOA_IMPLEMENTATION +#define QOA_NO_STDIO + +#include "qoa.h" diff --git a/thirdparty/misc/qoa.h b/thirdparty/misc/qoa.h index 23612bb0bfb4..f0f44214d813 100644 --- a/thirdparty/misc/qoa.h +++ b/thirdparty/misc/qoa.h @@ -140,14 +140,14 @@ typedef struct { #endif } qoa_desc; -inline unsigned int qoa_encode_header(qoa_desc *qoa, unsigned char *bytes); -inline unsigned int qoa_encode_frame(const short *sample_data, qoa_desc *qoa, unsigned int frame_len, unsigned char *bytes); -inline void *qoa_encode(const short *sample_data, qoa_desc *qoa, unsigned int *out_len); +unsigned int qoa_encode_header(qoa_desc *qoa, unsigned char *bytes); +unsigned int qoa_encode_frame(const short *sample_data, qoa_desc *qoa, unsigned int frame_len, unsigned char *bytes); +void *qoa_encode(const short *sample_data, qoa_desc *qoa, unsigned int *out_len); -inline unsigned int qoa_max_frame_size(qoa_desc *qoa); -inline unsigned int qoa_decode_header(const unsigned char *bytes, int size, qoa_desc *qoa); -inline unsigned int qoa_decode_frame(const unsigned char *bytes, unsigned int size, qoa_desc *qoa, short *sample_data, unsigned int *frame_len); -inline short *qoa_decode(const unsigned char *bytes, int size, qoa_desc *file); +unsigned int qoa_max_frame_size(qoa_desc *qoa); +unsigned int qoa_decode_header(const unsigned char *bytes, int size, qoa_desc *qoa); +unsigned int qoa_decode_frame(const unsigned char *bytes, unsigned int size, qoa_desc *qoa, short *sample_data, unsigned int *frame_len); +short *qoa_decode(const unsigned char *bytes, int size, qoa_desc *file); #ifndef QOA_NO_STDIO @@ -395,7 +395,7 @@ unsigned int qoa_encode_frame(const short *sample_data, qoa_desc *qoa, unsigned qoa_uint64_t best_error = -1; #endif qoa_uint64_t best_slice = 0; - qoa_lms_t best_lms = {}; + qoa_lms_t best_lms; int best_scalefactor = 0; for (int sfi = 0; sfi < 16; sfi++) { @@ -500,7 +500,7 @@ void *qoa_encode(const short *sample_data, qoa_desc *qoa, unsigned int *out_len) num_frames * QOA_LMS_LEN * 4 * qoa->channels + /* 4 * 4 bytes lms state per channel */ num_slices * 8 * qoa->channels; /* 8 byte slices */ - unsigned char *bytes = (unsigned char *)QOA_MALLOC(encoded_size); + unsigned char *bytes = QOA_MALLOC(encoded_size); for (unsigned int c = 0; c < qoa->channels; c++) { /* Set the initial LMS weights to {0, 0, -1, 2}. This helps with the @@ -626,12 +626,14 @@ unsigned int qoa_decode_frame(const unsigned char *bytes, unsigned int size, qoa qoa_uint64_t slice = qoa_read_u64(bytes, &p); int scalefactor = (slice >> 60) & 0xf; + slice <<= 4; + int slice_start = sample_index * channels + c; int slice_end = qoa_clamp(sample_index + QOA_SLICE_LEN, 0, samples) * channels + c; for (int si = slice_start; si < slice_end; si += channels) { int predicted = qoa_lms_predict(&qoa->lms[c]); - int quantized = (slice >> 57) & 0x7; + int quantized = (slice >> 61) & 0x7; int dequantized = qoa_dequant_tab[scalefactor][quantized]; int reconstructed = qoa_clamp_s16(predicted + dequantized); @@ -655,7 +657,7 @@ short *qoa_decode(const unsigned char *bytes, int size, qoa_desc *qoa) { /* Calculate the required size of the sample buffer and allocate */ int total_samples = qoa->samples * qoa->channels; - short *sample_data = (short *)QOA_MALLOC(total_samples * sizeof(short)); + short *sample_data = QOA_MALLOC(total_samples * sizeof(short)); unsigned int sample_index = 0; unsigned int frame_len; diff --git a/thirdparty/thorvg/AUTHORS b/thirdparty/thorvg/AUTHORS index e00e91a6967b..a15f3262a82b 100644 --- a/thirdparty/thorvg/AUTHORS +++ b/thirdparty/thorvg/AUTHORS @@ -28,6 +28,10 @@ Nattu Adnan Gabor Kiss-Vamosi Lorcán Mc Donagh Lucas Niu -Francisco Ramírez +Francisco Ramírez Abdelrahman Ashraf Neo Xu +Thaddeus Crews +Josh Soref +Elliott Sales de Andrade +Łukasz Pomietło diff --git a/thirdparty/thorvg/inc/config.h b/thirdparty/thorvg/inc/config.h index fc2faca29f95..6df6f52d0476 100644 --- a/thirdparty/thorvg/inc/config.h +++ b/thirdparty/thorvg/inc/config.h @@ -15,5 +15,5 @@ // For internal debugging: //#define THORVG_LOG_ENABLED -#define THORVG_VERSION_STRING "0.14.10" +#define THORVG_VERSION_STRING "0.15.5" #endif diff --git a/thirdparty/thorvg/inc/thorvg.h b/thirdparty/thorvg/inc/thorvg.h index 4303092a5ea7..1ee898ca6ff2 100644 --- a/thirdparty/thorvg/inc/thorvg.h +++ b/thirdparty/thorvg/inc/thorvg.h @@ -157,7 +157,7 @@ enum class FillRule enum class CompositeMethod { None = 0, ///< No composition is applied. - ClipPath, ///< The intersection of the source and the target is determined and only the resulting pixels from the source are rendered. Note that ClipPath only supports the Shape type. + ClipPath, ///< The intersection of the source and the target is determined and only the resulting pixels from the source are rendered. Note that ClipPath only supports the Shape type. @deprecated Use Paint::clip() instead. AlphaMask, ///< Alpha Masking using the compositing target's pixels as an alpha value. InvAlphaMask, ///< Alpha Masking using the complement to the compositing target's pixels as an alpha value. LumaMask, ///< Alpha Masking using the grayscale (0.2125R + 0.7154G + 0.0721*B) of the compositing target's pixels. @since 0.9 @@ -178,24 +178,46 @@ enum class CompositeMethod * * @see Paint::blend() * - * @note Experimental API + * @since 0.15 */ enum class BlendMethod : uint8_t { Normal = 0, ///< Perform the alpha blending(default). S if (Sa == 255), otherwise (Sa * S) + (255 - Sa) * D - Add, ///< Simply adds pixel values of one layer with the other. (S + D) - Screen, ///< The values of the pixels in the two layers are inverted, multiplied, and then inverted again. (S + D) - (S * D) Multiply, ///< Takes the RGB channel values from 0 to 255 of each pixel in the top layer and multiples them with the values for the corresponding pixel from the bottom layer. (S * D) + Screen, ///< The values of the pixels in the two layers are inverted, multiplied, and then inverted again. (S + D) - (S * D) Overlay, ///< Combines Multiply and Screen blend modes. (2 * S * D) if (2 * D < Da), otherwise (Sa * Da) - 2 * (Da - S) * (Sa - D) - Difference, ///< Subtracts the bottom layer from the top layer or the other way around, to always get a non-negative value. (S - D) if (S > D), otherwise (D - S) - Exclusion, ///< The result is twice the product of the top and bottom layers, subtracted from their sum. s + d - (2 * s * d) - SrcOver, ///< Replace the bottom layer with the top layer. Darken, ///< Creates a pixel that retains the smallest components of the top and bottom layer pixels. min(S, D) Lighten, ///< Only has the opposite action of Darken Only. max(S, D) ColorDodge, ///< Divides the bottom layer by the inverted top layer. D / (255 - S) ColorBurn, ///< Divides the inverted bottom layer by the top layer, and then inverts the result. 255 - (255 - D) / S HardLight, ///< The same as Overlay but with the color roles reversed. (2 * S * D) if (S < Sa), otherwise (Sa * Da) - 2 * (Da - S) * (Sa - D) - SoftLight ///< The same as Overlay but with applying pure black or white does not result in pure black or white. (1 - 2 * S) * (D ^ 2) + (2 * S * D) + SoftLight, ///< The same as Overlay but with applying pure black or white does not result in pure black or white. (1 - 2 * S) * (D ^ 2) + (2 * S * D) + Difference, ///< Subtracts the bottom layer from the top layer or the other way around, to always get a non-negative value. (S - D) if (S > D), otherwise (D - S) + Exclusion, ///< The result is twice the product of the top and bottom layers, subtracted from their sum. s + d - (2 * s * d) + Hue, ///< Reserved. Not supported. + Saturation, ///< Reserved. Not supported. + Color, ///< Reserved. Not supported. + Luminosity, ///< Reserved. Not supported. + Add, ///< Simply adds pixel values of one layer with the other. (S + D) + HardMix ///< Reserved. Not supported. +}; + + +/** + * @brief Enumeration that defines methods used for Scene Effects. + * + * This enum provides options to apply various post-processing effects to a scene. + * Scene effects are typically applied to modify the final appearance of a rendered scene, such as blurring. + * + * @see Scene::push(SceneEffect effect, ...) + * + * @note Experimental API + */ +enum class SceneEffect : uint8_t +{ + ClearAll = 0, ///< Reset all previously applied scene effects, restoring the scene to its original state. + GaussianBlur, ///< Apply a blur effect with a Gaussian filter. Param(3) = {sigma(float)[> 0], direction(int)[both: 0 / horizontal: 1 / vertical: 2], border(int)[duplicate: 0 / wrap: 1], quality(int)[0 - 100]} + DropShadow ///< Apply a drop shadow effect with a Gaussian Blur filter. Param(8) = {color_R(int)[0 - 255], color_G(int)[0 - 255], color_B(int)[0 - 255], opacity(int)[0 - 255], angle(float)[0 - 360], distance(float), blur_sigma(float)[> 0], quality(int)[0 - 100]} }; @@ -206,7 +228,29 @@ enum class CanvasEngine { Sw = (1 << 1), ///< CPU rasterizer. Gl = (1 << 2), ///< OpenGL rasterizer. - Wg = (1 << 3), ///< WebGPU rasterizer. (Experimental API) + Wg = (1 << 3), ///< WebGPU rasterizer. @since 0.15 +}; + + +/** + * @brief Enumeration specifying the ThorVG class type value. + * + * ThorVG's drawing objects can return class type values, allowing you to identify the specific class of each object. + * + * @see Paint::type() + * @see Fill::type() + * + * @note Experimental API + */ +enum class Type : uint8_t +{ + Undefined = 0, ///< Unkown class + Shape, ///< Shape class + Scene, ///< Scene class + Picture, ///< Picture class + Text, ///< Text class + LinearGradient = 10, ///< LinearGradient class + RadialGradient ///< RadialGradient class }; @@ -274,7 +318,7 @@ class TVG_API Paint /** * @brief Sets the values by which the object is moved in a two-dimensional space. * - * The origin of the coordinate system is in the upper left corner of the canvas. + * The origin of the coordinate system is in the upper-left corner of the canvas. * The horizontal and vertical axes point to the right and down, respectively. * * @param[in] x The value of the horizontal shift. @@ -312,7 +356,6 @@ class TVG_API Paint * @param[in] o The opacity value in the range [0 ~ 255], where 0 is completely transparent and 255 is opaque. * * @note Setting the opacity with this API may require multiple render pass for composition. It is recommended to avoid changing the opacity if possible. - * @note ClipPath won't use the opacity value. (see: enum class CompositeMethod::ClipPath) */ Result opacity(uint8_t o) noexcept; @@ -324,6 +367,20 @@ class TVG_API Paint */ Result composite(std::unique_ptr target, CompositeMethod method) noexcept; + /** + * @brief Clip the drawing region of the paint object. + * + * This function restricts the drawing area of the paint object to the specified shape's paths. + * + * @param[in] clipper The shape object as the clipper. + * + * @retval Result::NonSupport If the @p clipper type is not Shape. + * + * @note @p clipper only supports the Shape type. + * @note Experimental API + */ + Result clip(std::unique_ptr clipper) noexcept; + /** * @brief Sets the blending method for the paint object. * @@ -386,22 +443,15 @@ class TVG_API Paint CompositeMethod composite(const Paint** target) const noexcept; /** - * @brief Retrieves the current blending method applied to the paint object. + * @brief Returns the ID value of this class. * - * @return The currently set blending method. + * This method can be used to check the current concrete instance type. * - * @note Experimental API - */ - BlendMethod blend() const noexcept; - - /** - * @brief Return the unique id value of the paint instance. - * - * This method can be called for checking the current concrete instance type. + * @return The class type ID of the Paint instance. * - * @return The type id of the Paint instance. + * @since Experimental API */ - uint32_t identifier() const noexcept; + virtual Type type() const noexcept = 0; /** * @brief Unique ID of this instance. @@ -412,6 +462,11 @@ class TVG_API Paint */ uint32_t id = 0; + /** + * @see Paint::type() + */ + TVG_DEPRECATED uint32_t identifier() const noexcept; + _TVG_DECLARE_PRIVATE(Paint); }; @@ -503,13 +558,20 @@ class TVG_API Fill Fill* duplicate() const noexcept; /** - * @brief Return the unique id value of the Fill instance. + * @brief Returns the ID value of this class. + * + * This method can be used to check the current concrete instance type. * - * This method can be called for checking the current concrete instance type. + * @return The class type ID of the Fill instance. * - * @return The type id of the Fill instance. + * @since Experimental API + */ + virtual Type type() const noexcept = 0; + + /** + * @see Fill::type() */ - uint32_t identifier() const noexcept; + TVG_DEPRECATED uint32_t identifier() const noexcept; _TVG_DECLARE_PRIVATE(Fill); }; @@ -538,7 +600,7 @@ class TVG_API Canvas * * This function provides the list of paint nodes, allowing users a direct opportunity to modify the scene tree. * - * @warning Please avoid accessing the paints during Canvas update/draw. You can access them after calling sync(). + * @warning Please avoid accessing the paints during Canvas update/draw. You can access them after calling sync(). * @see Canvas::sync() * * @note Experimental API @@ -614,7 +676,7 @@ class TVG_API Canvas * @warning It's not allowed to change the viewport during Canvas::push() - Canvas::sync() or Canvas::update() - Canvas::sync(). * * @note When resetting the target, the viewport will also be reset to the target size. - * @note Experimental API + * @since 0.15 */ virtual Result viewport(int32_t x, int32_t y, int32_t w, int32_t h) noexcept; @@ -686,13 +748,20 @@ class TVG_API LinearGradient final : public Fill static std::unique_ptr gen() noexcept; /** - * @brief Return the unique id value of this class. + * @brief Returns the ID value of this class. * - * This method can be referred for identifying the LinearGradient class type. + * This method can be used to check the current concrete instance type. * - * @return The type id of the LinearGradient class. + * @return The class type ID of the LinearGradient instance. + * + * @since Experimental API + */ + Type type() const noexcept override; + + /** + * @see LinearGradient::type() */ - static uint32_t identifier() noexcept; + TVG_DEPRECATED static uint32_t identifier() noexcept; _TVG_DECLARE_PRIVATE(LinearGradient); }; @@ -744,13 +813,20 @@ class TVG_API RadialGradient final : public Fill static std::unique_ptr gen() noexcept; /** - * @brief Return the unique id value of this class. + * @brief Returns the ID value of this class. * - * This method can be referred for identifying the RadialGradient class type. + * This method can be used to check the current concrete instance type. * - * @return The type id of the RadialGradient class. + * @return The class type ID of the LinearGradient instance. + * + * @since Experimental API + */ + Type type() const noexcept override; + + /** + * @see RadialGradient::type() */ - static uint32_t identifier() noexcept; + TVG_DEPRECATED static uint32_t identifier() noexcept; _TVG_DECLARE_PRIVATE(RadialGradient); }; @@ -774,11 +850,11 @@ class TVG_API Shape final : public Paint ~Shape(); /** - * @brief Resets the properties of the shape path. + * @brief Resets the shape path. * - * The transformation matrix, the color, the fill and the stroke properties are retained. + * The transformation matrix, color, fill, and stroke properties are retained. * - * @note The memory, where the path data is stored, is not deallocated at this stage for caching effect. + * @note The memory where the path data is stored is not deallocated at this stage to allow for caching. */ Result reset() noexcept; @@ -836,15 +912,15 @@ class TVG_API Shape final : public Paint * The rectangle with rounded corners can be achieved by setting non-zero values to @p rx and @p ry arguments. * The @p rx and @p ry values specify the radii of the ellipse defining the rounding of the corners. * - * The position of the rectangle is specified by the coordinates of its upper left corner - @p x and @p y arguments. + * The position of the rectangle is specified by the coordinates of its upper-left corner - @p x and @p y arguments. * * The rectangle is treated as a new sub-path - it is not connected with the previous sub-path. * * The value of the current point is set to (@p x + @p rx, @p y) - in case @p rx is greater * than @p w/2 the current point is set to (@p x + @p w/2, @p y) * - * @param[in] x The horizontal coordinate of the upper left corner of the rectangle. - * @param[in] y The vertical coordinate of the upper left corner of the rectangle. + * @param[in] x The horizontal coordinate of the upper-left corner of the rectangle. + * @param[in] y The vertical coordinate of the upper-left corner of the rectangle. * @param[in] w The width of the rectangle. * @param[in] h The height of the rectangle. * @param[in] rx The x-axis radius of the ellipse defining the rounded corners of the rectangle. @@ -886,7 +962,7 @@ class TVG_API Shape final : public Paint * * @note Setting @p sweep value greater than 360 degrees, is equivalent to calling appendCircle(cx, cy, radius, radius). */ - Result appendArc(float cx, float cy, float radius, float startAngle, float sweep, bool pie) noexcept; + TVG_DEPRECATED Result appendArc(float cx, float cy, float radius, float startAngle, float sweep, bool pie) noexcept; /** * @brief Appends a given sub-path to the path. @@ -999,7 +1075,6 @@ class TVG_API Shape final : public Paint * @param[in] a The alpha channel value in the range [0 ~ 255], where 0 is completely transparent and 255 is opaque. The default value is 0. * * @note Either a solid color or a gradient fill is applied, depending on what was set as last. - * @note ClipPath won't use the fill values. (see: enum class CompositeMethod::ClipPath) */ Result fill(uint8_t r, uint8_t g, uint8_t b, uint8_t a = 255) noexcept; @@ -1130,18 +1205,6 @@ class TVG_API Shape final : public Paint */ float strokeMiterlimit() const noexcept; - /** - * @brief Gets the trim of the stroke along the defined path segment. - * - * @param[out] begin The starting point of the segment to display along the path. - * @param[out] end Specifies the end of the segment to display along the path. - * - * @return @c true if trimming is applied simultaneously to all paths of the shape, @c false otherwise. - * - * @note Experimental API - */ - bool strokeTrim(float* begin, float* end) const noexcept; - /** * @brief Creates a new Shape object. * @@ -1150,13 +1213,20 @@ class TVG_API Shape final : public Paint static std::unique_ptr gen() noexcept; /** - * @brief Return the unique id value of this class. + * @brief Returns the ID value of this class. * - * This method can be referred for identifying the Shape class type. + * This method can be used to check the current concrete instance type. * - * @return The type id of the Shape class. + * @return The class type ID of the Shape instance. + * + * @since Experimental API */ - static uint32_t identifier() noexcept; + Type type() const noexcept override; + + /** + * @see Shape::type() + */ + TVG_DEPRECATED static uint32_t identifier() noexcept; _TVG_DECLARE_PRIVATE(Shape); }; @@ -1213,7 +1283,7 @@ class TVG_API Picture final : public Paint * @retval Result::InvalidArguments In case no data are provided or the @p size is zero or less. * @retval Result::NonSupport When trying to load a file with an unknown extension. * - * @warning: It's the user responsibility to release the @p data memory. + * @warning It's the user responsibility to release the @p data memory. * * @note If you are unsure about the MIME type, you can provide an empty value like @c "", and thorvg will attempt to figure it out. * @since 0.5 @@ -1251,9 +1321,9 @@ class TVG_API Picture final : public Paint * @param[in] data A pointer to a memory location where the content of the picture raw data is stored. * @param[in] w The width of the image @p data in pixels. * @param[in] h The height of the image @p data in pixels. + * @param[in] premultiplied If @c true, the given image data is alpha-premultiplied. * @param[in] copy If @c true the data are copied into the engine local buffer, otherwise they are not. * - * @note It expects premultiplied alpha data. * @since 0.9 */ Result load(uint32_t* data, uint32_t w, uint32_t h, bool copy) noexcept; @@ -1281,13 +1351,20 @@ class TVG_API Picture final : public Paint static std::unique_ptr gen() noexcept; /** - * @brief Return the unique id value of this class. + * @brief Returns the ID value of this class. * - * This method can be referred for identifying the Picture class type. + * This method can be used to check the current concrete instance type. * - * @return The type id of the Picture class. + * @return The class type ID of the Picture instance. + * + * @since Experimental API + */ + Type type() const noexcept override; + + /** + * @see Picture::type() */ - static uint32_t identifier() noexcept; + TVG_DEPRECATED static uint32_t identifier() noexcept; _TVG_DECLARE_ACCESSOR(Animation); _TVG_DECLARE_PRIVATE(Picture); @@ -1331,9 +1408,9 @@ class TVG_API Scene final : public Paint * * This function provides the list of paint nodes, allowing users a direct opportunity to modify the scene tree. * - * @warning Please avoid accessing the paints during Scene update/draw. You can access them after calling Canvas::sync(). + * @warning Please avoid accessing the paints during Scene update/draw. You can access them after calling Canvas::sync(). * @see Canvas::sync() - * @see Scene::push() + * @see Scene::push(std::unique_ptr paint) * @see Scene::clear() * * @note Experimental API @@ -1352,6 +1429,20 @@ class TVG_API Scene final : public Paint */ Result clear(bool free = true) noexcept; + /** + * @brief Apply a post-processing effect to the scene. + * + * This function adds a specified scene effect, such as clearing all effects or applying a Gaussian blur, + * to the scene after it has been rendered. Multiple effects can be applied in sequence. + * + * @param[in] effect The scene effect to apply. Options are defined in the SceneEffect enum. + * For example, use SceneEffect::GaussianBlur to apply a blur with specific parameters. + * @param[in] ... Additional variadic parameters required for certain effects (e.g., sigma and direction for GaussianBlur). + * + * @note Experimental API + */ + Result push(SceneEffect effect, ...) noexcept; + /** * @brief Creates a new Scene object. * @@ -1360,13 +1451,20 @@ class TVG_API Scene final : public Paint static std::unique_ptr gen() noexcept; /** - * @brief Return the unique id value of this class. + * @brief Returns the ID value of this class. * - * This method can be referred for identifying the Scene class type. + * This method can be used to check the current concrete instance type. * - * @return The type id of the Scene class. + * @return The class type ID of the Scene instance. + * + * @since Experimental API */ - static uint32_t identifier() noexcept; + Type type() const noexcept override; + + /** + * @see Scene::type() + */ + TVG_DEPRECATED static uint32_t identifier() noexcept; _TVG_DECLARE_PRIVATE(Scene); }; @@ -1377,7 +1475,7 @@ class TVG_API Scene final : public Paint * * @brief A class to represent text objects in a graphical context, allowing for rendering and manipulation of unicode text. * - * @note Experimental API + * @since 0.15 */ class TVG_API Text final : public Paint { @@ -1422,7 +1520,7 @@ class TVG_API Text final : public Paint * * @see Text::font() * - * @note Experimental API + * @since 0.15 */ Result fill(uint8_t r, uint8_t g, uint8_t b) noexcept; @@ -1434,9 +1532,9 @@ class TVG_API Text final : public Paint * @param[in] f The unique pointer to the gradient fill. * * @note Either a solid color or a gradient fill is applied, depending on what was set as last. - * @note Experimental API - * * @see Text::font() + * + * @since 0.15 */ Result fill(std::unique_ptr f) noexcept; @@ -1452,9 +1550,9 @@ class TVG_API Text final : public Paint * @retval Result::InvalidArguments In case the @p path is invalid. * @retval Result::NonSupport When trying to load a file with an unknown extension. * - * @note Experimental API - * * @see Text::unload(const std::string& path) + * + * @since 0.15 */ static Result load(const std::string& path) noexcept; @@ -1475,13 +1573,13 @@ class TVG_API Text final : public Paint * @retval Result::NonSupport When trying to load a file with an unsupported extension. * @retval Result::InsufficientCondition If attempting to unload the font data that has not been previously loaded. * - * @warning: It's the user responsibility to release the @p data memory. + * @warning It's the user responsibility to release the @p data memory. * * @note To unload the font data loaded using this API, pass the proper @p name and @c nullptr as @p data. * @note If you are unsure about the MIME type, you can provide an empty value like @c "", and thorvg will attempt to figure it out. - * @note Experimental API - * * @see Text::font(const char* name, float size, const char* style) + * + * @note 0.15 */ static Result load(const char* name, const char* data, uint32_t size, const std::string& mimeType = "ttf", bool copy = false) noexcept; @@ -1495,9 +1593,9 @@ class TVG_API Text final : public Paint * @retval Result::InsufficientCondition Fails if the loader is not initialized. * * @note If the font data is currently in use, it will not be immediately unloaded. - * @note Experimental API - * * @see Text::load(const std::string& path) + * + * @since 0.15 */ static Result unload(const std::string& path) noexcept; @@ -1506,18 +1604,20 @@ class TVG_API Text final : public Paint * * @return A new Text object. * - * @note Experimental API + * @since 0.15 */ static std::unique_ptr gen() noexcept; /** - * @brief Return the unique id value of this class. + * @brief Returns the ID value of this class. + * + * This method can be used to check the current concrete instance type. * - * This method can be referred for identifying the Text class type. + * @return The class type ID of the Text instance. * - * @return The type id of the Text class. + * @since Experimental API */ - static uint32_t identifier() noexcept; + Type type() const noexcept override; _TVG_DECLARE_PRIVATE(Text); }; @@ -1616,8 +1716,6 @@ class TVG_API SwCanvas final : public Canvas * * @brief A class for the rendering graphic elements with a GL raster engine. * - * @warning Please do not use it. This class is not fully supported yet. - * * @since 0.14 */ class TVG_API GlCanvas final : public Canvas @@ -1666,7 +1764,7 @@ class TVG_API GlCanvas final : public Canvas * * @warning Please do not use it. This class is not fully supported yet. * - * @note Experimental API + * @since 0.15 */ class TVG_API WgCanvas final : public Canvas { @@ -1680,6 +1778,7 @@ class TVG_API WgCanvas final : public Canvas * @param[in] surface WGPUSurface, handle to a presentable surface. * @param[in] w The width of the surface. * @param[in] h The height of the surface. + * @param[in] device WGPUDevice, a desired handle for the wgpu device. If it is @c nullptr, ThorVG will assign an appropriate device internally. * * @retval Result::InsufficientCondition if the canvas is performing rendering. Please ensure the canvas is synced. * @retval Result::NonSupport In case the wg engine is not supported. @@ -1689,14 +1788,14 @@ class TVG_API WgCanvas final : public Canvas * @see Canvas::viewport() * @see Canvas::sync() */ - Result target(void* instance, void* surface, uint32_t w, uint32_t h) noexcept; + Result target(void* instance, void* surface, uint32_t w, uint32_t h, void* device = nullptr) noexcept; /** * @brief Creates a new WgCanvas object. * * @return A new WgCanvas object. * - * @note Experimental API + * @since 0.15 */ static std::unique_ptr gen() noexcept; @@ -1752,7 +1851,7 @@ class TVG_API Initializer final * * @return The version of the engine in the format major.minor.micro, or a @p nullptr in case of an internal error. * - * @note Experimental API + * @since 0.15 */ static const char* version(uint32_t* major, uint32_t* minor, uint32_t* micro) noexcept; @@ -1857,6 +1956,7 @@ class TVG_API Animation * @note Animation allows a range from 0.0 to 1.0. @p end should not be higher than @p begin. * @note If a marker has been specified, its range will be disregarded. * @see LottieAnimation::segment(const char* marker) + * * @note Experimental API */ Result segment(float begin, float end) noexcept; @@ -1895,7 +1995,7 @@ class TVG_API Animation * It's useful when you need to save the composed scene or image from a paint object and recreate it later. * * The file format is decided by the extension name(i.e. "*.tvg") while the supported formats depend on the TVG packaging environment. - * If it doesn't support the file format, the save() method returns the @c Result::NonSuppport result. + * If it doesn't support the file format, the save() method returns the @c Result::NonSupport result. * * Once you export a paint to the file successfully, you can recreate it using the Picture class. * diff --git a/thirdparty/thorvg/patches/pr2740-renderer-crash-hotfix.patch b/thirdparty/thorvg/patches/pr2740-renderer-crash-hotfix.patch deleted file mode 100644 index 50b1e1e4a77f..000000000000 --- a/thirdparty/thorvg/patches/pr2740-renderer-crash-hotfix.patch +++ /dev/null @@ -1,45 +0,0 @@ -From 8009c75465e5b35da2d5f53532bc65f6df202a3a Mon Sep 17 00:00:00 2001 -From: Hermet Park -Date: Tue, 17 Sep 2024 11:35:48 +0900 -Subject: [PATCH] renderer: hotfix a crash - -prevent a nullptr memory access -regression by f5337015e971d24379d2ee664895503ab8945e13 - -issue: https://github.com/godotengine/godot/issues/97078 ---- - src/renderer/tvgShape.h | 6 ++++-- - 2 files changed, 4 insertions(+), 4 deletions(-) - -diff --git a/src/renderer/tvgShape.h b/src/renderer/tvgShape.h -index 221931dee..e120a85c6 100644 ---- a/src/renderer/tvgShape.h -+++ b/src/renderer/tvgShape.h -@@ -51,8 +51,9 @@ struct Shape::Impl - - bool render(RenderMethod* renderer) - { -+ if (!rd) return false; -+ - Compositor* cmp = nullptr; -- bool ret; - - renderer->blend(shape->blend()); - -@@ -61,7 +62,7 @@ struct Shape::Impl - renderer->beginComposite(cmp, CompositeMethod::None, opacity); - } - -- ret = renderer->renderShape(rd); -+ auto ret = renderer->renderShape(rd); - if (cmp) renderer->endComposite(cmp); - return ret; - } -@@ -117,6 +118,7 @@ struct Shape::Impl - - RenderRegion bounds(RenderMethod* renderer) - { -+ if (!rd) return {0, 0, 0, 0}; - return renderer->region(rd); - } - diff --git a/thirdparty/thorvg/patches/revert-tvgLines-bezier-precision-change.patch b/thirdparty/thorvg/patches/revert-tvgLines-bezier-precision-change.patch index dd6c8ba5e701..c36836eadc1b 100644 --- a/thirdparty/thorvg/patches/revert-tvgLines-bezier-precision-change.patch +++ b/thirdparty/thorvg/patches/revert-tvgLines-bezier-precision-change.patch @@ -1,10 +1,10 @@ -diff --git a/thirdparty/thorvg/src/common/tvgLines.cpp b/thirdparty/thorvg/src/common/tvgLines.cpp -index 49d992f127..9d704900a5 100644 ---- a/thirdparty/thorvg/src/common/tvgLines.cpp -+++ b/thirdparty/thorvg/src/common/tvgLines.cpp +diff --git a/thirdparty/thorvg/src/common/tvgMath.cpp b/thirdparty/thorvg/src/common/tvgMath.cpp +index cb7f24ff40..f27f69faeb 100644 +--- a/thirdparty/thorvg/src/common/tvgMath.cpp ++++ b/thirdparty/thorvg/src/common/tvgMath.cpp @@ -79,7 +79,7 @@ float _bezAt(const Bezier& bz, float at, float length, LengthFunc lineLengthFunc Bezier left; - bezSplitLeft(right, t, left); + right.split(t, left); length = _bezLength(left, lineLengthFunc); - if (fabsf(length - at) < BEZIER_EPSILON || fabsf(smallest - biggest) < 1e-3f) { + if (fabsf(length - at) < BEZIER_EPSILON || fabsf(smallest - biggest) < BEZIER_EPSILON) { diff --git a/thirdparty/thorvg/src/common/tvgCompressor.cpp b/thirdparty/thorvg/src/common/tvgCompressor.cpp index aebe9a4ef164..714f21e07c9d 100644 --- a/thirdparty/thorvg/src/common/tvgCompressor.cpp +++ b/thirdparty/thorvg/src/common/tvgCompressor.cpp @@ -468,7 +468,7 @@ size_t b64Decode(const char* encoded, const size_t len, char** decoded) encoded += 4; } *decoded = output; - return reserved; + return idx; } diff --git a/thirdparty/thorvg/src/common/tvgLines.cpp b/thirdparty/thorvg/src/common/tvgLines.cpp deleted file mode 100644 index 9d704900a50e..000000000000 --- a/thirdparty/thorvg/src/common/tvgLines.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/* - * Copyright (c) 2020 - 2024 the ThorVG project. All rights reserved. - - * 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 "tvgMath.h" -#include "tvgLines.h" - -#define BEZIER_EPSILON 1e-2f - -/************************************************************************/ -/* Internal Class Implementation */ -/************************************************************************/ - -static float _lineLengthApprox(const Point& pt1, const Point& pt2) -{ - /* approximate sqrt(x*x + y*y) using alpha max plus beta min algorithm. - With alpha = 1, beta = 3/8, giving results with the largest error less - than 7% compared to the exact value. */ - Point diff = {pt2.x - pt1.x, pt2.y - pt1.y}; - if (diff.x < 0) diff.x = -diff.x; - if (diff.y < 0) diff.y = -diff.y; - return (diff.x > diff.y) ? (diff.x + diff.y * 0.375f) : (diff.y + diff.x * 0.375f); -} - - -static float _lineLength(const Point& pt1, const Point& pt2) -{ - Point diff = {pt2.x - pt1.x, pt2.y - pt1.y}; - return sqrtf(diff.x * diff.x + diff.y * diff.y); -} - - -template -float _bezLength(const Bezier& cur, LengthFunc lineLengthFunc) -{ - Bezier left, right; - auto len = lineLengthFunc(cur.start, cur.ctrl1) + lineLengthFunc(cur.ctrl1, cur.ctrl2) + lineLengthFunc(cur.ctrl2, cur.end); - auto chord = lineLengthFunc(cur.start, cur.end); - - if (fabsf(len - chord) > BEZIER_EPSILON) { - tvg::bezSplit(cur, left, right); - return _bezLength(left, lineLengthFunc) + _bezLength(right, lineLengthFunc); - } - return len; -} - - -template -float _bezAt(const Bezier& bz, float at, float length, LengthFunc lineLengthFunc) -{ - auto biggest = 1.0f; - auto smallest = 0.0f; - auto t = 0.5f; - - //just in case to prevent an infinite loop - if (at <= 0) return 0.0f; - if (at >= length) return 1.0f; - - while (true) { - auto right = bz; - Bezier left; - bezSplitLeft(right, t, left); - length = _bezLength(left, lineLengthFunc); - if (fabsf(length - at) < BEZIER_EPSILON || fabsf(smallest - biggest) < BEZIER_EPSILON) { - break; - } - if (length < at) { - smallest = t; - t = (t + biggest) * 0.5f; - } else { - biggest = t; - t = (smallest + t) * 0.5f; - } - } - return t; -} - - -/************************************************************************/ -/* External Class Implementation */ -/************************************************************************/ - -namespace tvg -{ - -float lineLength(const Point& pt1, const Point& pt2) -{ - return _lineLength(pt1, pt2); -} - - -void lineSplitAt(const Line& cur, float at, Line& left, Line& right) -{ - auto len = lineLength(cur.pt1, cur.pt2); - auto dx = ((cur.pt2.x - cur.pt1.x) / len) * at; - auto dy = ((cur.pt2.y - cur.pt1.y) / len) * at; - left.pt1 = cur.pt1; - left.pt2.x = left.pt1.x + dx; - left.pt2.y = left.pt1.y + dy; - right.pt1 = left.pt2; - right.pt2 = cur.pt2; -} - - -void bezSplit(const Bezier& cur, Bezier& left, Bezier& right) -{ - auto c = (cur.ctrl1.x + cur.ctrl2.x) * 0.5f; - left.ctrl1.x = (cur.start.x + cur.ctrl1.x) * 0.5f; - right.ctrl2.x = (cur.ctrl2.x + cur.end.x) * 0.5f; - left.start.x = cur.start.x; - right.end.x = cur.end.x; - left.ctrl2.x = (left.ctrl1.x + c) * 0.5f; - right.ctrl1.x = (right.ctrl2.x + c) * 0.5f; - left.end.x = right.start.x = (left.ctrl2.x + right.ctrl1.x) * 0.5f; - - c = (cur.ctrl1.y + cur.ctrl2.y) * 0.5f; - left.ctrl1.y = (cur.start.y + cur.ctrl1.y) * 0.5f; - right.ctrl2.y = (cur.ctrl2.y + cur.end.y) * 0.5f; - left.start.y = cur.start.y; - right.end.y = cur.end.y; - left.ctrl2.y = (left.ctrl1.y + c) * 0.5f; - right.ctrl1.y = (right.ctrl2.y + c) * 0.5f; - left.end.y = right.start.y = (left.ctrl2.y + right.ctrl1.y) * 0.5f; -} - - -float bezLength(const Bezier& cur) -{ - return _bezLength(cur, _lineLength); -} - - -float bezLengthApprox(const Bezier& cur) -{ - return _bezLength(cur, _lineLengthApprox); -} - - -void bezSplitLeft(Bezier& cur, float at, Bezier& left) -{ - left.start = cur.start; - - left.ctrl1.x = cur.start.x + at * (cur.ctrl1.x - cur.start.x); - left.ctrl1.y = cur.start.y + at * (cur.ctrl1.y - cur.start.y); - - left.ctrl2.x = cur.ctrl1.x + at * (cur.ctrl2.x - cur.ctrl1.x); //temporary holding spot - left.ctrl2.y = cur.ctrl1.y + at * (cur.ctrl2.y - cur.ctrl1.y); //temporary holding spot - - cur.ctrl2.x = cur.ctrl2.x + at * (cur.end.x - cur.ctrl2.x); - cur.ctrl2.y = cur.ctrl2.y + at * (cur.end.y - cur.ctrl2.y); - - cur.ctrl1.x = left.ctrl2.x + at * (cur.ctrl2.x - left.ctrl2.x); - cur.ctrl1.y = left.ctrl2.y + at * (cur.ctrl2.y - left.ctrl2.y); - - left.ctrl2.x = left.ctrl1.x + at * (left.ctrl2.x - left.ctrl1.x); - left.ctrl2.y = left.ctrl1.y + at * (left.ctrl2.y - left.ctrl1.y); - - left.end.x = cur.start.x = left.ctrl2.x + at * (cur.ctrl1.x - left.ctrl2.x); - left.end.y = cur.start.y = left.ctrl2.y + at * (cur.ctrl1.y - left.ctrl2.y); -} - - -float bezAt(const Bezier& bz, float at, float length) -{ - return _bezAt(bz, at, length, _lineLength); -} - - -float bezAtApprox(const Bezier& bz, float at, float length) -{ - return _bezAt(bz, at, length, _lineLengthApprox); -} - - -void bezSplitAt(const Bezier& cur, float at, Bezier& left, Bezier& right) -{ - right = cur; - auto t = bezAt(right, at, bezLength(right)); - bezSplitLeft(right, t, left); -} - - -Point bezPointAt(const Bezier& bz, float t) -{ - Point cur; - auto it = 1.0f - t; - - auto ax = bz.start.x * it + bz.ctrl1.x * t; - auto bx = bz.ctrl1.x * it + bz.ctrl2.x * t; - auto cx = bz.ctrl2.x * it + bz.end.x * t; - ax = ax * it + bx * t; - bx = bx * it + cx * t; - cur.x = ax * it + bx * t; - - float ay = bz.start.y * it + bz.ctrl1.y * t; - float by = bz.ctrl1.y * it + bz.ctrl2.y * t; - float cy = bz.ctrl2.y * it + bz.end.y * t; - ay = ay * it + by * t; - by = by * it + cy * t; - cur.y = ay * it + by * t; - - return cur; -} - - -float bezAngleAt(const Bezier& bz, float t) -{ - if (t < 0 || t > 1) return 0; - - //derivate - // p'(t) = 3 * (-(1-2t+t^2) * p0 + (1 - 4 * t + 3 * t^2) * p1 + (2 * t - 3 * - // t^2) * p2 + t^2 * p3) - float mt = 1.0f - t; - float d = t * t; - float a = -mt * mt; - float b = 1 - 4 * t + 3 * d; - float c = 2 * t - 3 * d; - - Point pt ={a * bz.start.x + b * bz.ctrl1.x + c * bz.ctrl2.x + d * bz.end.x, a * bz.start.y + b * bz.ctrl1.y + c * bz.ctrl2.y + d * bz.end.y}; - pt.x *= 3; - pt.y *= 3; - - return mathRad2Deg(mathAtan2(pt.y, pt.x)); -} - - -} diff --git a/thirdparty/thorvg/src/common/tvgLines.h b/thirdparty/thorvg/src/common/tvgLines.h deleted file mode 100644 index d900782b5627..000000000000 --- a/thirdparty/thorvg/src/common/tvgLines.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright (c) 2020 - 2024 the ThorVG project. All rights reserved. - - * 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 _TVG_LINES_H_ -#define _TVG_LINES_H_ - -#include "tvgCommon.h" - -namespace tvg -{ - -struct Line -{ - Point pt1; - Point pt2; -}; - -float lineLength(const Point& pt1, const Point& pt2); -void lineSplitAt(const Line& cur, float at, Line& left, Line& right); - - -struct Bezier -{ - Point start; - Point ctrl1; - Point ctrl2; - Point end; -}; - -void bezSplit(const Bezier&cur, Bezier& left, Bezier& right); -float bezLength(const Bezier& cur); -void bezSplitLeft(Bezier& cur, float at, Bezier& left); -float bezAt(const Bezier& bz, float at, float length); -void bezSplitAt(const Bezier& cur, float at, Bezier& left, Bezier& right); -Point bezPointAt(const Bezier& bz, float t); -float bezAngleAt(const Bezier& bz, float t); - -float bezLengthApprox(const Bezier& cur); -float bezAtApprox(const Bezier& bz, float at, float length); -} - -#endif //_TVG_LINES_H_ diff --git a/thirdparty/thorvg/src/common/tvgMath.cpp b/thirdparty/thorvg/src/common/tvgMath.cpp index c03b54e5f8b9..f27f69faebb2 100644 --- a/thirdparty/thorvg/src/common/tvgMath.cpp +++ b/thirdparty/thorvg/src/common/tvgMath.cpp @@ -22,11 +22,88 @@ #include "tvgMath.h" -//see: https://en.wikipedia.org/wiki/Remez_algorithm -float mathAtan2(float y, float x) +#define BEZIER_EPSILON 1e-2f + + +/************************************************************************/ +/* Internal Class Implementation */ +/************************************************************************/ + +static float _lineLengthApprox(const Point& pt1, const Point& pt2) { - if (y == 0.0f && x == 0.0f) return 0.0f; + /* approximate sqrt(x*x + y*y) using alpha max plus beta min algorithm. + With alpha = 1, beta = 3/8, giving results with the largest error less + than 7% compared to the exact value. */ + Point diff = {pt2.x - pt1.x, pt2.y - pt1.y}; + if (diff.x < 0) diff.x = -diff.x; + if (diff.y < 0) diff.y = -diff.y; + return (diff.x > diff.y) ? (diff.x + diff.y * 0.375f) : (diff.y + diff.x * 0.375f); +} + + +static float _lineLength(const Point& pt1, const Point& pt2) +{ + Point diff = {pt2.x - pt1.x, pt2.y - pt1.y}; + return sqrtf(diff.x * diff.x + diff.y * diff.y); +} + + +template +float _bezLength(const Bezier& cur, LengthFunc lineLengthFunc) +{ + Bezier left, right; + auto len = lineLengthFunc(cur.start, cur.ctrl1) + lineLengthFunc(cur.ctrl1, cur.ctrl2) + lineLengthFunc(cur.ctrl2, cur.end); + auto chord = lineLengthFunc(cur.start, cur.end); + + if (fabsf(len - chord) > BEZIER_EPSILON) { + cur.split(left, right); + return _bezLength(left, lineLengthFunc) + _bezLength(right, lineLengthFunc); + } + return len; +} + + +template +float _bezAt(const Bezier& bz, float at, float length, LengthFunc lineLengthFunc) +{ + auto biggest = 1.0f; + auto smallest = 0.0f; + auto t = 0.5f; + + //just in case to prevent an infinite loop + if (at <= 0) return 0.0f; + if (at >= length) return 1.0f; + + while (true) { + auto right = bz; + Bezier left; + right.split(t, left); + length = _bezLength(left, lineLengthFunc); + if (fabsf(length - at) < BEZIER_EPSILON || fabsf(smallest - biggest) < BEZIER_EPSILON) { + break; + } + if (length < at) { + smallest = t; + t = (t + biggest) * 0.5f; + } else { + biggest = t; + t = (smallest + t) * 0.5f; + } + } + return t; +} + + +/************************************************************************/ +/* External Class Implementation */ +/************************************************************************/ +namespace tvg { + +//https://en.wikipedia.org/wiki/Remez_algorithm +float atan2(float y, float x) +{ + if (y == 0.0f && x == 0.0f) return 0.0f; auto a = std::min(fabsf(x), fabsf(y)) / std::max(fabsf(x), fabsf(y)); auto s = a * a; auto r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a; @@ -37,7 +114,7 @@ float mathAtan2(float y, float x) } -bool mathInverse(const Matrix* m, Matrix* out) +bool inverse(const Matrix* m, Matrix* out) { auto det = m->e11 * (m->e22 * m->e33 - m->e32 * m->e23) - m->e12 * (m->e21 * m->e33 - m->e23 * m->e31) + @@ -60,7 +137,7 @@ bool mathInverse(const Matrix* m, Matrix* out) } -bool mathIdentity(const Matrix* m) +bool identity(const Matrix* m) { if (m->e11 != 1.0f || m->e12 != 0.0f || m->e13 != 0.0f || m->e21 != 0.0f || m->e22 != 1.0f || m->e23 != 0.0f || @@ -71,7 +148,7 @@ bool mathIdentity(const Matrix* m) } -void mathRotate(Matrix* m, float degree) +void rotate(Matrix* m, float degree) { if (degree == 0.0f) return; @@ -108,9 +185,9 @@ Matrix operator*(const Matrix& lhs, const Matrix& rhs) bool operator==(const Matrix& lhs, const Matrix& rhs) { - if (!mathEqual(lhs.e11, rhs.e11) || !mathEqual(lhs.e12, rhs.e12) || !mathEqual(lhs.e13, rhs.e13) || - !mathEqual(lhs.e21, rhs.e21) || !mathEqual(lhs.e22, rhs.e22) || !mathEqual(lhs.e23, rhs.e23) || - !mathEqual(lhs.e31, rhs.e31) || !mathEqual(lhs.e32, rhs.e32) || !mathEqual(lhs.e33, rhs.e33)) { + if (!tvg::equal(lhs.e11, rhs.e11) || !tvg::equal(lhs.e12, rhs.e12) || !tvg::equal(lhs.e13, rhs.e13) || + !tvg::equal(lhs.e21, rhs.e21) || !tvg::equal(lhs.e22, rhs.e22) || !tvg::equal(lhs.e23, rhs.e23) || + !tvg::equal(lhs.e31, rhs.e31) || !tvg::equal(lhs.e32, rhs.e32) || !tvg::equal(lhs.e33, rhs.e33)) { return false; } return true; @@ -133,9 +210,165 @@ Point operator*(const Point& pt, const Matrix& m) return {tx, ty}; } -uint8_t mathLerp(const uint8_t &start, const uint8_t &end, float t) + +Point normal(const Point& p1, const Point& p2) +{ + auto dir = p2 - p1; + auto len = length(dir); + if (tvg::zero(len)) return {}; + + auto unitDir = dir / len; + return {-unitDir.y, unitDir.x}; +} + + +float Line::length() const +{ + return _lineLength(pt1, pt2); +} + + +void Line::split(float at, Line& left, Line& right) const +{ + auto len = length(); + auto dx = ((pt2.x - pt1.x) / len) * at; + auto dy = ((pt2.y - pt1.y) / len) * at; + left.pt1 = pt1; + left.pt2.x = left.pt1.x + dx; + left.pt2.y = left.pt1.y + dy; + right.pt1 = left.pt2; + right.pt2 = pt2; +} + + +void Bezier::split(Bezier& left, Bezier& right) const +{ + auto c = (ctrl1.x + ctrl2.x) * 0.5f; + left.ctrl1.x = (start.x + ctrl1.x) * 0.5f; + right.ctrl2.x = (ctrl2.x + end.x) * 0.5f; + left.start.x = start.x; + right.end.x = end.x; + left.ctrl2.x = (left.ctrl1.x + c) * 0.5f; + right.ctrl1.x = (right.ctrl2.x + c) * 0.5f; + left.end.x = right.start.x = (left.ctrl2.x + right.ctrl1.x) * 0.5f; + + c = (ctrl1.y + ctrl2.y) * 0.5f; + left.ctrl1.y = (start.y + ctrl1.y) * 0.5f; + right.ctrl2.y = (ctrl2.y + end.y) * 0.5f; + left.start.y = start.y; + right.end.y = end.y; + left.ctrl2.y = (left.ctrl1.y + c) * 0.5f; + right.ctrl1.y = (right.ctrl2.y + c) * 0.5f; + left.end.y = right.start.y = (left.ctrl2.y + right.ctrl1.y) * 0.5f; +} + + +void Bezier::split(float at, Bezier& left, Bezier& right) const +{ + right = *this; + auto t = right.at(at, right.length()); + right.split(t, left); +} + + +float Bezier::length() const +{ + return _bezLength(*this, _lineLength); +} + + +float Bezier::lengthApprox() const +{ + return _bezLength(*this, _lineLengthApprox); +} + + +void Bezier::split(float t, Bezier& left) +{ + left.start = start; + + left.ctrl1.x = start.x + t * (ctrl1.x - start.x); + left.ctrl1.y = start.y + t * (ctrl1.y - start.y); + + left.ctrl2.x = ctrl1.x + t * (ctrl2.x - ctrl1.x); //temporary holding spot + left.ctrl2.y = ctrl1.y + t * (ctrl2.y - ctrl1.y); //temporary holding spot + + ctrl2.x = ctrl2.x + t * (end.x - ctrl2.x); + ctrl2.y = ctrl2.y + t * (end.y - ctrl2.y); + + ctrl1.x = left.ctrl2.x + t * (ctrl2.x - left.ctrl2.x); + ctrl1.y = left.ctrl2.y + t * (ctrl2.y - left.ctrl2.y); + + left.ctrl2.x = left.ctrl1.x + t * (left.ctrl2.x - left.ctrl1.x); + left.ctrl2.y = left.ctrl1.y + t * (left.ctrl2.y - left.ctrl1.y); + + left.end.x = start.x = left.ctrl2.x + t * (ctrl1.x - left.ctrl2.x); + left.end.y = start.y = left.ctrl2.y + t * (ctrl1.y - left.ctrl2.y); +} + + +float Bezier::at(float at, float length) const +{ + return _bezAt(*this, at, length, _lineLength); +} + + +float Bezier::atApprox(float at, float length) const +{ + return _bezAt(*this, at, length, _lineLengthApprox); +} + + +Point Bezier::at(float t) const +{ + Point cur; + auto it = 1.0f - t; + + auto ax = start.x * it + ctrl1.x * t; + auto bx = ctrl1.x * it + ctrl2.x * t; + auto cx = ctrl2.x * it + end.x * t; + ax = ax * it + bx * t; + bx = bx * it + cx * t; + cur.x = ax * it + bx * t; + + float ay = start.y * it + ctrl1.y * t; + float by = ctrl1.y * it + ctrl2.y * t; + float cy = ctrl2.y * it + end.y * t; + ay = ay * it + by * t; + by = by * it + cy * t; + cur.y = ay * it + by * t; + + return cur; +} + + +float Bezier::angle(float t) const +{ + if (t < 0 || t > 1) return 0; + + //derivate + // p'(t) = 3 * (-(1-2t+t^2) * p0 + (1 - 4 * t + 3 * t^2) * p1 + (2 * t - 3 * + // t^2) * p2 + t^2 * p3) + float mt = 1.0f - t; + float d = t * t; + float a = -mt * mt; + float b = 1 - 4 * t + 3 * d; + float c = 2 * t - 3 * d; + + Point pt ={a * start.x + b * ctrl1.x + c * ctrl2.x + d * end.x, a * start.y + b * ctrl1.y + c * ctrl2.y + d * end.y}; + pt.x *= 3; + pt.y *= 3; + + return rad2deg(tvg::atan2(pt.y, pt.x)); +} + + +uint8_t lerp(const uint8_t &start, const uint8_t &end, float t) { auto result = static_cast(start + (end - start) * t); - mathClamp(result, 0, 255); + tvg::clamp(result, 0, 255); return static_cast(result); } + +} + diff --git a/thirdparty/thorvg/src/common/tvgMath.h b/thirdparty/thorvg/src/common/tvgMath.h index 50786754a159..a917998256fd 100644 --- a/thirdparty/thorvg/src/common/tvgMath.h +++ b/thirdparty/thorvg/src/common/tvgMath.h @@ -29,47 +29,47 @@ #include #include "tvgCommon.h" +namespace tvg +{ + #define MATH_PI 3.14159265358979323846f #define MATH_PI2 1.57079632679489661923f #define FLOAT_EPSILON 1.0e-06f //1.192092896e-07f #define PATH_KAPPA 0.552284f -#define mathMin(x, y) (((x) < (y)) ? (x) : (y)) -#define mathMax(x, y) (((x) > (y)) ? (x) : (y)) - - /************************************************************************/ /* General functions */ /************************************************************************/ -float mathAtan2(float y, float x); +float atan2(float y, float x); -static inline float mathDeg2Rad(float degree) + +static inline float deg2rad(float degree) { return degree * (MATH_PI / 180.0f); } -static inline float mathRad2Deg(float radian) +static inline float rad2deg(float radian) { return radian * (180.0f / MATH_PI); } -static inline bool mathZero(float a) +static inline bool zero(float a) { return (fabsf(a) <= FLOAT_EPSILON) ? true : false; } -static inline bool mathEqual(float a, float b) +static inline bool equal(float a, float b) { - return mathZero(a - b); + return tvg::zero(a - b); } template -static inline void mathClamp(T& v, const T& min, const T& max) +static inline void clamp(T& v, const T& min, const T& max) { if (v < min) v = min; else if (v > max) v = max; @@ -79,27 +79,27 @@ static inline void mathClamp(T& v, const T& min, const T& max) /* Matrix functions */ /************************************************************************/ -void mathRotate(Matrix* m, float degree); -bool mathInverse(const Matrix* m, Matrix* out); -bool mathIdentity(const Matrix* m); +void rotate(Matrix* m, float degree); +bool inverse(const Matrix* m, Matrix* out); +bool identity(const Matrix* m); Matrix operator*(const Matrix& lhs, const Matrix& rhs); bool operator==(const Matrix& lhs, const Matrix& rhs); -static inline bool mathRightAngle(const Matrix& m) +static inline bool rightAngle(const Matrix& m) { - auto radian = fabsf(mathAtan2(m.e21, m.e11)); - if (radian < FLOAT_EPSILON || mathEqual(radian, MATH_PI2) || mathEqual(radian, MATH_PI)) return true; + auto radian = fabsf(tvg::atan2(m.e21, m.e11)); + if (radian < FLOAT_EPSILON || tvg::equal(radian, MATH_PI2) || tvg::equal(radian, MATH_PI)) return true; return false; } -static inline bool mathSkewed(const Matrix& m) +static inline bool skewed(const Matrix& m) { - return !mathZero(m.e21 + m.e12); + return !tvg::zero(m.e21 + m.e12); } -static inline void mathIdentity(Matrix* m) +static inline void identity(Matrix* m) { m->e11 = 1.0f; m->e12 = 0.0f; @@ -113,14 +113,14 @@ static inline void mathIdentity(Matrix* m) } -static inline void mathScale(Matrix* m, float sx, float sy) +static inline void scale(Matrix* m, float sx, float sy) { m->e11 *= sx; m->e22 *= sy; } -static inline void mathScaleR(Matrix* m, float x, float y) +static inline void scaleR(Matrix* m, float x, float y) { if (x != 1.0f) { m->e11 *= x; @@ -133,14 +133,14 @@ static inline void mathScaleR(Matrix* m, float x, float y) } -static inline void mathTranslate(Matrix* m, float x, float y) +static inline void translate(Matrix* m, float x, float y) { m->e13 += x; m->e23 += y; } -static inline void mathTranslateR(Matrix* m, float x, float y) +static inline void translateR(Matrix* m, float x, float y) { if (x == 0.0f && y == 0.0f) return; m->e13 += (x * m->e11 + y * m->e12); @@ -160,7 +160,7 @@ static inline void operator*=(Matrix& lhs, const Matrix& rhs) } -static inline void mathLog(const Matrix& m) +static inline void log(const Matrix& m) { TVGLOG("COMMON", "Matrix: [%f %f %f] [%f %f %f] [%f %f %f]", m.e11, m.e12, m.e13, m.e21, m.e22, m.e23, m.e31, m.e32, m.e33); } @@ -172,15 +172,21 @@ static inline void mathLog(const Matrix& m) void operator*=(Point& pt, const Matrix& m); Point operator*(const Point& pt, const Matrix& m); +Point normal(const Point& p1, const Point& p2); +static inline float cross(const Point& lhs, const Point& rhs) +{ + return lhs.x * rhs.y - rhs.x * lhs.y; +} -static inline bool mathZero(const Point& p) + +static inline bool zero(const Point& p) { - return mathZero(p.x) && mathZero(p.y); + return tvg::zero(p.x) && tvg::zero(p.y); } -static inline float mathLength(const Point* a, const Point* b) +static inline float length(const Point* a, const Point* b) { auto x = b->x - a->x; auto y = b->y - a->y; @@ -192,7 +198,7 @@ static inline float mathLength(const Point* a, const Point* b) } -static inline float mathLength(const Point& a) +static inline float length(const Point& a) { return sqrtf(a.x * a.x + a.y * a.y); } @@ -200,7 +206,7 @@ static inline float mathLength(const Point& a) static inline bool operator==(const Point& lhs, const Point& rhs) { - return mathEqual(lhs.x, rhs.x) && mathEqual(lhs.y, rhs.y); + return tvg::equal(lhs.x, rhs.x) && tvg::equal(lhs.y, rhs.y); } @@ -240,32 +246,61 @@ static inline Point operator/(const Point& lhs, const float rhs) } -static inline Point mathNormal(const Point& p1, const Point& p2) +static inline void log(const Point& pt) { - auto dir = p2 - p1; - auto len = mathLength(dir); - if (mathZero(len)) return {}; - - auto unitDir = dir / len; - return {-unitDir.y, unitDir.x}; + TVGLOG("COMMON", "Point: [%f %f]", pt.x, pt.y); } -static inline void mathLog(const Point& pt) +/************************************************************************/ +/* Line functions */ +/************************************************************************/ + +struct Line { - TVGLOG("COMMON", "Point: [%f %f]", pt.x, pt.y); -} + Point pt1; + Point pt2; + + void split(float at, Line& left, Line& right) const; + float length() const; +}; + + +/************************************************************************/ +/* Bezier functions */ +/************************************************************************/ + +struct Bezier +{ + Point start; + Point ctrl1; + Point ctrl2; + Point end; + + void split(float t, Bezier& left); + void split(Bezier& left, Bezier& right) const; + void split(float at, Bezier& left, Bezier& right) const; + float length() const; + float lengthApprox() const; + float at(float at, float length) const; + float atApprox(float at, float length) const; + Point at(float t) const; + float angle(float t) const; +}; + /************************************************************************/ /* Interpolation functions */ /************************************************************************/ template -static inline T mathLerp(const T &start, const T &end, float t) +static inline T lerp(const T &start, const T &end, float t) { return static_cast(start + (end - start) * t); } -uint8_t mathLerp(const uint8_t &start, const uint8_t &end, float t); +uint8_t lerp(const uint8_t &start, const uint8_t &end, float t); + +} #endif //_TVG_MATH_H_ diff --git a/thirdparty/thorvg/src/common/tvgStr.cpp b/thirdparty/thorvg/src/common/tvgStr.cpp index 3f1896680960..1ebdd41c5e92 100644 --- a/thirdparty/thorvg/src/common/tvgStr.cpp +++ b/thirdparty/thorvg/src/common/tvgStr.cpp @@ -207,16 +207,6 @@ float strToFloat(const char *nPtr, char **endPtr) return 0.0f; } - -int str2int(const char* str, size_t n) -{ - int ret = 0; - for(size_t i = 0; i < n; ++i) { - ret = ret * 10 + (str[i] - '0'); - } - return ret; -} - char* strDuplicate(const char *str, size_t n) { auto len = strlen(str); diff --git a/thirdparty/thorvg/src/common/tvgStr.h b/thirdparty/thorvg/src/common/tvgStr.h index 56fdf86fb2d5..6b16b4c7610c 100644 --- a/thirdparty/thorvg/src/common/tvgStr.h +++ b/thirdparty/thorvg/src/common/tvgStr.h @@ -29,7 +29,6 @@ namespace tvg { float strToFloat(const char *nPtr, char **endPtr); //convert to float -int str2int(const char* str, size_t n); //convert to integer char* strDuplicate(const char *str, size_t n); //copy the string char* strAppend(char* lhs, const char* rhs, size_t n); //append the rhs to the lhs char* strDirname(const char* path); //return the full directory name diff --git a/thirdparty/thorvg/src/loaders/external_png/tvgPngLoader.cpp b/thirdparty/thorvg/src/loaders/external_png/tvgPngLoader.cpp index 6c4c8410e82f..49c9f6e8aa2e 100644 --- a/thirdparty/thorvg/src/loaders/external_png/tvgPngLoader.cpp +++ b/thirdparty/thorvg/src/loaders/external_png/tvgPngLoader.cpp @@ -86,10 +86,10 @@ bool PngLoader::read() if (cs == ColorSpace::ARGB8888 || cs == ColorSpace::ARGB8888S) { image->format = PNG_FORMAT_BGRA; - surface.cs = ColorSpace::ARGB8888; + surface.cs = ColorSpace::ARGB8888S; } else { image->format = PNG_FORMAT_RGBA; - surface.cs = ColorSpace::ABGR8888; + surface.cs = ColorSpace::ABGR8888S; } auto buffer = static_cast(malloc(PNG_IMAGE_SIZE((*image)))); diff --git a/thirdparty/thorvg/src/loaders/external_webp/tvgWebpLoader.cpp b/thirdparty/thorvg/src/loaders/external_webp/tvgWebpLoader.cpp index fbaaea874319..0db7d2d233f8 100644 --- a/thirdparty/thorvg/src/loaders/external_webp/tvgWebpLoader.cpp +++ b/thirdparty/thorvg/src/loaders/external_webp/tvgWebpLoader.cpp @@ -134,7 +134,7 @@ bool WebpLoader::read() } -Surface* WebpLoader::bitmap() +RenderSurface* WebpLoader::bitmap() { this->done(); diff --git a/thirdparty/thorvg/src/loaders/external_webp/tvgWebpLoader.h b/thirdparty/thorvg/src/loaders/external_webp/tvgWebpLoader.h index ad8fb5a69954..3d44edac3a18 100644 --- a/thirdparty/thorvg/src/loaders/external_webp/tvgWebpLoader.h +++ b/thirdparty/thorvg/src/loaders/external_webp/tvgWebpLoader.h @@ -36,7 +36,7 @@ class WebpLoader : public ImageLoader, public Task bool open(const char* data, uint32_t size, bool copy) override; bool read() override; - Surface* bitmap() override; + RenderSurface* bitmap() override; private: void run(unsigned tid) override; diff --git a/thirdparty/thorvg/src/loaders/jpg/tvgJpgLoader.cpp b/thirdparty/thorvg/src/loaders/jpg/tvgJpgLoader.cpp index 86a46245d988..cb1306b71a3e 100644 --- a/thirdparty/thorvg/src/loaders/jpg/tvgJpgLoader.cpp +++ b/thirdparty/thorvg/src/loaders/jpg/tvgJpgLoader.cpp @@ -125,7 +125,7 @@ bool JpgLoader::close() } -Surface* JpgLoader::bitmap() +RenderSurface* JpgLoader::bitmap() { this->done(); return ImageLoader::bitmap(); diff --git a/thirdparty/thorvg/src/loaders/jpg/tvgJpgLoader.h b/thirdparty/thorvg/src/loaders/jpg/tvgJpgLoader.h index 05cbb54c852f..85f6d25dc392 100644 --- a/thirdparty/thorvg/src/loaders/jpg/tvgJpgLoader.h +++ b/thirdparty/thorvg/src/loaders/jpg/tvgJpgLoader.h @@ -46,7 +46,7 @@ class JpgLoader : public ImageLoader, public Task bool read() override; bool close() override; - Surface* bitmap() override; + RenderSurface* bitmap() override; }; #endif //_TVG_JPG_LOADER_H_ diff --git a/thirdparty/thorvg/src/loaders/jpg/tvgJpgd.cpp b/thirdparty/thorvg/src/loaders/jpg/tvgJpgd.cpp index 6f2bd916d5ea..92e23698af2c 100644 --- a/thirdparty/thorvg/src/loaders/jpg/tvgJpgd.cpp +++ b/thirdparty/thorvg/src/loaders/jpg/tvgJpgd.cpp @@ -36,6 +36,8 @@ #include #include #include + +#include "tvgCommon.h" #include "tvgJpgd.h" #ifdef _MSC_VER @@ -70,7 +72,7 @@ enum jpgd_status JPGD_BAD_DHT_COUNTS = -256, JPGD_BAD_DHT_INDEX, JPGD_BAD_DHT_MARKER, JPGD_BAD_DQT_MARKER, JPGD_BAD_DQT_TABLE, JPGD_BAD_PRECISION, JPGD_BAD_HEIGHT, JPGD_BAD_WIDTH, JPGD_TOO_MANY_COMPONENTS, JPGD_BAD_SOF_LENGTH, JPGD_BAD_VARIABLE_MARKER, JPGD_BAD_DRI_LENGTH, JPGD_BAD_SOS_LENGTH, - JPGD_BAD_SOS_COMP_ID, JPGD_W_EXTRA_BYTES_BEFORE_MARKER, JPGD_NO_ARITHMITIC_SUPPORT, JPGD_UNEXPECTED_MARKER, + JPGD_BAD_SOS_COMP_ID, JPGD_W_EXTRA_BYTES_BEFORE_MARKER, JPGD_NO_ARITHMETIC_SUPPORT, JPGD_UNEXPECTED_MARKER, JPGD_NOT_JPEG, JPGD_UNSUPPORTED_MARKER, JPGD_BAD_DQT_LENGTH, JPGD_TOO_MANY_BLOCKS, JPGD_UNDEFINED_QUANT_TABLE, JPGD_UNDEFINED_HUFF_TABLE, JPGD_NOT_SINGLE_SCAN, JPGD_UNSUPPORTED_COLORSPACE, JPGD_UNSUPPORTED_SAMP_FACTORS, JPGD_DECODE_ERROR, JPGD_BAD_RESTART_MARKER, JPGD_ASSERTION_ERROR, @@ -1382,9 +1384,9 @@ int jpeg_decoder::process_markers() read_dht_marker(); break; } - // No arithmitic support - dumb patents! + // No arithmetic support - dumb patents! case M_DAC: { - stop_decoding(JPGD_NO_ARITHMITIC_SUPPORT); + stop_decoding(JPGD_NO_ARITHMETIC_SUPPORT); break; } case M_DQT: { @@ -1466,8 +1468,8 @@ void jpeg_decoder::locate_sof_marker() read_sof_marker(); break; } - case M_SOF9: { /* Arithmitic coding */ - stop_decoding(JPGD_NO_ARITHMITIC_SUPPORT); + case M_SOF9: { /* Arithmetic coding */ + stop_decoding(JPGD_NO_ARITHMETIC_SUPPORT); break; } default: { @@ -1738,7 +1740,8 @@ void jpeg_decoder::transform_mcu_expand(int mcu_row) DCT_Upsample::R_S<8, 8>::calc(R, S, pSrc_ptr); break; default: - JPGD_ASSERT(false); + TVGERR("JPG", "invalid transform_mcu_expand"); + return; } DCT_Upsample::Matrix44 a(P + Q); P -= Q; DCT_Upsample::Matrix44& b = P; @@ -1831,7 +1834,7 @@ void jpeg_decoder::process_restart() int i; int c = 0; - // Align to a byte boundry + // Align to a byte boundary // FIXME: Is this really necessary? get_bits_no_markers() never reads in markers! //get_bits_no_markers(m_bits_left & 7); diff --git a/thirdparty/thorvg/src/loaders/svg/tvgSvgLoader.cpp b/thirdparty/thorvg/src/loaders/svg/tvgSvgLoader.cpp index 492cebe3a319..1ab3043c24e8 100644 --- a/thirdparty/thorvg/src/loaders/svg/tvgSvgLoader.cpp +++ b/thirdparty/thorvg/src/loaders/svg/tvgSvgLoader.cpp @@ -177,6 +177,7 @@ static float _toFloat(const SvgParser* svgParse, const char* str, SvgParserLengt else if (strstr(str, "%")) { if (type == SvgParserLengthType::Vertical) parsedValue = (parsedValue / 100.0f) * svgParse->global.h; else if (type == SvgParserLengthType::Horizontal) parsedValue = (parsedValue / 100.0f) * svgParse->global.w; + else if (type == SvgParserLengthType::Diagonal) parsedValue = (sqrtf(powf(svgParse->global.w, 2) + powf(svgParse->global.h, 2)) / sqrtf(2.0f)) * (parsedValue / 100.0f); else //if other than it's radius { float max = svgParse->global.w; @@ -593,15 +594,15 @@ static bool _hslToRgb(float hue, float saturation, float brightness, uint8_t* re saturation = saturation > 0 ? std::min(saturation, 1.0f) : 0.0f; brightness = brightness > 0 ? std::min(brightness, 1.0f) : 0.0f; - if (mathZero(saturation)) _red = _green = _blue = brightness; + if (tvg::zero(saturation)) _red = _green = _blue = brightness; else { - if (mathEqual(hue, 360.0)) hue = 0.0f; + if (tvg::equal(hue, 360.0)) hue = 0.0f; hue /= 60.0f; v = (brightness <= 0.5f) ? (brightness * (1.0f + saturation)) : (brightness + saturation - (brightness * saturation)); p = brightness + brightness - v; - if (!mathZero(v)) sv = (v - p) / v; + if (!tvg::zero(v)) sv = (v - p) / v; else sv = 0; i = static_cast(hue); @@ -858,8 +859,8 @@ static Matrix* _parseTransformationMatrix(const char* value) //Transform to signed. points[0] = fmodf(points[0], 360.0f); if (points[0] < 0) points[0] += 360.0f; - auto c = cosf(mathDeg2Rad(points[0])); - auto s = sinf(mathDeg2Rad(points[0])); + auto c = cosf(deg2rad(points[0])); + auto s = sinf(deg2rad(points[0])); if (ptCount == 1) { Matrix tmp = { c, -s, 0, s, c, 0, 0, 0, 1 }; *matrix *= tmp; @@ -882,12 +883,12 @@ static Matrix* _parseTransformationMatrix(const char* value) *matrix *= tmp; } else if (state == MatrixState::SkewX) { if (ptCount != 1) goto error; - auto deg = tanf(mathDeg2Rad(points[0])); + auto deg = tanf(deg2rad(points[0])); Matrix tmp = { 1, deg, 0, 0, 1, 0, 0, 0, 1 }; *matrix *= tmp; } else if (state == MatrixState::SkewY) { if (ptCount != 1) goto error; - auto deg = tanf(mathDeg2Rad(points[0])); + auto deg = tanf(deg2rad(points[0])); Matrix tmp = { 1, 0, 0, deg, 1, 0, 0, 0, 1 }; *matrix *= tmp; } @@ -1066,7 +1067,7 @@ static void _handleStrokeDashOffsetAttr(SvgLoaderData* loader, SvgNode* node, co static void _handleStrokeWidthAttr(SvgLoaderData* loader, SvgNode* node, const char* value) { node->style->stroke.flags = (node->style->stroke.flags | SvgStrokeFlags::Width); - node->style->stroke.width = _toFloat(loader->svgParse, value, SvgParserLengthType::Horizontal); + node->style->stroke.width = _toFloat(loader->svgParse, value, SvgParserLengthType::Diagonal); } @@ -1614,7 +1615,7 @@ static constexpr struct } circleTags[] = { {"cx", SvgParserLengthType::Horizontal, sizeof("cx"), offsetof(SvgCircleNode, cx)}, {"cy", SvgParserLengthType::Vertical, sizeof("cy"), offsetof(SvgCircleNode, cy)}, - {"r", SvgParserLengthType::Other, sizeof("r"), offsetof(SvgCircleNode, r)} + {"r", SvgParserLengthType::Diagonal, sizeof("r"), offsetof(SvgCircleNode, r)} }; @@ -2554,6 +2555,18 @@ static SvgStyleGradient* _createRadialGradient(SvgLoaderData* loader, const char } +static SvgColor* _findLatestColor(const SvgLoaderData* loader) +{ + auto parent = loader->stack.count > 0 ? loader->stack.last() : loader->doc; + + while (parent != nullptr) { + if (parent->style->curColorSet) return &parent->style->color; + parent = parent->parent; + } + return nullptr; +} + + static bool _attrParseStopsStyle(void* data, const char* key, const char* value) { SvgLoaderData* loader = (SvgLoaderData*)data; @@ -2563,7 +2576,13 @@ static bool _attrParseStopsStyle(void* data, const char* key, const char* value) stop->a = _toOpacity(value); loader->svgParse->flags = (loader->svgParse->flags | SvgStopStyleFlags::StopOpacity); } else if (!strcmp(key, "stop-color")) { - if (_toColor(value, &stop->r, &stop->g, &stop->b, nullptr)) { + if (!strcmp(value, "currentColor")) { + if (auto latestColor = _findLatestColor(loader)) { + stop->r = latestColor->r; + stop->g = latestColor->g; + stop->b = latestColor->b; + } + } else if (_toColor(value, &stop->r, &stop->g, &stop->b, nullptr)) { loader->svgParse->flags = (loader->svgParse->flags | SvgStopStyleFlags::StopColor); } } else { @@ -2586,7 +2605,13 @@ static bool _attrParseStops(void* data, const char* key, const char* value) stop->a = _toOpacity(value); } } else if (!strcmp(key, "stop-color")) { - if (!(loader->svgParse->flags & SvgStopStyleFlags::StopColor)) { + if (!strcmp(value, "currentColor")) { + if (auto latestColor = _findLatestColor(loader)) { + stop->r = latestColor->r; + stop->g = latestColor->g; + stop->b = latestColor->b; + } + } else if (!(loader->svgParse->flags & SvgStopStyleFlags::StopColor)) { _toColor(value, &stop->r, &stop->g, &stop->b, nullptr); } } else if (!strcmp(key, "style")) { @@ -3341,7 +3366,7 @@ static void _svgLoaderParserXmlOpen(SvgLoaderData* loader, const char* content, else parent = loader->doc; if (!strcmp(tagName, "style")) { // TODO: For now only the first style node is saved. After the css id selector - // is introduced this if condition shouldin't be necessary any more + // is introduced this if condition shouldn't be necessary any more if (!loader->cssStyle) { node = method(loader, nullptr, attrs, attrsLength, simpleXmlParseAttributes); loader->cssStyle = node; diff --git a/thirdparty/thorvg/src/loaders/svg/tvgSvgLoaderCommon.h b/thirdparty/thorvg/src/loaders/svg/tvgSvgLoaderCommon.h index 5661c8ae82ce..e64d7afb4113 100644 --- a/thirdparty/thorvg/src/loaders/svg/tvgSvgLoaderCommon.h +++ b/thirdparty/thorvg/src/loaders/svg/tvgSvgLoaderCommon.h @@ -215,6 +215,7 @@ enum class SvgParserLengthType { Vertical, Horizontal, + Diagonal, //In case of, for example, radius of radial gradient Other }; diff --git a/thirdparty/thorvg/src/loaders/svg/tvgSvgPath.cpp b/thirdparty/thorvg/src/loaders/svg/tvgSvgPath.cpp index 115e81aee122..57442139cdff 100644 --- a/thirdparty/thorvg/src/loaders/svg/tvgSvgPath.cpp +++ b/thirdparty/thorvg/src/loaders/svg/tvgSvgPath.cpp @@ -126,7 +126,7 @@ void _pathAppendArcTo(Array* cmds, Array* pts, Point* cur, P rx = fabsf(rx); ry = fabsf(ry); - angle = mathDeg2Rad(angle); + angle = deg2rad(angle); cosPhi = cosf(angle); sinPhi = sinf(angle); dx2 = (sx - x) / 2.0f; @@ -190,14 +190,14 @@ void _pathAppendArcTo(Array* cmds, Array* pts, Point* cur, P cx += (sx + x) / 2.0f; cy += (sy + y) / 2.0f; - //Sstep 4 (F6.5.4) + //Step 4 (F6.5.4) //We dont' use arccos (as per w3c doc), see //http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm //Note: atan2 (0.0, 1.0) == 0.0 - at = mathAtan2(((y1p - cyp) / ry), ((x1p - cxp) / rx)); + at = tvg::atan2(((y1p - cyp) / ry), ((x1p - cxp) / rx)); theta1 = (at < 0.0f) ? 2.0f * MATH_PI + at : at; - nat = mathAtan2(((-y1p - cyp) / ry), ((-x1p - cxp) / rx)); + nat = tvg::atan2(((-y1p - cyp) / ry), ((-x1p - cxp) / rx)); deltaTheta = (nat < at) ? 2.0f * MATH_PI - at + nat : nat - at; if (sweep) { @@ -469,12 +469,12 @@ static bool _processCommand(Array* cmds, Array* pts, char cm } case 'a': case 'A': { - if (mathZero(arr[0]) || mathZero(arr[1])) { + if (tvg::zero(arr[0]) || tvg::zero(arr[1])) { Point p = {arr[5], arr[6]}; cmds->push(PathCommand::LineTo); pts->push(p); *cur = {arr[5], arr[6]}; - } else if (!mathEqual(cur->x, arr[5]) || !mathEqual(cur->y, arr[6])) { + } else if (!tvg::equal(cur->x, arr[5]) || !tvg::equal(cur->y, arr[6])) { _pathAppendArcTo(cmds, pts, cur, curCtl, arr[5], arr[6], fabsf(arr[0]), fabsf(arr[1]), arr[2], arr[3], arr[4]); *cur = *curCtl = {arr[5], arr[6]}; *isQuadratic = false; diff --git a/thirdparty/thorvg/src/loaders/svg/tvgSvgSceneBuilder.cpp b/thirdparty/thorvg/src/loaders/svg/tvgSvgSceneBuilder.cpp index b048695a234c..00c7408a7e88 100644 --- a/thirdparty/thorvg/src/loaders/svg/tvgSvgSceneBuilder.cpp +++ b/thirdparty/thorvg/src/loaders/svg/tvgSvgSceneBuilder.cpp @@ -153,7 +153,7 @@ static unique_ptr _applyRadialGradientProperty(SvgStyleGradient* if (isTransform) finalTransform = *g->transform; if (g->userSpace) { - //The radius scalling is done according to the Units section: + //The radius scaling is done according to the Units section: //https://www.w3.org/TR/2015/WD-SVG2-20150915/coords.html g->radial->cx = g->radial->cx * vBox.w; g->radial->cy = g->radial->cy * vBox.h; @@ -215,7 +215,7 @@ static bool _appendClipUseNode(SvgLoaderData& loaderData, SvgNode* node, Shape* } if (child->transform) finalTransform = *child->transform * finalTransform; - return _appendClipShape(loaderData, child, shape, vBox, svgPath, mathIdentity((const Matrix*)(&finalTransform)) ? nullptr : &finalTransform); + return _appendClipShape(loaderData, child, shape, vBox, svgPath, identity((const Matrix*)(&finalTransform)) ? nullptr : &finalTransform); } @@ -272,8 +272,7 @@ static void _applyComposition(SvgLoaderData& loaderData, Paint* paint, const Svg if (valid) { Matrix finalTransform = _compositionTransform(paint, node, compNode, SvgNodeType::ClipPath); comp->transform(finalTransform); - - paint->composite(std::move(comp), CompositeMethod::ClipPath); + paint->clip(std::move(comp)); } node->style->clipPath.applying = false; @@ -714,7 +713,6 @@ static Matrix _calculateAspectRatioMatrix(AspectRatioAlign align, AspectRatioMee static unique_ptr _useBuildHelper(SvgLoaderData& loaderData, const SvgNode* node, const Box& vBox, const string& svgPath, int depth, bool* isMaskWhite) { - unique_ptr finalScene; auto scene = _sceneBuildHelper(loaderData, node, vBox, svgPath, false, depth + 1, isMaskWhite); // mUseTransform = mUseTransform * mTranslate @@ -736,10 +734,10 @@ static unique_ptr _useBuildHelper(SvgLoaderData& loaderData, const SvgNod auto vh = (symbol.hasViewBox ? symbol.vh : height); Matrix mViewBox = {1, 0, 0, 0, 1, 0, 0, 0, 1}; - if ((!mathEqual(width, vw) || !mathEqual(height, vh)) && vw > 0 && vh > 0) { + if ((!tvg::equal(width, vw) || !tvg::equal(height, vh)) && vw > 0 && vh > 0) { Box box = {symbol.vx, symbol.vy, vw, vh}; mViewBox = _calculateAspectRatioMatrix(symbol.align, symbol.meetOrSlice, width, height, box); - } else if (!mathZero(symbol.vx) || !mathZero(symbol.vy)) { + } else if (!tvg::zero(symbol.vx) || !tvg::zero(symbol.vy)) { mViewBox = {1, 0, -symbol.vx, 0, 1, -symbol.vy, 0, 0, 1}; } @@ -751,9 +749,7 @@ static unique_ptr _useBuildHelper(SvgLoaderData& loaderData, const SvgNod mSceneTransform = mUseTransform * mSceneTransform; scene->transform(mSceneTransform); - if (node->node.use.symbol->node.symbol.overflowVisible) { - finalScene = std::move(scene); - } else { + if (!node->node.use.symbol->node.symbol.overflowVisible) { auto viewBoxClip = Shape::gen(); viewBoxClip->appendRect(0, 0, width, height, 0, 0); @@ -764,21 +760,13 @@ static unique_ptr _useBuildHelper(SvgLoaderData& loaderData, const SvgNod } viewBoxClip->transform(mClipTransform); - auto compositeLayer = Scene::gen(); - compositeLayer->composite(std::move(viewBoxClip), CompositeMethod::ClipPath); - compositeLayer->push(std::move(scene)); - - auto root = Scene::gen(); - root->push(std::move(compositeLayer)); - - finalScene = std::move(root); + scene->clip(std::move(viewBoxClip)); } } else { scene->transform(mUseTransform); - finalScene = std::move(scene); } - return finalScene; + return scene; } @@ -821,7 +809,7 @@ static unique_ptr _textBuildHelper(SvgLoaderData& loaderData, const SvgNod Matrix textTransform = {1, 0, 0, 0, 1, 0, 0, 0, 1}; if (node->transform) textTransform = *node->transform; - mathTranslateR(&textTransform, node->node.text.x, node->node.text.y - textNode->fontSize); + translateR(&textTransform, node->node.text.x, node->node.text.y - textNode->fontSize); text->transform(textTransform); //TODO: handle def values of font and size as used in a system? @@ -926,10 +914,10 @@ Scene* svgSceneBuild(SvgLoaderData& loaderData, Box vBox, float w, float h, Aspe if (!(viewFlag & SvgViewFlag::Viewbox)) _updateInvalidViewSize(docNode.get(), vBox, w, h, viewFlag); - if (!mathEqual(w, vBox.w) || !mathEqual(h, vBox.h)) { + if (!tvg::equal(w, vBox.w) || !tvg::equal(h, vBox.h)) { Matrix m = _calculateAspectRatioMatrix(align, meetOrSlice, w, h, vBox); docNode->transform(m); - } else if (!mathZero(vBox.x) || !mathZero(vBox.y)) { + } else if (!tvg::zero(vBox.x) || !tvg::zero(vBox.y)) { docNode->translate(-vBox.x, -vBox.y); } @@ -937,7 +925,7 @@ Scene* svgSceneBuild(SvgLoaderData& loaderData, Box vBox, float w, float h, Aspe viewBoxClip->appendRect(0, 0, w, h); auto compositeLayer = Scene::gen(); - compositeLayer->composite(std::move(viewBoxClip), CompositeMethod::ClipPath); + compositeLayer->clip(std::move(viewBoxClip)); compositeLayer->push(std::move(docNode)); auto root = Scene::gen(); diff --git a/thirdparty/thorvg/src/loaders/svg/tvgXmlParser.cpp b/thirdparty/thorvg/src/loaders/svg/tvgXmlParser.cpp index 09fc8aaac137..da1cdae9e0e7 100644 --- a/thirdparty/thorvg/src/loaders/svg/tvgXmlParser.cpp +++ b/thirdparty/thorvg/src/loaders/svg/tvgXmlParser.cpp @@ -492,13 +492,13 @@ bool simpleXmlParseW3CAttribute(const char* buf, unsigned bufLength, simpleXMLAt key[0] = '\0'; val[0] = '\0'; - if (next == nullptr && sep != nullptr) { + if (sep != nullptr && next == nullptr) { memcpy(key, buf, sep - buf); key[sep - buf] = '\0'; memcpy(val, sep + 1, end - sep - 1); val[end - sep - 1] = '\0'; - } else if (sep < next && sep != nullptr) { + } else if (sep != nullptr && sep < next) { memcpy(key, buf, sep - buf); key[sep - buf] = '\0'; @@ -522,8 +522,9 @@ bool simpleXmlParseW3CAttribute(const char* buf, unsigned bufLength, simpleXMLAt } } + if (!next) break; buf = next + 1; - } while (next != nullptr); + } while (true); return true; } diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwCommon.h b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwCommon.h index ab1fc18b58a5..9371ae6c5ad3 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwCommon.h +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwCommon.h @@ -113,7 +113,7 @@ struct SwSpan uint8_t coverage; }; -struct SwRleData +struct SwRle { SwSpan *spans; uint32_t alloc; @@ -211,8 +211,8 @@ struct SwShape SwOutline* outline = nullptr; SwStroke* stroke = nullptr; SwFill* fill = nullptr; - SwRleData* rle = nullptr; - SwRleData* strokeRle = nullptr; + SwRle* rle = nullptr; + SwRle* strokeRle = nullptr; SwBBox bbox; //Keep it boundary without stroke region. Using for optimal filling. bool fastTrack = false; //Fast Track: axis-aligned rectangle without any clips? @@ -221,7 +221,7 @@ struct SwShape struct SwImage { SwOutline* outline = nullptr; - SwRleData* rle = nullptr; + SwRle* rle = nullptr; union { pixel_t* data; //system based data pointer uint32_t* buf32; //for explicit 32bits channels @@ -244,13 +244,13 @@ typedef uint8_t(*SwAlpha)(uint8_t*); //bl struct SwCompositor; -struct SwSurface : Surface +struct SwSurface : RenderSurface { SwJoin join; SwAlpha alphas[4]; //Alpha:2, InvAlpha:3, Luma:4, InvLuma:5 SwBlender blender = nullptr; //blender (optional) SwCompositor* compositor = nullptr; //compositor (optional) - BlendMethod blendMethod; //blending method (uint8_t) + BlendMethod blendMethod = BlendMethod::Normal; SwAlpha alpha(CompositeMethod method) { @@ -262,7 +262,7 @@ struct SwSurface : Surface { } - SwSurface(const SwSurface* rhs) : Surface(rhs) + SwSurface(const SwSurface* rhs) : RenderSurface(rhs) { join = rhs->join; memcpy(alphas, rhs->alphas, sizeof(alphas)); @@ -272,7 +272,7 @@ struct SwSurface : Surface } }; -struct SwCompositor : Compositor +struct SwCompositor : RenderCompositor { SwSurface* recoverSfc; //Recover surface when composition is started SwCompositor* recoverCmp; //Recover compositor when composition is done @@ -488,13 +488,14 @@ SwFixed mathAtan(const SwPoint& pt); SwFixed mathCos(SwFixed angle); SwFixed mathSin(SwFixed angle); void mathSplitCubic(SwPoint* base); +void mathSplitLine(SwPoint* base); SwFixed mathDiff(SwFixed angle1, SwFixed angle2); SwFixed mathLength(const SwPoint& pt); -bool mathSmallCubic(const SwPoint* base, SwFixed& angleIn, SwFixed& angleMid, SwFixed& angleOut); +int mathCubicAngle(const SwPoint* base, SwFixed& angleIn, SwFixed& angleMid, SwFixed& angleOut); SwFixed mathMean(SwFixed angle1, SwFixed angle2); SwPoint mathTransform(const Point* to, const Matrix& transform); bool mathUpdateOutlineBBox(const SwOutline* outline, const SwBBox& clipRegion, SwBBox& renderRegion, bool fastTrack); -bool mathClipBBox(const SwBBox& clipper, SwBBox& clipee); +bool mathClipBBox(const SwBBox& clipper, SwBBox& clippee); void shapeReset(SwShape* shape); bool shapePrepare(SwShape* shape, const RenderShape* rshape, const Matrix& transform, const SwBBox& clipRegion, SwBBox& renderRegion, SwMpool* mpool, unsigned tid, bool hasComposite); @@ -541,13 +542,13 @@ void fillRadial(const SwFill* fill, uint32_t* dst, uint32_t y, uint32_t x, uint3 void fillRadial(const SwFill* fill, uint32_t* dst, uint32_t y, uint32_t x, uint32_t len, SwBlender op, SwBlender op2, uint8_t a); //blending + BlendingMethod(op2) ver. void fillRadial(const SwFill* fill, uint32_t* dst, uint32_t y, uint32_t x, uint32_t len, uint8_t* cmp, SwAlpha alpha, uint8_t csize, uint8_t opacity); //matting ver. -SwRleData* rleRender(SwRleData* rle, const SwOutline* outline, const SwBBox& renderRegion, bool antiAlias); -SwRleData* rleRender(const SwBBox* bbox); -void rleFree(SwRleData* rle); -void rleReset(SwRleData* rle); -void rleMerge(SwRleData* rle, SwRleData* clip1, SwRleData* clip2); -void rleClipPath(SwRleData* rle, const SwRleData* clip); -void rleClipRect(SwRleData* rle, const SwBBox* clip); +SwRle* rleRender(SwRle* rle, const SwOutline* outline, const SwBBox& renderRegion, bool antiAlias); +SwRle* rleRender(const SwBBox* bbox); +void rleFree(SwRle* rle); +void rleReset(SwRle* rle); +void rleMerge(SwRle* rle, SwRle* clip1, SwRle* clip2); +void rleClip(SwRle* rle, const SwRle* clip); +void rleClip(SwRle* rle, const SwBBox* clip); SwMpool* mpoolInit(uint32_t threads); bool mpoolTerm(SwMpool* mpool); @@ -567,9 +568,17 @@ bool rasterStroke(SwSurface* surface, SwShape* shape, uint8_t r, uint8_t g, uint bool rasterGradientStroke(SwSurface* surface, SwShape* shape, const Fill* fdata, uint8_t opacity); bool rasterClear(SwSurface* surface, uint32_t x, uint32_t y, uint32_t w, uint32_t h, pixel_t val = 0); void rasterPixel32(uint32_t *dst, uint32_t val, uint32_t offset, int32_t len); +void rasterTranslucentPixel32(uint32_t* dst, uint32_t* src, uint32_t len, uint8_t opacity); +void rasterPixel32(uint32_t* dst, uint32_t* src, uint32_t len, uint8_t opacity); void rasterGrayscale8(uint8_t *dst, uint8_t val, uint32_t offset, int32_t len); -void rasterUnpremultiply(Surface* surface); -void rasterPremultiply(Surface* surface); -bool rasterConvertCS(Surface* surface, ColorSpace to); +void rasterXYFlip(uint32_t* src, uint32_t* dst, int32_t stride, int32_t w, int32_t h, const SwBBox& bbox, bool flipped); +void rasterUnpremultiply(RenderSurface* surface); +void rasterPremultiply(RenderSurface* surface); +bool rasterConvertCS(RenderSurface* surface, ColorSpace to); + +bool effectGaussianBlur(SwCompositor* cmp, SwSurface* surface, const RenderEffectGaussianBlur* params); +bool effectGaussianBlurPrepare(RenderEffectGaussianBlur* effect); +bool effectDropShadow(SwCompositor* cmp, SwSurface* surfaces[2], const RenderEffectDropShadow* params, uint8_t opacity, bool direct); +bool effectDropShadowPrepare(RenderEffectDropShadow* effect); #endif /* _TVG_SW_COMMON_H_ */ diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwFill.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwFill.cpp index 631294ad40be..f70ba7a13d61 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwFill.cpp +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwFill.cpp @@ -66,15 +66,15 @@ static void _calculateCoefficients(const SwFill* fill, uint32_t x, uint32_t y, f static uint32_t _estimateAAMargin(const Fill* fdata) { constexpr float marginScalingFactor = 800.0f; - if (fdata->identifier() == TVG_CLASS_ID_RADIAL) { + if (fdata->type() == Type::RadialGradient) { auto radius = P(static_cast(fdata))->r; - return mathZero(radius) ? 0 : static_cast(marginScalingFactor / radius); + return tvg::zero(radius) ? 0 : static_cast(marginScalingFactor / radius); } auto grad = P(static_cast(fdata)); Point p1 {grad->x1, grad->y1}; Point p2 {grad->x2, grad->y2}; - auto length = mathLength(&p1, &p2); - return mathZero(length) ? 0 : static_cast(marginScalingFactor / length); + auto len = length(&p1, &p2); + return tvg::zero(len) ? 0 : static_cast(marginScalingFactor / len); } @@ -217,7 +217,7 @@ bool _prepareLinear(SwFill* fill, const LinearGradient* linear, const Matrix& tr auto len = fill->linear.dx * fill->linear.dx + fill->linear.dy * fill->linear.dy; if (len < FLOAT_EPSILON) { - if (mathZero(fill->linear.dx) && mathZero(fill->linear.dy)) { + if (tvg::zero(fill->linear.dx) && tvg::zero(fill->linear.dy)) { fill->solid = true; } return true; @@ -228,7 +228,7 @@ bool _prepareLinear(SwFill* fill, const LinearGradient* linear, const Matrix& tr fill->linear.offset = -fill->linear.dx * x1 - fill->linear.dy * y1; auto gradTransform = linear->transform(); - bool isTransformation = !mathIdentity((const Matrix*)(&gradTransform)); + bool isTransformation = !identity((const Matrix*)(&gradTransform)); if (isTransformation) { gradTransform = transform * gradTransform; @@ -239,7 +239,7 @@ bool _prepareLinear(SwFill* fill, const LinearGradient* linear, const Matrix& tr if (isTransformation) { Matrix invTransform; - if (!mathInverse(&gradTransform, &invTransform)) return false; + if (!inverse(&gradTransform, &invTransform)) return false; fill->linear.offset += fill->linear.dx * invTransform.e13 + fill->linear.dy * invTransform.e23; @@ -261,7 +261,7 @@ bool _prepareRadial(SwFill* fill, const RadialGradient* radial, const Matrix& tr auto fy = P(radial)->fy; auto fr = P(radial)->fr; - if (mathZero(r)) { + if (tvg::zero(r)) { fill->solid = true; return true; } @@ -295,7 +295,7 @@ bool _prepareRadial(SwFill* fill, const RadialGradient* radial, const Matrix& tr if (fill->radial.a > 0) fill->radial.invA = 1.0f / fill->radial.a; auto gradTransform = radial->transform(); - bool isTransformation = !mathIdentity((const Matrix*)(&gradTransform)); + bool isTransformation = !identity((const Matrix*)(&gradTransform)); if (isTransformation) gradTransform = transform * gradTransform; else { @@ -305,7 +305,7 @@ bool _prepareRadial(SwFill* fill, const RadialGradient* radial, const Matrix& tr if (isTransformation) { Matrix invTransform; - if (!mathInverse(&gradTransform, &invTransform)) return false; + if (!inverse(&gradTransform, &invTransform)) return false; fill->radial.a11 = invTransform.e11; fill->radial.a12 = invTransform.e12; fill->radial.a13 = invTransform.e13; @@ -553,7 +553,7 @@ void fillLinear(const SwFill* fill, uint32_t* dst, uint32_t y, uint32_t x, uint3 float inc = (fill->linear.dx) * (GRADIENT_STOP_SIZE - 1); if (opacity == 255) { - if (mathZero(inc)) { + if (tvg::zero(inc)) { auto color = _fixedPixel(fill, static_cast(t * FIXPT_SIZE)); for (uint32_t i = 0; i < len; ++i, ++dst, cmp += csize) { *dst = opBlendNormal(color, *dst, alpha(cmp)); @@ -584,7 +584,7 @@ void fillLinear(const SwFill* fill, uint32_t* dst, uint32_t y, uint32_t x, uint3 } } } else { - if (mathZero(inc)) { + if (tvg::zero(inc)) { auto color = _fixedPixel(fill, static_cast(t * FIXPT_SIZE)); for (uint32_t i = 0; i < len; ++i, ++dst, cmp += csize) { *dst = opBlendNormal(color, *dst, MULTIPLY(alpha(cmp), opacity)); @@ -626,7 +626,7 @@ void fillLinear(const SwFill* fill, uint8_t* dst, uint32_t y, uint32_t x, uint32 float t = (fill->linear.dx * rx + fill->linear.dy * ry + fill->linear.offset) * (GRADIENT_STOP_SIZE - 1); float inc = (fill->linear.dx) * (GRADIENT_STOP_SIZE - 1); - if (mathZero(inc)) { + if (tvg::zero(inc)) { auto src = MULTIPLY(a, A(_fixedPixel(fill, static_cast(t * FIXPT_SIZE)))); for (uint32_t i = 0; i < len; ++i, ++dst) { *dst = maskOp(src, *dst, ~src); @@ -643,7 +643,7 @@ void fillLinear(const SwFill* fill, uint8_t* dst, uint32_t y, uint32_t x, uint32 auto t2 = static_cast(t * FIXPT_SIZE); auto inc2 = static_cast(inc * FIXPT_SIZE); for (uint32_t j = 0; j < len; ++j, ++dst) { - auto src = MULTIPLY(_fixedPixel(fill, t2), a); + auto src = MULTIPLY(A(_fixedPixel(fill, t2)), a); *dst = maskOp(src, *dst, ~src); t2 += inc2; } @@ -651,7 +651,7 @@ void fillLinear(const SwFill* fill, uint8_t* dst, uint32_t y, uint32_t x, uint32 } else { uint32_t counter = 0; while (counter++ < len) { - auto src = MULTIPLY(_pixel(fill, t / GRADIENT_STOP_SIZE), a); + auto src = MULTIPLY(A(_pixel(fill, t / GRADIENT_STOP_SIZE)), a); *dst = maskOp(src, *dst, ~src); ++dst; t += inc; @@ -668,7 +668,7 @@ void fillLinear(const SwFill* fill, uint8_t* dst, uint32_t y, uint32_t x, uint32 float t = (fill->linear.dx * rx + fill->linear.dy * ry + fill->linear.offset) * (GRADIENT_STOP_SIZE - 1); float inc = (fill->linear.dx) * (GRADIENT_STOP_SIZE - 1); - if (mathZero(inc)) { + if (tvg::zero(inc)) { auto src = A(_fixedPixel(fill, static_cast(t * FIXPT_SIZE))); src = MULTIPLY(src, a); for (uint32_t i = 0; i < len; ++i, ++dst, ++cmp) { @@ -715,7 +715,7 @@ void fillLinear(const SwFill* fill, uint32_t* dst, uint32_t y, uint32_t x, uint3 float t = (fill->linear.dx * rx + fill->linear.dy * ry + fill->linear.offset) * (GRADIENT_STOP_SIZE - 1); float inc = (fill->linear.dx) * (GRADIENT_STOP_SIZE - 1); - if (mathZero(inc)) { + if (tvg::zero(inc)) { auto color = _fixedPixel(fill, static_cast(t * FIXPT_SIZE)); for (uint32_t i = 0; i < len; ++i, ++dst) { *dst = op(color, *dst, a); @@ -755,7 +755,7 @@ void fillLinear(const SwFill* fill, uint32_t* dst, uint32_t y, uint32_t x, uint3 float t = (fill->linear.dx * rx + fill->linear.dy * ry + fill->linear.offset) * (GRADIENT_STOP_SIZE - 1); float inc = (fill->linear.dx) * (GRADIENT_STOP_SIZE - 1); - if (mathZero(inc)) { + if (tvg::zero(inc)) { auto color = _fixedPixel(fill, static_cast(t * FIXPT_SIZE)); if (a == 255) { for (uint32_t i = 0; i < len; ++i, ++dst) { @@ -828,9 +828,9 @@ bool fillGenColorTable(SwFill* fill, const Fill* fdata, const Matrix& transform, fill->spread = fdata->spread(); - if (fdata->identifier() == TVG_CLASS_ID_LINEAR) { + if (fdata->type() == Type::LinearGradient) { if (!_prepareLinear(fill, static_cast(fdata), transform)) return false; - } else if (fdata->identifier() == TVG_CLASS_ID_RADIAL) { + } else if (fdata->type() == Type::RadialGradient) { if (!_prepareRadial(fill, static_cast(fdata), transform)) return false; } diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwImage.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwImage.cpp index 3fc64ce036a9..d2c02bb932df 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwImage.cpp +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwImage.cpp @@ -29,7 +29,7 @@ static inline bool _onlyShifted(const Matrix& m) { - if (mathEqual(m.e11, 1.0f) && mathEqual(m.e22, 1.0f) && mathZero(m.e12) && mathZero(m.e21)) return true; + if (tvg::equal(m.e11, 1.0f) && tvg::equal(m.e22, 1.0f) && tvg::zero(m.e12) && tvg::zero(m.e21)) return true; return false; } @@ -86,7 +86,7 @@ bool imagePrepare(SwImage* image, const Matrix& transform, const SwBBox& clipReg auto scaleY = sqrtf((transform.e22 * transform.e22) + (transform.e12 * transform.e12)); image->scale = (fabsf(scaleX - scaleY) > 0.01f) ? 1.0f : scaleX; - if (mathZero(transform.e12) && mathZero(transform.e21)) image->scaled = true; + if (tvg::zero(transform.e12) && tvg::zero(transform.e21)) image->scaled = true; else image->scaled = false; } diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwMath.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwMath.cpp index fb809c4f7e91..1ff99f6aece9 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwMath.cpp +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwMath.cpp @@ -44,7 +44,7 @@ SwFixed mathMean(SwFixed angle1, SwFixed angle2) } -bool mathSmallCubic(const SwPoint* base, SwFixed& angleIn, SwFixed& angleMid, SwFixed& angleOut) +int mathCubicAngle(const SwPoint* base, SwFixed& angleIn, SwFixed& angleMid, SwFixed& angleOut) { auto d1 = base[2] - base[3]; auto d2 = base[1] - base[2]; @@ -54,7 +54,7 @@ bool mathSmallCubic(const SwPoint* base, SwFixed& angleIn, SwFixed& angleMid, Sw if (d2.small()) { if (d3.small()) { angleIn = angleMid = angleOut = 0; - return true; + return -1; //ignoreable } else { angleIn = angleMid = angleOut = mathAtan(d3); } @@ -90,8 +90,8 @@ bool mathSmallCubic(const SwPoint* base, SwFixed& angleIn, SwFixed& angleMid, Sw auto theta1 = abs(mathDiff(angleIn, angleMid)); auto theta2 = abs(mathDiff(angleMid, angleOut)); - if ((theta1 < (SW_ANGLE_PI / 8)) && (theta2 < (SW_ANGLE_PI / 8))) return true; - return false; + if ((theta1 < (SW_ANGLE_PI / 8)) && (theta2 < (SW_ANGLE_PI / 8))) return 0; //small size + return 1; } @@ -179,7 +179,7 @@ SwFixed mathTan(SwFixed angle) SwFixed mathAtan(const SwPoint& pt) { if (pt.zero()) return 0; - return SwFixed(mathAtan2(TO_FLOAT(pt.y), TO_FLOAT(pt.x)) * (180.0f / MATH_PI) * 65536.0f); + return SwFixed(tvg::atan2(TO_FLOAT(pt.y), TO_FLOAT(pt.x)) * (180.0f / MATH_PI) * 65536.0f); } @@ -242,6 +242,15 @@ void mathSplitCubic(SwPoint* base) } +void mathSplitLine(SwPoint* base) +{ + base[2] = base[1]; + + base[1].x = (base[0].x + base[1].x) >> 1; + base[1].y = (base[0].y + base[1].y) >> 1; +} + + SwFixed mathDiff(SwFixed angle1, SwFixed angle2) { auto delta = angle2 - angle1; @@ -263,19 +272,19 @@ SwPoint mathTransform(const Point* to, const Matrix& transform) } -bool mathClipBBox(const SwBBox& clipper, SwBBox& clipee) +bool mathClipBBox(const SwBBox& clipper, SwBBox& clippee) { - clipee.max.x = (clipee.max.x < clipper.max.x) ? clipee.max.x : clipper.max.x; - clipee.max.y = (clipee.max.y < clipper.max.y) ? clipee.max.y : clipper.max.y; - clipee.min.x = (clipee.min.x > clipper.min.x) ? clipee.min.x : clipper.min.x; - clipee.min.y = (clipee.min.y > clipper.min.y) ? clipee.min.y : clipper.min.y; + clippee.max.x = (clippee.max.x < clipper.max.x) ? clippee.max.x : clipper.max.x; + clippee.max.y = (clippee.max.y < clipper.max.y) ? clippee.max.y : clipper.max.y; + clippee.min.x = (clippee.min.x > clipper.min.x) ? clippee.min.x : clipper.min.x; + clippee.min.y = (clippee.min.y > clipper.min.y) ? clippee.min.y : clipper.min.y; //Check valid region - if (clipee.max.x - clipee.min.x < 1 && clipee.max.y - clipee.min.y < 1) return false; + if (clippee.max.x - clippee.min.x < 1 && clippee.max.y - clippee.min.y < 1) return false; //Check boundary - if (clipee.min.x >= clipper.max.x || clipee.min.y >= clipper.max.y || - clipee.max.x <= clipper.min.x || clipee.max.y <= clipper.min.y) return false; + if (clippee.min.x >= clipper.max.x || clippee.min.y >= clipper.max.y || + clippee.max.x <= clipper.min.x || clippee.max.y <= clipper.min.y) return false; return true; } @@ -303,9 +312,7 @@ bool mathUpdateOutlineBBox(const SwOutline* outline, const SwBBox& clipRegion, S if (yMin > pt->y) yMin = pt->y; if (yMax < pt->y) yMax = pt->y; } - //Since no antialiasing is applied in the Fast Track case, - //the rasterization region has to be rearranged. - //https://github.com/Samsung/thorvg/issues/916 + if (fastTrack) { renderRegion.min.x = static_cast(nearbyint(xMin / 64.0f)); renderRegion.max.x = static_cast(nearbyint(xMax / 64.0f)); diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwPostEffect.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwPostEffect.cpp new file mode 100644 index 000000000000..fd8e532e12a3 --- /dev/null +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwPostEffect.cpp @@ -0,0 +1,410 @@ +/* + * Copyright (c) 2024 the ThorVG project. All rights reserved. + + * 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 "tvgMath.h" +#include "tvgSwCommon.h" + +/************************************************************************/ +/* Gaussian Blur Implementation */ +/************************************************************************/ + +struct SwGaussianBlur +{ + static constexpr int MAX_LEVEL = 3; + int level; + int kernel[MAX_LEVEL]; +}; + + +static void _gaussianExtendRegion(RenderRegion& region, int extra, int8_t direction) +{ + //bbox region expansion for feathering + if (direction != 2) { + region.x = -extra; + region.w = extra * 2; + } + if (direction != 1) { + region.y = -extra; + region.h = extra * 2; + } +} + + +static int _gaussianEdgeWrap(int end, int idx) +{ + auto r = idx % end; + return (r < 0) ? end + r : r; +} + + +static int _gaussianEdgeExtend(int end, int idx) +{ + if (idx < 0) return 0; + else if (idx >= end) return end - 1; + return idx; +} + + +static int _gaussianRemap(int end, int idx, int border) +{ + if (border == 1) return _gaussianEdgeWrap(end, idx); + return _gaussianEdgeExtend(end, idx); +} + + +//TODO: SIMD OPTIMIZATION? +static void _gaussianFilter(uint8_t* dst, uint8_t* src, int32_t stride, int32_t w, int32_t h, const SwBBox& bbox, int32_t dimension, int border, bool flipped) +{ + if (flipped) { + src += (bbox.min.x * stride + bbox.min.y) << 2; + dst += (bbox.min.x * stride + bbox.min.y) << 2; + } else { + src += (bbox.min.y * stride + bbox.min.x) << 2; + dst += (bbox.min.y * stride + bbox.min.x) << 2; + } + + auto iarr = 1.0f / (dimension + dimension + 1); + + #pragma omp parallel for + for (int y = 0; y < h; ++y) { + auto p = y * stride; + auto i = p * 4; //current index + auto l = -(dimension + 1); //left index + auto r = dimension; //right index + int acc[4] = {0, 0, 0, 0}; //sliding accumulator + + //initial accumulation + for (int x = l; x < r; ++x) { + auto id = (_gaussianRemap(w, x, border) + p) * 4; + acc[0] += src[id++]; + acc[1] += src[id++]; + acc[2] += src[id++]; + acc[3] += src[id]; + } + //perform filtering + for (int x = 0; x < w; ++x, ++r, ++l) { + auto rid = (_gaussianRemap(w, r, border) + p) * 4; + auto lid = (_gaussianRemap(w, l, border) + p) * 4; + acc[0] += src[rid++] - src[lid++]; + acc[1] += src[rid++] - src[lid++]; + acc[2] += src[rid++] - src[lid++]; + acc[3] += src[rid] - src[lid]; + dst[i++] = static_cast(acc[0] * iarr + 0.5f); + dst[i++] = static_cast(acc[1] * iarr + 0.5f); + dst[i++] = static_cast(acc[2] * iarr + 0.5f); + dst[i++] = static_cast(acc[3] * iarr + 0.5f); + } + } +} + + +static int _gaussianInit(SwGaussianBlur* data, float sigma, int quality) +{ + const auto MAX_LEVEL = SwGaussianBlur::MAX_LEVEL; + + if (tvg::zero(sigma)) return 0; + + data->level = int(SwGaussianBlur::MAX_LEVEL * ((quality - 1) * 0.01f)) + 1; + + //compute box kernel sizes + auto wl = (int) sqrt((12 * sigma / MAX_LEVEL) + 1); + if (wl % 2 == 0) --wl; + auto wu = wl + 2; + auto mi = (12 * sigma - MAX_LEVEL * wl * wl - 4 * MAX_LEVEL * wl - 3 * MAX_LEVEL) / (-4 * wl - 4); + auto m = int(mi + 0.5f); + auto extends = 0; + + for (int i = 0; i < data->level; i++) { + data->kernel[i] = ((i < m ? wl : wu) - 1) / 2; + extends += data->kernel[i]; + } + + return extends; +} + + +bool effectGaussianBlurPrepare(RenderEffectGaussianBlur* params) +{ + auto rd = (SwGaussianBlur*)malloc(sizeof(SwGaussianBlur)); + + auto extends = _gaussianInit(rd, params->sigma * params->sigma, params->quality); + + //invalid + if (extends == 0) { + params->invalid = true; + free(rd); + return false; + } + + _gaussianExtendRegion(params->extend, extends, params->direction); + + params->rd = rd; + + return true; +} + + +bool effectGaussianBlur(SwCompositor* cmp, SwSurface* surface, const RenderEffectGaussianBlur* params) +{ + if (cmp->image.channelSize != sizeof(uint32_t)) { + TVGERR("SW_ENGINE", "Not supported grayscale Gaussian Blur!"); + return false; + } + + auto& buffer = surface->compositor->image; + auto data = static_cast(params->rd); + auto& bbox = cmp->bbox; + auto w = (bbox.max.x - bbox.min.x); + auto h = (bbox.max.y - bbox.min.y); + auto stride = cmp->image.stride; + auto front = cmp->image.buf32; + auto back = buffer.buf32; + auto swapped = false; + + TVGLOG("SW_ENGINE", "GaussianFilter region(%ld, %ld, %ld, %ld) params(%f %d %d), level(%d)", bbox.min.x, bbox.min.y, bbox.max.x, bbox.max.y, params->sigma, params->direction, params->border, data->level); + + /* It is best to take advantage of the Gaussian blur’s separable property + by dividing the process into two passes. horizontal and vertical. + We can expect fewer calculations. */ + + //horizontal + if (params->direction != 2) { + for (int i = 0; i < data->level; ++i) { + _gaussianFilter(reinterpret_cast(back), reinterpret_cast(front), stride, w, h, bbox, data->kernel[i], params->border, false); + std::swap(front, back); + swapped = !swapped; + } + } + + //vertical. x/y flipping and horionztal access is pretty compatible with the memory architecture. + if (params->direction != 1) { + rasterXYFlip(front, back, stride, w, h, bbox, false); + std::swap(front, back); + + for (int i = 0; i < data->level; ++i) { + _gaussianFilter(reinterpret_cast(back), reinterpret_cast(front), stride, h, w, bbox, data->kernel[i], params->border, true); + std::swap(front, back); + swapped = !swapped; + } + + rasterXYFlip(front, back, stride, h, w, bbox, true); + std::swap(front, back); + } + + if (swapped) std::swap(cmp->image.buf8, buffer.buf8); + + return true; +} + +/************************************************************************/ +/* Drop Shadow Implementation */ +/************************************************************************/ + +struct SwDropShadow : SwGaussianBlur +{ + SwPoint offset; +}; + + +//TODO: SIMD OPTIMIZATION? +static void _dropShadowFilter(uint32_t* dst, uint32_t* src, int stride, int w, int h, const SwBBox& bbox, int32_t dimension, uint32_t color, bool flipped) +{ + if (flipped) { + src += (bbox.min.x * stride + bbox.min.y); + dst += (bbox.min.x * stride + bbox.min.y); + } else { + src += (bbox.min.y * stride + bbox.min.x); + dst += (bbox.min.y * stride + bbox.min.x); + } + auto iarr = 1.0f / (dimension + dimension + 1); + + #pragma omp parallel for + for (int y = 0; y < h; ++y) { + auto p = y * stride; + auto i = p; //current index + auto l = -(dimension + 1); //left index + auto r = dimension; //right index + int acc = 0; //sliding accumulator + + //initial accumulation + for (int x = l; x < r; ++x) { + auto id = _gaussianEdgeExtend(w, x) + p; + acc += A(src[id]); + } + //perform filtering + for (int x = 0; x < w; ++x, ++r, ++l) { + auto rid = _gaussianEdgeExtend(w, r) + p; + auto lid = _gaussianEdgeExtend(w, l) + p; + acc += A(src[rid]) - A(src[lid]); + dst[i++] = ALPHA_BLEND(color, static_cast(acc * iarr + 0.5f)); + } + } +} + + +static void _dropShadowShift(uint32_t* dst, uint32_t* src, int stride, SwBBox& region, SwPoint& offset, uint8_t opacity, bool direct) +{ + src += (region.min.y * stride + region.min.x); + dst += (region.min.y * stride + region.min.x); + + auto w = region.max.x - region.min.x; + auto h = region.max.y - region.min.y; + auto translucent = (direct || opacity < 255); + + //shift offset + if (region.min.x + offset.x < 0) src -= offset.x; + else dst += offset.x; + + if (region.min.y + offset.y < 0) src -= (offset.y * stride); + else dst += (offset.y * stride); + + for (auto y = 0; y < h; ++y) { + if (translucent) rasterTranslucentPixel32(dst, src, w, opacity); + else rasterPixel32(dst, src, w, opacity); + src += stride; + dst += stride; + } +} + + +static void _dropShadowExtendRegion(RenderRegion& region, int extra, SwPoint& offset) +{ + //bbox region expansion for feathering + region.x = -extra; + region.w = extra * 2; + region.y = -extra; + region.h = extra * 2; + + region.x = std::min(region.x + (int32_t)offset.x, region.x); + region.y = std::min(region.y + (int32_t)offset.y, region.y); + region.w += abs(offset.x); + region.h += abs(offset.y); +} + + +bool effectDropShadowPrepare(RenderEffectDropShadow* params) +{ + auto rd = (SwDropShadow*)malloc(sizeof(SwDropShadow)); + + //compute box kernel sizes + auto extends = _gaussianInit(rd, params->sigma * params->sigma, params->quality); + + //invalid + if (extends == 0 || params->color[3] == 0) { + params->invalid = true; + free(rd); + return false; + } + + //offset + if (params->distance > 0.0f) { + auto radian = tvg::deg2rad(90.0f - params->angle); + rd->offset = {(SwCoord)(params->distance * cosf(radian)), (SwCoord)(-1.0f * params->distance * sinf(radian))}; + } else { + rd->offset = {0, 0}; + } + + //bbox region expansion for feathering + _dropShadowExtendRegion(params->extend, extends, rd->offset); + + params->rd = rd; + + return true; +} + + +//A quite same integration with effectGaussianBlur(). See it for detailed comments. +//surface[0]: the original image, to overlay it into the filtered image. +//surface[1]: temporary buffer for generating the filtered image. +bool effectDropShadow(SwCompositor* cmp, SwSurface* surface[2], const RenderEffectDropShadow* params, uint8_t opacity, bool direct) +{ + if (cmp->image.channelSize != sizeof(uint32_t)) { + TVGERR("SW_ENGINE", "Not supported grayscale Drop Shadow!"); + return false; + } + + //FIXME: if the body is partially visible due to clipping, the shadow also becomes partially visible. + + auto data = static_cast(params->rd); + auto& bbox = cmp->bbox; + auto w = (bbox.max.x - bbox.min.x); + auto h = (bbox.max.y - bbox.min.y); + + //outside the screen + if (abs(data->offset.x) >= w || abs(data->offset.y) >= h) return true; + + SwImage* buffer[] = {&surface[0]->compositor->image, &surface[1]->compositor->image}; + auto color = cmp->recoverSfc->join(params->color[0], params->color[1], params->color[2], 255); + auto stride = cmp->image.stride; + auto front = cmp->image.buf32; + auto back = buffer[1]->buf32; + opacity = MULTIPLY(params->color[3], opacity); + + TVGLOG("SW_ENGINE", "DropShadow region(%ld, %ld, %ld, %ld) params(%f %f %f), level(%d)", bbox.min.x, bbox.min.y, bbox.max.x, bbox.max.y, params->angle, params->distance, params->sigma, data->level); + + //saving the original image in order to overlay it into the filtered image. + _dropShadowFilter(back, front, stride, w, h, bbox, data->kernel[0], color, false); + std::swap(front, buffer[0]->buf32); + std::swap(front, back); + + //horizontal + for (int i = 1; i < data->level; ++i) { + _dropShadowFilter(back, front, stride, w, h, bbox, data->kernel[i], color, false); + std::swap(front, back); + } + + //vertical + rasterXYFlip(front, back, stride, w, h, bbox, false); + std::swap(front, back); + + for (int i = 0; i < data->level; ++i) { + _dropShadowFilter(back, front, stride, h, w, bbox, data->kernel[i], color, true); + std::swap(front, back); + } + + rasterXYFlip(front, back, stride, h, w, bbox, true); + std::swap(cmp->image.buf32, back); + + //draw to the main surface directly + if (direct) { + _dropShadowShift(cmp->recoverSfc->buf32, cmp->image.buf32, stride, bbox, data->offset, opacity, direct); + std::swap(cmp->image.buf32, buffer[0]->buf32); + return true; + } + + //draw to the intermediate surface + rasterClear(surface[1], bbox.min.x, bbox.min.y, w, h); + _dropShadowShift(buffer[1]->buf32, cmp->image.buf32, stride, bbox, data->offset, opacity, direct); + std::swap(cmp->image.buf32, buffer[1]->buf32); + + //compositing shadow and body + auto s = buffer[0]->buf32 + (bbox.min.y * buffer[0]->stride + bbox.min.x); + auto d = cmp->image.buf32 + (bbox.min.y * cmp->image.stride + bbox.min.x); + + for (auto y = 0; y < h; ++y) { + rasterTranslucentPixel32(d, s, w, 255); + s += buffer[0]->stride; + d += cmp->image.stride; + } + + return true; +} diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRaster.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRaster.cpp index b9e95f88cf79..18ffc18e1e0e 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRaster.cpp +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRaster.cpp @@ -490,7 +490,7 @@ static bool _rasterRect(SwSurface* surface, const SwBBox& region, uint8_t r, uin /* Rle */ /************************************************************************/ -static bool _rasterCompositeMaskedRle(SwSurface* surface, SwRleData* rle, SwMask maskOp, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool _rasterCompositeMaskedRle(SwSurface* surface, SwRle* rle, SwMask maskOp, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { auto span = rle->spans; auto cbuffer = surface->compositor->image.buf8; @@ -510,7 +510,7 @@ static bool _rasterCompositeMaskedRle(SwSurface* surface, SwRleData* rle, SwMask } -static bool _rasterDirectMaskedRle(SwSurface* surface, SwRleData* rle, SwMask maskOp, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool _rasterDirectMaskedRle(SwSurface* surface, SwRle* rle, SwMask maskOp, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { auto span = rle->spans; auto cbuffer = surface->compositor->image.buf8; @@ -531,7 +531,7 @@ static bool _rasterDirectMaskedRle(SwSurface* surface, SwRleData* rle, SwMask ma } -static bool _rasterMaskedRle(SwSurface* surface, SwRleData* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool _rasterMaskedRle(SwSurface* surface, SwRle* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { TVGLOG("SW_ENGINE", "Masked(%d) Rle", (int)surface->compositor->method); @@ -545,7 +545,7 @@ static bool _rasterMaskedRle(SwSurface* surface, SwRleData* rle, uint8_t r, uint } -static bool _rasterMattedRle(SwSurface* surface, SwRleData* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool _rasterMattedRle(SwSurface* surface, SwRle* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { TVGLOG("SW_ENGINE", "Matted(%d) Rle", (int)surface->compositor->method); @@ -568,10 +568,8 @@ static bool _rasterMattedRle(SwSurface* surface, SwRleData* rle, uint8_t r, uint *dst = tmp + ALPHA_BLEND(*dst, IA(tmp)); } } - return true; - } //8bit grayscale - if (surface->channelSize == sizeof(uint8_t)) { + } else if (surface->channelSize == sizeof(uint8_t)) { uint8_t src; for (uint32_t i = 0; i < rle->size; ++i, ++span) { auto dst = &surface->buf8[span->y * surface->stride + span->x]; @@ -582,13 +580,12 @@ static bool _rasterMattedRle(SwSurface* surface, SwRleData* rle, uint8_t r, uint *dst = INTERPOLATE8(src, *dst, alpha(cmp)); } } - return true; } - return false; + return true; } -static bool _rasterBlendingRle(SwSurface* surface, const SwRleData* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool _rasterBlendingRle(SwSurface* surface, const SwRle* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { if (surface->channelSize != sizeof(uint32_t)) return false; @@ -612,7 +609,7 @@ static bool _rasterBlendingRle(SwSurface* surface, const SwRleData* rle, uint8_t } -static bool _rasterTranslucentRle(SwSurface* surface, const SwRleData* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool _rasterTranslucentRle(SwSurface* surface, const SwRle* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { #if defined(THORVG_AVX_VECTOR_SUPPORT) return avxRasterTranslucentRle(surface, rle, r, g, b, a); @@ -624,7 +621,7 @@ static bool _rasterTranslucentRle(SwSurface* surface, const SwRleData* rle, uint } -static bool _rasterSolidRle(SwSurface* surface, const SwRleData* rle, uint8_t r, uint8_t g, uint8_t b) +static bool _rasterSolidRle(SwSurface* surface, const SwRle* rle, uint8_t r, uint8_t g, uint8_t b) { auto span = rle->spans; @@ -661,7 +658,7 @@ static bool _rasterSolidRle(SwSurface* surface, const SwRleData* rle, uint8_t r, } -static bool _rasterRle(SwSurface* surface, SwRleData* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool _rasterRle(SwSurface* surface, SwRle* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { if (!rle) return false; @@ -697,66 +694,9 @@ static bool _rasterRle(SwSurface* surface, SwRleData* rle, uint8_t r, uint8_t g, auto sx = (x) * itransform->e11 + itransform->e13 - 0.49f; \ if (sx <= -0.5f || (uint32_t)(sx + 0.5f) >= image->w) continue; \ - -#if 0 //Enable it when GRAYSCALE image is supported -static bool _rasterCompositeScaledMaskedRleImage(SwSurface* surface, const SwImage* image, const Matrix* itransform, const SwBBox& region, SwMask maskOp, uint8_t opacity) -{ - auto scaleMethod = image->scale < DOWN_SCALE_TOLERANCE ? _interpDownScaler : _interpUpScaler; - auto sampleSize = _sampleSize(image->scale); - auto span = image->rle->spans; - int32_t miny = 0, maxy = 0; - - for (uint32_t i = 0; i < image->rle->size; ++i, ++span) { - SCALED_IMAGE_RANGE_Y(span->y) - auto cmp = &surface->compositor->image.buf8[span->y * surface->compositor->image.stride + span->x]; - auto a = MULTIPLY(span->coverage, opacity); - for (uint32_t x = static_cast(span->x); x < static_cast(span->x) + span->len; ++x, ++cmp) { - SCALED_IMAGE_RANGE_X - auto src = scaleMethod(image->buf8, image->stride, image->w, image->h, sx, sy, miny, maxy, sampleSize); - if (a < 255) src = MULTIPLY(src, a); - *cmp = maskOp(src, *cmp, ~src); - } - } - return true; -} - - -static bool _rasterDirectScaledMaskedRleImage(SwSurface* surface, const SwImage* image, const Matrix* itransform, const SwBBox& region, SwMask maskOp, uint8_t opacity) -{ - auto scaleMethod = image->scale < DOWN_SCALE_TOLERANCE ? _interpDownScaler : _interpUpScaler; - auto sampleSize = _sampleSize(image->scale); - auto span = image->rle->spans; - int32_t miny = 0, maxy = 0; - - for (uint32_t i = 0; i < image->rle->size; ++i, ++span) { - SCALED_IMAGE_RANGE_Y(span->y) - auto cmp = &surface->compositor->image.buf8[span->y * surface->compositor->image.stride + span->x]; - auto dst = &surface->buf8[span->y * surface->stride + span->x]; - auto a = MULTIPLY(span->coverage, opacity); - for (uint32_t x = static_cast(span->x); x < static_cast(span->x) + span->len; ++x, ++cmp, ++dst) { - SCALED_IMAGE_RANGE_X - auto src = scaleMethod(image->buf8, image->stride, image->w, image->h, sx, sy, miny, maxy, sampleSize); - if (a < 255) src = MULTIPLY(src, a); - src = maskOp(src, *cmp, 0); //not use alpha - *dst = src + MULTIPLY(*dst, ~src); - } - } - return _compositeMaskImage(surface, &surface->compositor->image, surface->compositor->bbox); -} -#endif - static bool _rasterScaledMaskedRleImage(SwSurface* surface, const SwImage* image, const Matrix* itransform, const SwBBox& region, uint8_t opacity) { -#if 0 //Enable it when GRAYSCALE image is supported - TVGLOG("SW_ENGINE", "Scaled Masked(%d) Rle Image", (int)surface->compositor->method); - - //8bit masking channels composition - if (surface->channelSize != sizeof(uint8_t)) return false; - - auto maskOp = _getMaskOp(surface->compositor->method); - if (_direct(surface->compositor->method)) return _rasterDirectScaledMaskedRleImage(surface, image, itransform, region, maskOp, opacity); - else return _rasterCompositeScaledMaskedRleImage(surface, image, itransform, region, maskOp, opacity); -#endif + TVGERR("SW_ENGINE", "Not Supported Scaled Masked(%d) Rle Image", (int)surface->compositor->method); return false; } @@ -784,7 +724,6 @@ static bool _rasterScaledMattedRleImage(SwSurface* surface, const SwImage* image *dst = src + ALPHA_BLEND(*dst, IA(src)); } } - return true; } @@ -851,7 +790,7 @@ static bool _scaledRleImage(SwSurface* surface, const SwImage* image, const Matr Matrix itransform; - if (!mathInverse(&transform, &itransform)) return true; + if (!inverse(&transform, &itransform)) return true; if (_compositing(surface)) { if (_matting(surface)) return _rasterScaledMattedRleImage(surface, image, &itransform, region, opacity); @@ -869,75 +808,6 @@ static bool _scaledRleImage(SwSurface* surface, const SwImage* image, const Matr /* RLE Direct Image */ /************************************************************************/ -#if 0 //Enable it when GRAYSCALE image is supported -static bool _rasterCompositeDirectMaskedRleImage(SwSurface* surface, const SwImage* image, SwMask maskOp, uint8_t opacity) -{ - auto span = image->rle->spans; - auto cbuffer = surface->compositor->image.buf8; - auto ctride = surface->compositor->image.stride; - - for (uint32_t i = 0; i < image->rle->size; ++i, ++span) { - auto src = image->buf8 + (span->y + image->oy) * image->stride + (span->x + image->ox); - auto cmp = &cbuffer[span->y * ctride + span->x]; - auto alpha = MULTIPLY(span->coverage, opacity); - if (alpha == 255) { - for (uint32_t x = 0; x < span->len; ++x, ++src, ++cmp) { - *cmp = maskOp(*src, *cmp, ~*src); - } - } else { - for (uint32_t x = 0; x < span->len; ++x, ++src, ++cmp) { - auto tmp = MULTIPLY(*src, alpha); - *cmp = maskOp(*src, *cmp, ~tmp); - } - } - } - return _compositeMaskImage(surface, &surface->compositor->image, surface->compositor->bbox); -} - - -static bool _rasterDirectDirectMaskedRleImage(SwSurface* surface, const SwImage* image, SwMask maskOp, uint8_t opacity) -{ - auto span = image->rle->spans; - auto cbuffer = surface->compositor->image.buf8; - auto ctride = surface->compositor->image.stride; - - for (uint32_t i = 0; i < image->rle->size; ++i, ++span) { - auto src = image->buf8 + (span->y + image->oy) * image->stride + (span->x + image->ox); - auto cmp = &cbuffer[span->y * ctride + span->x]; - auto dst = &surface->buf8[span->y * surface->stride + span->x]; - auto alpha = MULTIPLY(span->coverage, opacity); - if (alpha == 255) { - for (uint32_t x = 0; x < span->len; ++x, ++src, ++cmp, ++dst) { - auto tmp = maskOp(*src, *cmp, 0); //not use alpha - *dst = INTERPOLATE8(tmp, *dst, (255 - tmp)); - } - } else { - for (uint32_t x = 0; x < span->len; ++x, ++src, ++cmp, ++dst) { - auto tmp = maskOp(MULTIPLY(*src, alpha), *cmp, 0); //not use alpha - *dst = INTERPOLATE8(tmp, *dst, (255 - tmp)); - } - } - } - return true; -} -#endif - -static bool _rasterDirectMaskedRleImage(SwSurface* surface, const SwImage* image, uint8_t opacity) -{ -#if 0 //Enable it when GRAYSCALE image is supported - TVGLOG("SW_ENGINE", "Direct Masked(%d) Rle Image", (int)surface->compositor->method); - - //8bit masking channels composition - if (surface->channelSize != sizeof(uint8_t)) return false; - - auto maskOp = _getMaskOp(surface->compositor->method); - if (_direct(surface->compositor->method)) _rasterDirectDirectMaskedRleImage(surface, image, maskOp, opacity); - else return _rasterCompositeDirectMaskedRleImage(surface, image, maskOp, opacity); -#endif - return false; -} - - static bool _rasterDirectMattedRleImage(SwSurface* surface, const SwImage* image, uint8_t opacity) { TVGLOG("SW_ENGINE", "Direct Matted(%d) Rle Image", (int)surface->compositor->method); @@ -999,21 +869,19 @@ static bool _rasterDirectRleImage(SwSurface* surface, const SwImage* image, uint auto dst = &surface->buf32[span->y * surface->stride + span->x]; auto img = image->buf32 + (span->y + image->oy) * image->stride + (span->x + image->ox); auto alpha = MULTIPLY(span->coverage, opacity); - if (alpha == 255) { - for (uint32_t x = 0; x < span->len; ++x, ++dst, ++img) { - *dst = *img + ALPHA_BLEND(*dst, IA(*img)); - } - } else { - for (uint32_t x = 0; x < span->len; ++x, ++dst, ++img) { - auto src = ALPHA_BLEND(*img, alpha); - *dst = src + ALPHA_BLEND(*dst, IA(src)); - } - } + rasterTranslucentPixel32(dst, img, span->len, alpha); } return true; } +static bool _rasterDirectMaskedRleImage(SwSurface* surface, const SwImage* image, uint8_t opacity) +{ + TVGERR("SW_ENGINE", "Not Supported Direct Masked(%d) Rle Image", (int)surface->compositor->method); + return false; +} + + static bool _directRleImage(SwSurface* surface, const SwImage* image, uint8_t opacity) { if (surface->channelSize == sizeof(uint8_t)) { @@ -1037,67 +905,9 @@ static bool _directRleImage(SwSurface* surface, const SwImage* image, uint8_t op /*Scaled Image */ /************************************************************************/ -#if 0 //Enable it when GRAYSCALE image is supported -static bool _rasterCompositeScaledMaskedImage(SwSurface* surface, const SwImage* image, const Matrix* itransform, const SwBBox& region, SwMask maskOp, uint8_t opacity) -{ - auto scaleMethod = image->scale < DOWN_SCALE_TOLERANCE ? _interpDownScaler : _interpUpScaler; - auto sampleSize = _sampleSize(image->scale); - auto cstride = surface->compositor->image.stride; - auto cbuffer = surface->compositor->image.buf8 + (region.min.y * cstride + region.min.x); - int32_t miny = 0, maxy = 0; - - for (auto y = region.min.y; y < region.max.y; ++y) { - SCALED_IMAGE_RANGE_Y(y) - auto cmp = cbuffer; - for (auto x = region.min.x; x < region.max.x; ++x, ++cmp) { - SCALED_IMAGE_RANGE_X - auto src = scaleMethod(image->buf8, image->stride, image->w, image->h, sx, sy, miny, maxy, sampleSize); - if (opacity < 255) src = MULTIPLY(src, opacity); - *cmp = maskOp(src, *cmp, ~src); - } - cbuffer += cstride; - } - return _compositeMaskImage(surface, &surface->compositor->image, surface->compositor->bbox); -} - - -static bool _rasterDirectScaledMaskedImage(SwSurface* surface, const SwImage* image, const Matrix* itransform, const SwBBox& region, SwMask maskOp, uint8_t opacity) -{ - auto scaleMethod = image->scale < DOWN_SCALE_TOLERANCE ? _interpDownScaler : _interpUpScaler; - auto sampleSize = _sampleSize(image->scale); - auto cstride = surface->compositor->image.stride; - auto cbuffer = surface->compositor->image.buf8 + (region.min.y * cstride + region.min.x); - auto dbuffer = surface->buf8 + (region.min.y * surface->stride + region.min.x); - int32_t miny = 0, maxy = 0; - - for (auto y = region.min.y; y < region.max.y; ++y) { - SCALED_IMAGE_RANGE_Y(y) - auto cmp = cbuffer; - auto dst = dbuffer; - for (auto x = region.min.x; x < region.max.x; ++x, ++cmp, ++dst) { - SCALED_IMAGE_RANGE_X - auto src = scaleMethod(image->buf8, image->stride, image->w, image->h, sx, sy, miny, maxy, sampleSize); - if (opacity < 255) src = MULTIPLY(src, opacity); - src = maskOp(src, *cmp, 0); //not use alpha - *dst = src + MULTIPLY(*dst, ~src); - } - cbuffer += cstride; - dbuffer += surface->stride; - } - return true; -} -#endif - static bool _rasterScaledMaskedImage(SwSurface* surface, const SwImage* image, const Matrix* itransform, const SwBBox& region, uint8_t opacity) { - TVGERR("SW_ENGINE", "Not supported ScaledMaskedImage!"); -#if 0 //Enable it when GRAYSCALE image is supported - TVGLOG("SW_ENGINE", "Scaled Masked(%d) Image [Region: %lu %lu %lu %lu]", (int)surface->compositor->method, region.min.x, region.min.y, region.max.x - region.min.x, region.max.y - region.min.y); - - auto maskOp = _getMaskOp(surface->compositor->method); - if (_direct(surface->compositor->method)) return _rasterDirectScaledMaskedImage(surface, image, itransform, region, maskOp, opacity); - else return _rasterCompositeScaledMaskedImage(surface, image, itransform, region, maskOp, opacity); -#endif + TVGERR("SW_ENGINE", "Not Supported Scaled Masked Image!"); return false; } @@ -1202,7 +1012,7 @@ static bool _scaledImage(SwSurface* surface, const SwImage* image, const Matrix& { Matrix itransform; - if (!mathInverse(&transform, &itransform)) return true; + if (!inverse(&transform, &itransform)) return true; if (_compositing(surface)) { if (_matting(surface)) return _rasterScaledMattedImage(surface, image, &itransform, region, opacity); @@ -1220,78 +1030,9 @@ static bool _scaledImage(SwSurface* surface, const SwImage* image, const Matrix& /* Direct Image */ /************************************************************************/ -#if 0 //Enable it when GRAYSCALE image is supported -static bool _rasterCompositeDirectMaskedImage(SwSurface* surface, const SwImage* image, const SwBBox& region, SwMask maskOp, uint8_t opacity) -{ - auto h = static_cast(region.max.y - region.min.y); - auto w = static_cast(region.max.x - region.min.x); - auto cstride = surface->compositor->image.stride; - - auto cbuffer = surface->compositor->image.buf8 + (region.min.y * cstride + region.min.x); //compositor buffer - auto sbuffer = image->buf8 + (region.min.y + image->oy) * image->stride + (region.min.x + image->ox); - - for (uint32_t y = 0; y < h; ++y) { - auto cmp = cbuffer; - auto src = sbuffer; - if (opacity == 255) { - for (uint32_t x = 0; x < w; ++x, ++src, ++cmp) { - *cmp = maskOp(*src, *cmp, ~*src); - } - } else { - for (uint32_t x = 0; x < w; ++x, ++src, ++cmp) { - auto tmp = MULTIPLY(*src, opacity); - *cmp = maskOp(tmp, *cmp, ~tmp); - } - } - cbuffer += cstride; - sbuffer += image->stride; - } - return _compositeMaskImage(surface, &surface->compositor->image, surface->compositor->bbox); -} - - -static bool _rasterDirectDirectMaskedImage(SwSurface* surface, const SwImage* image, const SwBBox& region, SwMask maskOp, uint8_t opacity) -{ - auto h = static_cast(region.max.y - region.min.y); - auto w = static_cast(region.max.x - region.min.x); - auto cstride = surface->compositor->image.stride; - - auto cbuffer = surface->compositor->image.buf32 + (region.min.y * cstride + region.min.x); //compositor buffer - auto dbuffer = surface->buf8 + (region.min.y * surface->stride + region.min.x); //destination buffer - auto sbuffer = image->buf8 + (region.min.y + image->oy) * image->stride + (region.min.x + image->ox); - - for (uint32_t y = 0; y < h; ++y) { - auto cmp = cbuffer; - auto dst = dbuffer; - auto src = sbuffer; - if (opacity == 255) { - for (uint32_t x = 0; x < w; ++x, ++src, ++cmp, ++dst) { - auto tmp = maskOp(*src, *cmp, 0); //not use alpha - *dst = tmp + MULTIPLY(*dst, ~tmp); - } - } else { - for (uint32_t x = 0; x < w; ++x, ++src, ++cmp, ++dst) { - auto tmp = maskOp(MULTIPLY(*src, opacity), *cmp, 0); //not use alpha - *dst = tmp + MULTIPLY(*dst, ~tmp); - } - } - cbuffer += cstride; - dbuffer += surface->stride; - sbuffer += image->stride; - } - return true; -} -#endif - static bool _rasterDirectMaskedImage(SwSurface* surface, const SwImage* image, const SwBBox& region, uint8_t opacity) { - TVGERR("SW_ENGINE", "Not Supported: Direct Masked(%d) Image [Region: %lu %lu %lu %lu]", (int)surface->compositor->method, region.min.x, region.min.y, region.max.x - region.min.x, region.max.y - region.min.y); - -#if 0 //Enable it when GRAYSCALE image is supported - auto maskOp = _getMaskOp(surface->compositor->method); - if (_direct(surface->compositor->method)) return _rasterDirectDirectMaskedImage(surface, image, region, maskOp, opacity); - else return _rasterCompositeDirectMaskedImage(surface, image, region, maskOp, opacity); -#endif + TVGERR("SW_ENGINE", "Not Supported: Direct Masked Image"); return false; } @@ -1394,37 +1135,24 @@ static bool _rasterDirectImage(SwSurface* surface, const SwImage* image, const S //32bits channels if (surface->channelSize == sizeof(uint32_t)) { auto dbuffer = &surface->buf32[region.min.y * surface->stride + region.min.x]; - for (auto y = region.min.y; y < region.max.y; ++y) { - auto dst = dbuffer; - auto src = sbuffer; - if (opacity == 255) { - for (auto x = region.min.x; x < region.max.x; x++, dst++, src++) { - *dst = *src + ALPHA_BLEND(*dst, IA(*src)); - } - } else { - for (auto x = region.min.x; x < region.max.x; ++x, ++dst, ++src) { - auto tmp = ALPHA_BLEND(*src, opacity); - *dst = tmp + ALPHA_BLEND(*dst, IA(tmp)); - } - } + rasterTranslucentPixel32(dbuffer, sbuffer, region.max.x - region.min.x, opacity); dbuffer += surface->stride; sbuffer += image->stride; } //8bits grayscale } else if (surface->channelSize == sizeof(uint8_t)) { auto dbuffer = &surface->buf8[region.min.y * surface->stride + region.min.x]; - for (auto y = region.min.y; y < region.max.y; ++y, dbuffer += surface->stride, sbuffer += image->stride) { auto dst = dbuffer; auto src = sbuffer; if (opacity == 255) { for (auto x = region.min.x; x < region.max.x; ++x, ++dst, ++src) { - *dst = *src + MULTIPLY(*dst, ~*src); + *dst = *src + MULTIPLY(*dst, IA(*src)); } } else { for (auto x = region.min.x; x < region.max.x; ++x, ++dst, ++src) { - *dst = INTERPOLATE8(*src, *dst, opacity); + *dst = INTERPOLATE8(A(*src), *dst, opacity); } } } @@ -1602,13 +1330,23 @@ static bool _rasterBlendingGradientRect(SwSurface* surface, const SwBBox& region template static bool _rasterTranslucentGradientRect(SwSurface* surface, const SwBBox& region, const SwFill* fill) { - auto buffer = surface->buf32 + (region.min.y * surface->stride) + region.min.x; auto h = static_cast(region.max.y - region.min.y); auto w = static_cast(region.max.x - region.min.x); - for (uint32_t y = 0; y < h; ++y) { - fillMethod()(fill, buffer, region.min.y + y, region.min.x, w, opBlendPreNormal, 255); - buffer += surface->stride; + //32 bits + if (surface->channelSize == sizeof(uint32_t)) { + auto buffer = surface->buf32 + (region.min.y * surface->stride) + region.min.x; + for (uint32_t y = 0; y < h; ++y) { + fillMethod()(fill, buffer, region.min.y + y, region.min.x, w, opBlendPreNormal, 255); + buffer += surface->stride; + } + //8 bits + } else if (surface->channelSize == sizeof(uint8_t)) { + auto buffer = surface->buf8 + (region.min.y * surface->stride) + region.min.x; + for (uint32_t y = 0; y < h; ++y) { + fillMethod()(fill, buffer, region.min.y + y, region.min.x, w, _opMaskAdd, 255); + buffer += surface->stride; + } } return true; } @@ -1617,12 +1355,23 @@ static bool _rasterTranslucentGradientRect(SwSurface* surface, const SwBBox& reg template static bool _rasterSolidGradientRect(SwSurface* surface, const SwBBox& region, const SwFill* fill) { - auto buffer = surface->buf32 + (region.min.y * surface->stride) + region.min.x; auto w = static_cast(region.max.x - region.min.x); auto h = static_cast(region.max.y - region.min.y); - for (uint32_t y = 0; y < h; ++y) { - fillMethod()(fill, buffer + y * surface->stride, region.min.y + y, region.min.x, w, opBlendSrcOver, 255); + //32 bits + if (surface->channelSize == sizeof(uint32_t)) { + auto buffer = surface->buf32 + (region.min.y * surface->stride) + region.min.x; + for (uint32_t y = 0; y < h; ++y) { + fillMethod()(fill, buffer, region.min.y + y, region.min.x, w, opBlendSrcOver, 255); + buffer += surface->stride; + } + //8 bits + } else if (surface->channelSize == sizeof(uint8_t)) { + auto buffer = surface->buf8 + (region.min.y * surface->stride) + region.min.x; + for (uint32_t y = 0; y < h; ++y) { + fillMethod()(fill, buffer, region.min.y + y, region.min.x, w, _opMaskNone, 255); + buffer += surface->stride; + } } return true; } @@ -1663,7 +1412,7 @@ static bool _rasterRadialGradientRect(SwSurface* surface, const SwBBox& region, /************************************************************************/ template -static bool _rasterCompositeGradientMaskedRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill, SwMask maskOp) +static bool _rasterCompositeGradientMaskedRle(SwSurface* surface, const SwRle* rle, const SwFill* fill, SwMask maskOp) { auto span = rle->spans; auto cstride = surface->compositor->image.stride; @@ -1678,7 +1427,7 @@ static bool _rasterCompositeGradientMaskedRle(SwSurface* surface, const SwRleDat template -static bool _rasterDirectGradientMaskedRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill, SwMask maskOp) +static bool _rasterDirectGradientMaskedRle(SwSurface* surface, const SwRle* rle, const SwFill* fill, SwMask maskOp) { auto span = rle->spans; auto cstride = surface->compositor->image.stride; @@ -1695,7 +1444,7 @@ static bool _rasterDirectGradientMaskedRle(SwSurface* surface, const SwRleData* template -static bool _rasterGradientMaskedRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill) +static bool _rasterGradientMaskedRle(SwSurface* surface, const SwRle* rle, const SwFill* fill) { auto method = surface->compositor->method; @@ -1710,7 +1459,7 @@ static bool _rasterGradientMaskedRle(SwSurface* surface, const SwRleData* rle, c template -static bool _rasterGradientMattedRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill) +static bool _rasterGradientMattedRle(SwSurface* surface, const SwRle* rle, const SwFill* fill) { TVGLOG("SW_ENGINE", "Matted(%d) Rle Linear Gradient", (int)surface->compositor->method); @@ -1729,7 +1478,7 @@ static bool _rasterGradientMattedRle(SwSurface* surface, const SwRleData* rle, c template -static bool _rasterBlendingGradientRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill) +static bool _rasterBlendingGradientRle(SwSurface* surface, const SwRle* rle, const SwFill* fill) { auto span = rle->spans; @@ -1742,7 +1491,7 @@ static bool _rasterBlendingGradientRle(SwSurface* surface, const SwRleData* rle, template -static bool _rasterTranslucentGradientRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill) +static bool _rasterTranslucentGradientRle(SwSurface* surface, const SwRle* rle, const SwFill* fill) { auto span = rle->spans; @@ -1757,7 +1506,7 @@ static bool _rasterTranslucentGradientRle(SwSurface* surface, const SwRleData* r } else if (surface->channelSize == sizeof(uint8_t)) { for (uint32_t i = 0; i < rle->size; ++i, ++span) { auto dst = &surface->buf8[span->y * surface->stride + span->x]; - fillMethod()(fill, dst, span->y, span->x, span->len, _opMaskAdd, 255); + fillMethod()(fill, dst, span->y, span->x, span->len, _opMaskAdd, span->coverage); } } return true; @@ -1765,7 +1514,7 @@ static bool _rasterTranslucentGradientRle(SwSurface* surface, const SwRleData* r template -static bool _rasterSolidGradientRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill) +static bool _rasterSolidGradientRle(SwSurface* surface, const SwRle* rle, const SwFill* fill) { auto span = rle->spans; @@ -1789,7 +1538,7 @@ static bool _rasterSolidGradientRle(SwSurface* surface, const SwRleData* rle, co } -static bool _rasterLinearGradientRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill) +static bool _rasterLinearGradientRle(SwSurface* surface, const SwRle* rle, const SwFill* fill) { if (!rle) return false; @@ -1806,7 +1555,7 @@ static bool _rasterLinearGradientRle(SwSurface* surface, const SwRleData* rle, c } -static bool _rasterRadialGradientRle(SwSurface* surface, const SwRleData* rle, const SwFill* fill) +static bool _rasterRadialGradientRle(SwSurface* surface, const SwRle* rle, const SwFill* fill) { if (!rle) return false; @@ -1814,9 +1563,9 @@ static bool _rasterRadialGradientRle(SwSurface* surface, const SwRleData* rle, c if (_matting(surface)) return _rasterGradientMattedRle(surface, rle, fill); else return _rasterGradientMaskedRle(surface, rle, fill); } else if (_blending(surface)) { - _rasterBlendingGradientRle(surface, rle, fill); + return _rasterBlendingGradientRle(surface, rle, fill); } else { - if (fill->translucent) _rasterTranslucentGradientRle(surface, rle, fill); + if (fill->translucent) return _rasterTranslucentGradientRle(surface, rle, fill); else return _rasterSolidGradientRle(surface, rle, fill); } return false; @@ -1827,6 +1576,19 @@ static bool _rasterRadialGradientRle(SwSurface* surface, const SwRleData* rle, c /* External Class Implementation */ /************************************************************************/ +void rasterTranslucentPixel32(uint32_t* dst, uint32_t* src, uint32_t len, uint8_t opacity) +{ + //TODO: Support SIMD accelerations + cRasterTranslucentPixels(dst, src, len, opacity); +} + + +void rasterPixel32(uint32_t* dst, uint32_t* src, uint32_t len, uint8_t opacity) +{ + //TODO: Support SIMD accelerations + cRasterPixels(dst, src, len, opacity); +} + void rasterGrayscale8(uint8_t *dst, uint8_t val, uint32_t offset, int32_t len) { @@ -1905,7 +1667,7 @@ bool rasterClear(SwSurface* surface, uint32_t x, uint32_t y, uint32_t w, uint32_ } -void rasterUnpremultiply(Surface* surface) +void rasterUnpremultiply(RenderSurface* surface) { if (surface->channelSize != sizeof(uint32_t)) return; @@ -1935,7 +1697,7 @@ void rasterUnpremultiply(Surface* surface) } -void rasterPremultiply(Surface* surface) +void rasterPremultiply(RenderSurface* surface) { ScopedLock lock(surface->key); if (surface->premultiplied || (surface->channelSize != sizeof(uint32_t))) return; @@ -1965,13 +1727,13 @@ bool rasterGradientShape(SwSurface* surface, SwShape* shape, const Fill* fdata, return a > 0 ? rasterShape(surface, shape, color->r, color->g, color->b, a) : true; } - auto id = fdata->identifier(); + auto type = fdata->type(); if (shape->fastTrack) { - if (id == TVG_CLASS_ID_LINEAR) return _rasterLinearGradientRect(surface, shape->bbox, shape->fill); - else if (id == TVG_CLASS_ID_RADIAL)return _rasterRadialGradientRect(surface, shape->bbox, shape->fill); + if (type == Type::LinearGradient) return _rasterLinearGradientRect(surface, shape->bbox, shape->fill); + else if (type == Type::RadialGradient)return _rasterRadialGradientRect(surface, shape->bbox, shape->fill); } else { - if (id == TVG_CLASS_ID_LINEAR) return _rasterLinearGradientRle(surface, shape->rle, shape->fill); - else if (id == TVG_CLASS_ID_RADIAL) return _rasterRadialGradientRle(surface, shape->rle, shape->fill); + if (type == Type::LinearGradient) return _rasterLinearGradientRle(surface, shape->rle, shape->fill); + else if (type == Type::RadialGradient) return _rasterRadialGradientRle(surface, shape->rle, shape->fill); } return false; } @@ -1986,9 +1748,9 @@ bool rasterGradientStroke(SwSurface* surface, SwShape* shape, const Fill* fdata, return a > 0 ? rasterStroke(surface, shape, color->r, color->g, color->b, a) : true; } - auto id = fdata->identifier(); - if (id == TVG_CLASS_ID_LINEAR) return _rasterLinearGradientRle(surface, shape->strokeRle, shape->stroke->fill); - else if (id == TVG_CLASS_ID_RADIAL) return _rasterRadialGradientRle(surface, shape->strokeRle, shape->stroke->fill); + auto type = fdata->type(); + if (type == Type::LinearGradient) return _rasterLinearGradientRle(surface, shape->strokeRle, shape->stroke->fill); + else if (type == Type::RadialGradient) return _rasterRadialGradientRle(surface, shape->strokeRle, shape->stroke->fill); return false; } @@ -2027,12 +1789,12 @@ bool rasterImage(SwSurface* surface, SwImage* image, const Matrix& transform, co } -bool rasterConvertCS(Surface* surface, ColorSpace to) +bool rasterConvertCS(RenderSurface* surface, ColorSpace to) { ScopedLock lock(surface->key); if (surface->cs == to) return true; - //TOOD: Support SIMD accelerations + //TODO: Support SIMD accelerations auto from = surface->cs; if (((from == ColorSpace::ABGR8888) || (from == ColorSpace::ABGR8888S)) && ((to == ColorSpace::ARGB8888) || (to == ColorSpace::ARGB8888S))) { @@ -2045,3 +1807,39 @@ bool rasterConvertCS(Surface* surface, ColorSpace to) } return false; } + + +//TODO: SIMD OPTIMIZATION? +void rasterXYFlip(uint32_t* src, uint32_t* dst, int32_t stride, int32_t w, int32_t h, const SwBBox& bbox, bool flipped) +{ + constexpr int BLOCK = 8; //experimental decision + + if (flipped) { + src += ((bbox.min.x * stride) + bbox.min.y); + dst += ((bbox.min.y * stride) + bbox.min.x); + } else { + src += ((bbox.min.y * stride) + bbox.min.x); + dst += ((bbox.min.x * stride) + bbox.min.y); + } + + #pragma omp parallel for + for (int x = 0; x < w; x += BLOCK) { + auto bx = std::min(w, x + BLOCK) - x; + auto in = &src[x]; + auto out = &dst[x * stride]; + for (int y = 0; y < h; y += BLOCK) { + auto p = &in[y * stride]; + auto q = &out[y]; + auto by = std::min(h, y + BLOCK) - y; + for (int xx = 0; xx < bx; ++xx) { + for (int yy = 0; yy < by; ++yy) { + *q = *p; + p += stride; + ++q; + } + p += 1 - by * stride; + q += stride - by; + } + } + } +} diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterAvx.h b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterAvx.h index a072a88819a9..79cab043f243 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterAvx.h +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterAvx.h @@ -158,7 +158,7 @@ static bool avxRasterTranslucentRect(SwSurface* surface, const SwBBox& region, u } -static bool avxRasterTranslucentRle(SwSurface* surface, const SwRleData* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool avxRasterTranslucentRle(SwSurface* surface, const SwRle* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { auto span = rle->spans; @@ -185,7 +185,7 @@ static bool avxRasterTranslucentRle(SwSurface* surface, const SwRleData* rle, ui } //2. fill the aligned memory using avx - N_32BITS_IN_128REG pixels processed at once - //In order to avoid unneccessary avx variables declarations a check is made whether there are any iterations at all + //In order to avoid unnecessary avx variables declarations a check is made whether there are any iterations at all uint32_t iterations = (span->len - notAligned) / N_32BITS_IN_128REG; uint32_t avxFilled = 0; if (iterations > 0) { diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterC.h b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterC.h index 6d0bd9383dad..d79da0e4d8ce 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterC.h +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterC.h @@ -20,6 +20,38 @@ * SOFTWARE. */ + +template +static void inline cRasterTranslucentPixels(PIXEL_T* dst, PIXEL_T* src, uint32_t len, uint32_t opacity) +{ + //TODO: 64bits faster? + if (opacity == 255) { + for (uint32_t x = 0; x < len; ++x, ++dst, ++src) { + *dst = *src + ALPHA_BLEND(*dst, IA(*src)); + } + } else { + for (uint32_t x = 0; x < len; ++x, ++dst, ++src) { + auto tmp = ALPHA_BLEND(*src, opacity); + *dst = tmp + ALPHA_BLEND(*dst, IA(tmp)); + } + } +} + + +template +static void inline cRasterPixels(PIXEL_T* dst, PIXEL_T* src, uint32_t len, uint32_t opacity) +{ + //TODO: 64bits faster? + if (opacity == 255) { + for (uint32_t x = 0; x < len; ++x, ++dst, ++src) { + *dst = *src; + } + } else { + cRasterTranslucentPixels(dst, src, len, opacity); + } +} + + template static void inline cRasterPixels(PIXEL_T* dst, PIXEL_T val, uint32_t offset, int32_t len) { @@ -60,7 +92,7 @@ static void inline cRasterPixels(PIXEL_T* dst, PIXEL_T val, uint32_t offset, int } -static bool inline cRasterTranslucentRle(SwSurface* surface, const SwRleData* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool inline cRasterTranslucentRle(SwSurface* surface, const SwRle* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { auto span = rle->spans; @@ -125,7 +157,7 @@ static bool inline cRasterTranslucentRect(SwSurface* surface, const SwBBox& regi } -static bool inline cRasterABGRtoARGB(Surface* surface) +static bool inline cRasterABGRtoARGB(RenderSurface* surface) { TVGLOG("SW_ENGINE", "Convert ColorSpace ABGR - ARGB [Size: %d x %d]", surface->w, surface->h); @@ -156,7 +188,7 @@ static bool inline cRasterABGRtoARGB(Surface* surface) } -static bool inline cRasterARGBtoABGR(Surface* surface) +static bool inline cRasterARGBtoABGR(RenderSurface* surface) { //exactly same with ABGRtoARGB return cRasterABGRtoARGB(surface); diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterNeon.h b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterNeon.h index 91cf7743c190..fe693b7f33ac 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterNeon.h +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterNeon.h @@ -89,7 +89,7 @@ static void neonRasterPixel32(uint32_t *dst, uint32_t val, uint32_t offset, int3 } -static bool neonRasterTranslucentRle(SwSurface* surface, const SwRleData* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +static bool neonRasterTranslucentRle(SwSurface* surface, const SwRle* rle, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { auto span = rle->spans; diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterTexmap.h b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterTexmap.h index 88ef2118f278..1162edc8381c 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterTexmap.h +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRasterTexmap.h @@ -20,6 +20,17 @@ * SOFTWARE. */ +struct Vertex +{ + Point pt; + Point uv; +}; + +struct Polygon +{ + Vertex vertex[3]; +}; + struct AALine { int32_t x[2]; @@ -53,10 +64,12 @@ static bool _arrange(const SwImage* image, const SwBBox* region, int& yStart, in regionBottom = image->rle->spans[image->rle->size - 1].y; } + if (yStart >= regionBottom) return false; + if (yStart < regionTop) yStart = regionTop; if (yEnd > regionBottom) yEnd = regionBottom; - return yEnd > yStart; + return true; } @@ -673,7 +686,7 @@ static void _rasterPolygonImage(SwSurface* surface, const SwImage* image, const auto denom = ((x[2] - x[0]) * (y[1] - y[0]) - (x[1] - x[0]) * (y[2] - y[0])); //Skip poly if it's an infinitely thin line - if (mathZero(denom)) return; + if (tvg::zero(denom)) return; denom = 1 / denom; //Reciprocal for speeding up dudx = ((u[2] - u[0]) * (y[1] - y[0]) - (u[1] - u[0]) * (y[2] - y[0])) * denom; @@ -689,8 +702,8 @@ static void _rasterPolygonImage(SwSurface* surface, const SwImage* image, const //Determine which side of the polygon the longer edge is on auto side = (dxdy[1] > dxdy[0]) ? true : false; - if (mathEqual(y[0], y[1])) side = x[0] > x[1]; - if (mathEqual(y[1], y[2])) side = x[2] > x[1]; + if (tvg::equal(y[0], y[1])) side = x[0] > x[1]; + if (tvg::equal(y[1], y[2])) side = x[2] > x[1]; auto regionTop = region ? region->min.y : image->rle->spans->y; //Normal Image or Rle Image? auto compositing = _compositing(surface); //Composition required diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRenderer.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRenderer.cpp index 116c4e813527..180f3cc37a2b 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRenderer.cpp +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRenderer.cpp @@ -20,6 +20,9 @@ * SOFTWARE. */ +#ifdef THORVG_SW_OPENMP_SUPPORT + #include +#endif #include #include "tvgMath.h" #include "tvgSwCommon.h" @@ -38,7 +41,7 @@ struct SwTask : Task { SwSurface* surface = nullptr; SwMpool* mpool = nullptr; - SwBBox bbox = {{0, 0}, {0, 0}}; //Whole Rendering Region + SwBBox bbox; //Rendering Region Matrix transform; Array clips; RenderUpdateFlag flags = RenderUpdateFlag::None; @@ -65,9 +68,7 @@ struct SwTask : Task } virtual void dispose() = 0; - virtual bool clip(SwRleData* target) = 0; - virtual SwRleData* rle() = 0; - + virtual bool clip(SwRle* target) = 0; virtual ~SwTask() {} }; @@ -92,38 +93,34 @@ struct SwShapeTask : SwTask if (!rshape->stroke) return 0.0f; auto width = rshape->stroke->width; - if (mathZero(width)) return 0.0f; + if (tvg::zero(width)) return 0.0f; if (!rshape->stroke->fill && (MULTIPLY(rshape->stroke->color[3], opacity) == 0)) return 0.0f; - if (mathZero(rshape->stroke->trim.begin - rshape->stroke->trim.end)) return 0.0f; + if (tvg::zero(rshape->stroke->trim.begin - rshape->stroke->trim.end)) return 0.0f; return (width * sqrt(transform.e11 * transform.e11 + transform.e12 * transform.e12)); } - bool clip(SwRleData* target) override + bool clip(SwRle* target) override { - if (shape.fastTrack) rleClipRect(target, &bbox); - else if (shape.rle) rleClipPath(target, shape.rle); + if (shape.fastTrack) rleClip(target, &bbox); + else if (shape.rle) rleClip(target, shape.rle); else return false; return true; } - SwRleData* rle() override - { - if (!shape.rle && shape.fastTrack) { - shape.rle = rleRender(&shape.bbox); - } - return shape.rle; - } - void run(unsigned tid) override { - if (opacity == 0 && !clipper) return; //Invisible + //Invisible + if (opacity == 0 && !clipper) { + bbox.reset(); + return; + } auto strokeWidth = validStrokeWidth(); - bool visibleFill = false; - auto clipRegion = bbox; + SwBBox renderRegion{}; + auto visibleFill = false; //This checks also for the case, if the invisible shape turned to visible by alpha. auto prepareShape = false; @@ -135,10 +132,11 @@ struct SwShapeTask : SwTask rshape->fillColor(nullptr, nullptr, nullptr, &alpha); alpha = MULTIPLY(alpha, opacity); visibleFill = (alpha > 0 || rshape->fill); + shapeReset(&shape); if (visibleFill || clipper) { - shapeReset(&shape); - if (!shapePrepare(&shape, rshape, transform, clipRegion, bbox, mpool, tid, clips.count > 0 ? true : false)) { + if (!shapePrepare(&shape, rshape, transform, bbox, renderRegion, mpool, tid, clips.count > 0 ? true : false)) { visibleFill = false; + renderRegion.reset(); } } } @@ -159,8 +157,8 @@ struct SwShapeTask : SwTask if (flags & (RenderUpdateFlag::Path | RenderUpdateFlag::Stroke | RenderUpdateFlag::Transform)) { if (strokeWidth > 0.0f) { shapeResetStroke(&shape, rshape, transform); - if (!shapeGenStrokeRle(&shape, rshape, transform, clipRegion, bbox, mpool, tid)) goto err; + if (!shapeGenStrokeRle(&shape, rshape, transform, bbox, renderRegion, mpool, tid)) goto err; if (auto fill = rshape->strokeFill()) { auto ctable = (flags & RenderUpdateFlag::GradientStroke) ? true : false; if (ctable) shapeResetStrokeFill(&shape); @@ -184,9 +182,13 @@ struct SwShapeTask : SwTask //Clip stroke rle if (shape.strokeRle && !clipper->clip(shape.strokeRle)) goto err; } + + bbox = renderRegion; //sync + return; err: + bbox.reset(); shapeReset(&shape); shapeDelOutline(&shape, mpool, tid); } @@ -201,20 +203,14 @@ struct SwShapeTask : SwTask struct SwImageTask : SwTask { SwImage image; - Surface* source; //Image source + RenderSurface* source; //Image source - bool clip(SwRleData* target) override + bool clip(SwRle* target) override { TVGERR("SW_ENGINE", "Image is used as ClipPath?"); return true; } - SwRleData* rle() override - { - TVGERR("SW_ENGINE", "Image is used as Scene ClipPath?"); - return nullptr; - } - void run(unsigned tid) override { auto clipRegion = bbox; @@ -452,27 +448,18 @@ bool SwRenderer::blend(BlendMethod method) surface->blendMethod = method; switch (method) { - case BlendMethod::Add: - surface->blender = opBlendAdd; - break; - case BlendMethod::Screen: - surface->blender = opBlendScreen; + case BlendMethod::Normal: + surface->blender = nullptr; break; case BlendMethod::Multiply: surface->blender = opBlendMultiply; break; + case BlendMethod::Screen: + surface->blender = opBlendScreen; + break; case BlendMethod::Overlay: surface->blender = opBlendOverlay; break; - case BlendMethod::Difference: - surface->blender = opBlendDifference; - break; - case BlendMethod::Exclusion: - surface->blender = opBlendExclusion; - break; - case BlendMethod::SrcOver: - surface->blender = opBlendSrcOver; - break; case BlendMethod::Darken: surface->blender = opBlendDarken; break; @@ -491,7 +478,17 @@ bool SwRenderer::blend(BlendMethod method) case BlendMethod::SoftLight: surface->blender = opBlendSoftLight; break; + case BlendMethod::Difference: + surface->blender = opBlendDifference; + break; + case BlendMethod::Exclusion: + surface->blender = opBlendExclusion; + break; + case BlendMethod::Add: + surface->blender = opBlendAdd; + break; default: + TVGLOG("SW_ENGINE", "Non supported blending option = %d", (int) method); surface->blender = nullptr; break; } @@ -505,7 +502,7 @@ RenderRegion SwRenderer::region(RenderData data) } -bool SwRenderer::beginComposite(Compositor* cmp, CompositeMethod method, uint8_t opacity) +bool SwRenderer::beginComposite(RenderCompositor* cmp, CompositeMethod method, uint8_t opacity) { if (!cmp) return false; auto p = static_cast(cmp); @@ -543,31 +540,19 @@ bool SwRenderer::mempool(bool shared) } -const Surface* SwRenderer::mainSurface() +const RenderSurface* SwRenderer::mainSurface() { return surface; } -Compositor* SwRenderer::target(const RenderRegion& region, ColorSpace cs) +SwSurface* SwRenderer::request(int channelSize) { - auto x = region.x; - auto y = region.y; - auto w = region.w; - auto h = region.h; - auto sw = static_cast(surface->w); - auto sh = static_cast(surface->h); - - //Out of boundary - if (x >= sw || y >= sh || x + w < 0 || y + h < 0) return nullptr; - SwSurface* cmp = nullptr; - auto reqChannelSize = CHANNEL_SIZE(cs); - //Use cached data for (auto p = compositors.begin(); p < compositors.end(); ++p) { - if ((*p)->compositor->valid && (*p)->compositor->image.channelSize == reqChannelSize) { + if ((*p)->compositor->valid && (*p)->compositor->image.channelSize == channelSize) { cmp = *p; break; } @@ -578,18 +563,48 @@ Compositor* SwRenderer::target(const RenderRegion& region, ColorSpace cs) //Inherits attributes from main surface cmp = new SwSurface(surface); cmp->compositor = new SwCompositor; - - //TODO: We can optimize compositor surface size from (surface->stride x surface->h) to Parameter(w x h) - cmp->compositor->image.data = (pixel_t*)malloc(reqChannelSize * surface->stride * surface->h); - cmp->channelSize = cmp->compositor->image.channelSize = reqChannelSize; + cmp->compositor->image.data = (pixel_t*)malloc(channelSize * surface->stride * surface->h); + cmp->compositor->image.w = surface->w; + cmp->compositor->image.h = surface->h; + cmp->compositor->image.stride = surface->stride; + cmp->compositor->image.direct = true; + cmp->compositor->valid = true; + cmp->channelSize = cmp->compositor->image.channelSize = channelSize; + cmp->w = cmp->compositor->image.w; + cmp->h = cmp->compositor->image.h; compositors.push(cmp); } + //Sync. This may have been modified by post-processing. + cmp->data = cmp->compositor->image.data; + + return cmp; +} + + +RenderCompositor* SwRenderer::target(const RenderRegion& region, ColorSpace cs) +{ + auto x = region.x; + auto y = region.y; + auto w = region.w; + auto h = region.h; + auto sw = static_cast(surface->w); + auto sh = static_cast(surface->h); + + //Out of boundary + if (x >= sw || y >= sh || x + w < 0 || y + h < 0) return nullptr; + + auto cmp = request(CHANNEL_SIZE(cs)); + //Boundary Check + if (x < 0) x = 0; + if (y < 0) y = 0; if (x + w > sw) w = (sw - x); if (y + h > sh) h = (sh - y); + if (w == 0 || h == 0) return nullptr; + cmp->compositor->recoverSfc = surface; cmp->compositor->recoverCmp = surface->compositor; cmp->compositor->valid = false; @@ -597,14 +612,6 @@ Compositor* SwRenderer::target(const RenderRegion& region, ColorSpace cs) cmp->compositor->bbox.min.y = y; cmp->compositor->bbox.max.x = x + w; cmp->compositor->bbox.max.y = y + h; - cmp->compositor->image.stride = surface->stride; - cmp->compositor->image.w = surface->w; - cmp->compositor->image.h = surface->h; - cmp->compositor->image.direct = true; - - cmp->data = cmp->compositor->image.data; - cmp->w = cmp->compositor->image.w; - cmp->h = cmp->compositor->image.h; /* TODO: Currently, only blending might work. Blending and composition must be handled together. */ @@ -618,7 +625,7 @@ Compositor* SwRenderer::target(const RenderRegion& region, ColorSpace cs) } -bool SwRenderer::endComposite(Compositor* cmp) +bool SwRenderer::endComposite(RenderCompositor* cmp) { if (!cmp) return false; @@ -639,6 +646,40 @@ bool SwRenderer::endComposite(Compositor* cmp) } +bool SwRenderer::prepare(RenderEffect* effect) +{ + switch (effect->type) { + case SceneEffect::GaussianBlur: return effectGaussianBlurPrepare(static_cast(effect)); + case SceneEffect::DropShadow: return effectDropShadowPrepare(static_cast(effect)); + default: return false; + } +} + + +bool SwRenderer::effect(RenderCompositor* cmp, const RenderEffect* effect, uint8_t opacity, bool direct) +{ + if (effect->invalid) return false; + + auto p = static_cast(cmp); + + switch (effect->type) { + case SceneEffect::GaussianBlur: { + return effectGaussianBlur(p, request(surface->channelSize), static_cast(effect)); + } + case SceneEffect::DropShadow: { + auto cmp1 = request(surface->channelSize); + cmp1->compositor->valid = false; + auto cmp2 = request(surface->channelSize); + SwSurface* surfaces[] = {cmp1, cmp2}; + auto ret = effectDropShadow(p, surfaces, static_cast(effect), opacity, direct); + cmp1->compositor->valid = true; + return ret; + } + default: return false; + } +} + + ColorSpace SwRenderer::colorSpace() { if (surface) return surface->cs; @@ -681,10 +722,10 @@ void* SwRenderer::prepareCommon(SwTask* task, const Matrix& transform, const Arr task->surface = surface; task->mpool = mpool; task->flags = flags; - task->bbox.min.x = mathMax(static_cast(0), static_cast(vport.x)); - task->bbox.min.y = mathMax(static_cast(0), static_cast(vport.y)); - task->bbox.max.x = mathMin(static_cast(surface->w), static_cast(vport.x + vport.w)); - task->bbox.max.y = mathMin(static_cast(surface->h), static_cast(vport.y + vport.h)); + task->bbox.min.x = std::max(static_cast(0), static_cast(vport.x)); + task->bbox.min.y = std::max(static_cast(0), static_cast(vport.y)); + task->bbox.max.x = std::min(static_cast(surface->w), static_cast(vport.x + vport.w)); + task->bbox.max.y = std::min(static_cast(surface->h), static_cast(vport.y + vport.h)); if (!task->pushed) { task->pushed = true; @@ -697,7 +738,7 @@ void* SwRenderer::prepareCommon(SwTask* task, const Matrix& transform, const Arr } -RenderData SwRenderer::prepare(Surface* surface, RenderData data, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flags) +RenderData SwRenderer::prepare(RenderSurface* surface, RenderData data, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flags) { //prepare task auto task = static_cast(data); @@ -748,6 +789,10 @@ bool SwRenderer::init(uint32_t threads) int32_t SwRenderer::init() { +#ifdef THORVG_SW_OPENMP_SUPPORT + omp_set_num_threads(TaskScheduler::threads()); +#endif + return initEngineCnt; } diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRenderer.h b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRenderer.h index fcd8ad46205f..bd6beb8d85ce 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRenderer.h +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRenderer.h @@ -37,7 +37,7 @@ class SwRenderer : public RenderMethod { public: RenderData prepare(const RenderShape& rshape, RenderData data, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flags, bool clipper) override; - RenderData prepare(Surface* surface, RenderData data, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flags) override; + RenderData prepare(RenderSurface* surface, RenderData data, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flags) override; bool preRender() override; bool renderShape(RenderData data) override; bool renderImage(RenderData data) override; @@ -48,18 +48,21 @@ class SwRenderer : public RenderMethod bool viewport(const RenderRegion& vp) override; bool blend(BlendMethod method) override; ColorSpace colorSpace() override; - const Surface* mainSurface() override; + const RenderSurface* mainSurface() override; bool clear() override; bool sync() override; bool target(pixel_t* data, uint32_t stride, uint32_t w, uint32_t h, ColorSpace cs); bool mempool(bool shared); - Compositor* target(const RenderRegion& region, ColorSpace cs) override; - bool beginComposite(Compositor* cmp, CompositeMethod method, uint8_t opacity) override; - bool endComposite(Compositor* cmp) override; + RenderCompositor* target(const RenderRegion& region, ColorSpace cs) override; + bool beginComposite(RenderCompositor* cmp, CompositeMethod method, uint8_t opacity) override; + bool endComposite(RenderCompositor* cmp) override; void clearCompositors(); + bool prepare(RenderEffect* effect) override; + bool effect(RenderCompositor* cmp, const RenderEffect* effect, uint8_t opacity, bool direct) override; + static SwRenderer* gen(); static bool init(uint32_t threads); static int32_t init(); @@ -76,6 +79,7 @@ class SwRenderer : public RenderMethod SwRenderer(); ~SwRenderer(); + SwSurface* request(int channelSize); RenderData prepareCommon(SwTask* task, const Matrix& transform, const Array& clips, uint8_t opacity, RenderUpdateFlag flags); }; diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRle.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRle.cpp index 42b08de6a581..3e4ad679a8af 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRle.cpp +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwRle.cpp @@ -217,7 +217,7 @@ struct Cell struct RleWorker { - SwRleData* rle; + SwRle* rle; SwPoint cellPos; SwPoint cellMin; @@ -235,6 +235,7 @@ struct RleWorker SwPoint pos; SwPoint bezStack[32 * 3 + 1]; + SwPoint lineStack[32 + 1]; int levStack[32]; SwOutline* outline; @@ -297,7 +298,7 @@ static inline SwCoord HYPOT(SwPoint pt) } -static void _horizLine(RleWorker& rw, SwCoord x, SwCoord y, SwCoord area, SwCoord acount) +static void _horizLine(RleWorker& rw, SwCoord x, SwCoord y, SwCoord area, SwCoord aCount) { x += rw.cellMin.x; y += rw.cellMin.y; @@ -341,11 +342,11 @@ static void _horizLine(RleWorker& rw, SwCoord x, SwCoord y, SwCoord area, SwCoor if ((span->coverage == coverage) && (span->y == y) && (span->x + span->len == x)) { //Clip x range SwCoord xOver = 0; - if (x + acount >= rw.cellMax.x) xOver -= (x + acount - rw.cellMax.x); + if (x + aCount >= rw.cellMax.x) xOver -= (x + aCount - rw.cellMax.x); if (x < rw.cellMin.x) xOver -= (rw.cellMin.x - x); - //span->len += (acount + xOver) - 1; - span->len += (acount + xOver); + //span->len += (aCount + xOver) - 1; + span->len += (aCount + xOver); return; } } @@ -361,20 +362,20 @@ static void _horizLine(RleWorker& rw, SwCoord x, SwCoord y, SwCoord area, SwCoor //Clip x range SwCoord xOver = 0; - if (x + acount >= rw.cellMax.x) xOver -= (x + acount - rw.cellMax.x); + if (x + aCount >= rw.cellMax.x) xOver -= (x + aCount - rw.cellMax.x); if (x < rw.cellMin.x) { xOver -= (rw.cellMin.x - x); x = rw.cellMin.x; } //Nothing to draw - if (acount + xOver <= 0) return; + if (aCount + xOver <= 0) return; //add a span to the current list auto span = rle->spans + rle->size; span->x = x; span->y = y; - span->len = (acount + xOver); + span->len = (aCount + xOver); span->coverage = coverage; rle->size++; } @@ -513,98 +514,116 @@ static void _lineTo(RleWorker& rw, const SwPoint& to) return; } - auto diff = to - rw.pos; - auto f1 = rw.pos - SUBPIXELS(e1); - SwPoint f2; - - //inside one cell - if (e1 == e2) { - ; - //any horizontal line - } else if (diff.y == 0) { - e1.x = e2.x; - _setCell(rw, e1); - } else if (diff.x == 0) { - //vertical line up - if (diff.y > 0) { - do { - f2.y = ONE_PIXEL; - rw.cover += (f2.y - f1.y); - rw.area += (f2.y - f1.y) * f1.x * 2; - f1.y = 0; - ++e1.y; - _setCell(rw, e1); - } while(e1.y != e2.y); - //vertical line down + auto line = rw.lineStack; + line[0] = to; + line[1] = rw.pos; + + while (true) { + auto diff = line[0] - line[1]; + auto L = HYPOT(diff); + + if (L > SHRT_MAX) { + mathSplitLine(line); + ++line; + continue; + } + e1 = TRUNC(line[1]); + e2 = TRUNC(line[0]); + + auto f1 = line[1] - SUBPIXELS(e1); + SwPoint f2; + + //inside one cell + if (e1 == e2) { + ; + //any horizontal line + } else if (diff.y == 0) { + e1.x = e2.x; + _setCell(rw, e1); + } else if (diff.x == 0) { + //vertical line up + if (diff.y > 0) { + do { + f2.y = ONE_PIXEL; + rw.cover += (f2.y - f1.y); + rw.area += (f2.y - f1.y) * f1.x * 2; + f1.y = 0; + ++e1.y; + _setCell(rw, e1); + } while(e1.y != e2.y); + //vertical line down + } else { + do { + f2.y = 0; + rw.cover += (f2.y - f1.y); + rw.area += (f2.y - f1.y) * f1.x * 2; + f1.y = ONE_PIXEL; + --e1.y; + _setCell(rw, e1); + } while(e1.y != e2.y); + } + //any other line } else { + Area prod = diff.x * f1.y - diff.y * f1.x; + + /* These macros speed up repetitive divisions by replacing them + with multiplications and right shifts. */ + auto dx_r = static_cast(ULONG_MAX >> PIXEL_BITS) / (diff.x); + auto dy_r = static_cast(ULONG_MAX >> PIXEL_BITS) / (diff.y); + + /* The fundamental value `prod' determines which side and the */ + /* exact coordinate where the line exits current cell. It is */ + /* also easily updated when moving from one cell to the next. */ do { - f2.y = 0; - rw.cover += (f2.y - f1.y); - rw.area += (f2.y - f1.y) * f1.x * 2; - f1.y = ONE_PIXEL; - --e1.y; + auto px = diff.x * ONE_PIXEL; + auto py = diff.y * ONE_PIXEL; + + //left + if (prod <= 0 && prod - px > 0) { + f2 = {0, SW_UDIV(-prod, -dx_r)}; + prod -= py; + rw.cover += (f2.y - f1.y); + rw.area += (f2.y - f1.y) * (f1.x + f2.x); + f1 = {ONE_PIXEL, f2.y}; + --e1.x; + //up + } else if (prod - px <= 0 && prod - px + py > 0) { + prod -= px; + f2 = {SW_UDIV(-prod, dy_r), ONE_PIXEL}; + rw.cover += (f2.y - f1.y); + rw.area += (f2.y - f1.y) * (f1.x + f2.x); + f1 = {f2.x, 0}; + ++e1.y; + //right + } else if (prod - px + py <= 0 && prod + py >= 0) { + prod += py; + f2 = {ONE_PIXEL, SW_UDIV(prod, dx_r)}; + rw.cover += (f2.y - f1.y); + rw.area += (f2.y - f1.y) * (f1.x + f2.x); + f1 = {0, f2.y}; + ++e1.x; + //down + } else { + f2 = {SW_UDIV(prod, -dy_r), 0}; + prod += px; + rw.cover += (f2.y - f1.y); + rw.area += (f2.y - f1.y) * (f1.x + f2.x); + f1 = {f2.x, ONE_PIXEL}; + --e1.y; + } + _setCell(rw, e1); - } while(e1.y != e2.y); + + } while(e1 != e2); } - //any other line - } else { - Area prod = diff.x * f1.y - diff.y * f1.x; - - /* These macros speed up repetitive divisions by replacing them - with multiplications and right shifts. */ - auto dx_r = static_cast(ULONG_MAX >> PIXEL_BITS) / (diff.x); - auto dy_r = static_cast(ULONG_MAX >> PIXEL_BITS) / (diff.y); - - /* The fundamental value `prod' determines which side and the */ - /* exact coordinate where the line exits current cell. It is */ - /* also easily updated when moving from one cell to the next. */ - do { - auto px = diff.x * ONE_PIXEL; - auto py = diff.y * ONE_PIXEL; - - //left - if (prod <= 0 && prod - px > 0) { - f2 = {0, SW_UDIV(-prod, -dx_r)}; - prod -= py; - rw.cover += (f2.y - f1.y); - rw.area += (f2.y - f1.y) * (f1.x + f2.x); - f1 = {ONE_PIXEL, f2.y}; - --e1.x; - //up - } else if (prod - px <= 0 && prod - px + py > 0) { - prod -= px; - f2 = {SW_UDIV(-prod, dy_r), ONE_PIXEL}; - rw.cover += (f2.y - f1.y); - rw.area += (f2.y - f1.y) * (f1.x + f2.x); - f1 = {f2.x, 0}; - ++e1.y; - //right - } else if (prod - px + py <= 0 && prod + py >= 0) { - prod += py; - f2 = {ONE_PIXEL, SW_UDIV(prod, dx_r)}; - rw.cover += (f2.y - f1.y); - rw.area += (f2.y - f1.y) * (f1.x + f2.x); - f1 = {0, f2.y}; - ++e1.x; - //down - } else { - f2 = {SW_UDIV(prod, -dy_r), 0}; - prod += px; - rw.cover += (f2.y - f1.y); - rw.area += (f2.y - f1.y) * (f1.x + f2.x); - f1 = {f2.x, ONE_PIXEL}; - --e1.y; - } - _setCell(rw, e1); + f2 = {line[0].x - SUBPIXELS(e2.x), line[0].y - SUBPIXELS(e2.y)}; + rw.cover += (f2.y - f1.y); + rw.area += (f2.y - f1.y) * (f1.x + f2.x); + rw.pos = line[0]; - } while(e1 != e2); + if (line-- == rw.lineStack) return; } - - f2 = {to.x - SUBPIXELS(e2.x), to.y - SUBPIXELS(e2.y)}; - rw.cover += (f2.y - f1.y); - rw.area += (f2.y - f1.y) * (f1.x + f2.x); - rw.pos = to; } @@ -690,31 +709,27 @@ static void _decomposeOutline(RleWorker& rw) auto start = UPSCALE(outline->pts[first]); auto pt = outline->pts.data + first; auto types = outline->types.data + first; + ++types; _moveTo(rw, UPSCALE(outline->pts[first])); while (pt < limit) { - ++pt; - ++types; - //emit a single line_to if (types[0] == SW_CURVE_TYPE_POINT) { + ++pt; + ++types; _lineTo(rw, UPSCALE(*pt)); //types cubic } else { - pt += 2; - types += 2; - - if (pt <= limit) { - _cubicTo(rw, UPSCALE(pt[-2]), UPSCALE(pt[-1]), UPSCALE(pt[0])); - continue; - } - _cubicTo(rw, UPSCALE(pt[-2]), UPSCALE(pt[-1]), start); - goto close; + pt += 3; + types += 3; + if (pt <= limit) _cubicTo(rw, UPSCALE(pt[-2]), UPSCALE(pt[-1]), UPSCALE(pt[0])); + else if (pt - 1 == limit) _cubicTo(rw, UPSCALE(pt[-2]), UPSCALE(pt[-1]), start); + else goto close; } } - _lineTo(rw, start); close: + _lineTo(rw, start); first = last + 1; } } @@ -731,7 +746,7 @@ static int _genRle(RleWorker& rw) } -static SwSpan* _intersectSpansRegion(const SwRleData *clip, const SwRleData *target, SwSpan *outSpans, uint32_t outSpansCnt) +static SwSpan* _intersectSpansRegion(const SwRle *clip, const SwRle *target, SwSpan *outSpans, uint32_t outSpansCnt) { auto out = outSpans; auto spans = target->spans; @@ -740,7 +755,7 @@ static SwSpan* _intersectSpansRegion(const SwRleData *clip, const SwRleData *tar auto clipEnd = clip->spans + clip->size; while (spans < end && clipSpans < clipEnd) { - //align y cooridnates. + //align y-coordinates. if (clipSpans->y > spans->y) { ++spans; continue; @@ -750,7 +765,7 @@ static SwSpan* _intersectSpansRegion(const SwRleData *clip, const SwRleData *tar continue; } - //Try clipping with all clip spans which have a same y coordinate. + //Try clipping with all clip spans which have a same y-coordinate. auto temp = clipSpans; while(temp < clipEnd && outSpansCnt > 0 && temp->y == clipSpans->y) { auto sx1 = spans->x; @@ -783,7 +798,7 @@ static SwSpan* _intersectSpansRegion(const SwRleData *clip, const SwRleData *tar } -static SwSpan* _intersectSpansRect(const SwBBox *bbox, const SwRleData *targetRle, SwSpan *outSpans, uint32_t outSpansCnt) +static SwSpan* _intersectSpansRect(const SwBBox *bbox, const SwRle *targetRle, SwSpan *outSpans, uint32_t outSpansCnt) { auto out = outSpans; auto spans = targetRle->spans; @@ -822,7 +837,7 @@ static SwSpan* _intersectSpansRect(const SwBBox *bbox, const SwRleData *targetRl } -void _replaceClipSpan(SwRleData *rle, SwSpan* clippedSpans, uint32_t size) +void _replaceClipSpan(SwRle *rle, SwSpan* clippedSpans, uint32_t size) { free(rle->spans); rle->spans = clippedSpans; @@ -834,7 +849,7 @@ void _replaceClipSpan(SwRleData *rle, SwSpan* clippedSpans, uint32_t size) /* External Class Implementation */ /************************************************************************/ -SwRleData* rleRender(SwRleData* rle, const SwOutline* outline, const SwBBox& renderRegion, bool antiAlias) +SwRle* rleRender(SwRle* rle, const SwOutline* outline, const SwBBox& renderRegion, bool antiAlias) { constexpr auto RENDER_POOL_SIZE = 16384L; constexpr auto BAND_SIZE = 40; @@ -862,7 +877,7 @@ SwRleData* rleRender(SwRleData* rle, const SwOutline* outline, const SwBBox& ren rw.bandShoot = 0; rw.antiAlias = antiAlias; - if (!rle) rw.rle = reinterpret_cast(calloc(1, sizeof(SwRleData))); + if (!rle) rw.rle = reinterpret_cast(calloc(1, sizeof(SwRle))); else rw.rle = rle; //Generate RLE @@ -953,12 +968,12 @@ SwRleData* rleRender(SwRleData* rle, const SwOutline* outline, const SwBBox& ren } -SwRleData* rleRender(const SwBBox* bbox) +SwRle* rleRender(const SwBBox* bbox) { auto width = static_cast(bbox->max.x - bbox->min.x); auto height = static_cast(bbox->max.y - bbox->min.y); - auto rle = static_cast(malloc(sizeof(SwRleData))); + auto rle = static_cast(malloc(sizeof(SwRle))); rle->spans = static_cast(malloc(sizeof(SwSpan) * height)); rle->size = height; rle->alloc = height; @@ -975,14 +990,14 @@ SwRleData* rleRender(const SwBBox* bbox) } -void rleReset(SwRleData* rle) +void rleReset(SwRle* rle) { if (!rle) return; rle->size = 0; } -void rleFree(SwRleData* rle) +void rleFree(SwRle* rle) { if (!rle) return; if (rle->spans) free(rle->spans); @@ -990,7 +1005,7 @@ void rleFree(SwRleData* rle) } -void rleClipPath(SwRleData *rle, const SwRleData *clip) +void rleClip(SwRle *rle, const SwRle *clip) { if (rle->size == 0 || clip->size == 0) return; auto spanCnt = rle->size > clip->size ? rle->size : clip->size; @@ -999,11 +1014,11 @@ void rleClipPath(SwRleData *rle, const SwRleData *clip) _replaceClipSpan(rle, spans, spansEnd - spans); - TVGLOG("SW_ENGINE", "Using ClipPath!"); + TVGLOG("SW_ENGINE", "Using Path Clipping!"); } -void rleClipRect(SwRleData *rle, const SwBBox* clip) +void rleClip(SwRle *rle, const SwBBox* clip) { if (rle->size == 0) return; auto spans = static_cast(malloc(sizeof(SwSpan) * (rle->size))); @@ -1011,5 +1026,5 @@ void rleClipRect(SwRleData *rle, const SwBBox* clip) _replaceClipSpan(rle, spans, spansEnd - spans); - TVGLOG("SW_ENGINE", "Using ClipRect!"); + TVGLOG("SW_ENGINE", "Using Box Clipping!"); } diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwShape.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwShape.cpp index 24c4a9e37255..4408db0b86f2 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwShape.cpp +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwShape.cpp @@ -22,7 +22,6 @@ #include "tvgSwCommon.h" #include "tvgMath.h" -#include "tvgLines.h" /************************************************************************/ /* Internal Class Implementation */ @@ -102,9 +101,9 @@ static bool _outlineClose(SwOutline& outline) static void _dashLineTo(SwDashStroke& dash, const Point* to, const Matrix& transform) { Line cur = {dash.ptCur, *to}; - auto len = lineLength(cur.pt1, cur.pt2); + auto len = cur.length(); - if (mathZero(len)) { + if (tvg::zero(len)) { _outlineMoveTo(*dash.outline, &dash.ptCur, transform); //draw the current line fully } else if (len <= dash.curLen) { @@ -122,7 +121,7 @@ static void _dashLineTo(SwDashStroke& dash, const Point* to, const Matrix& trans Line left, right; if (dash.curLen > 0) { len -= dash.curLen; - lineSplitAt(cur, dash.curLen, left, right); + cur.split(dash.curLen, left, right); if (!dash.curOpGap) { if (dash.move || dash.pattern[dash.curIdx] - dash.curLen < FLOAT_EPSILON) { _outlineMoveTo(*dash.outline, &left.pt1, transform); @@ -163,10 +162,10 @@ static void _dashLineTo(SwDashStroke& dash, const Point* to, const Matrix& trans static void _dashCubicTo(SwDashStroke& dash, const Point* ctrl1, const Point* ctrl2, const Point* to, const Matrix& transform) { Bezier cur = {dash.ptCur, *ctrl1, *ctrl2, *to}; - auto len = bezLength(cur); + auto len = cur.length(); //draw the current line fully - if (mathZero(len)) { + if (tvg::zero(len)) { _outlineMoveTo(*dash.outline, &dash.ptCur, transform); } else if (len <= dash.curLen) { dash.curLen -= len; @@ -183,7 +182,7 @@ static void _dashCubicTo(SwDashStroke& dash, const Point* ctrl1, const Point* ct Bezier left, right; if (dash.curLen > 0) { len -= dash.curLen; - bezSplitAt(cur, dash.curLen, left, right); + cur.split(dash.curLen, left, right); if (!dash.curOpGap) { if (dash.move || dash.pattern[dash.curIdx] - dash.curLen < FLOAT_EPSILON) { _outlineMoveTo(*dash.outline, &left.start, transform); @@ -284,7 +283,7 @@ static float _outlineLength(const RenderShape* rshape, uint32_t shiftPts, uint32 if (cmdCnt <= 0 || ptsCnt <= 0) return 0.0f; const Point* close = nullptr; - auto length = 0.0f; + auto len = 0.0f; //must begin with moveTo if (cmds[0] == PathCommand::MoveTo) { @@ -297,30 +296,30 @@ static float _outlineLength(const RenderShape* rshape, uint32_t shiftPts, uint32 while (cmdCnt-- > 0) { switch (*cmds) { case PathCommand::Close: { - length += mathLength(pts - 1, close); - if (subpath) return length; + len += length(pts - 1, close); + if (subpath) return len; break; } case PathCommand::MoveTo: { - if (subpath) return length; + if (subpath) return len; close = pts; ++pts; break; } case PathCommand::LineTo: { - length += mathLength(pts - 1, pts); + len += length(pts - 1, pts); ++pts; break; } case PathCommand::CubicTo: { - length += bezLength({*(pts - 1), *pts, *(pts + 1), *(pts + 2)}); + len += Bezier{*(pts - 1), *pts, *(pts + 1), *(pts + 2)}.length(); pts += 3; break; } } ++cmds; } - return length; + return len; } @@ -355,7 +354,7 @@ static SwOutline* _genDashOutline(const RenderShape* rshape, const Matrix& trans //offset auto patternLength = 0.0f; uint32_t offIdx = 0; - if (!mathZero(offset)) { + if (!tvg::zero(offset)) { for (size_t i = 0; i < dash.cnt; ++i) patternLength += dash.pattern[i]; bool isOdd = dash.cnt % 2; if (isOdd) patternLength *= 2; @@ -499,7 +498,6 @@ bool shapePrepare(SwShape* shape, const RenderShape* rshape, const Matrix& trans if (!_genOutline(shape, rshape, transform, mpool, tid, hasComposite)) return false; if (!mathUpdateOutlineBBox(shape->outline, clipRegion, renderRegion, shape->fastTrack)) return false; - //Keep it for Rasterization Region shape->bbox = renderRegion; //Check valid region diff --git a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwStroke.cpp b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwStroke.cpp index 75ac96be04d4..e195f72adf2a 100644 --- a/thirdparty/thorvg/src/renderer/sw_engine/tvgSwStroke.cpp +++ b/thirdparty/thorvg/src/renderer/sw_engine/tvgSwStroke.cpp @@ -441,13 +441,23 @@ static void _cubicTo(SwStroke& stroke, const SwPoint& ctrl1, const SwPoint& ctrl //initialize with current direction angleIn = angleOut = angleMid = stroke.angleIn; - if (arc < limit && !mathSmallCubic(arc, angleIn, angleMid, angleOut)) { + auto valid = mathCubicAngle(arc, angleIn, angleMid, angleOut); + + //valid size + if (valid > 0 && arc < limit) { if (stroke.firstPt) stroke.angleIn = angleIn; mathSplitCubic(arc); arc += 3; continue; } + //ignoreable size + if (valid < 0 && arc == bezStack) { + stroke.center = to; + return; + } + + //small size if (firstArc) { firstArc = false; //process corner if necessary @@ -662,7 +672,7 @@ static void _beginSubPath(SwStroke& stroke, const SwPoint& to, bool closed) /* Determine if we need to check whether the border radius is greater than the radius of curvature of a curve, to handle this case specially. This is only required if bevel joins or butt caps may be created because - round & miter joins and round & square caps cover the nagative sector + round & miter joins and round & square caps cover the negative sector created with wide strokes. */ if ((stroke.join != StrokeJoin::Round) || (!stroke.closedSubPath && stroke.cap == StrokeCap::Butt)) stroke.handleWideStrokes = true; @@ -715,7 +725,7 @@ static void _endSubPath(SwStroke& stroke) _addCap(stroke, stroke.subPathAngle + SW_ANGLE_PI, 0); /* now end the right subpath accordingly. The left one is rewind - and deosn't need further processing */ + and doesn't need further processing */ _borderClose(right, false); } } @@ -845,31 +855,25 @@ bool strokeParseOutline(SwStroke* stroke, const SwOutline& outline) //A contour cannot start with a cubic control point if (type == SW_CURVE_TYPE_CUBIC) return false; + ++types; auto closed = outline.closed.data ? outline.closed.data[i]: false; _beginSubPath(*stroke, start, closed); while (pt < limit) { - ++pt; - ++types; - - //emit a signel line_to + //emit a single line_to if (types[0] == SW_CURVE_TYPE_POINT) { + ++pt; + ++types; _lineTo(*stroke, *pt); //types cubic } else { - if (pt + 1 > limit || types[1] != SW_CURVE_TYPE_CUBIC) return false; - - pt += 2; - types += 2; - - if (pt <= limit) { - _cubicTo(*stroke, pt[-2], pt[-1], pt[0]); - continue; - } - _cubicTo(*stroke, pt[-2], pt[-1], start); - goto close; + pt += 3; + types += 3; + if (pt <= limit) _cubicTo(*stroke, pt[-2], pt[-1], pt[0]); + else if (pt - 1 == limit) _cubicTo(*stroke, pt[-2], pt[-1], start); + else goto close; } } close: diff --git a/thirdparty/thorvg/src/renderer/tvgBinaryDesc.h b/thirdparty/thorvg/src/renderer/tvgBinaryDesc.h index 29f84eb82a6d..e40859b6db91 100644 --- a/thirdparty/thorvg/src/renderer/tvgBinaryDesc.h +++ b/thirdparty/thorvg/src/renderer/tvgBinaryDesc.h @@ -36,7 +36,7 @@ using TvgBinFlag = TvgBinByte; #define TVG_HEADER_SIZE 33 //TVG_HEADER_SIGNATURE_LENGTH + TVG_HEADER_VERSION_LENGTH + 2*SIZE(float) + TVG_HEADER_RESERVED_LENGTH + TVG_HEADER_COMPRESS_SIZE #define TVG_HEADER_SIGNATURE "ThorVG" #define TVG_HEADER_SIGNATURE_LENGTH 6 -#define TVG_HEADER_VERSION "001200" //Major 00, Minor 12, Micro 00 +#define TVG_HEADER_VERSION "001500" //Major 00, Minor 15, Micro 00 #define TVG_HEADER_VERSION_LENGTH 6 #define TVG_HEADER_RESERVED_LENGTH 1 //Storing flags for extensions #define TVG_HEADER_COMPRESS_SIZE 12 //TVG_HEADER_UNCOMPRESSED_SIZE + TVG_HEADER_COMPRESSED_SIZE + TVG_HEADER_COMPRESSED_SIZE_BITS diff --git a/thirdparty/thorvg/src/renderer/tvgCanvas.h b/thirdparty/thorvg/src/renderer/tvgCanvas.h index 199e823034f7..c5d2127f9c63 100644 --- a/thirdparty/thorvg/src/renderer/tvgCanvas.h +++ b/thirdparty/thorvg/src/renderer/tvgCanvas.h @@ -26,7 +26,7 @@ #include "tvgPaint.h" -enum Status : uint8_t {Synced = 0, Updating, Drawing, Damanged}; +enum Status : uint8_t {Synced = 0, Updating, Drawing, Damaged}; struct Canvas::Impl { @@ -42,7 +42,7 @@ struct Canvas::Impl ~Impl() { - //make it sure any deffered jobs + //make it sure any deferred jobs renderer->sync(); renderer->clear(); @@ -61,7 +61,7 @@ struct Canvas::Impl Result push(unique_ptr paint) { - //You can not push paints during rendering. + //You cannot push paints during rendering. if (status == Status::Drawing) return Result::InsufficientCondition; auto p = paint.release(); @@ -91,7 +91,7 @@ struct Canvas::Impl Array clips; auto flag = RenderUpdateFlag::None; - if (status == Status::Damanged || force) flag = RenderUpdateFlag::All; + if (status == Status::Damaged || force) flag = RenderUpdateFlag::All; auto m = Matrix{1, 0, 0, 0, 1, 0, 0, 0, 1}; @@ -108,7 +108,7 @@ struct Canvas::Impl Result draw() { - if (status == Status::Damanged) update(nullptr, false); + if (status == Status::Damaged) update(nullptr, false); if (status == Status::Drawing || paints.empty() || !renderer->preRender()) return Result::InsufficientCondition; bool rendered = false; @@ -124,7 +124,7 @@ struct Canvas::Impl Result sync() { - if (status == Status::Synced || status == Status::Damanged) return Result::InsufficientCondition; + if (status == Status::Synced || status == Status::Damaged) return Result::InsufficientCondition; if (renderer->sync()) { status = Status::Synced; @@ -136,7 +136,7 @@ struct Canvas::Impl Result viewport(int32_t x, int32_t y, int32_t w, int32_t h) { - if (status != Status::Damanged && status != Status::Synced) return Result::InsufficientCondition; + if (status != Status::Damaged && status != Status::Synced) return Result::InsufficientCondition; RenderRegion val = {x, y, w, h}; //intersect if the target buffer is already set. @@ -147,7 +147,7 @@ struct Canvas::Impl if (vport == val) return Result::Success; renderer->viewport(val); vport = val; - status = Status::Damanged; + status = Status::Damaged; return Result::Success; } }; diff --git a/thirdparty/thorvg/src/renderer/tvgCommon.h b/thirdparty/thorvg/src/renderer/tvgCommon.h index 15a2cc4ef045..527221625b66 100644 --- a/thirdparty/thorvg/src/renderer/tvgCommon.h +++ b/thirdparty/thorvg/src/renderer/tvgCommon.h @@ -54,15 +54,6 @@ using namespace tvg; #define strdup _strdup #endif -//TVG class identifier values -#define TVG_CLASS_ID_UNDEFINED 0 -#define TVG_CLASS_ID_SHAPE 1 -#define TVG_CLASS_ID_SCENE 2 -#define TVG_CLASS_ID_PICTURE 3 -#define TVG_CLASS_ID_LINEAR 4 -#define TVG_CLASS_ID_RADIAL 5 -#define TVG_CLASS_ID_TEXT 6 - enum class FileType { Png = 0, Jpg, Webp, Tvg, Svg, Lottie, Ttf, Raw, Gif, Unknown }; using Size = Point; diff --git a/thirdparty/thorvg/src/renderer/tvgFill.cpp b/thirdparty/thorvg/src/renderer/tvgFill.cpp index ea1010051ee5..19edff5a2cd6 100644 --- a/thirdparty/thorvg/src/renderer/tvgFill.cpp +++ b/thirdparty/thorvg/src/renderer/tvgFill.cpp @@ -155,15 +155,14 @@ Fill* Fill::duplicate() const noexcept } -uint32_t Fill::identifier() const noexcept +TVG_DEPRECATED uint32_t Fill::identifier() const noexcept { - return pImpl->id; + return (uint32_t) type(); } RadialGradient::RadialGradient():pImpl(new Impl()) { - Fill::pImpl->id = TVG_CLASS_ID_RADIAL; Fill::pImpl->method(new FillDup(pImpl)); } @@ -196,15 +195,20 @@ unique_ptr RadialGradient::gen() noexcept } -uint32_t RadialGradient::identifier() noexcept +TVG_DEPRECATED uint32_t RadialGradient::identifier() noexcept { - return TVG_CLASS_ID_RADIAL; + return (uint32_t) Type::RadialGradient; +} + + +Type RadialGradient::type() const noexcept +{ + return Type::RadialGradient; } LinearGradient::LinearGradient():pImpl(new Impl()) { - Fill::pImpl->id = TVG_CLASS_ID_LINEAR; Fill::pImpl->method(new FillDup(pImpl)); } @@ -243,8 +247,13 @@ unique_ptr LinearGradient::gen() noexcept } -uint32_t LinearGradient::identifier() noexcept +TVG_DEPRECATED uint32_t LinearGradient::identifier() noexcept { - return TVG_CLASS_ID_LINEAR; + return (uint32_t) Type::LinearGradient; } + +Type LinearGradient::type() const noexcept +{ + return Type::LinearGradient; +} diff --git a/thirdparty/thorvg/src/renderer/tvgFill.h b/thirdparty/thorvg/src/renderer/tvgFill.h index 47f0c051c025..f249356aa2a7 100644 --- a/thirdparty/thorvg/src/renderer/tvgFill.h +++ b/thirdparty/thorvg/src/renderer/tvgFill.h @@ -55,7 +55,6 @@ struct Fill::Impl uint32_t cnt = 0; FillSpread spread; DuplicateMethod* dup = nullptr; - uint8_t id; ~Impl() { diff --git a/thirdparty/thorvg/src/renderer/tvgGlCanvas.cpp b/thirdparty/thorvg/src/renderer/tvgGlCanvas.cpp index 82666b7ae31f..24e2fb8b1beb 100644 --- a/thirdparty/thorvg/src/renderer/tvgGlCanvas.cpp +++ b/thirdparty/thorvg/src/renderer/tvgGlCanvas.cpp @@ -62,7 +62,7 @@ GlCanvas::~GlCanvas() Result GlCanvas::target(int32_t id, uint32_t w, uint32_t h) noexcept { #ifdef THORVG_GL_RASTER_SUPPORT - if (Canvas::pImpl->status != Status::Damanged && Canvas::pImpl->status != Status::Synced) { + if (Canvas::pImpl->status != Status::Damaged && Canvas::pImpl->status != Status::Synced) { return Result::InsufficientCondition; } @@ -75,7 +75,7 @@ Result GlCanvas::target(int32_t id, uint32_t w, uint32_t h) noexcept renderer->viewport(Canvas::pImpl->vport); //Paints must be updated again with this new target. - Canvas::pImpl->status = Status::Damanged; + Canvas::pImpl->status = Status::Damaged; return Result::Success; #endif diff --git a/thirdparty/thorvg/src/renderer/tvgLoadModule.h b/thirdparty/thorvg/src/renderer/tvgLoadModule.h index 1b81d81a4f70..a9c1a6854428 100644 --- a/thirdparty/thorvg/src/renderer/tvgLoadModule.h +++ b/thirdparty/thorvg/src/renderer/tvgLoadModule.h @@ -80,14 +80,14 @@ struct ImageLoader : LoadModule static ColorSpace cs; //desired value float w = 0, h = 0; //default image size - Surface surface; + RenderSurface surface; ImageLoader(FileType type) : LoadModule(type) {} virtual bool animatable() { return false; } //true if this loader supports animation. virtual Paint* paint() { return nullptr; } - virtual Surface* bitmap() + virtual RenderSurface* bitmap() { if (surface.data) return &surface; return nullptr; diff --git a/thirdparty/thorvg/src/renderer/tvgLoader.cpp b/thirdparty/thorvg/src/renderer/tvgLoader.cpp index 6a81ddcdbb31..db51fc215a0b 100644 --- a/thirdparty/thorvg/src/renderer/tvgLoader.cpp +++ b/thirdparty/thorvg/src/renderer/tvgLoader.cpp @@ -294,10 +294,10 @@ LoadModule* LoaderMgr::loader(const string& path, bool* invalid) { *invalid = false; - //TODO: lottie is not sharable. + //TODO: svg & lottie is not sharable. auto allowCache = true; auto ext = path.substr(path.find_last_of(".") + 1); - if (!ext.compare("json")) allowCache = false; + if (!ext.compare("svg") || !ext.compare("json")) allowCache = false; if (allowCache) { if (auto loader = _findFromCache(path)) return loader; @@ -317,7 +317,7 @@ LoadModule* LoaderMgr::loader(const string& path, bool* invalid) } delete(loader); } - //Unkown MimeType. Try with the candidates in the order + //Unknown MimeType. Try with the candidates in the order for (int i = 0; i < static_cast(FileType::Raw); i++) { if (auto loader = _find(static_cast(i))) { if (loader->open(path)) { @@ -392,7 +392,7 @@ LoadModule* LoaderMgr::loader(const char* data, uint32_t size, const string& mim } } } - //Unkown MimeType. Try with the candidates in the order + //Unknown MimeType. Try with the candidates in the order for (int i = 0; i < static_cast(FileType::Raw); i++) { auto loader = _find(static_cast(i)); if (loader) { diff --git a/thirdparty/thorvg/src/renderer/tvgPaint.cpp b/thirdparty/thorvg/src/renderer/tvgPaint.cpp index 37813b19ef7e..536e18785211 100644 --- a/thirdparty/thorvg/src/renderer/tvgPaint.cpp +++ b/thirdparty/thorvg/src/renderer/tvgPaint.cpp @@ -32,11 +32,11 @@ /************************************************************************/ #define PAINT_METHOD(ret, METHOD) \ - switch (id) { \ - case TVG_CLASS_ID_SHAPE: ret = P((Shape*)paint)->METHOD; break; \ - case TVG_CLASS_ID_SCENE: ret = P((Scene*)paint)->METHOD; break; \ - case TVG_CLASS_ID_PICTURE: ret = P((Picture*)paint)->METHOD; break; \ - case TVG_CLASS_ID_TEXT: ret = P((Text*)paint)->METHOD; break; \ + switch (paint->type()) { \ + case Type::Shape: ret = P((Shape*)paint)->METHOD; break; \ + case Type::Scene: ret = P((Scene*)paint)->METHOD; break; \ + case Type::Picture: ret = P((Picture*)paint)->METHOD; break; \ + case Type::Text: ret = P((Text*)paint)->METHOD; break; \ default: ret = {}; \ } @@ -91,8 +91,8 @@ static Result _compFastTrack(RenderMethod* renderer, Paint* cmpTarget, const Mat //No rotation and no skewing, still can try out clipping the rect region. auto tryClip = false; - if ((!mathRightAngle(pm) || mathSkewed(pm))) tryClip = true; - if ((!mathRightAngle(rm) || mathSkewed(rm))) tryClip = true; + if ((!rightAngle(pm) || skewed(pm))) tryClip = true; + if ((!rightAngle(rm) || skewed(rm))) tryClip = true; if (tryClip) return _clipRect(renderer, pts, pm, rm, before); @@ -102,8 +102,8 @@ static Result _compFastTrack(RenderMethod* renderer, Paint* cmpTarget, const Mat auto pt3 = pts + 2; auto pt4 = pts + 3; - if ((mathEqual(pt1->x, pt2->x) && mathEqual(pt2->y, pt3->y) && mathEqual(pt3->x, pt4->x) && mathEqual(pt1->y, pt4->y)) || - (mathEqual(pt2->x, pt3->x) && mathEqual(pt1->y, pt2->y) && mathEqual(pt1->x, pt4->x) && mathEqual(pt3->y, pt4->y))) { + if ((tvg::equal(pt1->x, pt2->x) && tvg::equal(pt2->y, pt3->y) && tvg::equal(pt3->x, pt4->x) && tvg::equal(pt1->y, pt4->y)) || + (tvg::equal(pt2->x, pt3->x) && tvg::equal(pt1->y, pt2->y) && tvg::equal(pt1->x, pt4->x) && tvg::equal(pt3->y, pt4->y))) { RenderRegion after; @@ -164,6 +164,7 @@ Paint* Paint::Impl::duplicate(Paint* ret) ret->pImpl->opacity = opacity; if (compData) ret->pImpl->composite(ret, compData->target->duplicate(), compData->method); + if (clipper) ret->pImpl->clip(clipper->duplicate()); return ret; } @@ -172,7 +173,7 @@ Paint* Paint::Impl::duplicate(Paint* ret) bool Paint::Impl::rotate(float degree) { if (tr.overriding) return false; - if (mathEqual(degree, tr.degree)) return true; + if (tvg::equal(degree, tr.degree)) return true; tr.degree = degree; renderFlag |= RenderUpdateFlag::Transform; @@ -183,7 +184,7 @@ bool Paint::Impl::rotate(float degree) bool Paint::Impl::scale(float factor) { if (tr.overriding) return false; - if (mathEqual(factor, tr.scale)) return true; + if (tvg::equal(factor, tr.scale)) return true; tr.scale = factor; renderFlag |= RenderUpdateFlag::Transform; @@ -194,7 +195,7 @@ bool Paint::Impl::scale(float factor) bool Paint::Impl::translate(float x, float y) { if (tr.overriding) return false; - if (mathEqual(x, tr.m.e13) && mathEqual(y, tr.m.e23)) return true; + if (tvg::equal(x, tr.m.e13) && tvg::equal(y, tr.m.e23)) return true; tr.m.e13 = x; tr.m.e23 = y; renderFlag |= RenderUpdateFlag::Transform; @@ -207,11 +208,9 @@ bool Paint::Impl::render(RenderMethod* renderer) { if (opacity == 0) return true; - Compositor* cmp = nullptr; + RenderCompositor* cmp = nullptr; - /* Note: only ClipPath is processed in update() step. - Create a composition image. */ - if (compData && compData->method != CompositeMethod::ClipPath && !(compData->target->pImpl->ctxFlag & ContextFlag::FastTrack)) { + if (compData && !(compData->target->pImpl->ctxFlag & ContextFlag::FastTrack)) { RenderRegion region; PAINT_METHOD(region, bounds(renderer)); @@ -248,43 +247,49 @@ RenderData Paint::Impl::update(RenderMethod* renderer, const Matrix& pm, Arraytarget; auto method = compData->method; P(target)->ctxFlag &= ~ContextFlag::FastTrack; //reset - /* If the transformation has no rotational factors and the ClipPath/Alpha(InvAlpha)Masking involves a simple rectangle, - we can optimize by using the viewport instead of the regular ClipPath/AlphaMasking sequence for improved performance. */ - auto tryFastTrack = false; - if (target->identifier() == TVG_CLASS_ID_SHAPE) { - if (method == CompositeMethod::ClipPath) tryFastTrack = true; - else { - auto shape = static_cast(target); - uint8_t a; - shape->fillColor(nullptr, nullptr, nullptr, &a); - //no gradient fill & no compositions of the composition target. - if (!shape->fill() && !(PP(shape)->compData)) { - if (method == CompositeMethod::AlphaMask && a == 255 && PP(shape)->opacity == 255) tryFastTrack = true; - else if (method == CompositeMethod::InvAlphaMask && (a == 0 || PP(shape)->opacity == 0)) tryFastTrack = true; - } - } - if (tryFastTrack) { - viewport = renderer->viewport(); - if ((compFastTrack = _compFastTrack(renderer, target, pm, viewport)) == Result::Success) { - P(target)->ctxFlag |= ContextFlag::FastTrack; + /* If the transformation has no rotational factors and the Alpha(InvAlpha)Masking involves a simple rectangle, + we can optimize by using the viewport instead of the regular AlphaMasking sequence for improved performance. */ + if (target->type() == Type::Shape) { + auto shape = static_cast(target); + uint8_t a; + shape->fillColor(nullptr, nullptr, nullptr, &a); + //no gradient fill & no compositions of the composition target. + if (!shape->fill() && !(PP(shape)->compData)) { + if ((method == CompositeMethod::AlphaMask && a == 255 && PP(shape)->opacity == 255) || (method == CompositeMethod::InvAlphaMask && (a == 0 || PP(shape)->opacity == 0))) { + viewport = renderer->viewport(); + if ((compFastTrack = _compFastTrack(renderer, target, pm, viewport)) == Result::Success) { + P(target)->ctxFlag |= ContextFlag::FastTrack; + } } } } if (compFastTrack == Result::InsufficientCondition) { - childClipper = compData->method == CompositeMethod::ClipPath ? true : false; - trd = P(target)->update(renderer, pm, clips, 255, pFlag, childClipper); - if (childClipper) clips.push(trd); + trd = P(target)->update(renderer, pm, clips, 255, pFlag, false); + } + } + + /* 2. Clipping */ + if (this->clipper) { + P(this->clipper)->ctxFlag &= ~ContextFlag::FastTrack; //reset + viewport = renderer->viewport(); + /* TODO: Intersect the clipper's clipper, if both are FastTrack. + Update the subsequent clipper first and check its ctxFlag. */ + if (!P(this->clipper)->clipper && (compFastTrack = _compFastTrack(renderer, this->clipper, pm, viewport)) == Result::Success) { + P(this->clipper)->ctxFlag |= ContextFlag::FastTrack; + } + if (compFastTrack == Result::InsufficientCondition) { + trd = P(this->clipper)->update(renderer, pm, clips, 255, pFlag, true); + clips.push(trd); } } - /* 2. Main Update */ + /* 3. Main Update */ auto newFlag = static_cast(pFlag | renderFlag); renderFlag = RenderUpdateFlag::None; opacity = MULTIPLY(opacity, this->opacity); @@ -294,9 +299,9 @@ RenderData Paint::Impl::update(RenderMethod* renderer, const Matrix& pm, Arrayviewport(viewport); - else if (childClipper) clips.pop(); + else if (this->clipper) clips.pop(); return rd; } @@ -308,7 +313,7 @@ bool Paint::Impl::bounds(float* x, float* y, float* w, float* h, bool transforme const auto& m = this->transform(origin); //Case: No transformed, quick return! - if (!transformed || mathIdentity(&m)) { + if (!transformed || identity(&m)) { PAINT_METHOD(ret, bounds(x, y, w, h, stroking)); return ret; } @@ -351,12 +356,18 @@ bool Paint::Impl::bounds(float* x, float* y, float* w, float* h, bool transforme void Paint::Impl::reset() { + if (clipper) { + delete(clipper); + clipper = nullptr; + } + if (compData) { if (P(compData->target)->unref() == 0) delete(compData->target); free(compData); compData = nullptr; } - mathIdentity(&tr.m); + + tvg::identity(&tr.m); tr.degree = 0.0f; tr.scale = 1.0f; tr.overriding = false; @@ -437,15 +448,27 @@ Paint* Paint::duplicate() const noexcept } -Result Paint::composite(std::unique_ptr target, CompositeMethod method) noexcept +Result Paint::clip(std::unique_ptr clipper) noexcept { - if (method == CompositeMethod::ClipPath && target && target->identifier() != TVG_CLASS_ID_SHAPE) { - TVGERR("RENDERER", "ClipPath only allows the Shape!"); + auto p = clipper.release(); + + if (p && p->type() != Type::Shape) { + TVGERR("RENDERER", "Clipping only supports the Shape!"); return Result::NonSupport; } + pImpl->clip(p); + return Result::Success; +} + + +Result Paint::composite(std::unique_ptr target, CompositeMethod method) noexcept +{ + //TODO: remove. Keep this for the backward compatibility + if (target && method == CompositeMethod::ClipPath) return clip(std::move(target)); auto p = target.release(); if (pImpl->composite(this, p, method)) return Result::Success; + delete(p); return Result::InvalidArguments; } @@ -457,6 +480,11 @@ CompositeMethod Paint::composite(const Paint** target) const noexcept if (target) *target = pImpl->compData->target; return pImpl->compData->method; } else { + //TODO: remove. Keep this for the backward compatibility + if (pImpl->clipper) { + if (target) *target = pImpl->clipper; + return CompositeMethod::ClipPath; + } if (target) *target = nullptr; return CompositeMethod::None; } @@ -480,14 +508,17 @@ uint8_t Paint::opacity() const noexcept } -uint32_t Paint::identifier() const noexcept +TVG_DEPRECATED uint32_t Paint::identifier() const noexcept { - return pImpl->id; + return (uint32_t) type(); } Result Paint::blend(BlendMethod method) noexcept { + //TODO: Remove later + if (method == BlendMethod::Hue || method == BlendMethod::Saturation || method == BlendMethod::Color || method == BlendMethod::Luminosity || method == BlendMethod::HardMix) return Result::NonSupport; + if (pImpl->blendMethod != method) { pImpl->blendMethod = method; pImpl->renderFlag |= RenderUpdateFlag::Blend; @@ -495,9 +526,3 @@ Result Paint::blend(BlendMethod method) noexcept return Result::Success; } - - -BlendMethod Paint::blend() const noexcept -{ - return pImpl->blendMethod; -} diff --git a/thirdparty/thorvg/src/renderer/tvgPaint.h b/thirdparty/thorvg/src/renderer/tvgPaint.h index e43ca239bb7a..d78e9bb3d1c2 100644 --- a/thirdparty/thorvg/src/renderer/tvgPaint.h +++ b/thirdparty/thorvg/src/renderer/tvgPaint.h @@ -49,6 +49,7 @@ namespace tvg { Paint* paint = nullptr; Composite* compData = nullptr; + Paint* clipper = nullptr; RenderMethod* renderer = nullptr; struct { Matrix m; //input matrix @@ -67,8 +68,8 @@ namespace tvg m.e31 = 0.0f; m.e32 = 0.0f; m.e33 = 1.0f; - mathScale(&m, scale, scale); - mathRotate(&m, degree); + tvg::scale(&m, scale, scale); + tvg::rotate(&m, degree); } } tr; BlendMethod blendMethod; @@ -76,7 +77,6 @@ namespace tvg uint8_t ctxFlag; uint8_t opacity; uint8_t refCnt = 0; //reference count - uint8_t id; //TODO: deprecated, remove it Impl(Paint* pnt) : paint(pnt) { @@ -89,6 +89,7 @@ namespace tvg if (P(compData->target)->unref() == 0) delete(compData->target); free(compData); } + if (clipper && P(clipper)->unref() == 0) delete(clipper); if (renderer && (renderer->unref() == 0)) delete(renderer); } @@ -106,7 +107,7 @@ namespace tvg bool transform(const Matrix& m) { - tr.m = m; + if (&tr.m != &m) tr.m = m; tr.overriding = true; renderFlag |= RenderUpdateFlag::Transform; @@ -121,6 +122,20 @@ namespace tvg return tr.m; } + void clip(Paint* clp) + { + if (this->clipper) { + P(this->clipper)->unref(); + if (this->clipper != clp && P(this->clipper)->refCnt == 0) { + delete(this->clipper); + } + } + this->clipper = clp; + if (!clp) return; + + P(clipper)->ref(); + } + bool composite(Paint* source, Paint* target, CompositeMethod method) { //Invalid case diff --git a/thirdparty/thorvg/src/renderer/tvgPicture.cpp b/thirdparty/thorvg/src/renderer/tvgPicture.cpp index 223cc5b026f2..d3e31d198a3c 100644 --- a/thirdparty/thorvg/src/renderer/tvgPicture.cpp +++ b/thirdparty/thorvg/src/renderer/tvgPicture.cpp @@ -20,6 +20,7 @@ * SOFTWARE. */ +#include "tvgPaint.h" #include "tvgPicture.h" /************************************************************************/ @@ -73,11 +74,11 @@ bool Picture::Impl::needComposition(uint8_t opacity) bool Picture::Impl::render(RenderMethod* renderer) { bool ret = false; - renderer->blend(picture->blend()); + renderer->blend(PP(picture)->blendMethod); if (surface) return renderer->renderImage(rd); else if (paint) { - Compositor* cmp = nullptr; + RenderCompositor* cmp = nullptr; if (needComp) { cmp = renderer->target(bounds(renderer), renderer->colorSpace()); renderer->beginComposite(cmp, CompositeMethod::None, 255); @@ -134,7 +135,6 @@ Result Picture::Impl::load(ImageLoader* loader) Picture::Picture() : pImpl(new Impl(this)) { - Paint::pImpl->id = TVG_CLASS_ID_PICTURE; } @@ -150,9 +150,15 @@ unique_ptr Picture::gen() noexcept } -uint32_t Picture::identifier() noexcept +TVG_DEPRECATED uint32_t Picture::identifier() noexcept { - return TVG_CLASS_ID_PICTURE; + return (uint32_t) Type::Picture; +} + + +Type Picture::type() const noexcept +{ + return Type::Picture; } diff --git a/thirdparty/thorvg/src/renderer/tvgPicture.h b/thirdparty/thorvg/src/renderer/tvgPicture.h index 3a4880cabaa3..bbbc43910595 100644 --- a/thirdparty/thorvg/src/renderer/tvgPicture.h +++ b/thirdparty/thorvg/src/renderer/tvgPicture.h @@ -60,7 +60,7 @@ struct Picture::Impl ImageLoader* loader = nullptr; Paint* paint = nullptr; //vector picture uses - Surface* surface = nullptr; //bitmap picture uses + RenderSurface* surface = nullptr; //bitmap picture uses RenderData rd = nullptr; //engine data float w = 0, h = 0; Picture* picture = nullptr; diff --git a/thirdparty/thorvg/src/renderer/tvgRender.h b/thirdparty/thorvg/src/renderer/tvgRender.h index b0ee42db8d0d..eae44a2e8ad9 100644 --- a/thirdparty/thorvg/src/renderer/tvgRender.h +++ b/thirdparty/thorvg/src/renderer/tvgRender.h @@ -24,6 +24,7 @@ #define _TVG_RENDER_H_ #include +#include #include "tvgCommon.h" #include "tvgArray.h" #include "tvgLock.h" @@ -36,9 +37,8 @@ using pixel_t = uint32_t; enum RenderUpdateFlag : uint8_t {None = 0, Path = 1, Color = 2, Gradient = 4, Stroke = 8, Transform = 16, Image = 32, GradientStroke = 64, Blend = 128, All = 255}; -struct Surface; - -enum ColorSpace +//TODO: Move this in public header unifying with SwCanvas::Colorspace +enum ColorSpace : uint8_t { ABGR8888 = 0, //The channels are joined in the order: alpha, blue, green, red. Colors are alpha-premultiplied. ARGB8888, //The channels are joined in the order: alpha, red, green, blue. Colors are alpha-premultiplied. @@ -48,7 +48,7 @@ enum ColorSpace Unsupported //TODO: Change to the default, At the moment, we put it in the last to align with SwCanvas::Colorspace. }; -struct Surface +struct RenderSurface { union { pixel_t* data = nullptr; //system based data pointer @@ -62,11 +62,11 @@ struct Surface uint8_t channelSize = 0; bool premultiplied = false; //Alpha-premultiplied - Surface() + RenderSurface() { } - Surface(const Surface* rhs) + RenderSurface(const RenderSurface* rhs) { data = rhs->data; stride = rhs->stride; @@ -80,21 +80,10 @@ struct Surface }; -struct Compositor +struct RenderCompositor { CompositeMethod method; - uint8_t opacity; -}; - -struct Vertex -{ - Point pt; - Point uv; -}; - -struct Polygon -{ - Vertex vertex[3]; + uint8_t opacity; }; struct RenderRegion @@ -270,11 +259,66 @@ struct RenderShape float strokeMiterlimit() const { if (!stroke) return 4.0f; - return stroke->miterlimit;; } }; +struct RenderEffect +{ + RenderData rd = nullptr; + RenderRegion extend = {0, 0, 0, 0}; + SceneEffect type; + bool invalid = false; + + virtual ~RenderEffect() + { + free(rd); + } +}; + +struct RenderEffectGaussianBlur : RenderEffect +{ + float sigma; + uint8_t direction; //0: both, 1: horizontal, 2: vertical + uint8_t border; //0: duplicate, 1: wrap + uint8_t quality; //0 ~ 100 (optional) + + static RenderEffectGaussianBlur* gen(va_list& args) + { + auto inst = new RenderEffectGaussianBlur; + inst->sigma = std::max((float) va_arg(args, double), 0.0f); + inst->direction = std::min(va_arg(args, int), 2); + inst->border = std::min(va_arg(args, int), 1); + inst->quality = std::min(va_arg(args, int), 100); + inst->type = SceneEffect::GaussianBlur; + return inst; + } +}; + +struct RenderEffectDropShadow : RenderEffect +{ + uint8_t color[4]; //rgba + float angle; + float distance; + float sigma; + uint8_t quality; //0 ~ 100 (optional) + + static RenderEffectDropShadow* gen(va_list& args) + { + auto inst = new RenderEffectDropShadow; + inst->color[0] = va_arg(args, int); + inst->color[1] = va_arg(args, int); + inst->color[2] = va_arg(args, int); + inst->color[3] = std::min(va_arg(args, int), 255); + inst->angle = (float) va_arg(args, double); + inst->distance = (float) va_arg(args, double); + inst->sigma = std::max((float) va_arg(args, double), 0.0f); + inst->quality = std::min(va_arg(args, int), 100); + inst->type = SceneEffect::DropShadow; + return inst; + } +}; + class RenderMethod { private: @@ -287,7 +331,7 @@ class RenderMethod virtual ~RenderMethod() {} virtual RenderData prepare(const RenderShape& rshape, RenderData data, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flags, bool clipper) = 0; - virtual RenderData prepare(Surface* surface, RenderData data, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flags) = 0; + virtual RenderData prepare(RenderSurface* surface, RenderData data, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flags) = 0; virtual bool preRender() = 0; virtual bool renderShape(RenderData data) = 0; virtual bool renderImage(RenderData data) = 0; @@ -298,14 +342,17 @@ class RenderMethod virtual bool viewport(const RenderRegion& vp) = 0; virtual bool blend(BlendMethod method) = 0; virtual ColorSpace colorSpace() = 0; - virtual const Surface* mainSurface() = 0; + virtual const RenderSurface* mainSurface() = 0; virtual bool clear() = 0; virtual bool sync() = 0; - virtual Compositor* target(const RenderRegion& region, ColorSpace cs) = 0; - virtual bool beginComposite(Compositor* cmp, CompositeMethod method, uint8_t opacity) = 0; - virtual bool endComposite(Compositor* cmp) = 0; + virtual RenderCompositor* target(const RenderRegion& region, ColorSpace cs) = 0; + virtual bool beginComposite(RenderCompositor* cmp, CompositeMethod method, uint8_t opacity) = 0; + virtual bool endComposite(RenderCompositor* cmp) = 0; + + virtual bool prepare(RenderEffect* effect) = 0; + virtual bool effect(RenderCompositor* cmp, const RenderEffect* effect, uint8_t opacity, bool direct) = 0; }; static inline bool MASK_REGION_MERGING(CompositeMethod method) @@ -374,7 +421,6 @@ static inline uint8_t MULTIPLY(uint8_t c, uint8_t a) return (((c) * (a) + 0xff) >> 8); } - } #endif //_TVG_RENDER_H_ diff --git a/thirdparty/thorvg/src/renderer/tvgSaver.cpp b/thirdparty/thorvg/src/renderer/tvgSaver.cpp index 79302f69fa56..993fe6d80f8d 100644 --- a/thirdparty/thorvg/src/renderer/tvgSaver.cpp +++ b/thirdparty/thorvg/src/renderer/tvgSaver.cpp @@ -20,6 +20,7 @@ * SOFTWARE. */ +#include #include "tvgCommon.h" #include "tvgSaveModule.h" #include "tvgPaint.h" @@ -122,7 +123,7 @@ Result Saver::save(std::unique_ptr paint, const string& path, bool compre auto p = paint.release(); if (!p) return Result::MemoryCorruption; - //Already on saving an other resource. + //Already on saving another resource. if (pImpl->saveModule) { if (P(p)->refCnt == 0) delete(p); return Result::InsufficientCondition; @@ -160,12 +161,12 @@ Result Saver::save(unique_ptr animation, const string& path, uint32_t //animation holds the picture, it must be 1 at the bottom. auto remove = PP(a->picture())->refCnt <= 1 ? true : false; - if (mathZero(a->totalFrame())) { + if (tvg::zero(a->totalFrame())) { if (remove) delete(a); return Result::InsufficientCondition; } - //Already on saving an other resource. + //Already on saving another resource. if (pImpl->saveModule) { if (remove) delete(a); return Result::InsufficientCondition; diff --git a/thirdparty/thorvg/src/renderer/tvgScene.cpp b/thirdparty/thorvg/src/renderer/tvgScene.cpp index f5809cf93b56..ce169d33ba67 100644 --- a/thirdparty/thorvg/src/renderer/tvgScene.cpp +++ b/thirdparty/thorvg/src/renderer/tvgScene.cpp @@ -20,15 +20,32 @@ * SOFTWARE. */ +#include #include "tvgScene.h" +/************************************************************************/ +/* Internal Class Implementation */ +/************************************************************************/ + +Result Scene::Impl::resetEffects() +{ + if (effects) { + for (auto e = effects->begin(); e < effects->end(); ++e) { + delete(*e); + } + delete(effects); + effects = nullptr; + } + return Result::Success; +} + + /************************************************************************/ /* External Class Implementation */ /************************************************************************/ Scene::Scene() : pImpl(new Impl(this)) { - Paint::pImpl->id = TVG_CLASS_ID_SCENE; } @@ -44,9 +61,15 @@ unique_ptr Scene::gen() noexcept } -uint32_t Scene::identifier() noexcept +TVG_DEPRECATED uint32_t Scene::identifier() noexcept +{ + return (uint32_t) Type::Scene; +} + + +Type Scene::type() const noexcept { - return TVG_CLASS_ID_SCENE; + return Type::Scene; } @@ -54,7 +77,11 @@ Result Scene::push(unique_ptr paint) noexcept { auto p = paint.release(); if (!p) return Result::MemoryCorruption; - PP(p)->ref(); + P(p)->ref(); + + //Relocated the paint to the current scene space + P(p)->renderFlag |= RenderUpdateFlag::Transform; + pImpl->paints.push_back(p); return Result::Success; @@ -79,3 +106,34 @@ list& Scene::paints() noexcept { return pImpl->paints; } + + +Result Scene::push(SceneEffect effect, ...) noexcept +{ + if (effect == SceneEffect::ClearAll) return pImpl->resetEffects(); + + if (!pImpl->effects) pImpl->effects = new Array; + + va_list args; + va_start(args, effect); + + RenderEffect* re = nullptr; + + switch (effect) { + case SceneEffect::GaussianBlur: { + re = RenderEffectGaussianBlur::gen(args); + break; + } + case SceneEffect::DropShadow: { + re = RenderEffectDropShadow::gen(args); + break; + } + default: break; + } + + if (!re) return Result::InvalidArguments; + + pImpl->effects->push(re); + + return Result::Success; +} diff --git a/thirdparty/thorvg/src/renderer/tvgScene.h b/thirdparty/thorvg/src/renderer/tvgScene.h index 190ecd31b91d..7972ae33fb10 100644 --- a/thirdparty/thorvg/src/renderer/tvgScene.h +++ b/thirdparty/thorvg/src/renderer/tvgScene.h @@ -23,10 +23,9 @@ #ifndef _TVG_SCENE_H_ #define _TVG_SCENE_H_ -#include +#include "tvgMath.h" #include "tvgPaint.h" - struct SceneIterator : Iterator { list* paints; @@ -61,8 +60,10 @@ struct Scene::Impl list paints; RenderData rd = nullptr; Scene* scene = nullptr; - uint8_t opacity; //for composition - bool needComp = false; //composite or not + RenderRegion vport = {0, 0, INT32_MAX, INT32_MAX}; + Array* effects = nullptr; + uint8_t opacity; //for composition + bool needComp = false; //composite or not Impl(Scene* s) : scene(s) { @@ -70,6 +71,8 @@ struct Scene::Impl ~Impl() { + resetEffects(); + for (auto paint : paints) { if (P(paint)->unref() == 0) delete(paint); } @@ -83,12 +86,15 @@ struct Scene::Impl { if (opacity == 0 || paints.empty()) return false; + //post effects requires composition + if (effects) return true; + //Masking may require composition (even if opacity == 255) auto compMethod = scene->composite(nullptr); if (compMethod != CompositeMethod::None && compMethod != CompositeMethod::ClipPath) return true; //Blending may require composition (even if opacity == 255) - if (scene->blend() != BlendMethod::Normal) return true; + if (PP(scene)->blendMethod != BlendMethod::Normal) return true; //Half translucent requires intermediate composition. if (opacity == 255) return false; @@ -96,31 +102,34 @@ struct Scene::Impl //If scene has several children or only scene, it may require composition. //OPTIMIZE: the bitmap type of the picture would not need the composition. //OPTIMIZE: a single paint of a scene would not need the composition. - if (paints.size() == 1 && paints.front()->identifier() == TVG_CLASS_ID_SHAPE) return false; + if (paints.size() == 1 && paints.front()->type() == Type::Shape) return false; return true; } RenderData update(RenderMethod* renderer, const Matrix& transform, Array& clips, uint8_t opacity, RenderUpdateFlag flag, TVG_UNUSED bool clipper) { + this->vport = renderer->viewport(); + if ((needComp = needComposition(opacity))) { /* Overriding opacity value. If this scene is half-translucent, - It must do intermeidate composition with that opacity value. */ + It must do intermediate composition with that opacity value. */ this->opacity = opacity; opacity = 255; } for (auto paint : paints) { paint->pImpl->update(renderer, transform, clips, opacity, flag, false); } + return nullptr; } bool render(RenderMethod* renderer) { - Compositor* cmp = nullptr; + RenderCompositor* cmp = nullptr; auto ret = true; - renderer->blend(scene->blend()); + renderer->blend(PP(scene)->blendMethod); if (needComp) { cmp = renderer->target(bounds(renderer), renderer->colorSpace()); @@ -131,7 +140,16 @@ struct Scene::Impl ret &= paint->pImpl->render(renderer); } - if (cmp) renderer->endComposite(cmp); + if (cmp) { + //Apply post effects if any. + if (effects) { + auto direct = effects->count == 1 ? true : false; + for (auto e = effects->begin(); e < effects->end(); ++e) { + renderer->effect(cmp, *e, opacity, direct); + } + } + renderer->endComposite(cmp); + } return ret; } @@ -155,7 +173,23 @@ struct Scene::Impl if (y2 < region.y + region.h) y2 = (region.y + region.h); } - return {x1, y1, (x2 - x1), (y2 - y1)}; + //Extends the render region if post effects require + int32_t ex = 0, ey = 0, ew = 0, eh = 0; + if (effects) { + for (auto e = effects->begin(); e < effects->end(); ++e) { + auto effect = *e; + if (effect->rd || renderer->prepare(effect)) { + ex = std::min(ex, effect->extend.x); + ey = std::min(ey, effect->extend.y); + ew = std::max(ew, effect->extend.w); + eh = std::max(eh, effect->extend.h); + } + } + } + + auto ret = RenderRegion{x1 + ex, y1 + ey, (x2 - x1) + ew, (y2 - y1) + eh}; + ret.intersect(this->vport); + return ret; } bool bounds(float* px, float* py, float* pw, float* ph, bool stroking) @@ -203,6 +237,8 @@ struct Scene::Impl dup->paints.push_back(cdup); } + if (effects) TVGERR("RENDERER", "TODO: Duplicate Effects?"); + return scene; } @@ -218,6 +254,8 @@ struct Scene::Impl { return new SceneIterator(&paints); } + + Result resetEffects(); }; #endif //_TVG_SCENE_H_ diff --git a/thirdparty/thorvg/src/renderer/tvgShape.cpp b/thirdparty/thorvg/src/renderer/tvgShape.cpp index 3b9293a00e05..269d951f05a3 100644 --- a/thirdparty/thorvg/src/renderer/tvgShape.cpp +++ b/thirdparty/thorvg/src/renderer/tvgShape.cpp @@ -34,7 +34,6 @@ Shape :: Shape() : pImpl(new Impl(this)) { - Paint::pImpl->id = TVG_CLASS_ID_SHAPE; } @@ -52,7 +51,13 @@ unique_ptr Shape::gen() noexcept uint32_t Shape::identifier() noexcept { - return TVG_CLASS_ID_SHAPE; + return (uint32_t) Type::Shape; +} + + +Type Shape::type() const noexcept +{ + return Type::Shape; } @@ -151,14 +156,14 @@ Result Shape::appendCircle(float cx, float cy, float rx, float ry) noexcept } -Result Shape::appendArc(float cx, float cy, float radius, float startAngle, float sweep, bool pie) noexcept +TVG_DEPRECATED Result Shape::appendArc(float cx, float cy, float radius, float startAngle, float sweep, bool pie) noexcept { //just circle if (sweep >= 360.0f || sweep <= -360.0f) return appendCircle(cx, cy, radius, radius); const float arcPrecision = 1e-5f; - startAngle = mathDeg2Rad(startAngle); - sweep = mathDeg2Rad(sweep); + startAngle = deg2rad(startAngle); + sweep = deg2rad(sweep); auto nCurves = static_cast(fabsf(sweep / MATH_PI2)); if (fabsf(sweep / MATH_PI2) - nCurves > arcPrecision) ++nCurves; @@ -409,12 +414,6 @@ Result Shape::strokeTrim(float begin, float end, bool simultaneous) noexcept } -bool Shape::strokeTrim(float* begin, float* end) const noexcept -{ - return pImpl->strokeTrim(begin, end); -} - - Result Shape::fill(FillRule r) noexcept { pImpl->rs.rule = r; diff --git a/thirdparty/thorvg/src/renderer/tvgShape.h b/thirdparty/thorvg/src/renderer/tvgShape.h index 440fb312b6c0..42f815206062 100644 --- a/thirdparty/thorvg/src/renderer/tvgShape.h +++ b/thirdparty/thorvg/src/renderer/tvgShape.h @@ -53,9 +53,9 @@ struct Shape::Impl { if (!rd) return false; - Compositor* cmp = nullptr; + RenderCompositor* cmp = nullptr; - renderer->blend(shape->blend()); + renderer->blend(PP(shape)->blendMethod); if (needComp) { cmp = renderer->target(bounds(renderer), renderer->colorSpace()); @@ -83,7 +83,7 @@ struct Shape::Impl auto method = shape->composite(&target); if (!target || method == CompositeMethod::ClipPath) return false; if (target->pImpl->opacity == 255 || target->pImpl->opacity == 0) { - if (target->identifier() == TVG_CLASS_ID_SHAPE) { + if (target->type() == Type::Shape) { auto shape = static_cast(target); if (!shape->fill()) { uint8_t r, g, b, a; @@ -106,7 +106,7 @@ struct Shape::Impl if ((needComp = needComposition(opacity))) { /* Overriding opacity value. If this scene is half-translucent, - It must do intermeidate composition with that opacity value. */ + It must do intermediate composition with that opacity value. */ this->opacity = opacity; opacity = 255; } @@ -219,7 +219,7 @@ struct Shape::Impl rs.stroke = new RenderStroke(); } - if (mathEqual(rs.stroke->trim.begin, begin) && mathEqual(rs.stroke->trim.end, end) && + if (tvg::equal(rs.stroke->trim.begin, begin) && tvg::equal(rs.stroke->trim.end, end) && rs.stroke->trim.simultaneous == simultaneous) return; rs.stroke->trim.begin = begin; diff --git a/thirdparty/thorvg/src/renderer/tvgSwCanvas.cpp b/thirdparty/thorvg/src/renderer/tvgSwCanvas.cpp index d762492f22dc..6c4b6da1de44 100644 --- a/thirdparty/thorvg/src/renderer/tvgSwCanvas.cpp +++ b/thirdparty/thorvg/src/renderer/tvgSwCanvas.cpp @@ -82,7 +82,7 @@ Result SwCanvas::mempool(MempoolPolicy policy) noexcept Result SwCanvas::target(uint32_t* buffer, uint32_t stride, uint32_t w, uint32_t h, Colorspace cs) noexcept { #ifdef THORVG_SW_RASTER_SUPPORT - if (Canvas::pImpl->status != Status::Damanged && Canvas::pImpl->status != Status::Synced) { + if (Canvas::pImpl->status != Status::Damaged && Canvas::pImpl->status != Status::Synced) { return Result::InsufficientCondition; } @@ -98,7 +98,7 @@ Result SwCanvas::target(uint32_t* buffer, uint32_t stride, uint32_t w, uint32_t ImageLoader::cs = static_cast(cs); //Paints must be updated again with this new target. - Canvas::pImpl->status = Status::Damanged; + Canvas::pImpl->status = Status::Damaged; return Result::Success; #endif diff --git a/thirdparty/thorvg/src/renderer/tvgText.cpp b/thirdparty/thorvg/src/renderer/tvgText.cpp index d54c78783c5d..b324b95049e9 100644 --- a/thirdparty/thorvg/src/renderer/tvgText.cpp +++ b/thirdparty/thorvg/src/renderer/tvgText.cpp @@ -37,7 +37,6 @@ Text::Text() : pImpl(new Impl(this)) { - Paint::pImpl->id = TVG_CLASS_ID_TEXT; } @@ -111,7 +110,7 @@ unique_ptr Text::gen() noexcept } -uint32_t Text::identifier() noexcept +Type Text::type() const noexcept { - return TVG_CLASS_ID_TEXT; + return Type::Text; } diff --git a/thirdparty/thorvg/src/renderer/tvgText.h b/thirdparty/thorvg/src/renderer/tvgText.h index cb9a76c05218..11e01b58ce6d 100644 --- a/thirdparty/thorvg/src/renderer/tvgText.h +++ b/thirdparty/thorvg/src/renderer/tvgText.h @@ -90,7 +90,7 @@ struct Text::Impl bool render(RenderMethod* renderer) { if (!loader) return true; - renderer->blend(paint->blend()); + renderer->blend(PP(paint)->blendMethod); return PP(shape)->render(renderer); } @@ -115,7 +115,7 @@ struct Text::Impl auto fill = P(shape)->rs.fill; if (fill && P(shape)->flag & RenderUpdateFlag::Gradient) { auto scale = 1.0f / loader->scale; - if (fill->identifier() == TVG_CLASS_ID_LINEAR) { + if (fill->type() == Type::LinearGradient) { P(static_cast(fill))->x1 *= scale; P(static_cast(fill))->y1 *= scale; P(static_cast(fill))->x2 *= scale; diff --git a/thirdparty/thorvg/src/renderer/tvgWgCanvas.cpp b/thirdparty/thorvg/src/renderer/tvgWgCanvas.cpp index 067e35b1f0d8..991f73fc542c 100644 --- a/thirdparty/thorvg/src/renderer/tvgWgCanvas.cpp +++ b/thirdparty/thorvg/src/renderer/tvgWgCanvas.cpp @@ -40,7 +40,7 @@ struct WgCanvas::Impl /************************************************************************/ #ifdef THORVG_WG_RASTER_SUPPORT -WgCanvas::WgCanvas() : Canvas(WgRenderer::gen()), pImpl(new Impl) +WgCanvas::WgCanvas() : Canvas(WgRenderer::gen()), pImpl(nullptr) #else WgCanvas::WgCanvas() : Canvas(nullptr), pImpl(nullptr) #endif @@ -50,14 +50,17 @@ WgCanvas::WgCanvas() : Canvas(nullptr), pImpl(nullptr) WgCanvas::~WgCanvas() { - delete pImpl; +#ifdef THORVG_WG_RASTER_SUPPORT + auto renderer = static_cast(Canvas::pImpl->renderer); + renderer->target(nullptr, 0, 0); +#endif } -Result WgCanvas::target(void* instance, void* surface, uint32_t w, uint32_t h) noexcept +Result WgCanvas::target(void* instance, void* surface, uint32_t w, uint32_t h, void* device) noexcept { #ifdef THORVG_WG_RASTER_SUPPORT - if (Canvas::pImpl->status != Status::Damanged && Canvas::pImpl->status != Status::Synced) { + if (Canvas::pImpl->status != Status::Damaged && Canvas::pImpl->status != Status::Synced) { return Result::InsufficientCondition; } @@ -67,12 +70,12 @@ Result WgCanvas::target(void* instance, void* surface, uint32_t w, uint32_t h) n auto renderer = static_cast(Canvas::pImpl->renderer); if (!renderer) return Result::MemoryCorruption; - if (!renderer->target((WGPUInstance)instance, (WGPUSurface)surface, w, h)) return Result::Unknown; + if (!renderer->target((WGPUInstance)instance, (WGPUSurface)surface, w, h, (WGPUDevice)device)) return Result::Unknown; Canvas::pImpl->vport = {0, 0, (int32_t)w, (int32_t)h}; renderer->viewport(Canvas::pImpl->vport); //Paints must be updated again with this new target. - Canvas::pImpl->status = Status::Damanged; + Canvas::pImpl->status = Status::Damaged; return Result::Success; #endif diff --git a/thirdparty/thorvg/update-thorvg.sh b/thirdparty/thorvg/update-thorvg.sh index 2c5d84d266cb..f9953f2fc9e5 100755 --- a/thirdparty/thorvg/update-thorvg.sh +++ b/thirdparty/thorvg/update-thorvg.sh @@ -1,6 +1,6 @@ #!/bin/bash -e -VERSION=0.14.10 +VERSION=0.15.5 # Uncomment and set a git hash to use specific commit instead of tag. #GIT_COMMIT= diff --git a/thirdparty/ufbx/ufbx.c b/thirdparty/ufbx/ufbx.c index 2d13c78bd8c3..77bd3dea623f 100644 --- a/thirdparty/ufbx/ufbx.c +++ b/thirdparty/ufbx/ufbx.c @@ -20,6 +20,21 @@ // UFBX_TRACE Log calls of `ufbxi_check()` for tracing execution // UFBX_LITTLE_ENDIAN=0/1 Explicitly define little/big endian architecture // UFBX_PATH_SEPARATOR='' Specify default platform path separator +// UFBX_NO_SSE Do not try to include SSE + +// Dependencies: +// UFBX_NO_MALLOC Disable default malloc/realloc/free +// UFBX_NO_STDIO Disable stdio FILE API +// UFBX_EXTERNAL_MALLOC Link to external ufbx_malloc() interface +// UFBX_EXTERNAL_STDIO Link to external ufbx_stdio_() interface +// UFBX_EXTERNAL_MATH Link to external interface +// UFBX_EXTERNAL_STRING Link to external interface + +// Freestanding: +// UFBX_MATH_PREFIX='ufbx_' Prefix for external functions used +// UFBX_STRING_PREFIX='ufbx_' Prefix for external functions used +// UFBX_NO_LIBC Do not include libc (implies UFBX_EXTERNAL_MATH/STRING/MALLOC/STDIO by default) +// UFBX_NO_LIBC_TYPES Do not include any libc headers, you must define all types in // Mostly internal for debugging: // UFBX_STATIC_ANALYSIS Enable static analysis augmentation @@ -175,28 +190,71 @@ // -- Headers -#include -#include -#include -#include -#include -#include - -#if !defined(UFBX_NO_MATH_H) - #include - #define UFBX_INFINITY INFINITY - #define UFBX_NAN NAN +// Legacy mapping +#if !defined(UFBX_EXTERNAL_MATH) && defined(UFBX_NO_MATH_H) + #define UFBX_EXTERNAL_MATH #endif -#if !defined(UFBX_MATH_PREFIX) - #define UFBX_MATH_PREFIX +#if !defined(UFBX_NO_LIBC_TYPES) + #include #endif -#define ufbxi_math_cat2(a, b) a##b -#define ufbxi_math_cat(a, b) ufbxi_math_cat2(a, b) -#define ufbxi_math_fn(name) ufbxi_math_cat(UFBX_MATH_PREFIX, name) +#if !defined(UFBX_NO_LIBC) + #if !defined(UFBX_NO_FLOAT_H) + #include + #endif + #if !defined(UFBX_EXTERNAL_MATH) + #include + #endif + #if !defined(UFBX_EXTERNAL_STRING) + #include + #endif + #if !defined(UFBX_NO_STDIO) && !defined(UFBX_EXTERNAL_STDIO) + #include + #endif + #if !defined(UFBX_NO_MALLOC) && !defined(UFBX_EXTERNAL_MALLOC) + #include + #endif +#else + #if !defined(UFBX_EXTERNAL_MATH) && !defined(UFBX_NO_EXTERNAL_MATH) + #define UFBX_EXTERNAL_MATH + #endif + #if !defined(UFBX_EXTERNAL_STRING) && !defined(UFBX_NO_EXTERNAL_STRING) + #define UFBX_EXTERNAL_STRING + #endif + #if !defined(UFBX_EXTERNAL_MALLOC) && !defined(UFBX_NO_EXTERNAL_MALLOC) && !defined(UFBX_NO_MALLOC) + #define UFBX_EXTERNAL_MALLOC + #endif + #if !defined(UFBX_EXTERNAL_STDIO) && !defined(UFBX_NO_EXTERNAL_STDIO) && !defined(UFBX_NO_STDIO) + #define UFBX_EXTERNAL_STDIO + #endif +#endif + +#if defined(UFBX_EXTERNAL_STRING) && !defined(UFBX_STRING_PREFIX) + #define UFBX_STRING_PREFIX ufbx_ +#endif -#if !defined(UFBX_NO_MATH_DEFINES) +#if !defined(UFBX_EXTERNAL_MATH) + #if !defined(UFBX_MATH_PREFIX) + #define UFBX_MATH_PREFIX + #endif +#endif + +#define ufbxi_pre_cat2(a, b) a##b +#define ufbxi_pre_cat(a, b) ufbxi_pre_cat2(a, b) + +// -- External functions + +#ifndef ufbx_extern_abi + #if defined(UFBX_STATIC) + #define ufbx_extern_abi static + #else + #define ufbx_extern_abi + #endif +#endif + +#if defined(UFBX_MATH_PREFIX) + #define ufbxi_math_fn(name) ufbxi_pre_cat(UFBX_MATH_PREFIX, name) #define ufbx_sqrt ufbxi_math_fn(sqrt) #define ufbx_fabs ufbxi_math_fn(fabs) #define ufbx_pow ufbxi_math_fn(pow) @@ -216,31 +274,123 @@ #define ufbx_isnan ufbxi_math_fn(isnan) #endif -#if defined(UFBX_NO_MATH_H) && !defined(UFBX_NO_MATH_DECLARATIONS) - double ufbx_sqrt(double x); - double ufbx_sin(double x); - double ufbx_cos(double x); - double ufbx_tan(double x); - double ufbx_asin(double x); - double ufbx_acos(double x); - double ufbx_atan(double x); - double ufbx_atan2(double y, double x); - double ufbx_pow(double x, double y); - double ufbx_fmin(double a, double b); - double ufbx_fmax(double a, double b); - double ufbx_fabs(double x); - double ufbx_copysign(double x, double y); - double ufbx_nextafter(double x, double y); - double ufbx_rint(double x); - double ufbx_ceil(double x); - int ufbx_isnan(double x); +#if !defined(UFBX_NO_EXTERNAL_DEFINES) + +#if defined(__cplusplus) +extern "C" { +#endif + +#if defined(UFBX_EXTERNAL_MATH) + ufbx_extern_abi double ufbx_sqrt(double x); + ufbx_extern_abi double ufbx_sin(double x); + ufbx_extern_abi double ufbx_cos(double x); + ufbx_extern_abi double ufbx_tan(double x); + ufbx_extern_abi double ufbx_asin(double x); + ufbx_extern_abi double ufbx_acos(double x); + ufbx_extern_abi double ufbx_atan(double x); + ufbx_extern_abi double ufbx_atan2(double y, double x); + ufbx_extern_abi double ufbx_pow(double x, double y); + ufbx_extern_abi double ufbx_fmin(double a, double b); + ufbx_extern_abi double ufbx_fmax(double a, double b); + ufbx_extern_abi double ufbx_fabs(double x); + ufbx_extern_abi double ufbx_copysign(double x, double y); + ufbx_extern_abi double ufbx_nextafter(double x, double y); + ufbx_extern_abi double ufbx_rint(double x); + ufbx_extern_abi double ufbx_ceil(double x); + ufbx_extern_abi int ufbx_isnan(double x); +#endif + +#if defined(UFBX_EXTERNAL_STRING) + ufbx_extern_abi size_t ufbx_strlen(const char *str); + ufbx_extern_abi void *ufbx_memcpy(void *dst, const void *src, size_t count); + ufbx_extern_abi void *ufbx_memmove(void *dst, const void *src, size_t count); + ufbx_extern_abi void *ufbx_memset(void *dst, int ch, size_t count); + ufbx_extern_abi const void *ufbx_memchr(const void *ptr, int value, size_t count); + ufbx_extern_abi int ufbx_memcmp(const void *a, const void *b, size_t count); + ufbx_extern_abi int ufbx_strcmp(const char *a, const char *b); + ufbx_extern_abi int ufbx_strncmp(const char *a, const char *b, size_t count); +#endif + +#if defined(UFBX_EXTERNAL_MALLOC) + ufbx_extern_abi void *ufbx_malloc(size_t size); + ufbx_extern_abi void *ufbx_realloc(void *ptr, size_t old_size, size_t new_size); + ufbx_extern_abi void ufbx_free(void *ptr, size_t old_size); +#endif + +#if defined(UFBX_EXTERNAL_STDIO) + ufbx_extern_abi void *ufbx_stdio_open(const char *path, size_t path_len); + ufbx_extern_abi size_t ufbx_stdio_read(void *file, void *data, size_t size); + ufbx_extern_abi bool ufbx_stdio_skip(void *file, size_t size); + ufbx_extern_abi uint64_t ufbx_stdio_size(void *file); + ufbx_extern_abi void ufbx_stdio_close(void *file); +#endif + +#if defined(__cplusplus) +} +#endif + #endif #if !defined(UFBX_INFINITY) - #define UFBX_INFINITY (1e+300 * 1e+300) + #if defined(INFINITY) + #define UFBX_INFINITY INFINITY + #else + #define UFBX_INFINITY (1e+300 * 1e+300) + #endif #endif #if !defined(UFBX_NAN) - #define UFBX_NAN (UFBX_INFINITY * 0.0f) + #if defined(NAN) + #define UFBX_NAN NAN + #else + #define UFBX_NAN (UFBX_INFINITY * 0.0f) + #endif +#endif +#if !defined(UFBX_FLT_EPSILON) + #if defined(FLT_EPSILON) + #define UFBX_FLT_EPSILON FLT_EPSILON + #else + #define UFBX_FLT_EPSILON 1.192092896e-07f + #endif +#endif +#if !defined(UFBX_FLT_EVAL_METHOD) + #if defined(FLT_EVAL_METHOD) + #define UFBX_FLT_EVAL_METHOD FLT_EVAL_METHOD + #elif defined(__FLT_EVAL_METHOD__) + #define UFBX_FLT_EVAL_METHOD __FLT_EVAL_METHOD__ + #elif defined(_MSC_VER) && (defined(_M_X64) || defined(_M_ARM64)) + #define UFBX_FLT_EVAL_METHOD 0 + #else + #define UFBX_FLT_EVAL_METHOD -1 + #endif +#endif + +#if defined(ufbx_malloc) || defined(ufbx_realloc) || defined(ufbx_free) + // User provided allocators + #if !defined(ufbx_malloc) || !defined(ufbx_realloc) || !defined(ufbx_free) + #error Inconsistent custom global allocator + #endif +#elif defined(UFBX_NO_MALLOC) + #define ufbx_malloc(size) ((void)(size), (void*)NULL) + #define ufbx_realloc(ptr, old_size, new_size) ((void)(ptr), (void)(old_size), (void)(new_size), (void*)NULL) + #define ufbx_free(ptr, old_size) ((void)(ptr), (void*)(old_size)) +#elif defined(UFBX_EXTERNAL_MALLOC) + // Nop +#else + #define ufbx_malloc(size) malloc((size)) + #define ufbx_realloc(ptr, old_size, new_size) realloc((ptr), (new_size)) + #define ufbx_free(ptr, old_size) free((ptr)) +#endif + +#if !defined(ufbx_panic_handler) + static void ufbxi_panic_handler(const char *message) + { + (void)message; + #if !defined(UFBX_NO_STDIO) && !defined(UFBX_EXTERNAL_STDIO) + fprintf(stderr, "ufbx panic: %s\n", message); + #endif + ufbx_assert(false && "ufbx panic: See stderr for more information"); + } + #define ufbx_panic_handler ufbxi_panic_handler #endif // -- Platform @@ -414,8 +564,16 @@ #if defined(UFBX_STATIC_ANALYSIS) bool ufbxi_analysis_opaque; #define ufbxi_maybe_null(ptr) (ufbxi_analysis_opaque ? (ptr) : NULL) + #define ufbxi_analysis_assert(cond) ufbx_assert(cond) #else #define ufbxi_maybe_null(ptr) (ptr) + #define ufbxi_analysis_assert(cond) (void)0 +#endif + +#if defined(UFBX_STATIC_ANALYSIS) || defined(UFBX_UBSAN) + #define ufbxi_maybe_uninit(cond, value, def) ((cond) ? (value) : (def)) +#else + #define ufbxi_maybe_uninit(cond, value, def) (value) #endif #if !defined(ufbxi_trace) @@ -442,7 +600,7 @@ #endif #endif -#if !defined(UFBX_STANDARD_C) && (defined(_MSC_VER) && defined(_M_X64)) || ((defined(__GNUC__) || defined(__clang__)) && defined(__x86_64__)) || defined(UFBX_USE_SSE) +#if !defined(UFBX_STANDARD_C) && !defined(UFBX_NO_SSE) && (defined(_MSC_VER) && defined(_M_X64) && !defined(_M_ARM64EC)) || ((defined(__GNUC__) || defined(__clang__)) && defined(__x86_64__)) || defined(UFBX_USE_SSE) #define UFBXI_HAS_SSE 1 #include #include @@ -450,6 +608,108 @@ #define UFBXI_HAS_SSE 0 #endif +// -- Atomic counter + +#define UFBXI_THREAD_SAFE 1 + +#if defined(__cplusplus) + #define ufbxi_extern_c extern "C" +#else + #define ufbxi_extern_c +#endif + +#if !defined(UFBX_STANDARD_C) && (defined(__GNUC__) || defined(__clang__) || defined(__INTEL_COMPILER)) + typedef size_t ufbxi_atomic_counter; + #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_inc(ptr) __sync_fetch_and_add((ptr), 1) + #define ufbxi_atomic_counter_dec(ptr) __sync_fetch_and_sub((ptr), 1) + #define ufbxi_atomic_counter_load(ptr) __sync_fetch_and_add((ptr), 0) // TODO: Proper atomic load +#elif !defined(UFBX_STANDARD_C) && defined(_MSC_VER) + #if defined(_M_X64) || defined(_M_ARM64) + ufbxi_extern_c __int64 _InterlockedIncrement64(__int64 volatile * lpAddend); + ufbxi_extern_c __int64 _InterlockedDecrement64(__int64 volatile * lpAddend); + ufbxi_extern_c __int64 _InterlockedExchangeAdd64(__int64 volatile * lpAddend, __int64 Value); + typedef volatile __int64 ufbxi_atomic_counter; + #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_inc(ptr) ((size_t)_InterlockedIncrement64(ptr) - 1) + #define ufbxi_atomic_counter_dec(ptr) ((size_t)_InterlockedDecrement64(ptr) + 1) + #define ufbxi_atomic_counter_load(ptr) ((size_t)_InterlockedExchangeAdd64((ptr), 0)) + #else + ufbxi_extern_c long __cdecl _InterlockedIncrement(long volatile * lpAddend); + ufbxi_extern_c long __cdecl _InterlockedDecrement(long volatile * lpAddend); + ufbxi_extern_c long __cdecl _InterlockedExchangeAdd(long volatile * lpAddend, long Value); + typedef volatile long ufbxi_atomic_counter; + #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_inc(ptr) ((size_t)_InterlockedIncrement(ptr) - 1) + #define ufbxi_atomic_counter_dec(ptr) ((size_t)_InterlockedDecrement(ptr) + 1) + #define ufbxi_atomic_counter_load(ptr) ((size_t)_InterlockedExchangeAdd((ptr), 0)) + #endif +#elif !defined(UFBX_STANDARD_C) && defined(__TINYC__) + #if defined(__x86_64__) || defined(_AMD64_) + static size_t ufbxi_tcc_atomic_add(volatile size_t *dst, size_t value) { + __asm__ __volatile__("lock; xaddq %0, %1;" : "+r" (value), "=m" (*dst) : "m" (dst)); + return value; + } + #elif defined(__i386__) || defined(_X86_) + static size_t ufbxi_tcc_atomic_add(volatile size_t *dst, size_t value) { + __asm__ __volatile__("lock; xaddl %0, %1;" : "+r" (value), "=m" (*dst) : "m" (dst)); + return value; + } + #else + #error Unexpected TCC architecture + #endif + typedef volatile size_t ufbxi_atomic_counter; + #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_inc(ptr) ufbxi_tcc_atomic_add((ptr), 1) + #define ufbxi_atomic_counter_dec(ptr) ufbxi_tcc_atomic_add((ptr), SIZE_MAX) + #define ufbxi_atomic_counter_load(ptr) ufbxi_tcc_atomic_add((ptr), 0) +#elif defined(__cplusplus) && (__cplusplus >= 201103L) + #include + #include + typedef struct { alignas(std::atomic_size_t) char data[sizeof(std::atomic_size_t)]; } ufbxi_atomic_counter; + #define ufbxi_atomic_counter_init(ptr) (new (&(ptr)->data) std::atomic_size_t(0)) + #define ufbxi_atomic_counter_free(ptr) (((std::atomic_size_t*)(ptr)->data)->~atomic()) + #define ufbxi_atomic_counter_inc(ptr) ((std::atomic_size_t*)(ptr)->data)->fetch_add(1) + #define ufbxi_atomic_counter_dec(ptr) ((std::atomic_size_t*)(ptr)->data)->fetch_sub(1) + #define ufbxi_atomic_counter_load(ptr) ((std::atomic_size_t*)(ptr)->data)->load(std::memory_order_acquire) +#elif defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L) && !defined(__STDC_NO_ATOMICS__) + #include + typedef volatile atomic_size_t ufbxi_atomic_counter; + #define ufbxi_atomic_counter_init(ptr) atomic_init(ptr, 0) + #define ufbxi_atomic_counter_free(ptr) (void)(ptr) + #define ufbxi_atomic_counter_inc(ptr) atomic_fetch_add((ptr), 1) + #define ufbxi_atomic_counter_dec(ptr) atomic_fetch_sub((ptr), 1) + #define ufbxi_atomic_counter_load(ptr) atomic_load_explicit((ptr), memory_order_acquire) +#else + typedef volatile size_t ufbxi_atomic_counter; + #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) + #define ufbxi_atomic_counter_inc(ptr) ((*(ptr))++) + #define ufbxi_atomic_counter_dec(ptr) ((*(ptr))--) + #define ufbxi_atomic_counter_load(ptr) (*(ptr)) + #undef UFBXI_THREAD_SAFE + #define UFBXI_THREAD_SAFE 0 +#endif + +// ^^ No references to before this point ^^ +// vv No more includes past this point vv + +#if defined(UFBX_STRING_PREFIX) + #define ufbxi_string_fn(name) ufbxi_pre_cat(UFBX_STRING_PREFIX, name) + #define strlen ufbxi_string_fn(strlen) + #define memcpy ufbxi_string_fn(memcpy) + #define memmove ufbxi_string_fn(memmove) + #define memset ufbxi_string_fn(memset) + #define memchr ufbxi_string_fn(memchr) + #define memcmp ufbxi_string_fn(memcmp) + #define strcmp ufbxi_string_fn(strcmp) + #define strncmp ufbxi_string_fn(strncmp) +#endif + #if !defined(UFBX_LITTLE_ENDIAN) #if !defined(UFBX_STANDARD_C) && (defined(_M_IX86) || defined(__i386__) || defined(_M_X64) || defined(__x86_64__) || defined(_M_ARM64) || defined(__aarch64__) || defined(__wasm__) || defined(__EMSCRIPTEN__)) #define UFBX_LITTLE_ENDIAN 1 @@ -570,7 +830,7 @@ ufbx_static_assert(sizeof_f64, sizeof(double) == 8); // -- Version -#define UFBX_SOURCE_VERSION ufbx_pack_version(0, 14, 3) +#define UFBX_SOURCE_VERSION ufbx_pack_version(0, 15, 0) ufbx_abi_data_def const uint32_t ufbx_source_version = UFBX_SOURCE_VERSION; ufbx_static_assert(source_header_version, UFBX_SOURCE_VERSION/1000u == UFBX_HEADER_VERSION/1000u); @@ -607,98 +867,16 @@ ufbx_static_assert(source_header_version, UFBX_SOURCE_VERSION/1000u == UFBX_HEAD #define ufbxi_wrap_shr64(a, b) ((a) >> ((b) & 63)) #endif -// -- Atomic counter - -#define UFBXI_THREAD_SAFE 1 - -#if defined(__cplusplus) - #define ufbxi_extern_c extern "C" -#else - #define ufbxi_extern_c -#endif - -#if !defined(UFBX_STANDARD_C) && (defined(__GNUC__) || defined(__clang__) || defined(__INTEL_COMPILER)) - typedef size_t ufbxi_atomic_counter; - #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_inc(ptr) __sync_fetch_and_add((ptr), 1) - #define ufbxi_atomic_counter_dec(ptr) __sync_fetch_and_sub((ptr), 1) - #define ufbxi_atomic_counter_load(ptr) __sync_fetch_and_add((ptr), 0) // TODO: Proper atomic load -#elif !defined(UFBX_STANDARD_C) && defined(_MSC_VER) - #if defined(_M_X64) || defined(_M_ARM64) - ufbxi_extern_c __int64 _InterlockedIncrement64(__int64 volatile * lpAddend); - ufbxi_extern_c __int64 _InterlockedDecrement64(__int64 volatile * lpAddend); - ufbxi_extern_c __int64 _InterlockedExchangeAdd64(__int64 volatile * lpAddend, __int64 Value); - typedef volatile __int64 ufbxi_atomic_counter; - #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_inc(ptr) ((size_t)_InterlockedIncrement64(ptr) - 1) - #define ufbxi_atomic_counter_dec(ptr) ((size_t)_InterlockedDecrement64(ptr) + 1) - #define ufbxi_atomic_counter_load(ptr) ((size_t)_InterlockedExchangeAdd64((ptr), 0)) - #else - ufbxi_extern_c long __cdecl _InterlockedIncrement(long volatile * lpAddend); - ufbxi_extern_c long __cdecl _InterlockedDecrement(long volatile * lpAddend); - ufbxi_extern_c long __cdecl _InterlockedExchangeAdd(long volatile * lpAddend, long Value); - typedef volatile long ufbxi_atomic_counter; - #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_inc(ptr) ((size_t)_InterlockedIncrement(ptr) - 1) - #define ufbxi_atomic_counter_dec(ptr) ((size_t)_InterlockedDecrement(ptr) + 1) - #define ufbxi_atomic_counter_load(ptr) ((size_t)_InterlockedExchangeAdd((ptr), 0)) - #endif -#elif !defined(UFBX_STANDARD_C) && defined(__TINYC__) - #if defined(__x86_64__) || defined(_AMD64_) - static size_t ufbxi_tcc_atomic_add(volatile size_t *dst, size_t value) { - __asm__ __volatile__("lock; xaddq %0, %1;" : "+r" (value), "=m" (*dst) : "m" (dst)); - return value; - } - #elif defined(__i386__) || defined(_X86_) - static size_t ufbxi_tcc_atomic_add(volatile size_t *dst, size_t value) { - __asm__ __volatile__("lock; xaddl %0, %1;" : "+r" (value), "=m" (*dst) : "m" (dst)); - return value; - } - #else - #error Unexpected TCC architecture - #endif - typedef volatile size_t ufbxi_atomic_counter; - #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_inc(ptr) ufbxi_tcc_atomic_add((ptr), 1) - #define ufbxi_atomic_counter_dec(ptr) ufbxi_tcc_atomic_add((ptr), SIZE_MAX) - #define ufbxi_atomic_counter_load(ptr) ufbxi_tcc_atomic_add((ptr), 0) -#elif defined(__cplusplus) && (__cplusplus >= 201103L) - #include - #include - typedef struct { alignas(std::atomic_size_t) char data[sizeof(std::atomic_size_t)]; } ufbxi_atomic_counter; - #define ufbxi_atomic_counter_init(ptr) (new (&(ptr)->data) std::atomic_size_t(0)) - #define ufbxi_atomic_counter_free(ptr) (((std::atomic_size_t*)(ptr)->data)->~atomic()) - #define ufbxi_atomic_counter_inc(ptr) ((std::atomic_size_t*)(ptr)->data)->fetch_add(1) - #define ufbxi_atomic_counter_dec(ptr) ((std::atomic_size_t*)(ptr)->data)->fetch_sub(1) - #define ufbxi_atomic_counter_load(ptr) ((std::atomic_size_t*)(ptr)->data)->load(std::memory_order_acquire) -#elif defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L) && !defined(__STDC_NO_ATOMICS__) - #include - typedef volatile atomic_size_t ufbxi_atomic_counter; - #define ufbxi_atomic_counter_init(ptr) atomic_init(ptr, 0) - #define ufbxi_atomic_counter_free(ptr) (void)(ptr) - #define ufbxi_atomic_counter_inc(ptr) atomic_fetch_add((ptr), 1) - #define ufbxi_atomic_counter_dec(ptr) atomic_fetch_sub((ptr), 1) - #define ufbxi_atomic_counter_load(ptr) atomic_load_explicit((ptr), memory_order_acquire) -#else - typedef volatile size_t ufbxi_atomic_counter; - #define ufbxi_atomic_counter_init(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_free(ptr) (*(ptr) = 0) - #define ufbxi_atomic_counter_inc(ptr) ((*(ptr))++) - #define ufbxi_atomic_counter_dec(ptr) ((*(ptr))--) - #define ufbxi_atomic_counter_load(ptr) (*(ptr)) - #undef UFBXI_THREAD_SAFE - #define UFBXI_THREAD_SAFE 0 -#endif - // -- Bit manipulation #if !defined(UFBX_STANDARD_C) && defined(_MSC_VER) && (defined(_M_X64) || defined(_M_IX86)) ufbxi_extern_c unsigned char _BitScanReverse(unsigned long * _Index, unsigned long _Mask); ufbxi_extern_c unsigned char _BitScanReverse64(unsigned long * _Index, unsigned __int64 _Mask); + static ufbxi_forceinline ufbxi_unused uint32_t ufbxi_lzcnt32(uint32_t v) { + unsigned long index; + _BitScanReverse(&index, (unsigned long)v); + return 31 - (uint32_t)index; + } static ufbxi_forceinline ufbxi_unused uint32_t ufbxi_lzcnt64(uint64_t v) { unsigned long index; #if defined(_M_X64) @@ -713,14 +891,26 @@ ufbx_static_assert(source_header_version, UFBX_SOURCE_VERSION/1000u == UFBX_HEAD return 63 - (uint32_t)index; } #elif !defined(UFBX_STANDARD_C) && (defined(__GNUC__) || defined(__clang__)) + #define ufbxi_lzcnt32(v) ((uint32_t)__builtin_clz((unsigned)(v))) #define ufbxi_lzcnt64(v) ((uint32_t)__builtin_clzll((unsigned long long)(v))) #else // DeBrujin table lookup - static const uint8_t ufbxi_lzcnt_table[] = { + static const uint8_t ufbxi_lzcnt32_table[] = { + 31, 22, 30, 21, 18, 10, 29, 2, 20, 17, 15, 13, 9, 6, 28, 1, 23, 19, 11, 3, 16, 14, 7, 24, 12, 4, 8, 25, 5, 26, 27, 0, + }; + static const uint8_t ufbxi_lzcnt64_table[] = { 63, 16, 62, 7, 15, 36, 61, 3, 6, 14, 22, 26, 35, 47, 60, 2, 9, 5, 28, 11, 13, 21, 42, 19, 25, 31, 34, 40, 46, 52, 59, 1, 17, 8, 37, 4, 23, 27, 48, 10, 29, 12, 43, 20, 32, 41, 53, 18, 38, 24, 49, 30, 44, 33, 54, 39, 50, 45, 55, 51, 56, 57, 58, 0, }; + static ufbxi_noinline ufbxi_unused uint32_t ufbxi_lzcnt32(uint32_t v) { + v |= v >> 1; + v |= v >> 2; + v |= v >> 4; + v |= v >> 8; + v |= v >> 16; + return ufbxi_lzcnt32_table[(v * 0x07c4acddu) >> 27]; + } static ufbxi_noinline ufbxi_unused uint32_t ufbxi_lzcnt64(uint64_t v) { v |= v >> 1; v |= v >> 2; @@ -728,10 +918,20 @@ ufbx_static_assert(source_header_version, UFBX_SOURCE_VERSION/1000u == UFBX_HEAD v |= v >> 8; v |= v >> 16; v |= v >> 32; - return ufbxi_lzcnt_table[(v * UINT64_C(0x03f79d71b4cb0a89)) >> 58]; + return ufbxi_lzcnt64_table[(v * UINT64_C(0x03f79d71b4cb0a89)) >> 58]; } #endif +// -- Bit conversion + +#if defined(__cplusplus) + #define ufbxi_bit_cast(m_dst_type, m_dst, m_src_type, m_src) memcpy(&(m_dst), &(m_src), sizeof(m_dst_type)) +#else + #define ufbxi_bit_cast(m_dst_type, m_dst, m_src_type, m_src) do { \ + union { m_dst_type mi_dst; m_src_type mi_src; } mi_union; \ + mi_union.mi_src = (m_src); (m_dst) = mi_union.mi_dst; } while (0) +#endif + // -- Debug #if defined(UFBX_DEBUG_BINARY_SEARCH) || defined(UFBX_REGRESSION) @@ -1081,321 +1281,389 @@ static ufbxi_noinline void ufbxi_unstable_sort(void *in_data, size_t size, size_ } // -- Float parsing -// -// Custom float parsing that handles floats up to (-)ddddddddddddddddddd.ddddddddddddddddddd -// If larger or scientific notation is used then it defers to `strtod()`. -// For the algorithm we need 128-bit division that is either provided by hardware on x64 or -// a custom implementation below. - -#if !defined(UFBX_STANDARD_C) && UFBXI_MSC_VER >= 1920 && defined(_M_X64) && !defined(__clang__) - ufbxi_extern_c extern unsigned __int64 __cdecl _udiv128(unsigned __int64 highdividend, - unsigned __int64 lowdividend, unsigned __int64 divisor, unsigned __int64 *remainder); - #define ufbxi_div128(a_hi, a_lo, b, p_rem) (_udiv128((a_hi), (a_lo), (b), (p_rem))) -#elif !defined(UFBX_STANDARD_C) && (defined(__GNUC__) || defined(__clang__)) && (defined(__x86_64__) || defined(_M_X64)) - static ufbxi_forceinline uint64_t ufbxi_div128(uint64_t a_hi, uint64_t a_lo, uint64_t b, uint64_t *p_rem) { - uint64_t quot, rem; - __asm__("divq %[v]" : "=a"(quot), "=d"(rem) : [v] "r"(b), "a"(a_lo), "d"(a_hi)); - *p_rem = rem; - return quot; - } -#else - static ufbxi_forceinline uint64_t ufbxi_div128(uint64_t a_hi, uint64_t a_lo, uint64_t b, uint64_t *p_rem) { - // Divide `(a_hi << 64 | a_lo)` by `b`, returns quotinent and stores reminder in `p_rem`. - // Based on TAOCP 2.4 multi-word division single algorithm digit step. - // - // Notation: - // b is the base (2^32) in this case - // aN is the Nth digit (base b) of a from the least significant digit - // { x y z } is a multi-digit number b^2*x + b*y + z - // ie. for a 64-bit number a = { a1 a0 } = b*a1 + a0 - // - // We do the division in two steps by dividing three digits in each iteration: - // - // q1, r = { a3 a2 a1 } / { b1 b0 } - // q0, r = { r1 r0 a0 } / { b1 b0 } - // - // In each step we want to compute the expression: - // - // q, r = { u2 u1 u0 } / { v1 v0 } - // - // However we cannot rely on being able to do `u96 / u64` division we estimate - // the result by considering only the leading digits: - // - // q^ = { u2 u1 } / v1 [A] - // r^ = { u2 u1 } % v1 = { u2 u1 } - v1 * q^ [B] - // - // As long as `v1 >= b/2` the estimate `q^` is at most two larger than the actual `q` - // (proof in TAOCP 2.4) so we can compute the correction amount `c`: - // - // q <= q^ <= q + 2 - // q = q^ - c [C] - // - // We can compute the final remainder (that must be non-negative) as follows: - // - // r = { u2 u1 u0 } - v*q - // r = { u2 u1 u0 } - v*(q^ - c) - // r = { u2 u1 u0 } - v*q^ + v*c - // r = { u2 u1 u0 } - { v1 v0 } * q^ + v*c - // r = b^2*u2 + b*u1 + u0 - b*v1*q^ - v0*q^ + v*c - // r = b*(b*u2 + u1 - v1*q^) + u0 - v0*q^ + v*c - // r = b*({ u2 u1 } - v1*q^) + u0 - v0*q^ + v*c - // r = b*r^ + u0 - v0*q^ + v*c - // r = { r^ u0 } - v0*q^ + v*c [D] - // - // As we know `0 <= c <= 2` we can first check if `r < 0` requiring `c >= 1`: - // - // { r^ u0 } - v0*q^ < 0 - // { r^ u0 } < v0*q^ [E] - // - // If we know that `r < 0` we can check if `r < -v` requiring `c = 2`: - // - // { r^ u0 } - v0*q^ < -v - // v0*q^ - { r^ u0 } > v [F] - // - - // First we need to make sure `v1 >= b/2`, we can do this by multiplying the whole - // expression by `2^shift` so that the high bit of `v` is set. - uint32_t shift = ufbxi_lzcnt64(b); - a_hi = (a_hi << shift) | (shift ? a_lo >> (64 - shift) : 0); - a_lo <<= shift; - b <<= shift; - - uint64_t v = b; - uint32_t v1 = (uint32_t)(v >> 32); - uint32_t v0 = (uint32_t)(v); - uint64_t q1, q0, r; - - // q1, r = { a3 a2 a1 } / { b1 b0 } - { - uint64_t u2_u1 = a_hi; - uint32_t u0 = (uint32_t)(a_lo >> 32u); - uint64_t qh = u2_u1 / v1; // q^ = { u2 u1 } / v1 [A] - uint64_t rh = u2_u1 % v1; // r^ = { u2 u1 } % v1 [B] - uint64_t rh_u0 = rh << 32u | u0; // { r^ u0 } - uint64_t v0qh = v0 * qh; // v0*q^ - uint32_t c = rh_u0 < v0qh ? 1 : 0; // { r^ u0 } < v0*q^ [E] - c += c & (v0qh - rh_u0 > v ? 1 : 0); // v0*q^ - { r^ u0 } > v [F] - q1 = qh - c; // q1 = q^ - c [C] - r = rh_u0 - v0qh + v*c; // r = { r^ u0 } - v0*q^ + v*c [D] - } +#define UFBXI_BIGINT_LIMB_BITS 32 +#define UFBXI_BIGINT_ACCUM_BITS (UFBXI_BIGINT_LIMB_BITS * 2) +#define UFBXI_BIGINT_LIMB_MAX (ufbxi_bigint_limb)(((ufbxi_bigint_accum)1 << UFBXI_BIGINT_LIMB_BITS) - 1) +typedef uint32_t ufbxi_bigint_limb; +typedef uint64_t ufbxi_bigint_accum; - // q0, r = { r1 r0 a0 } / { b1 b0 } - { - uint64_t u2_u1 = r; - uint32_t u0 = (uint32_t)a_lo; - - uint64_t qh = u2_u1 / v1; // q^ = { u2 u1 } / v1 [A] - uint64_t rh = u2_u1 % v1; // r^ = { u2 u1 } % v1 [B] - uint64_t rh_u0 = rh << 32u | u0; // { r^ u0 } - uint64_t v0qh = v0 * qh; // v0*q^ - uint32_t c = rh_u0 < v0qh ? 1 : 0; // { r^ u0 } < v0*q^ [E] - c += c & (v0qh - rh_u0 > v ? 1 : 0); // v0*q^ - { r^ u0 } > v [F] - q0 = qh - c; // q0 = q^ - c [C] - r = rh_u0 - v0qh + v*c; // r = { r^ u0 } - v0*q^ + v*c [D] - } +typedef struct { + ufbxi_bigint_limb *limbs; + uint32_t capacity; + uint32_t length; +} ufbxi_bigint; - // Un-normalize the remainder and return the quotinent - *p_rem = r >> shift; - return q1 << 32u | q0; - } -#endif +static ufbxi_bigint ufbxi_bigint_make(ufbxi_bigint_limb *limbs, size_t capacity) +{ + ufbxi_bigint bi = { limbs, (uint32_t)capacity }; + return bi; +} -typedef enum { - UFBXI_PARSE_DOUBLE_ALLOW_FAST_PATH = 0x1, - UFBXI_PARSE_DOUBLE_VERIFY_LENGTH = 0x2, -} ufbxi_parse_double_flag; +#define ufbxi_bigint_array(arr) ufbxi_bigint_make((arr), sizeof(arr) / sizeof(*(arr))) static const uint64_t ufbxi_pow5_tab[] = { - UINT64_C(0x8000000000000000), // 5^0 * 2^63 - UINT64_C(0xa000000000000000), // 5^1 * 2^61 - UINT64_C(0xc800000000000000), // 5^2 * 2^59 - UINT64_C(0xfa00000000000000), // 5^3 * 2^57 - UINT64_C(0x9c40000000000000), // 5^4 * 2^54 - UINT64_C(0xc350000000000000), // 5^5 * 2^52 - UINT64_C(0xf424000000000000), // 5^6 * 2^50 - UINT64_C(0x9896800000000000), // 5^7 * 2^47 - UINT64_C(0xbebc200000000000), // 5^8 * 2^45 - UINT64_C(0xee6b280000000000), // 5^9 * 2^43 - UINT64_C(0x9502f90000000000), // 5^10 * 2^40 - UINT64_C(0xba43b74000000000), // 5^11 * 2^38 - UINT64_C(0xe8d4a51000000000), // 5^12 * 2^36 - UINT64_C(0x9184e72a00000000), // 5^13 * 2^33 - UINT64_C(0xb5e620f480000000), // 5^14 * 2^31 - UINT64_C(0xe35fa931a0000000), // 5^15 * 2^29 - UINT64_C(0x8e1bc9bf04000000), // 5^16 * 2^26 - UINT64_C(0xb1a2bc2ec5000000), // 5^17 * 2^24 - UINT64_C(0xde0b6b3a76400000), // 5^18 * 2^22 - UINT64_C(0x8ac7230489e80000), // 5^19 * 2^19 - UINT64_C(0xad78ebc5ac620000), // 5^20 * 2^17 - UINT64_C(0xd8d726b7177a8000), // 5^21 * 2^15 - UINT64_C(0x878678326eac9000), // 5^22 * 2^12 - UINT64_C(0xa968163f0a57b400), // 5^23 * 2^10 - UINT64_C(0xd3c21bcecceda100), // 5^24 * 2^8 - UINT64_C(0x84595161401484a0), // 5^25 * 2^5 - UINT64_C(0xa56fa5b99019a5c8), // 5^26 * 2^3 - UINT64_C(0xcecb8f27f4200f3a), // 5^27 * 2^1 -}; -static const int8_t ufbxi_pow2_tab[] = { - 62, 59, 56, 53, 49, 46, 43, 39, 36, 33, 29, 26, 23, 19, 16, 13, 9, 6, 3, -1, -4, -7, -11, -14, -17, -21, -24, -27, + UINT64_C(0x1), UINT64_C(0x5), UINT64_C(0x19), UINT64_C(0x7d), UINT64_C(0x271), UINT64_C(0xc35), UINT64_C(0x3d09), UINT64_C(0x1312d), UINT64_C(0x5f5e1), + UINT64_C(0x1dcd65), UINT64_C(0x9502f9), UINT64_C(0x2e90edd), UINT64_C(0xe8d4a51), UINT64_C(0x48c27395), UINT64_C(0x16bcc41e9), UINT64_C(0x71afd498d), + UINT64_C(0x2386f26fc1), UINT64_C(0xb1a2bc2ec5), UINT64_C(0x3782dace9d9), UINT64_C(0x1158e460913d), UINT64_C(0x56bc75e2d631), UINT64_C(0x1b1ae4d6e2ef5), + UINT64_C(0x878678326eac9), UINT64_C(0x2a5a058fc295ed), UINT64_C(0xd3c21bcecceda1), UINT64_C(0x422ca8b0a00a425), UINT64_C(0x14adf4b7320334b9), UINT64_C(0x6765c793fa10079d), }; + static const double ufbxi_pow10_tab_f64[] = { 1e0, 1e1, 1e2, 1e3, 1e4, 1e5, 1e6, 1e7, 1e8, 1e9, 1e10, 1e11, 1e12, 1e13, 1e14, 1e15, 1e16, 1e17, 1e18, 1e19, 1e20, 1e21, 1e22, }; -static ufbxi_noinline uint32_t ufbxi_parse_double_init_flags() +static ufbxi_noinline void ufbxi_bigint_mad(ufbxi_bigint *bigint, ufbxi_bigint_accum multiplicand, ufbxi_bigint_accum addend) +{ + ufbxi_dev_assert((multiplicand | addend) >> (UFBXI_BIGINT_ACCUM_BITS - 1) == 0); + ufbxi_bigint b = *bigint; + ufbxi_bigint_limb m_lo = (ufbxi_bigint_limb)multiplicand; + ufbxi_bigint_limb m_hi = (ufbxi_bigint_limb)(multiplicand >> UFBXI_BIGINT_LIMB_BITS); + ufbxi_bigint_accum carry = addend; + for (uint32_t i = 0; i < b.length; i++) { + ufbxi_bigint_accum limb = (ufbxi_bigint_accum)b.limbs[i]; + ufbxi_bigint_accum lo = limb * m_lo + (carry & UFBXI_BIGINT_LIMB_MAX); + ufbxi_bigint_accum hi = limb * m_hi; + b.limbs[i] = (ufbxi_bigint_limb)lo; + carry = (carry >> 32u) + (lo >> 32u) + hi; + } + while (carry) { + b.limbs[b.length++] = (ufbxi_bigint_limb)carry; + ufbxi_dev_assert(b.length < b.capacity); + carry >>= 32u; + } + bigint->length = b.length; +} + +static ufbxi_noinline bool ufbxi_bigint_div(ufbxi_bigint *q, ufbxi_bigint *u, ufbxi_bigint *v) +{ + int32_t n = (int32_t)v->length; + int32_t m = (int32_t)u->length - n; + ufbxi_bigint_limb v_hi = v->limbs[v->length - 1]; + ufbxi_bigint_limb *un = u->limbs, *vn = v->limbs; + ufbxi_dev_assert(n >= 2 && m >= 1 && v_hi >> (UFBXI_BIGINT_LIMB_BITS - 1) != 0 && un[n+m - 1] >> (UFBXI_BIGINT_LIMB_BITS - 1) == 0); + un[n + m] = 0; + q->length = 0; + for (int32_t j = m - 1; j >= 0; j--) { + ufbxi_bigint_accum u_hi = ((ufbxi_bigint_accum)un[n+j] << UFBXI_BIGINT_LIMB_BITS) | un[n+j-1]; + ufbxi_bigint_accum t, qhat = u_hi / v_hi, rhat = u_hi % v_hi; + while (qhat >> UFBXI_BIGINT_LIMB_BITS != 0 || qhat*vn[n-2] > ((rhat<> UFBXI_BIGINT_LIMB_BITS != 0) break; + } + ufbxi_bigint_limb carry = 0; + for (int32_t i = 0; i < n; i++) { + ufbxi_bigint_accum p = qhat * vn[i]; + t = (ufbxi_bigint_accum)un[i+j] - carry - (ufbxi_bigint_limb)p; + un[i+j] = (ufbxi_bigint_limb)t; + carry = (ufbxi_bigint_limb)((p >> UFBXI_BIGINT_LIMB_BITS) - (t >> UFBXI_BIGINT_LIMB_BITS)); + } + t = (ufbxi_bigint_accum)un[j+n] - carry; + un[j+n] = (ufbxi_bigint_limb)t; + if (t >> UFBXI_BIGINT_LIMB_BITS != 0) { + qhat -= 1; + carry = 0; + for (int32_t i = 0; i < n; i++) { + t = (ufbxi_bigint_accum)un[i+j] + vn[i] + carry; + un[i+j] = (ufbxi_bigint_limb)t; + carry = (ufbxi_bigint_limb)(t >> UFBXI_BIGINT_LIMB_BITS); + } + un[j+n] += carry; + } + q->limbs[j] = (ufbxi_bigint_limb)qhat; + if (qhat && !q->length) { + ufbxi_dev_assert(j + 1 < (int32_t)q->capacity); + q->length = (uint32_t)(j + 1); + } + } + for (int32_t i = 0; i < n; i++) { + if (un[i]) return true; + } + return false; +} + +static void ufbxi_bigint_mul_pow5(ufbxi_bigint *b, uint32_t power) { - // We require evaluation in double precision, either for doubles (0) or always (1) - // and rounding to nearest, which we can check for with `1 + eps == 1 - eps`. - #if defined(FLT_EVAL_METHOD) - #if FLT_EVAL_METHOD == 0 || FLT_EVAL_METHOD == 1 - static volatile double ufbxi_volatile_eps = 2.2250738585072014e-308; - if (1.0 + ufbxi_volatile_eps == 1.0 - ufbxi_volatile_eps) return UFBXI_PARSE_DOUBLE_ALLOW_FAST_PATH; - #endif - #endif + for (; power > 27; power -= 27) { + ufbxi_bigint_mad(b, ufbxi_pow5_tab[27], 0); + } + ufbxi_bigint_mad(b, ufbxi_pow5_tab[power], 0); +} - return 0; +static ufbxi_noinline void ufbxi_bigint_shift_left(ufbxi_bigint *bigint, uint32_t amount) +{ + uint32_t words = amount / UFBXI_BIGINT_LIMB_BITS, bits = amount % UFBXI_BIGINT_LIMB_BITS; + ufbxi_bigint b = *bigint; + ufbxi_dev_assert(b.length + words + 1 < b.capacity && b.capacity >= 4); + uint32_t bits_down = UFBXI_BIGINT_LIMB_BITS - bits - 1; + bigint->length += words + (b.limbs[b.length - 1] >> 1 >> bits_down != 0 ? 1 : 0); + b.limbs[b.length] = 0; + if (b.length <= 3 && words <= 3) { + ufbxi_bigint_limb l0 = ufbxi_maybe_uninit(b.length >= 0, b.limbs[0], ~0u); + ufbxi_bigint_limb l1 = ufbxi_maybe_uninit(b.length >= 1, b.limbs[1], ~0u); + ufbxi_bigint_limb l2 = ufbxi_maybe_uninit(b.length >= 2, b.limbs[2], ~0u); + b.limbs[0] = 0; + b.limbs[1] = 0; + b.limbs[2] = 0; + b.limbs[words + 0] = l0 << bits; + b.limbs[words + 1] = (l1 << bits) | (l0 >> 1 >> bits_down); + b.limbs[words + 2] = (l2 << bits) | (l1 >> 1 >> bits_down); + b.limbs[words + 3] = (l2 >> 1 >> bits_down); + } else { + for (uint32_t i = b.length + 1; i-- > 1; ) { + b.limbs[i + words] = (b.limbs[i] << bits) | (b.limbs[i - 1] >> 1 >> bits_down); + } + b.limbs[words] = b.limbs[0] << bits; + for (uint32_t i = 0; i < words; i++) { + b.limbs[i] = 0; + } + } +} + +static ufbxi_bigint_limb ufbxi_bigint_top_limb(const ufbxi_bigint b, uint32_t index) { + return index < b.length ? b.limbs[b.length - 1 - index] : 0; +} + +static ufbxi_noinline uint64_t ufbxi_bigint_extract_high(const ufbxi_bigint b, int32_t *p_exponent, bool *p_tail) +{ + ufbxi_dev_assert(b.length != 0); + uint64_t result = 0; + const uint32_t limb_count = 64 / UFBXI_BIGINT_LIMB_BITS; + for (uint32_t i = 0; i < limb_count; i++) { + result = (result << UFBXI_BIGINT_LIMB_BITS) | ufbxi_bigint_top_limb(b, i); + } + uint32_t shift = ufbxi_lzcnt64(result); + result <<= shift; + ufbxi_bigint_limb lo = ufbxi_bigint_top_limb(b, limb_count); + if (shift > 0) { + result |= lo >> (UFBXI_BIGINT_LIMB_BITS - shift); + } + *p_tail |= (ufbxi_bigint_limb)(lo << shift) != 0; + for (uint32_t i = limb_count + 1; i < b.length; i++) { + *p_tail |= ufbxi_bigint_top_limb(b, i) != 0; + } + *p_exponent += (int32_t)(b.length * UFBXI_BIGINT_LIMB_BITS - shift - 1); + return result; } -static ufbxi_noinline double ufbxi_parse_double_slow(const char *str, char **end) +static uint64_t ufbxi_shift_right_round(uint64_t value, uint32_t shift, bool tail) { - // TODO: Locales - return strtod(str, end); + if (shift == 0) return value; + if (shift > 64) return 0; + uint64_t result = value >> (shift - 1); + uint64_t tail_mask = (UINT64_C(1) << (shift - 1)) - 1; + + bool r_odd = (result & 0x2) != 0; + bool r_round = (result & 0x1) != 0; + bool r_tail = tail || (value & tail_mask) != 0; + uint64_t round_bit = (r_round && (r_odd || r_tail)) ? 1u : 0u; + + return (result >> 1u) + round_bit; } +typedef enum { + UFBXI_PARSE_DOUBLE_ALLOW_FAST_PATH = 0x1, + UFBXI_PARSE_DOUBLE_VERIFY_LENGTH = 0x2, + UFBXI_PARSE_DOUBLE_AS_BINARY32 = 0x4, +} ufbxi_parse_double_flag; + static ufbxi_noinline double ufbxi_parse_double(const char *str, size_t max_length, char **end, uint32_t flags) { - // TODO: Use this for optimizing digit parsing - (void)max_length; + const uint32_t max_limbs = 14; - uint64_t integer = 0; - uint32_t n_integer = 0; - int32_t n_decimals = 0; - uint32_t n_exp = 0; - bool negative = false; + ufbxi_bigint_limb mantissa_limbs[42], divisor_limbs[42], quotient_limbs[42]; + ufbxi_bigint big_mantissa = ufbxi_bigint_array(mantissa_limbs); + ufbxi_bigint big_quotient = ufbxi_bigint_array(quotient_limbs); + int32_t dec_exponent = 0, has_dot = 0; + bool negative = false, tail = false, digits_valid = true; + uint64_t digits = 0; + uint32_t num_digits = 0; - // Parse /[+-]?[0-9]*(\.[0-9]*)([eE][+-]?[0-9]*)?/ retaining all digits - // in `integer` and number of decimals in `n_decimals`, exponent simply - // modifies `n_decimals` accordingly. const char *p = str; - if (*p == '-') { - negative = true; - p++; - } else if (*p == '+') { - p++; - } - while (((uint32_t)*p - '0') < 10) { - integer = integer * 10 + (uint64_t)(*p++ - '0'); - n_integer++; + if (*p == '+' || *p == '-') { + negative = *p++ == '-'; } - if (*p == '.') { - p++; - while (((uint32_t)*p - '0') < 10) { - integer = integer * 10 + (uint64_t)(*p++ - '0'); - n_integer++; - n_decimals++; + for (;;) { + char c = *p++; + if (c >= '0' && c <= '9') { + if (big_mantissa.length < max_limbs) { + digits = digits * 10 + (uint64_t)(c - '0'); + num_digits++; + if (num_digits >= 18) { + ufbxi_bigint_mad(&big_mantissa, ufbxi_pow5_tab[num_digits] << num_digits, digits); + digits = 0; + num_digits = 0; + digits_valid = false; + } + dec_exponent -= has_dot; + } else { + dec_exponent += 1 - has_dot; + } + } else if (c == '.' && !has_dot) { + has_dot = true; + } else { + break; } } - if ((*p | 0x20) == 'e') { + p--; + if (*p == 'e' || *p == 'E') { p++; - int32_t exp = 0; - int32_t exp_sign = -1; - if (*p == '-') { - p++; - exp_sign = 1; - } else if (*p == '+') { + bool exp_negative = false; + if (*p == '+' || *p == '-') { + exp_negative = *p == '-'; p++; } - while (((uint32_t)*p - '0') < 10) { - exp = exp * 10 + (int32_t)(*p++ - '0'); - n_exp++; + int32_t exp = 0; + for (;;) { + char c = *p; + if (c >= '0' && c <= '9') { + p++; + exp = exp * 10 + (c - '0'); + if (exp >= 10000) break; + } else { + break; + } } - n_decimals += exp * exp_sign; + dec_exponent += exp_negative ? -exp : exp; } - *end = (char*)p; + *end = (char*)p; // Check that the number is not potentially truncated. if (ufbxi_to_size(p - str) >= max_length && (flags & UFBXI_PARSE_DOUBLE_VERIFY_LENGTH) != 0) { *end = NULL; return 0.0; } - // Overflowed either 64-bit `integer` or 31-bit `exp`. - if (n_integer > 19 || n_exp > 9 || (integer >> 63) != 0) { - return ufbxi_parse_double_slow(str, end); - } - // Both power of 10 and integer are exactly representable as doubles // Powers of 10 are factored as 2*5, and 2^N can be always exactly represented. - if ((flags & UFBXI_PARSE_DOUBLE_ALLOW_FAST_PATH) != 0 && n_decimals >= -22 && n_decimals <= 22 && (integer >> 53) == 0) { + if ((flags & UFBXI_PARSE_DOUBLE_ALLOW_FAST_PATH) != 0 && big_mantissa.length == 0 && dec_exponent >= -22 && dec_exponent <= 22 && (digits >> 53) == 0) { double value; - if (n_decimals > 0) { - value = (double)integer / ufbxi_pow10_tab_f64[n_decimals]; + if (dec_exponent < 0) { + value = (double)digits / ufbxi_pow10_tab_f64[-dec_exponent]; } else { - value = (double)integer * ufbxi_pow10_tab_f64[-n_decimals]; + value = (double)digits * ufbxi_pow10_tab_f64[dec_exponent]; } return negative ? -value : value; } - // Cannot handle positive exponents here, fortunately the fast case should - // take care of most of them, for negative exponents we can only handle - // up to e-27 as `5^28 > 2^64` and cannot be used as a divisor below. - if (n_decimals < 0) { - return ufbxi_parse_double_slow(str, end); - } else if (!n_decimals || !integer) { - double value = (double)integer; - return negative ? -value : value; - } else if (n_decimals > 27) { - return ufbxi_parse_double_slow(str, end); - } - - // We want to compute `integer / 10^N` precisely, we can do this - // using 128-bit division `2^64 * dividend / divisor`: - // dividend = integer * 2^S (S set such that highest bit is 62) - // divisor = 10^N * 2^T (T set such that highest bit is 63) - // We have to compensate for the shifts in the exponent: - // (2^64 * integer * 2^S) / (10^N * 2^T) * 2^(-1 - S + T) - // To get larger exponent range split 10^N to 5^N * 2^N and move 2^N to the exponent - // (2^64 * integer * 2^S) / (5^N * 2^T) * 2^(-1 - S + T - N) - uint32_t shift = ufbxi_lzcnt64(integer) - 1; - uint64_t dividend = integer << shift; - uint64_t divisor = ufbxi_pow5_tab[n_decimals]; - int32_t exponent = (int32_t)ufbxi_pow2_tab[n_decimals] - (int32_t)shift; // (-1 + T - N) - S - uint64_t rem_hi; - uint64_t b_hi = ufbxi_div128(dividend, 0, divisor, &rem_hi); - - // Align the mantissa so that high bit is set, due to the shifting of the - // divisor and dividend the smallest result is `2^62 + N`, so we need to - // shift at most by one bit. - uint64_t b_bit = 1 - (b_hi >> 63u); - uint64_t mantissa = b_hi << b_bit; - exponent -= (int32_t)b_bit; - - // Round to 53 bits, accounting for potential remainder. - bool nonzero_tail = rem_hi != 0; - bool r_odd = mantissa & (1 << 11u); - bool r_round = mantissa & (1 << 10u); - bool r_tail = (mantissa & ((1 << 10u) - 1)) != 0 || nonzero_tail; - uint64_t round = (r_round && (r_odd || r_tail)) ? 1u : 0u; - - // Assemble the IEEE 754 binary64 number. - uint64_t bits - = (uint64_t)negative << 63u - | (uint64_t)(exponent + 1023) << 52u - | ((mantissa >> 11u) & ~(UINT64_C(1) << 52u)); - bits += round; - - // Type punning via unions is safe in C but in C++ the only safe way - // (pre std::bit_cast) is to use `memcpy()` and hope it gets optimized out. -#if defined(__cplusplus) - double result; - memcpy(&result, &bits, 8); - return result; -#else - union { uint64_t u; double d; } u_to_d; - u_to_d.u = bits; - return u_to_d.d; -#endif + if (big_mantissa.length == 0) { + big_mantissa.limbs[0] = (ufbxi_bigint_limb)digits; + big_mantissa.limbs[1] = (ufbxi_bigint_limb)(digits >> 32u); + big_mantissa.length = (digits >> 32u) ? 2 : digits ? 1 : 0; + if (big_mantissa.length == 0) return negative ? -0.0 : 0.0; + } else { + ufbxi_bigint_mad(&big_mantissa, ufbxi_pow5_tab[num_digits] << num_digits, digits); + } + + uint32_t enc_sign_shift = 63; + uint32_t enc_mantissa_bits = 53; + int32_t enc_max_exponent = 1023; + if (flags & UFBXI_PARSE_DOUBLE_AS_BINARY32) { + enc_sign_shift = 31; + enc_mantissa_bits = 24; + enc_max_exponent = 127; + } + + int32_t exponent = 0; + if (dec_exponent < 0) { + if (dec_exponent + (int32_t)big_mantissa.length * 10 <= -325) return negative ? -0.0 : 0.0; + + ufbxi_bigint big_divisor = ufbxi_bigint_array(divisor_limbs); + uint32_t pow5 = (uint32_t)-dec_exponent; + uint32_t initial_pow5 = pow5 <= 27 ? pow5 : 27; + uint64_t pow5_value = ufbxi_pow5_tab[initial_pow5]; + pow5 -= initial_pow5; + exponent += dec_exponent; + + if (pow5 == 0 && digits_valid && digits >> 63 == 0) { + uint32_t divisor_zeros = ufbxi_lzcnt64(pow5_value); + uint64_t mantissa_zeros = ufbxi_lzcnt64(digits) - 1; + uint64_t divisor_bits = pow5_value << divisor_zeros; + uint64_t mantissa_bits = digits << mantissa_zeros; + big_divisor.limbs[0] = (ufbxi_bigint_limb)divisor_bits; + big_divisor.limbs[1] = (ufbxi_bigint_limb)(divisor_bits >> 32u); + big_divisor.length = 2; + big_mantissa.limbs[0] = 0; + big_mantissa.limbs[1] = 0; + big_mantissa.limbs[2] = (ufbxi_bigint_limb)mantissa_bits; + big_mantissa.limbs[3] = (ufbxi_bigint_limb)(mantissa_bits >> 32u); + big_mantissa.length = 4; + exponent += (int32_t)divisor_zeros - (int32_t)mantissa_zeros - 64; + } else { + big_divisor.limbs[0] = (ufbxi_bigint_limb)pow5_value; + big_divisor.limbs[1] = (ufbxi_bigint_limb)(pow5_value >> 32u); + big_divisor.length = (pow5_value >> 32u) != 0 ? 2 : 1; + if (pow5 > 0) { + ufbxi_bigint_mul_pow5(&big_divisor, pow5); + } + + uint32_t divisor_zeros = ufbxi_lzcnt32(big_divisor.limbs[big_divisor.length - 1]); + if (big_divisor.length == 1) divisor_zeros += UFBXI_BIGINT_LIMB_BITS; + ufbxi_bigint_shift_left(&big_divisor, divisor_zeros); + uint32_t divisor_bits = big_divisor.length * UFBXI_BIGINT_LIMB_BITS; + + uint32_t mantissa_zeros = ufbxi_lzcnt32(big_mantissa.limbs[big_mantissa.length - 1]); + uint32_t mantissa_bits = big_mantissa.length * UFBXI_BIGINT_LIMB_BITS - mantissa_zeros; + uint32_t mantissa_min_bits = divisor_bits + enc_mantissa_bits + 2; + uint32_t mantissa_shift = mantissa_bits < mantissa_min_bits ? mantissa_min_bits - mantissa_bits : 0; + // Align mantissa to never have a high bit, this means we can skip the first digit during division. + mantissa_shift += ((mantissa_shift - mantissa_zeros) & (UFBXI_BIGINT_LIMB_BITS - 1)) == 0 ? 1 : 0; + if (mantissa_shift > 0) { + ufbxi_bigint_shift_left(&big_mantissa, mantissa_shift); + } + exponent += (int32_t)divisor_zeros - (int32_t)mantissa_shift; + } + + tail = ufbxi_bigint_div(&big_quotient, &big_mantissa, &big_divisor); + big_mantissa = big_quotient; + } else if (dec_exponent > 0) { + if (dec_exponent + (int32_t)(big_mantissa.length - 1) * 9 >= 310) return negative ? -UFBX_INFINITY : UFBX_INFINITY; + + exponent += dec_exponent; + ufbxi_bigint_mul_pow5(&big_mantissa, (uint32_t)dec_exponent); + } + + uint64_t mantissa = ufbxi_bigint_extract_high(big_mantissa, &exponent, &tail); + uint64_t sign_bit = (uint64_t)(negative ? 1u : 0u) << enc_sign_shift; + + uint32_t mantissa_shift = 64 - enc_mantissa_bits; + if (exponent > enc_max_exponent) { + return negative ? -UFBX_INFINITY : UFBX_INFINITY; + } else if (exponent <= -enc_max_exponent) { + mantissa_shift += (uint32_t)(-enc_max_exponent + 1 - exponent); + exponent = -enc_max_exponent + 1; + } + + mantissa = ufbxi_shift_right_round(mantissa, mantissa_shift, tail); + if (mantissa == 0) return negative ? -0.0 : 0.0; + + uint64_t bits = mantissa; + bits += (uint64_t)(exponent + enc_max_exponent - 1) << (enc_mantissa_bits - 1); + bits |= sign_bit; + + if (flags & UFBXI_PARSE_DOUBLE_AS_BINARY32) { + uint32_t bits_lo = (uint32_t)bits; + float result; + ufbxi_bit_cast(float, result, uint32_t, bits_lo); + return result; + } else { + double result; + ufbxi_bit_cast(double, result, uint64_t, bits); + return result; + } +} + +static ufbxi_noinline uint32_t ufbxi_parse_double_init_flags() +{ + // We require evaluation in double precision, either for doubles (0) or always (1) + // and rounding to nearest, which we can check for with `1 + eps == 1 - eps`. + #if UFBX_FLT_EVAL_METHOD == 0 || UFBX_FLT_EVAL_METHOD == 1 + static volatile double ufbxi_volatile_eps = 2.2250738585072014e-308; + if (1.0 + ufbxi_volatile_eps == 1.0 - ufbxi_volatile_eps) return UFBXI_PARSE_DOUBLE_ALLOW_FAST_PATH; + #endif + + return 0; } static ufbxi_forceinline int64_t ufbxi_parse_int64(const char *str, char **end) @@ -1421,6 +1689,24 @@ static ufbxi_forceinline int64_t ufbxi_parse_int64(const char *str, char **end) return negative ? -(int64_t)abs_val : (int64_t)abs_val; } +static ufbxi_noinline uint32_t ufbxi_parse_uint32_radix(const char *str, uint32_t radix) +{ + uint32_t value = 0; + for (const char *p = str; ; p++) { + char c = *p; + if (c >= '0' && c <= '9') { + value = value * radix + (uint32_t)(c - '0'); + } else if (radix == 16 && (c >= 'a' && c <= 'f')) { + value = value * radix + (uint32_t)(c + (10 - 'a')); + } else if (radix == 16 && (c >= 'A' && c <= 'F')) { + value = value * radix + (uint32_t)(c + (10 - 'A')); + } else { + break; + } + } + return value; +} + // -- DEFLATE implementation #if !defined(ufbx_inflate) @@ -2838,35 +3124,111 @@ ufbxi_extern_c ptrdiff_t ufbx_inflate(void *dst, size_t dst_size, const ufbx_inf uint32_t ref = (uint32_t)bits; ref = (ref>>24) | ((ref>>8)&0xff00) | ((ref<<8)&0xff0000) | (ref<<24); - uint32_t checksum = ufbxi_adler32(dc.out_begin, ufbxi_to_size(dc.out_ptr - dc.out_begin)); - if (ref != checksum) { - return -9; + uint32_t checksum = ufbxi_adler32(dc.out_begin, ufbxi_to_size(dc.out_ptr - dc.out_begin)); + if (ref != checksum) { + return -9; + } + } + } + + return dc.out_ptr - dc.out_begin; +} + +#endif // !defined(ufbx_inflate) + +// -- Printf + +typedef struct { + char *dst; + size_t length; + size_t pos; +} ufbxi_print_buffer; + +#define UFBXI_PRINT_UNSIGNED 0x1 +#define UFBXI_PRINT_STRING 0x2 +#define UFBXI_PRINT_SIZE_T 0x10 + +static void ufbxi_print_append(ufbxi_print_buffer *buf, size_t min_width, size_t max_width, const char *str) +{ + size_t width = 0; + for (width = 0; width < max_width; width++) { + if (!str[width]) break; + } + size_t pad = min_width > width ? min_width - width : 0; + for (size_t i = 0; i < pad; i++) { + if (buf->pos < buf->length) buf->dst[buf->pos++] = ' '; + } + for (size_t i = 0; i < width; i++) { + if (buf->pos < buf->length) buf->dst[buf->pos++] = str[i]; + } +} + +static char *ufbxi_print_format_int(char *buffer, uint64_t value) +{ + *--buffer = '\0'; + do { + uint32_t digit = (uint32_t)(value % 10); + value = value / 10; + *--buffer = (char)('0' + digit); + } while (value > 0); + return buffer; +} + +static void ufbxi_vprint(ufbxi_print_buffer *buf, const char *fmt, va_list args) +{ + char buffer[96]; // ufbxi_uninit + for (const char *p = fmt; *p;) { + if (*p == '%' && *++p != '%') { + size_t min_width = 0, max_width = SIZE_MAX; + if (*p == '*') { + p++; + min_width = (size_t)va_arg(args, int); + } + if (*p == '.') { + ufbxi_dev_assert(p[1] == '*'); + p += 2; + max_width = (size_t)va_arg(args, int); + } + uint32_t flags = 0; + switch (*p) { + case 'z': p++; flags |= UFBXI_PRINT_SIZE_T; break; + default: break; + } + switch (*p++) { + case 'u': flags |= UFBXI_PRINT_UNSIGNED; break; + case 's': flags |= UFBXI_PRINT_STRING; break; + default: break; + } + if (flags & UFBXI_PRINT_STRING) { + const char *str = va_arg(args, const char*); + ufbxi_print_append(buf, min_width, max_width, str); + } else if (flags & UFBXI_PRINT_UNSIGNED) { + uint64_t value = (flags & UFBXI_PRINT_SIZE_T) != 0 ? (uint64_t)va_arg(args, size_t) : (uint64_t)va_arg(args, uint32_t); + char *str = ufbxi_print_format_int(buffer + sizeof(buffer), value); + ufbxi_print_append(buf, min_width, max_width, str); + } else { + ufbxi_unreachable("Bad printf format"); } + } else { + if (buf->pos < buf->length) buf->dst[buf->pos++] = *p; + p++; } } - - return dc.out_ptr - dc.out_begin; + if (buf->length && buf->dst) { + size_t end = buf->pos <= buf->length - 1 ? buf->pos : buf->length - 1; + buf->dst[end] = '\0'; + } } -#endif // !defined(ufbx_inflate) - // -- Errors static const char ufbxi_empty_char[1] = { '\0' }; static ufbxi_noinline int ufbxi_vsnprintf(char *buf, size_t buf_size, const char *fmt, va_list args) { - int result = vsnprintf(buf, buf_size, fmt, args); - - if (result < 0) result = 0; - if ((size_t)result >= buf_size - 1) result = (int)buf_size - 1; - - // HACK: On some MSYS/MinGW implementations `vsnprintf` is broken and does - // not write the null terminator on truncation, it's always safe to do so - // let's just do it unconditionally here... - buf[result] = '\0'; - - return result; + ufbxi_print_buffer buffer = { buf, buf_size }; + ufbxi_vprint(&buffer, fmt, args); + return (int)ufbxi_min_sz(buffer.pos, buf_size - 1); } static ufbxi_noinline int ufbxi_snprintf(char *buf, size_t buf_size, const char *fmt, ...) @@ -2883,21 +3245,19 @@ static ufbxi_noinline void ufbxi_panicf_imp(ufbx_panic *panic, const char *fmt, if (panic && panic->did_panic) return; va_list args; // ufbxi_uninit - va_start(args, fmt); if (panic) { + va_start(args, fmt); panic->did_panic = true; panic->message_length = (size_t)ufbxi_vsnprintf(panic->message, sizeof(panic->message), fmt, args); + va_end(args); } else { - fprintf(stderr, "ufbx panic: "); - vfprintf(stderr, fmt, args); - fprintf(stderr, "\n"); - } - - va_end(args); + va_start(args, fmt); + char message[UFBX_PANIC_MESSAGE_LENGTH]; + ufbxi_vsnprintf(message, sizeof(message), fmt, args); + va_end(args); - if (!panic) { - ufbx_assert(false && "ufbx panic: See stderr for more information"); + ufbx_panic_handler(message); } } @@ -3046,7 +3406,7 @@ static ufbxi_noinline void ufbxi_clear_error(ufbx_error *err) #define ufbxi_fail_err_msg(err, desc, msg) return ufbxi_fail_imp_err(err, ufbxi_error_msg(desc, msg), ufbxi_function, ufbxi_line) #define ufbxi_report_err_msg(err, desc, msg) (void)ufbxi_fail_imp_err(err, ufbxi_error_msg(desc, msg), ufbxi_function, ufbxi_line) -static ufbxi_noinline void ufbxi_fix_error_type(ufbx_error *error, const char *default_desc) +static ufbxi_noinline void ufbxi_fix_error_type(ufbx_error *error, const char *default_desc, ufbx_error *p_error) { const char *desc = error->description.data; if (!desc) desc = default_desc; @@ -3096,6 +3456,9 @@ static ufbxi_noinline void ufbxi_fix_error_type(ufbx_error *error, const char *d } error->description.data = desc; error->description.length = strlen(desc); + if (p_error) { + memcpy(p_error, error, sizeof(ufbx_error)); + } } // -- Allocator @@ -3168,7 +3531,7 @@ static ufbxi_noinline void *ufbxi_alloc_size(ufbxi_allocator *ator, size_t size, } else if (ator->ator.allocator.realloc_fn) { ptr = ator->ator.allocator.realloc_fn(ator->ator.allocator.user, NULL, 0, total); } else { - ptr = malloc(total); + ptr = ufbx_malloc(total); } if (!ptr) { @@ -3215,7 +3578,7 @@ static ufbxi_noinline void *ufbxi_realloc_size(ufbxi_allocator *ator, size_t siz ator->ator.allocator.free_fn(ator->ator.allocator.user, old_ptr, old_total); } } else { - ptr = realloc(old_ptr, total); + ptr = ufbx_realloc(old_ptr, old_total, total); } ufbxi_check_return_err_msg(ator->error, ptr, NULL, "Out of memory"); @@ -3249,7 +3612,7 @@ static ufbxi_noinline void ufbxi_free_size(ufbxi_allocator *ator, size_t size, v ator->ator.allocator.realloc_fn(ator->ator.allocator.user, ptr, total, 0); } } else { - free(ptr); + ufbx_free(ptr, total); } } @@ -3888,7 +4251,7 @@ static ufbxi_noinline void ufbxi_map_init(ufbxi_map *map, ufbxi_allocator *ator, // allocation counts. We can work around this using a local allocator that doesn't // count the allocations. { - ufbxi_allocator *regression_ator = (ufbxi_allocator*)malloc(sizeof(ufbxi_allocator)); + ufbxi_allocator *regression_ator = (ufbxi_allocator*)ufbx_malloc(sizeof(ufbxi_allocator)); ufbx_assert(regression_ator); memset(regression_ator, 0, sizeof(ufbxi_allocator)); regression_ator->name = "regression"; @@ -3922,7 +4285,7 @@ static ufbxi_noinline void ufbxi_map_free(ufbxi_map *map) #if defined(UFBX_REGRESSION) if (regression_ator) { ufbxi_free_ator(regression_ator); - free(regression_ator); + ufbx_free(regression_ator, sizeof(ufbxi_allocator)); } #endif } @@ -5923,10 +6286,8 @@ typedef struct { // IO uint64_t data_offset; - ufbx_read_fn *read_fn; ufbx_skip_fn *skip_fn; - ufbx_close_fn *close_fn; void *read_user; char *read_buffer; @@ -6026,6 +6387,10 @@ typedef struct { // Temporary per-element flags uint8_t *tmp_element_flag; + // IO (cold) + ufbx_close_fn *close_fn; + ufbx_size_fn *size_fn; + ufbxi_ascii ascii; bool has_geometry_transform_nodes; @@ -6065,6 +6430,10 @@ typedef struct { ufbxi_warnings warnings; bool deferred_failure; + bool deferred_load; + + const char *load_filename; + size_t load_filename_len; bool parse_threaded; ufbxi_thread_pool thread_pool; @@ -6348,8 +6717,6 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_read_to(ufbxi_context *uc, void return 1; } -// -- File IO - static ufbxi_noinline void ufbxi_init_ator(ufbx_error *error, ufbxi_allocator *ator, const ufbx_allocator_opts *opts, const char *name) { ufbx_allocator_opts zero_opts; @@ -6369,19 +6736,56 @@ static ufbxi_noinline void ufbxi_init_ator(ufbx_error *error, ufbxi_allocator *a ator->name = name; } -static ufbxi_noinline FILE *ufbxi_fopen(const char *path, size_t path_len, ufbxi_allocator *tmp_ator) +typedef struct { + ufbx_error error; + + ufbxi_allocator *parent_ator; + ufbxi_allocator ator; +} ufbxi_file_context; + +static ufbxi_noinline void ufbxi_begin_file_context(ufbxi_file_context *fc, ufbx_open_file_context ctx, const ufbx_allocator_opts *ator_opts) { -#if !defined(UFBX_STANDARD_C) && defined(_WIN32) - wchar_t wpath_buf[256]; - wchar_t *wpath = NULL; + memset(fc, 0, sizeof(ufbxi_file_context)); + if (ctx) { + fc->parent_ator = (ufbxi_allocator*)ctx; + fc->ator = *fc->parent_ator; + fc->ator.error = &fc->error; + } else { + ufbxi_init_ator(&fc->error, &fc->ator, ator_opts, "file"); + } +} - if (path_len == SIZE_MAX) { - path_len = strlen(path); +static ufbxi_noinline void ufbxi_end_file_context(ufbxi_file_context *fc, ufbx_error *error, bool ok) +{ + if (fc->parent_ator) { + fc->ator.error = fc->parent_ator->error; + *fc->parent_ator = fc->ator; + } else { + ufbxi_free_ator(&fc->ator); + } + if (error) { + if (!ok) { + ufbxi_fix_error_type(&fc->error, "Failed to open file", error); + } else { + ufbxi_clear_error(error); + } } +} + +// -- File IO + +#if !defined(UFBX_NO_STDIO) && !defined(UFBX_EXTERNAL_STDIO) + +static ufbxi_noinline FILE *ufbxi_fopen(ufbxi_file_context *fc, const char *path, size_t path_len, bool null_terminated) +{ + FILE *file = NULL; +#if !defined(UFBX_STANDARD_C) && defined(_WIN32) + (void)null_terminated; + wchar_t wpath_buf[256], *wpath = NULL; // ufbxi_uninit if (path_len < ufbxi_arraycount(wpath_buf) - 1) { wpath = wpath_buf; } else { - wpath = ufbxi_alloc(tmp_ator, wchar_t, path_len + 1); + wpath = ufbxi_alloc(&fc->ator, wchar_t, path_len + 1); if (!wpath) return NULL; } @@ -6416,45 +6820,38 @@ static ufbxi_noinline FILE *ufbxi_fopen(const char *path, size_t path_len, ufbxi } wpath[wlen] = 0; - FILE *file = NULL; -#if UFBXI_MSC_VER >= 1400 - if (_wfopen_s(&file, wpath, L"rb") != 0) { - file = NULL; - } -#else - file = _wfopen(wpath, L"rb"); -#endif - + #if UFBXI_MSC_VER >= 1400 + if (_wfopen_s(&file, wpath, L"rb") != 0) file = NULL; + #else + file = _wfopen(wpath, L"rb"); + #endif if (wpath != wpath_buf) { - ufbxi_free(tmp_ator, wchar_t, wpath, path_len + 1); + ufbxi_free(&fc->ator, wchar_t, wpath, path_len + 1); } - - return file; #else - if (path_len == SIZE_MAX) { - return fopen(path, "rb"); - } - - char copy_buf[256]; // ufbxi_uninit - char *copy = NULL; - - if (path_len < ufbxi_arraycount(copy_buf) - 1) { - copy = copy_buf; + char copy_buf[256], *copy = NULL; // ufbxi_uninit + if (null_terminated) { + copy = (char*)path; } else { - copy = ufbxi_alloc(tmp_ator, char, path_len + 1); - if (!copy) return NULL; + if (path_len < ufbxi_arraycount(copy_buf) - 1) { + copy = copy_buf; + } else { + copy = ufbxi_alloc(&fc->ator, char, path_len + 1); + if (!copy) return NULL; + } + memcpy(copy, path, path_len); + copy[path_len] = '\0'; } - memcpy(copy, path, path_len); - copy[path_len] = '\0'; - - FILE *file = fopen(copy, "rb"); - - if (copy != copy_buf) { - ufbxi_free(tmp_ator, char, copy, path_len + 1); + file = fopen(copy, "rb"); + if (!null_terminated && copy != copy_buf) { + ufbxi_free(&fc->ator, char, copy, path_len + 1); } - - return file; #endif + if (!file) { + ufbxi_set_err_info(&fc->error, path, path_len); + ufbxi_report_err_msg(&fc->error, "file", "File not found"); + } + return file; } static uint64_t ufbxi_ftell(FILE *file) @@ -6466,20 +6863,20 @@ static uint64_t ufbxi_ftell(FILE *file) int64_t result = _ftelli64(file); if (result >= 0) return (uint64_t)result; #else - long result = ftell(file); + int64_t result = ftell(file); if (result >= 0) return (uint64_t)result; #endif return UINT64_MAX; } -static size_t ufbxi_file_read(void *user, void *data, size_t max_size) +static size_t ufbxi_stdio_read(void *user, void *data, size_t max_size) { FILE *file = (FILE*)user; if (ferror(file)) return SIZE_MAX; return fread(data, 1, max_size, file); } -static bool ufbxi_file_skip(void *user, size_t size) +static bool ufbxi_stdio_skip(void *user, size_t size) { FILE *file = (FILE*)user; ufbx_assert(size <= UFBXI_MAX_SKIP_SIZE); @@ -6488,12 +6885,94 @@ static bool ufbxi_file_skip(void *user, size_t size) return true; } -static void ufbxi_file_close(void *user) +static uint64_t ufbxi_stdio_size(void *user) +{ + FILE *file = (FILE*)user; + uint64_t result = 0; + uint64_t begin = ufbxi_ftell(file); + if (begin < UINT64_MAX) { + fpos_t pos; // ufbxi_uninit + if (fgetpos(file, &pos) == 0) { + if (fseek(file, 0, SEEK_END) == 0) { + uint64_t end = ufbxi_ftell(file); + if (end != UINT64_MAX && begin < end) { + result = end - begin; + } + // Both `rewind()` and `fsetpos()` to reset error and EOF + rewind(file); + fsetpos(file, &pos); + } + } + } + return result; +} + +static void ufbxi_stdio_close(void *user) { FILE *file = (FILE*)user; fclose(file); } +static ufbxi_noinline void ufbxi_stdio_init(ufbx_stream *stream, void *file, bool close) +{ + stream->read_fn = &ufbxi_stdio_read; + stream->skip_fn = &ufbxi_stdio_skip; + stream->size_fn = &ufbxi_stdio_size; + stream->close_fn = close ? &ufbxi_stdio_close : NULL; + stream->user = file; +} + +static ufbxi_noinline bool ufbxi_stdio_open(ufbxi_file_context *fc, ufbx_stream *stream, const char *path, size_t path_len, bool null_terminated) +{ + FILE *file = ufbxi_fopen(fc, path, path_len, null_terminated); + if (!file) return false; + ufbxi_stdio_init(stream, file, true); + return true; +} + +#elif defined(UFBX_EXTERNAL_STDIO) + +static ufbxi_noinline void ufbxi_stdio_init(ufbx_stream *stream, void *file, bool close) +{ + stream->read_fn = &ufbx_stdio_read; + stream->skip_fn = &ufbx_stdio_skip; + stream->size_fn = &ufbx_stdio_size; + stream->close_fn = close ? &ufbx_stdio_close : NULL; + stream->user = file; +} + +static ufbxi_noinline bool ufbxi_stdio_open(ufbxi_file_context *fc, ufbx_stream *stream, const char *path, size_t path_len, bool null_terminated) +{ + char copy_buf[256], *copy = NULL; // ufbxi_uninit + if (null_terminated) { + copy = (char*)path; + } else { + if (path_len < ufbxi_arraycount(copy_buf) - 1) { + copy = copy_buf; + } else { + copy = ufbxi_alloc(&fc->ator, char, path_len + 1); + if (!copy) return false; + } + memcpy(copy, path, path_len); + copy[path_len] = '\0'; + } + void *file = ufbx_stdio_open(copy, path_len); + if (!null_terminated && copy != copy_buf) { + ufbxi_free(&fc->ator, char, copy, path_len + 1); + } + if (!file) { + ufbxi_set_err_info(&fc->error, path, path_len); + ufbxi_report_err_msg(&fc->error, "file", "File not found"); + return false; + } + ufbxi_stdio_init(stream, file, true); + return true; +} + +#endif + +// -- Memory IO + typedef struct { const void *data; size_t size; @@ -6502,7 +6981,8 @@ typedef struct { // Own allocation information size_t self_size; - ufbxi_allocator ator; + ufbxi_allocator *parent_ator; + ufbxi_allocator local_ator; ufbx_error error; char data_copy[]; } ufbxi_memory_stream; @@ -6524,6 +7004,12 @@ static bool ufbxi_memory_skip(void *user, size_t size) return true; } +static uint64_t ufbxi_memory_size(void *user) +{ + ufbxi_memory_stream *stream = (ufbxi_memory_stream*)user; + return stream->size; +} + static void ufbxi_memory_close(void *user) { ufbxi_memory_stream *stream = (ufbxi_memory_stream*)user; @@ -6531,9 +7017,13 @@ static void ufbxi_memory_close(void *user) stream->close_cb.fn(stream->close_cb.user, (void*)stream->data, stream->size); } - ufbxi_allocator ator = stream->ator; - ufbxi_free(&ator, char, stream, stream->self_size); - ufbxi_free_ator(&ator); + if (stream->parent_ator) { + ufbxi_free(stream->parent_ator, char, stream, stream->self_size); + } else { + ufbxi_allocator ator = stream->local_ator; + ufbxi_free(&ator, char, stream, stream->self_size); + ufbxi_free_ator(&ator); + } } // -- XML @@ -6703,9 +7193,9 @@ static ufbxi_noinline int ufbxi_xml_read_until(ufbxi_xml_context *xc, ufbx_strin if (entity[0] == '#') { unsigned long code = 0; if (entity[1] == 'x') { - code = strtoul(entity + 2, NULL, 16); + code = ufbxi_parse_uint32_radix(entity + 2, 16); } else { - code = strtoul(entity + 1, NULL, 10); + code = ufbxi_parse_uint32_radix(entity + 1, 10); } char bytes[5] = { 0 }; @@ -9098,11 +9588,9 @@ ufbxi_nodiscard ufbxi_noinline static int ufbxi_ascii_next_token(ufbxi_context * token->value.i64 = ufbxi_parse_int64(token->str_data, &end); ufbxi_check(end == token->str_data + token->str_len - 1); } else if (token->type == UFBXI_ASCII_FLOAT) { - if (ua->parse_as_f32) { - token->value.f64 = strtof(token->str_data, &end); - } else { - token->value.f64 = ufbxi_parse_double(token->str_data, token->str_len, &end, uc->double_parse_flags); - } + uint32_t flags = uc->double_parse_flags; + if (ua->parse_as_f32) flags = UFBXI_PARSE_DOUBLE_AS_BINARY32; + token->value.f64 = ufbxi_parse_double(token->str_data, token->str_len, &end, flags); ufbxi_check(end == token->str_data + token->str_len - 1); } } @@ -9499,8 +9987,6 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_ascii_read_float_array(ufbxi_con *v = (float)val; } - // TODO: Collect ASCII numbers to deferred parse integer/string segments - // Try to parse the next value, we don't commit this until we find a comma after it above. char *num_end = NULL; size_t left = ufbxi_to_size(end - src_scan); @@ -15725,44 +16211,12 @@ ufbxi_nodiscard ufbxi_noinline static int ufbxi_resolve_relative_filename(ufbxi_ // Open file utility -static void *ufbxi_ator_alloc(void *user, size_t size) -{ - ufbxi_allocator *ator = (ufbxi_allocator*)user; - return ufbxi_alloc(ator, char, size); -} - -static void *ufbxi_ator_realloc(void *user, void *old_ptr, size_t old_size, size_t new_size) -{ - ufbxi_allocator *ator = (ufbxi_allocator*)user; - return ufbxi_realloc(ator, char, old_ptr, old_size, new_size); -} - -static void ufbxi_ator_free(void *user, void *ptr, size_t size) -{ - ufbxi_allocator *ator = (ufbxi_allocator*)user; - ufbxi_free(ator, char, ptr, size); -} - -static ufbxi_noinline void ufbxi_setup_ator_allocator(ufbx_allocator *allocator, ufbxi_allocator *ator) -{ - allocator->alloc_fn = &ufbxi_ator_alloc; - allocator->realloc_fn = &ufbxi_ator_realloc; - allocator->free_fn = &ufbxi_ator_free; - allocator->free_allocator_fn = NULL; - allocator->user = ator; -} - static ufbxi_noinline bool ufbxi_open_file(const ufbx_open_file_cb *cb, ufbx_stream *stream, const char *path, size_t path_len, const ufbx_blob *original_filename, ufbxi_allocator *ator, ufbx_open_file_type type) { if (!cb || !cb->fn) return false; ufbx_open_file_info info; // ufbxi_uninit - if (ator) { - ufbxi_setup_ator_allocator(&info.temp_allocator, ator); - } else { - memset(&info.temp_allocator, 0, sizeof(info.temp_allocator)); - } - + info.context = (uintptr_t)ator; if (original_filename) { info.original_filename = *original_filename; } else { @@ -17084,6 +17538,7 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_obj_load_mtl(ufbxi_context *uc) if (!has_stream && uc->opts.load_external_files && uc->opts.obj_search_mtl_by_filename && path.length > 4) { ufbx_string ext = { path.data + path.length - 4, 4 }; if (ufbxi_match(&ext, "\\c.obj")) { + ufbxi_analysis_assert(path.length < SIZE_MAX - 1); char *copy = ufbxi_push_copy(&uc->tmp, char, path.length + 1, path.data); ufbxi_check(copy); copy[path.length - 3] = copy[path.length - 3] == 'O' ? 'M' : 'm'; @@ -23237,9 +23692,9 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_cache_load_xml_imp(ufbxi_cache_c if (tag_fps) { ufbxi_xml_attrib *fps = ufbxi_xml_find_attrib(tag_fps, "TimePerFrame"); if (fps) { - int value = atoi(fps->value.data); + uint32_t value = ufbxi_parse_uint32_radix(fps->value.data, 10); if (value > 0) { - cc->xml_ticks_per_frame = (uint32_t)value; + cc->xml_ticks_per_frame = value; } } } @@ -23264,9 +23719,9 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_cache_load_xml_imp(ufbxi_cache_c ufbxi_xml_attrib *start_time = ufbxi_xml_find_attrib(tag, "StartTime"); ufbxi_xml_attrib *end_time = ufbxi_xml_find_attrib(tag, "EndTime"); if (sampling_rate && start_time && end_time) { - channel->sample_rate = (uint32_t)atoi(sampling_rate->value.data); - channel->start_time = (uint32_t)atoi(start_time->value.data); - channel->end_time = (uint32_t)atoi(end_time->value.data); + channel->sample_rate = ufbxi_parse_uint32_radix(sampling_rate->value.data, 10); + channel->start_time = ufbxi_parse_uint32_radix(start_time->value.data, 10); + channel->end_time = ufbxi_parse_uint32_radix(end_time->value.data, 10); channel->current_time = channel->start_time; channel->try_load = true; } @@ -23591,7 +24046,7 @@ ufbxi_noinline static ufbx_geometry_cache *ufbxi_cache_load(ufbxi_cache_context if (ok) { return &cc->imp->cache; } else { - ufbxi_fix_error_type(&cc->error, "Failed to load geometry cache"); + ufbxi_fix_error_type(&cc->error, "Failed to load geometry cache", NULL); if (!cc->owned_by_scene) { ufbxi_buf_free(&cc->string_pool.buf); ufbxi_free_ator(&cc->ator_result); @@ -24090,6 +24545,50 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_load_imp(ufbxi_context *uc) { // Check for deferred failure if (uc->deferred_failure) return 0; + if (uc->deferred_load) { + ufbx_stream stream = { 0 }; + ufbx_open_file_opts opts = { 0 }; + const char *filename = uc->load_filename; + size_t filename_len = uc->load_filename_len; + bool ok = false; + if (filename_len == SIZE_MAX) { + opts.filename_null_terminated = true; + filename_len = strlen(filename); + } + if (uc->opts.filename.length == 0 || uc->opts.filename.data == NULL) { + uc->opts.filename.data = filename; + uc->opts.filename.length = filename_len; + } + ufbx_error error; + error.type = UFBX_ERROR_NONE; + if (uc->opts.open_main_file_with_default || uc->opts.open_file_cb.fn == &ufbx_default_open_file) { + ufbx_open_file_context ctx = (ufbx_open_file_context)&uc->ator_tmp; + ok = ufbx_open_file_ctx(&stream, ctx, filename, filename_len, &opts, &error); + } else { + ok = ufbxi_open_file(&uc->opts.open_file_cb, &stream, uc->load_filename, filename_len, NULL, &uc->ator_tmp, UFBX_OPEN_FILE_MAIN_MODEL); + } + if (!ok) { + if (error.type != UFBX_ERROR_NONE) { + // cppcheck-suppress uninitStructMember + uc->error = error; + } else { + ufbxi_set_err_info(&uc->error, filename, filename_len); + } + ufbxi_fail_msg("open_file_fn()", "File not found"); + } + uc->read_fn = stream.read_fn; + uc->skip_fn = stream.skip_fn; + uc->size_fn = stream.size_fn; + uc->close_fn = stream.close_fn; + uc->read_user = stream.user; + } + + if (uc->opts.progress_cb.fn && uc->progress_bytes_total == 0 && uc->size_fn) { + uint64_t total = uc->size_fn(uc->read_user); + ufbxi_check(total != UINT64_MAX); + uc->progress_bytes_total = total; + } + ufbxi_check(uc->opts.path_separator >= 0x20 && uc->opts.path_separator <= 0x7e); ufbxi_check(ufbxi_fixup_opts_string(uc, &uc->opts.filename, false)); @@ -24431,34 +24930,24 @@ static ufbxi_noinline ufbx_scene *ufbxi_load(ufbxi_context *uc, const ufbx_load_ int ok = ufbxi_load_imp(uc); - ufbxi_free_temp(uc); - if (uc->close_fn) { uc->close_fn(uc->read_user); } + ufbxi_free_temp(uc); + if (ok) { if (p_error) { ufbxi_clear_error(p_error); } return &uc->scene_imp->scene; } else { - ufbxi_fix_error_type(&uc->error, "Failed to load"); - if (p_error) *p_error = uc->error; + ufbxi_fix_error_type(&uc->error, "Failed to load", p_error); ufbxi_free_result(uc); return NULL; } } -static ufbxi_noinline ufbx_scene *ufbxi_load_not_found(const char *filename, size_t filename_len, ufbx_error *p_error) -{ - ufbxi_context uc = { UFBX_ERROR_NONE }; - ufbxi_set_err_info(&uc.error, filename, filename_len); - ufbxi_report_err_msg(&uc.error, "File not found", "File not found"); - uc.deferred_failure = true; - return ufbxi_load(&uc, NULL, p_error); -} - // -- Animation evaluation static ufbxi_forceinline bool ufbxi_override_less_than_prop(const ufbx_prop_override *over, uint32_t element_id, const ufbx_prop *prop) @@ -25240,8 +25729,7 @@ ufbxi_nodiscard static ufbxi_noinline ufbx_scene *ufbxi_evaluate_scene(ufbxi_eva } return &ec->scene_imp->scene; } else { - ufbxi_fix_error_type(&ec->error, "Failed to evaluate"); - if (p_error) *p_error = ec->error; + ufbxi_fix_error_type(&ec->error, "Failed to evaluate", p_error); ufbxi_buf_free(&ec->tmp); ufbxi_buf_free(&ec->result); ufbxi_free_ator(&ec->ator_tmp); @@ -25310,11 +25798,11 @@ static bool ufbxi_prop_override_less(void *user, const void *va, const void *vb) return strcmp(a->prop_name.data, b->prop_name.data) < 0; } -static int ufbxi_cmp_transform_override(const void *va, const void *vb) +static bool ufbxi_transform_override_less(void *user, const void *va, const void *vb) { + (void)user; const ufbx_transform_override *a = (const ufbx_transform_override*)va, *b = (const ufbx_transform_override*)vb; - if (a->node_id != b->node_id) return a->node_id < b->node_id ? -1 : 1; - return 0; + return a->node_id < b->node_id; } ufbxi_nodiscard static ufbxi_noinline int ufbxi_create_anim_imp(ufbxi_create_anim_context *ac) @@ -25419,8 +25907,7 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_create_anim_imp(ufbxi_create_ani anim->transform_overrides.count = ac->opts.transform_overrides.count; anim->transform_overrides.data = ufbxi_push_copy(&ac->result, ufbx_transform_override, anim->transform_overrides.count, ac->opts.transform_overrides.data); ufbxi_check_err(&ac->error, anim->transform_overrides.data); - - qsort(anim->transform_overrides.data, anim->transform_overrides.count, sizeof(ufbx_transform_override), &ufbxi_cmp_transform_override); + ufbxi_unstable_sort(anim->transform_overrides.data, anim->transform_overrides.count, sizeof(ufbx_transform_override), &ufbxi_transform_override_less, NULL); } ac->imp = ufbxi_push(&ac->result, ufbxi_anim_imp, 1); @@ -25473,6 +25960,9 @@ typedef struct { ufbx_baked_node **baked_nodes; bool *nodes_to_bake; + char *tmp_arr; + size_t tmp_arr_size; + const ufbx_scene *scene; const ufbx_anim *anim; ufbx_bake_opts opts; @@ -25495,14 +25985,15 @@ typedef struct { ufbx_anim_value *anim_value; } ufbxi_bake_prop; -static int ufbxi_cmp_bake_prop(const void *va, const void *vb) +static bool ufbxi_bake_prop_less(void *user, const void *va, const void *vb) { + (void)user; const ufbxi_bake_prop *a = (const ufbxi_bake_prop*)va; const ufbxi_bake_prop *b = (const ufbxi_bake_prop*)vb; - if (a->sort_id != b->sort_id) return a->sort_id < b->sort_id ? -1 : 1; - if (a->element_id != b->element_id) return a->element_id < b->element_id ? -1 : 1; - if (a->prop_name != b->prop_name) return strcmp(a->prop_name, b->prop_name); - return a->anim_value < b->anim_value; + if (a->sort_id != b->sort_id) return a->sort_id < b->sort_id; + if (a->element_id != b->element_id) return a->element_id < b->element_id; + if (a->prop_name != b->prop_name) return strcmp(a->prop_name, b->prop_name) < 0; + return false; } ufbx_static_assert(bake_step_left, UFBX_BAKED_KEY_STEP_LEFT == 0x1); @@ -25518,13 +26009,6 @@ static ufbxi_forceinline int ufbxi_cmp_bake_time(ufbxi_bake_time a, ufbxi_bake_t return 0; } -static int ufbxi_cmp_bake_time_fn(const void *va, const void *vb) -{ - const ufbxi_bake_time a = *(const ufbxi_bake_time*)va; - const ufbxi_bake_time b = *(const ufbxi_bake_time*)vb; - return ufbxi_cmp_bake_time(a, b); -} - ufbxi_nodiscard static ufbxi_forceinline int ufbxi_bake_push_time(ufbxi_bake_context *bc, double time, uint32_t flags) { ufbxi_bake_time *p_key = ufbxi_push_fast(&bc->tmp_times, ufbxi_bake_time, 1); @@ -25609,6 +26093,13 @@ ufbxi_nodiscard static ufbxi_noinline bool ufbxi_in_list(const char *const *item return false; } +ufbxi_nodiscard static ufbxi_noinline int ufbxi_sort_bake_times(ufbxi_bake_context *bc, ufbxi_bake_time *times, size_t count) +{ + ufbxi_check_err(&bc->error, ufbxi_grow_array(&bc->ator_tmp, &bc->tmp_arr, &bc->tmp_arr_size, count * sizeof(ufbxi_bake_time))); + ufbxi_macro_stable_sort(ufbxi_bake_time, 32, times, bc->tmp_arr, count, ( ufbxi_cmp_bake_time(*a, *b) < 0 )); + return 1; +} + ufbxi_nodiscard static ufbxi_noinline int ufbxi_finalize_bake_times(ufbxi_bake_context *bc, ufbxi_bake_time_list *p_dst) { if (bc->layer_weight_times.count > 0) { @@ -25624,8 +26115,7 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_finalize_bake_times(ufbxi_bake_c ufbxi_bake_time *times = ufbxi_push_pop(&bc->tmp_prop, &bc->tmp_times, ufbxi_bake_time, num_times); ufbxi_check_err(&bc->error, times); - // TODO: Something better - qsort(times, num_times, sizeof(ufbxi_bake_time), &ufbxi_cmp_bake_time_fn); + ufbxi_check_err(&bc->error, ufbxi_sort_bake_times(bc, times, num_times)); // Deduplicate times if (num_times > 0) { @@ -25736,13 +26226,13 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_finalize_bake_times(ufbxi_bake_c #define ufbxi_add_epsilon(a, epsilon) ((a)>0 ? (a)*(epsilon) : (a)/(epsilon)) #define ufbxi_sub_epsilon(a, epsilon) ((a)>0 ? (a)/(epsilon) : (a)*(epsilon)) -static ufbxi_noinline bool ufbxi_postprocess_step(ufbxi_bake_context *bc, double prev_time, double next_time, double *p_time, uint32_t flags) +static ufbxi_noinline bool ufbxi_postprocess_step(ufbxi_bake_context *bc, double prev_time, double next_time, double *p_time, ufbx_baked_key_flags flags) { ufbxi_dev_assert((flags & (UFBX_BAKED_KEY_STEP_LEFT|UFBX_BAKED_KEY_STEP_RIGHT)) != 0); bool left = (flags & UFBX_BAKED_KEY_STEP_LEFT) != 0; double step = 0.001; - double epsilon = 1.0 + FLT_EPSILON * 4.0f; + double epsilon = 1.0 + UFBX_FLT_EPSILON * 4.0f; double time = *p_time; switch (bc->opts.step_handling) { @@ -25982,7 +26472,7 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_push_resampled_times(ufbxi_bake_ ufbxi_bake_time *times = ufbxi_push(&bc->tmp_times, ufbxi_bake_time, keys.count); ufbxi_check_err(&bc->error, times); for (size_t i = 0; i < keys.count; i++) { - uint32_t flags = keys.data[i].flags; + ufbx_baked_key_flags flags = keys.data[i].flags; double time = keys.data[i].time; if ((flags & UFBX_BAKED_KEY_STEP_LEFT) != 0 && i + 1 < keys.count && (keys.data[i + 1].flags & UFBX_BAKED_KEY_STEP_KEY) != 0) { time = keys.data[i + 1].time; @@ -26402,8 +26892,7 @@ ufbxi_nodiscard static ufbxi_noinline int ufbxi_bake_anim(ufbxi_bake_context *bc ufbxi_bake_prop *props = ufbxi_push_pop(&bc->tmp, &bc->tmp_bake_props, ufbxi_bake_prop, num_props); ufbxi_check_err(&bc->error, props); - // TODO: Macro unstable/non allocating sort - qsort(props, num_props, sizeof(ufbxi_bake_prop), &ufbxi_cmp_bake_prop); + ufbxi_unstable_sort(props, num_props, sizeof(ufbxi_bake_prop), &ufbxi_bake_prop_less, NULL); // Pre-bake layer weight times if (!bc->opts.ignore_layer_weight_animation) { @@ -27453,19 +27942,19 @@ ufbxi_noinline static uint32_t ufbxi_triangulate_ngon(ufbxi_ngon_context *nc, ui #endif -static int ufbxi_cmp_topo_index_prev_next(const void *va, const void *vb) +static bool ufbxi_topo_less_index_prev_next(void *user, const void *va, const void *vb) { + (void)user; const ufbx_topo_edge *a = (const ufbx_topo_edge*)va, *b = (const ufbx_topo_edge*)vb; - if ((int32_t)a->prev != (int32_t)b->prev) return (int32_t)a->prev < (int32_t)b->prev ? -1 : +1; - if ((int32_t)a->next != (int32_t)b->next) return (int32_t)a->next < (int32_t)b->next ? -1 : +1; - return 0; + if ((int32_t)a->prev != (int32_t)b->prev) return (int32_t)a->prev < (int32_t)b->prev; + return (int32_t)a->next < (int32_t)b->next; } -static int ufbxi_cmp_topo_index_index(const void *va, const void *vb) +static bool ufbxi_topo_less_index_index(void *user, const void *va, const void *vb) { + (void)user; const ufbx_topo_edge *a = (const ufbx_topo_edge*)va, *b = (const ufbx_topo_edge*)vb; - if ((int32_t)a->index != (int32_t)b->index) return (int32_t)a->index < (int32_t)b->index ? -1 : +1; - return 0; + return (int32_t)a->index < (int32_t)b->index; } ufbxi_noinline static void ufbxi_compute_topology(const ufbx_mesh *mesh, ufbx_topo_edge *topo) @@ -27494,8 +27983,7 @@ ufbxi_noinline static void ufbxi_compute_topology(const ufbx_mesh *mesh, ufbx_to } } - // TODO: Macro unstable/non allocating sort - qsort(topo, num_indices, sizeof(ufbx_topo_edge), &ufbxi_cmp_topo_index_prev_next); + ufbxi_unstable_sort(topo, num_indices, sizeof(ufbx_topo_edge), &ufbxi_topo_less_index_prev_next, NULL); if (mesh->edges.data) { for (uint32_t ei = 0; ei < mesh->num_edges; ei++) { @@ -27535,8 +28023,7 @@ ufbxi_noinline static void ufbxi_compute_topology(const ufbx_mesh *mesh, ufbx_to i0 = i1 + 1; } - // TODO: Macro unstable/non allocating sort - qsort(topo, num_indices, sizeof(ufbx_topo_edge), &ufbxi_cmp_topo_index_index); + ufbxi_unstable_sort(topo, num_indices, sizeof(ufbx_topo_edge), &ufbxi_topo_less_index_index, NULL); // Fix `prev` and `next` to the actual index values for (uint32_t fi = 0; fi < mesh->num_faces; fi++) { @@ -27702,12 +28189,13 @@ static int ufbxi_subdivide_sum_vec4(void *user, void *output, const ufbxi_subdiv return 1; } -static ufbxi_noinline int ufbxi_cmp_subdivision_weight(const void *va, const void *vb) +static ufbxi_noinline bool ufbxi_subdivision_weight_less(void *user, const void *va, const void *vb) { + (void)user; ufbx_subdivision_weight a = *(const ufbx_subdivision_weight*)va, b = *(const ufbx_subdivision_weight*)vb; ufbxi_dev_assert(a.index != b.index); - if (a.weight != b.weight) return a.weight > b.weight ? -1 : +1; - return a.index < b.index ? -1 : +1; + if (a.weight != b.weight) return a.weight > b.weight; + return a.index < b.index; } static int ufbxi_subdivide_sum_vertex_weights(void *user, void *output, const ufbxi_subdivide_input *inputs, size_t num_inputs) @@ -27743,7 +28231,7 @@ static int ufbxi_subdivide_sum_vertex_weights(void *user, void *output, const uf vertex_weights[vx] = 0.0f; } - qsort(tmp_weights, num_weights, sizeof(ufbx_subdivision_weight), ufbxi_cmp_subdivision_weight); + ufbxi_unstable_sort(tmp_weights, num_weights, sizeof(ufbx_subdivision_weight), ufbxi_subdivision_weight_less, NULL); if (sc->max_vertex_weights != SIZE_MAX) { num_weights = ufbxi_min_sz(sc->max_vertex_weights, num_weights); @@ -28823,8 +29311,7 @@ ufbxi_noinline static ufbx_mesh *ufbxi_subdivide_mesh(const ufbx_mesh *mesh, siz ufbxi_mesh_imp *imp = sc.imp; return &imp->mesh; } else { - ufbxi_fix_error_type(&sc.error, "Failed to subdivide"); - if (p_error) *p_error = sc.error; + ufbxi_fix_error_type(&sc.error, "Failed to subdivide", p_error); ufbxi_buf_free(&sc.result); ufbxi_free_ator(&sc.ator_tmp); ufbxi_free_ator(&sc.ator_result); @@ -28976,7 +29463,7 @@ static ufbxi_noinline size_t ufbxi_generate_indices(const ufbx_vertex_stream *us ufbxi_clear_error(error); } else { - ufbxi_fix_error_type(error, "Failed to generate indices"); + ufbxi_fix_error_type(error, "Failed to generate indices", NULL); } if (streams && streams != local_streams) { @@ -29169,29 +29656,43 @@ ufbx_abi_data_def const size_t ufbx_element_type_size[UFBX_ELEMENT_TYPE_COUNT] = sizeof(ufbx_metadata_object), }; -ufbx_abi bool ufbx_open_file(ufbx_stream *stream, const char *path, size_t path_len) +ufbx_abi bool ufbx_default_open_file(void *user, ufbx_stream *stream, const char *path, size_t path_len, const ufbx_open_file_info *info) { - ufbxi_allocator tmp_ator = { 0 }; - ufbx_error tmp_error = { UFBX_ERROR_NONE }; - ufbxi_init_ator(&tmp_error, &tmp_ator, NULL, "filename"); - FILE *f = ufbxi_fopen(path, path_len, &tmp_ator); - if (!f) return false; + (void)user; + return ufbx_open_file_ctx(stream, info->context, path, path_len, NULL, NULL); +} - stream->read_fn = &ufbxi_file_read; - stream->skip_fn = &ufbxi_file_skip; - stream->close_fn = &ufbxi_file_close; - stream->user = f; - return true; +ufbx_abi bool ufbx_open_file(ufbx_stream *stream, const char *path, size_t path_len, const ufbx_open_file_opts *opts, ufbx_error *error) +{ + return ufbx_open_file_ctx(stream, (ufbx_open_file_context)NULL, path, path_len, opts, error); } -ufbx_abi bool ufbx_default_open_file(void *user, ufbx_stream *stream, const char *path, size_t path_len, const ufbx_open_file_info *info) +ufbx_abi bool ufbx_open_file_ctx(ufbx_stream *stream, ufbx_open_file_context ctx, const char *path, size_t path_len, const ufbx_open_file_opts *opts, ufbx_error *error) { - (void)user; - (void)info; - return ufbx_open_file(stream, path, path_len); + bool ok = false; + ufbxi_file_context fc; // ufbxi_uninit + ufbxi_begin_file_context(&fc, ctx, NULL); + if (path_len == SIZE_MAX) path_len = strlen(path); +#if !defined(UFBX_NO_STDIO) + ok = ufbxi_stdio_open(&fc, stream, path, path_len, opts ? opts->filename_null_terminated : false); +#else + (void)stream; + (void)path; + (void)path_len; + (void)opts; + ufbxi_fmt_err_info(&fc.error, "UFBX_NO_STDIO"); + ufbxi_report_err_msg(&fc.error, "UFBX_NO_STDIO", "Feature disabled"); +#endif + ufbxi_end_file_context(&fc, error, ok); + return ok; } ufbx_abi bool ufbx_open_memory(ufbx_stream *stream, const void *data, size_t data_size, const ufbx_open_memory_opts *opts, ufbx_error *error) +{ + return ufbx_open_memory_ctx(stream, (ufbx_open_file_context)NULL, data, data_size, opts, error); +} + +ufbx_abi bool ufbx_open_memory_ctx(ufbx_stream *stream, ufbx_open_file_context ctx, const void *data, size_t data_size, const ufbx_open_memory_opts *opts, ufbx_error *error) { ufbx_open_memory_opts local_opts; // ufbxi_uninit if (!opts) { @@ -29200,22 +29701,17 @@ ufbx_abi bool ufbx_open_memory(ufbx_stream *stream, const void *data, size_t dat } ufbx_assert(opts->_begin_zero == 0 && opts->_end_zero == 0); - ufbx_error local_error = { UFBX_ERROR_NONE }; - if (!error) error = &local_error; - ufbxi_clear_error(error); - - ufbxi_allocator ator = { 0 }; - ufbxi_init_ator(error, &ator, &opts->allocator, "memory"); + ufbxi_file_context fc; // ufbxi_uninit + ufbxi_begin_file_context(&fc, ctx, &opts->allocator); size_t copy_size = opts->no_copy ? 0 : data_size; // Align the allocation size to 8 bytes to make sure the header is aligned. size_t self_size = ufbxi_align_to_mask(sizeof(ufbxi_memory_stream) + copy_size, 7); - void *memory = ufbxi_alloc(&ator, char, self_size); + void *memory = ufbxi_alloc(&fc.ator, char, self_size); if (!memory) { - ufbxi_free_ator(&ator); - ufbxi_fix_error_type(error, "Failed to open memory"); + ufbxi_end_file_context(&fc, error, false); return false; } @@ -29234,14 +29730,20 @@ ufbx_abi bool ufbx_open_memory(ufbx_stream *stream, const void *data, size_t dat } // Transplant the allocator in the result blob - mem->ator = ator; - mem->ator.error = &mem->error; + if (fc.parent_ator) { + mem->parent_ator = fc.parent_ator; + } else { + fc.parent_ator = &mem->local_ator; + } stream->read_fn = ufbxi_memory_read; stream->skip_fn = ufbxi_memory_skip; + stream->size_fn = ufbxi_memory_size; stream->close_fn = ufbxi_memory_close; stream->user = mem; + ufbxi_end_file_context(&fc, error, true); + return true; } @@ -29253,7 +29755,8 @@ ufbx_abi bool ufbx_is_thread_safe(void) ufbx_abi ufbx_scene *ufbx_load_memory(const void *data, size_t size, const ufbx_load_opts *opts, ufbx_error *error) { ufbxi_check_opts_ptr(ufbx_scene, opts, error); - ufbxi_context uc = { UFBX_ERROR_NONE }; + ufbxi_context uc; // ufbxi_uninit + memset(&uc, 0, sizeof(ufbxi_context)); uc.data_begin = uc.data = (const char *)data; uc.data_size = size; uc.progress_bytes_total = size; @@ -29268,42 +29771,12 @@ ufbx_abi ufbx_scene *ufbx_load_file(const char *filename, const ufbx_load_opts * ufbx_abi ufbx_scene *ufbx_load_file_len(const char *filename, size_t filename_len, const ufbx_load_opts *opts, ufbx_error *error) { ufbxi_check_opts_ptr(ufbx_scene, opts, error); - ufbx_load_opts opts_copy; - if (opts) { - opts_copy = *opts; - } else { - memset(&opts_copy, 0, sizeof(opts_copy)); - opts = &opts_copy; - } - if (opts_copy.filename.length == 0 || opts_copy.filename.data == NULL) { - opts_copy.filename.data = filename; - opts_copy.filename.length = filename_len; - } - - // Defer to `ufbx_load_stream()` if the user so prefers. - if (!opts->open_main_file_with_default && opts->open_file_cb.fn) { - ufbx_stream stream = { 0 }; - if (ufbxi_open_file(&opts->open_file_cb, &stream, filename, filename_len, NULL, NULL, UFBX_OPEN_FILE_MAIN_MODEL)) { - return ufbx_load_stream_prefix(&stream, NULL, 0, &opts_copy, error); - } else { - return ufbxi_load_not_found(filename, filename_len, error); - } - } - - ufbxi_allocator tmp_ator = { 0 }; - ufbx_error tmp_error = { UFBX_ERROR_NONE }; - ufbxi_init_ator(&tmp_error, &tmp_ator, opts ? &opts->temp_allocator : NULL, "filename"); - - FILE *file = ufbxi_fopen(filename, filename_len, &tmp_ator); - if (!file) { - return ufbxi_load_not_found(filename, filename_len, error); - } - - ufbx_scene *scene = ufbx_load_stdio(file, &opts_copy, error); - - fclose(file); - - return scene; + ufbxi_context uc; // ufbxi_uninit + memset(&uc, 0, sizeof(ufbxi_context)); + uc.deferred_load = true; + uc.load_filename = filename; + uc.load_filename_len = filename_len; + return ufbxi_load(&uc, opts, error); } ufbx_abi ufbx_scene *ufbx_load_stdio(void *file_void, const ufbx_load_opts *opts, ufbx_error *error) @@ -29313,37 +29786,24 @@ ufbx_abi ufbx_scene *ufbx_load_stdio(void *file_void, const ufbx_load_opts *opts ufbx_abi ufbx_scene *ufbx_load_stdio_prefix(void *file_void, const void *prefix, size_t prefix_size, const ufbx_load_opts *opts, ufbx_error *error) { - ufbxi_check_opts_ptr(ufbx_scene, opts, error); - FILE *file = (FILE*)file_void; - - ufbxi_context uc = { UFBX_ERROR_NONE }; - uc.data_begin = uc.data = (const char *)prefix; - uc.data_size = prefix_size; - uc.read_fn = &ufbxi_file_read; - uc.skip_fn = &ufbxi_file_skip; - uc.read_user = file; - - if (opts && opts->progress_cb.fn && opts->file_size_estimate == 0) { - uint64_t begin = ufbxi_ftell(file); - if (begin < UINT64_MAX) { - fpos_t pos; // ufbxi_uninit - if (fgetpos(file, &pos) == 0) { - if (fseek(file, 0, SEEK_END) == 0) { - uint64_t end = ufbxi_ftell(file); - if (end != UINT64_MAX && begin < end) { - uc.progress_bytes_total = end - begin; - } - - // Both `rewind()` and `fsetpos()` to reset error and EOF - rewind(file); - fsetpos(file, &pos); - } - } - } - } - - ufbx_scene *scene = ufbxi_load(&uc, opts, error); - return scene; +#if !defined(UFBX_NO_STDIO) + if (!file_void) return NULL; + ufbx_stream stream = { 0 }; + ufbxi_stdio_init(&stream, file_void, false); + return ufbx_load_stream_prefix(&stream, prefix, prefix_size, opts, error); +#else + (void)file_void; + (void)prefix; + (void)prefix_size; + (void)opts; + + ufbxi_context uc; // ufbxi_uninit + memset(&uc, 0, sizeof(ufbxi_context)); + ufbxi_fmt_err_info(&uc.error, "UFBX_NO_STDIO"); + ufbxi_report_err_msg(&uc.error, "UFBX_NO_STDIO", "Feature disabled"); + uc.deferred_failure = true; + return ufbxi_load(&uc, NULL, error); +#endif } ufbx_abi ufbx_scene *ufbx_load_stream(const ufbx_stream *stream, const ufbx_load_opts *opts, ufbx_error *error) @@ -29354,13 +29814,16 @@ ufbx_abi ufbx_scene *ufbx_load_stream(const ufbx_stream *stream, const ufbx_load ufbx_abi ufbx_scene *ufbx_load_stream_prefix(const ufbx_stream *stream, const void *prefix, size_t prefix_size, const ufbx_load_opts *opts, ufbx_error *error) { ufbxi_check_opts_ptr(ufbx_scene, opts, error); - ufbxi_context uc = { UFBX_ERROR_NONE }; + ufbxi_context uc; // ufbxi_uninit + memset(&uc, 0, sizeof(ufbxi_context)); uc.data_begin = uc.data = (const char *)prefix; uc.data_size = prefix_size; uc.read_fn = stream->read_fn; uc.skip_fn = stream->skip_fn; + uc.size_fn = stream->size_fn; uc.close_fn = stream->close_fn; uc.read_user = stream->user; + ufbx_scene *scene = ufbxi_load(&uc, opts, error); return scene; } @@ -29412,9 +29875,10 @@ ufbx_abi ufbxi_noinline size_t ufbx_format_error(char *dst, size_t dst_size, con } size_t stack_size = ufbxi_min_sz(error->stack_size, UFBX_ERROR_STACK_MAX_DEPTH); + int line_width = 6; for (size_t i = 0; i < stack_size; i++) { const ufbx_error_frame *frame = &error->stack[i]; - int num = ufbxi_snprintf(dst + offset, dst_size - offset, "%6u:%s: %s\n", frame->source_line, frame->function.data, frame->description.data); + int num = ufbxi_snprintf(dst + offset, dst_size - offset, "%*u:%s: %s\n", line_width, frame->source_line, frame->function.data, frame->description.data); if (num > 0) offset = ufbxi_min_sz(offset + (size_t)num, dst_size - 1); } @@ -29958,8 +30422,7 @@ ufbx_abi ufbx_anim *ufbx_create_anim(const ufbx_scene *scene, const ufbx_anim_op ufbxi_anim_imp *imp = ac.imp; return &imp->anim; } else { - ufbxi_fix_error_type(&ac.error, "Failed to create anim"); - if (error) *error = ac.error; + ufbxi_fix_error_type(&ac.error, "Failed to create anim", error); ufbxi_buf_free(&ac.result); ufbxi_free_ator(&ac.ator_result); return NULL; @@ -30014,6 +30477,7 @@ ufbx_abi ufbx_baked_anim *ufbx_bake_anim(const ufbx_scene *scene, const ufbx_ani ufbxi_buf_free(&bc.tmp_elements); ufbxi_buf_free(&bc.tmp_props); ufbxi_buf_free(&bc.tmp_bake_stack); + ufbxi_free(&bc.ator_tmp, char, bc.tmp_arr, bc.tmp_arr_size); ufbxi_free_ator(&bc.ator_tmp); if (ok) { @@ -30021,8 +30485,7 @@ ufbx_abi ufbx_baked_anim *ufbx_bake_anim(const ufbx_scene *scene, const ufbx_ani ufbxi_baked_anim_imp *imp = bc.imp; return &imp->bake; } else { - ufbxi_fix_error_type(&bc.error, "Failed to bake anim"); - if (error) *error = bc.error; + ufbxi_fix_error_type(&bc.error, "Failed to bake anim", error); ufbxi_buf_free(&bc.result); ufbxi_free_ator(&bc.ator_result); return NULL; @@ -31046,8 +31509,7 @@ ufbx_abi ufbx_line_curve *ufbx_tessellate_nurbs_curve(const ufbx_nurbs_curve *cu ufbxi_line_curve_imp *imp = tc.imp; return &imp->curve; } else { - ufbxi_fix_error_type(&tc.error, "Failed to tessellate"); - if (error) *error = tc.error; + ufbxi_fix_error_type(&tc.error, "Failed to tessellate", error); ufbxi_buf_free(&tc.result); ufbxi_free_ator(&tc.ator_result); return NULL; @@ -31087,8 +31549,7 @@ ufbx_abi ufbx_mesh *ufbx_tessellate_nurbs_surface(const ufbx_nurbs_surface *surf ufbxi_mesh_imp *imp = tc.imp; return &imp->mesh; } else { - ufbxi_fix_error_type(&tc.error, "Failed to tessellate"); - if (error) *error = tc.error; + ufbxi_fix_error_type(&tc.error, "Failed to tessellate", error); ufbxi_buf_free(&tc.result); ufbxi_free_ator(&tc.ator_result); return NULL; @@ -31230,7 +31691,7 @@ ufbx_abi void ufbx_catch_compute_topology(ufbx_panic *panic, const ufbx_mesh *me ufbx_abi uint32_t ufbx_catch_topo_next_vertex_edge(ufbx_panic *panic, const ufbx_topo_edge *topo, size_t num_topo, uint32_t index) { if (index == UFBX_NO_INDEX) return UFBX_NO_INDEX; - if (ufbxi_panicf(panic, (size_t)index < num_topo, "index (%d) out of bounds (%zu)", index, num_topo)) return UFBX_NO_INDEX; + if (ufbxi_panicf(panic, (size_t)index < num_topo, "index (%u) out of bounds (%zu)", index, num_topo)) return UFBX_NO_INDEX; uint32_t twin = topo[index].twin; if (twin == UFBX_NO_INDEX) return UFBX_NO_INDEX; if (ufbxi_panicf(panic, (size_t)twin < num_topo, "Corrupted topology structure")) return UFBX_NO_INDEX; @@ -31240,7 +31701,7 @@ ufbx_abi uint32_t ufbx_catch_topo_next_vertex_edge(ufbx_panic *panic, const ufbx ufbx_abi uint32_t ufbx_catch_topo_prev_vertex_edge(ufbx_panic *panic, const ufbx_topo_edge *topo, size_t num_topo, uint32_t index) { if (index == UFBX_NO_INDEX) return UFBX_NO_INDEX; - if (ufbxi_panicf(panic, (size_t)index < num_topo, "index (%d) out of bounds (%zu)", index, num_topo)) return UFBX_NO_INDEX; + if (ufbxi_panicf(panic, (size_t)index < num_topo, "index (%u) out of bounds (%zu)", index, num_topo)) return UFBX_NO_INDEX; return topo[topo[index].prev].twin; } @@ -31340,7 +31801,7 @@ ufbx_abi void ufbx_catch_compute_normals(ufbx_panic *panic, const ufbx_mesh *mes for (size_t ix = 0; ix < face.num_indices; ix++) { uint32_t index = normal_indices[face.index_begin + ix]; - if (ufbxi_panicf(panic, index < num_normals, "Normal index (%d) out of bounds (%zu) at %zu", index, num_normals, ix)) return; + if (ufbxi_panicf(panic, index < num_normals, "Normal index (%u) out of bounds (%zu) at %zu", index, num_normals, ix)) return; ufbx_vec3 *n = &normals[index]; *n = ufbxi_add3(*n, normal); @@ -31820,12 +32281,63 @@ ufbx_abi ufbx_audio_clip *ufbx_as_audio_clip(const ufbx_element *element) { retu ufbx_abi ufbx_pose *ufbx_as_pose(const ufbx_element *element) { return element && element->type == UFBX_ELEMENT_POSE ? (ufbx_pose*)element : NULL; } ufbx_abi ufbx_metadata_object *ufbx_as_metadata_object(const ufbx_element *element) { return element && element->type == UFBX_ELEMENT_METADATA_OBJECT ? (ufbx_metadata_object*)element : NULL; } +// -- String API + +ufbx_abi ufbx_prop *ufbx_find_prop(const ufbx_props *props, const char *name) { return ufbx_find_prop_len(props, name, strlen(name)); } +ufbx_abi ufbx_real ufbx_find_real(const ufbx_props *props, const char *name, ufbx_real def) { return ufbx_find_real_len(props, name, strlen(name), def); } +ufbx_abi ufbx_vec3 ufbx_find_vec3(const ufbx_props *props, const char *name, ufbx_vec3 def) { return ufbx_find_vec3_len(props, name, strlen(name), def); } +ufbx_abi int64_t ufbx_find_int(const ufbx_props *props, const char *name, int64_t def) { return ufbx_find_int_len(props, name, strlen(name), def); } +ufbx_abi bool ufbx_find_bool(const ufbx_props *props, const char *name, bool def) { return ufbx_find_bool_len(props, name, strlen(name), def); } +ufbx_abi ufbx_string ufbx_find_string(const ufbx_props *props, const char *name, ufbx_string def) { return ufbx_find_string_len(props, name, strlen(name), def); } +ufbx_abi ufbx_blob ufbx_find_blob(const ufbx_props *props, const char *name, ufbx_blob def) { return ufbx_find_blob_len(props, name, strlen(name), def); } +ufbx_abi ufbx_element *ufbx_find_prop_element(const ufbx_element *element, const char *name, ufbx_element_type type) { return ufbx_find_prop_element_len(element, name, strlen(name), type); } +ufbx_abi ufbx_element *ufbx_find_element(const ufbx_scene *scene, ufbx_element_type type, const char *name) { return ufbx_find_element_len(scene, type, name, strlen(name)); } +ufbx_abi ufbx_node *ufbx_find_node(const ufbx_scene *scene, const char *name) { return ufbx_find_node_len(scene, name, strlen(name)); } +ufbx_abi ufbx_anim_stack *ufbx_find_anim_stack(const ufbx_scene *scene, const char *name) { return ufbx_find_anim_stack_len(scene, name, strlen(name)); } +ufbx_abi ufbx_material *ufbx_find_material(const ufbx_scene *scene, const char *name) { return ufbx_find_material_len(scene, name, strlen(name)); } +ufbx_abi ufbx_anim_prop *ufbx_find_anim_prop(const ufbx_anim_layer *layer, const ufbx_element *element, const char *prop) { return ufbx_find_anim_prop_len(layer, element, prop, strlen(prop)); } +ufbx_abi ufbx_prop ufbx_evaluate_prop(const ufbx_anim *anim, const ufbx_element *element, const char *name, double time) { return ufbx_evaluate_prop_len(anim, element, name, strlen(name), time); } +ufbx_abi ufbx_texture *ufbx_find_prop_texture(const ufbx_material *material, const char *name) { return ufbx_find_prop_texture_len(material, name, strlen(name)); } +ufbx_abi ufbx_string ufbx_find_shader_prop(const ufbx_shader *shader, const char *name) { return ufbx_find_shader_prop_len(shader, name, strlen(name)); } +ufbx_abi ufbx_shader_prop_binding_list ufbx_find_shader_prop_bindings(const ufbx_shader *shader, const char *name) { return ufbx_find_shader_prop_bindings_len(shader, name, strlen(name)); } +ufbx_abi ufbx_shader_texture_input *ufbx_find_shader_texture_input(const ufbx_shader_texture *shader, const char *name) { return ufbx_find_shader_texture_input_len(shader, name, strlen(name)); } +ufbx_abi ufbx_dom_node *ufbx_dom_find(const ufbx_dom_node *parent, const char *name) { return ufbx_dom_find_len(parent, name, strlen(name)); } + +// -- Catch API + +ufbx_abi uint32_t ufbx_triangulate_face(uint32_t *indices, size_t num_indices, const ufbx_mesh *mesh, ufbx_face face) { + return ufbx_catch_triangulate_face(NULL, indices, num_indices, mesh, face); +} +ufbx_abi void ufbx_compute_topology(const ufbx_mesh *mesh, ufbx_topo_edge *topo, size_t num_topo) { + ufbx_catch_compute_topology(NULL, mesh, topo, num_topo); +} +ufbx_abi uint32_t ufbx_topo_next_vertex_edge(const ufbx_topo_edge *topo, size_t num_topo, uint32_t index) { + return ufbx_catch_topo_next_vertex_edge(NULL, topo, num_topo, index); +} +ufbx_abi uint32_t ufbx_topo_prev_vertex_edge(const ufbx_topo_edge *topo, size_t num_topo, uint32_t index) { + return ufbx_catch_topo_prev_vertex_edge(NULL, topo, num_topo, index); +} +ufbx_abi ufbx_vec3 ufbx_get_weighted_face_normal(const ufbx_vertex_vec3 *positions, ufbx_face face) { + return ufbx_catch_get_weighted_face_normal(NULL, positions, face); +} + #ifdef __cplusplus } #endif #endif +#if defined(UFBX_STRING_PREFIX) + #undef strlen + #undef memcpy + #undef memmove + #undef memset + #undef memchr + #undef memcmp + #undef strcmp + #undef strncmp +#endif + #if defined(_MSC_VER) #pragma warning(pop) #elif defined(__clang__) diff --git a/thirdparty/ufbx/ufbx.h b/thirdparty/ufbx/ufbx.h index 8d856edaad09..1fc9a103655b 100644 --- a/thirdparty/ufbx/ufbx.h +++ b/thirdparty/ufbx/ufbx.h @@ -9,10 +9,11 @@ // -- Headers -#include -#include -#include -#include +#if !defined(UFBX_NO_LIBC_TYPES) + #include + #include + #include +#endif // -- Platform @@ -99,7 +100,7 @@ // make sure that it is also used within `ufbx.c`. // Defining `UFBX_NO_ASSERT` to any value disables assertions. #ifndef ufbx_assert - #if defined(UFBX_NO_ASSERT) + #if defined(UFBX_NO_ASSERT) || defined(UFBX_NO_LIBC) #define ufbx_assert(cond) (void)0 #else #include @@ -266,7 +267,7 @@ struct ufbx_converter { }; // `ufbx_source_version` contains the version of the corresponding source file. // HINT: The version can be compared numerically to the result of `ufbx_pack_version()`, // for example `#if UFBX_VERSION >= ufbx_pack_version(0, 12, 0)`. -#define UFBX_HEADER_VERSION ufbx_pack_version(0, 14, 3) +#define UFBX_HEADER_VERSION ufbx_pack_version(0, 15, 0) #define UFBX_VERSION UFBX_HEADER_VERSION // -- Basic types @@ -3984,12 +3985,17 @@ typedef size_t ufbx_read_fn(void *user, void *data, size_t size); // Skip `size` bytes in the file. typedef bool ufbx_skip_fn(void *user, size_t size); +// Get the size of the file. +// Return `0` if unknown, `UINT64_MAX` if error. +typedef uint64_t ufbx_size_fn(void *user); + // Close the file typedef void ufbx_close_fn(void *user); typedef struct ufbx_stream { ufbx_read_fn *read_fn; // < Required ufbx_skip_fn *skip_fn; // < Optional: Will use `read_fn()` if missing + ufbx_size_fn *size_fn; // < Optional ufbx_close_fn *close_fn; // < Optional // Context passed to other functions @@ -4006,13 +4012,17 @@ typedef enum ufbx_open_file_type UFBX_ENUM_REPR { UFBX_ENUM_TYPE(ufbx_open_file_type, UFBX_OPEN_FILE_TYPE, UFBX_OPEN_FILE_OBJ_MTL); +typedef uintptr_t ufbx_open_file_context; + typedef struct ufbx_open_file_info { + // Context that can be passed to the following functions to use a shared allocator: + // ufbx_open_file_ctx() + // ufbx_open_memory_ctx() + ufbx_open_file_context context; + // Kind of file to load. ufbx_open_file_type type; - // Temporary allocator to use. - ufbx_allocator temp_allocator; - // Original filename in the file, not resolved or UTF-8 encoded. // NOTE: Not necessarily NULL-terminated! ufbx_blob original_filename; @@ -4030,6 +4040,19 @@ typedef struct ufbx_open_file_cb { (stream, path, path_len, info)) } ufbx_open_file_cb; +// Options for `ufbx_open_file()`. +typedef struct ufbx_open_file_opts { + uint32_t _begin_zero; + + // Allocator to allocate the memory with. + ufbx_allocator_opts allocator; + + // The filename is guaranteed to be NULL-terminated. + ufbx_unsafe bool filename_null_terminated; + + uint32_t _end_zero; +} ufbx_open_file_opts; + // Memory stream options typedef void ufbx_close_memory_fn(void *user, void *data, size_t data_size); @@ -5092,6 +5115,7 @@ ufbx_abi_data const size_t ufbx_element_type_size[UFBX_ELEMENT_TYPE_COUNT]; // Version of the source file, comparable to `UFBX_HEADER_VERSION` ufbx_abi_data const uint32_t ufbx_source_version; + // Practically always `true` (see below), if not you need to be careful with threads. // // Guaranteed to be `true` in _any_ of the following conditions: @@ -5160,23 +5184,23 @@ ufbx_abi size_t ufbx_format_error(char *dst, size_t dst_size, const ufbx_error * // Find a property `name` from `props`, returns `NULL` if not found. // Searches through `ufbx_props.defaults` as well. ufbx_abi ufbx_prop *ufbx_find_prop_len(const ufbx_props *props, const char *name, size_t name_len); -ufbx_inline ufbx_prop *ufbx_find_prop(const ufbx_props *props, const char *name) { return ufbx_find_prop_len(props, name, strlen(name));} +ufbx_abi ufbx_prop *ufbx_find_prop(const ufbx_props *props, const char *name); // Utility functions for finding the value of a property, returns `def` if not found. // NOTE: For `ufbx_string` you need to ensure the lifetime of the default is // sufficient as no copy is made. ufbx_abi ufbx_real ufbx_find_real_len(const ufbx_props *props, const char *name, size_t name_len, ufbx_real def); -ufbx_inline ufbx_real ufbx_find_real(const ufbx_props *props, const char *name, ufbx_real def) { return ufbx_find_real_len(props, name, strlen(name), def); } +ufbx_abi ufbx_real ufbx_find_real(const ufbx_props *props, const char *name, ufbx_real def); ufbx_abi ufbx_vec3 ufbx_find_vec3_len(const ufbx_props *props, const char *name, size_t name_len, ufbx_vec3 def); -ufbx_inline ufbx_vec3 ufbx_find_vec3(const ufbx_props *props, const char *name, ufbx_vec3 def) { return ufbx_find_vec3_len(props, name, strlen(name), def); } +ufbx_abi ufbx_vec3 ufbx_find_vec3(const ufbx_props *props, const char *name, ufbx_vec3 def); ufbx_abi int64_t ufbx_find_int_len(const ufbx_props *props, const char *name, size_t name_len, int64_t def); -ufbx_inline int64_t ufbx_find_int(const ufbx_props *props, const char *name, int64_t def) { return ufbx_find_int_len(props, name, strlen(name), def); } +ufbx_abi int64_t ufbx_find_int(const ufbx_props *props, const char *name, int64_t def); ufbx_abi bool ufbx_find_bool_len(const ufbx_props *props, const char *name, size_t name_len, bool def); -ufbx_inline bool ufbx_find_bool(const ufbx_props *props, const char *name, bool def) { return ufbx_find_bool_len(props, name, strlen(name), def); } +ufbx_abi bool ufbx_find_bool(const ufbx_props *props, const char *name, bool def); ufbx_abi ufbx_string ufbx_find_string_len(const ufbx_props *props, const char *name, size_t name_len, ufbx_string def); -ufbx_inline ufbx_string ufbx_find_string(const ufbx_props *props, const char *name, ufbx_string def) { return ufbx_find_string_len(props, name, strlen(name), def); } +ufbx_abi ufbx_string ufbx_find_string(const ufbx_props *props, const char *name, ufbx_string def); ufbx_abi ufbx_blob ufbx_find_blob_len(const ufbx_props *props, const char *name, size_t name_len, ufbx_blob def); -ufbx_inline ufbx_blob ufbx_find_blob(const ufbx_props *props, const char *name, ufbx_blob def) { return ufbx_find_blob_len(props, name, strlen(name), def); } +ufbx_abi ufbx_blob ufbx_find_blob(const ufbx_props *props, const char *name, ufbx_blob def); // Find property in `props` with concatendated `parts[num_parts]`. ufbx_abi ufbx_prop *ufbx_find_prop_concat(const ufbx_props *props, const ufbx_string *parts, size_t num_parts); @@ -5186,30 +5210,30 @@ ufbx_abi ufbx_element *ufbx_get_prop_element(const ufbx_element *element, const // Find an element connected to a property by name. ufbx_abi ufbx_element *ufbx_find_prop_element_len(const ufbx_element *element, const char *name, size_t name_len, ufbx_element_type type); -ufbx_inline ufbx_element *ufbx_find_prop_element(const ufbx_element *element, const char *name, ufbx_element_type type) { return ufbx_find_prop_element_len(element, name, strlen(name), type); } +ufbx_abi ufbx_element *ufbx_find_prop_element(const ufbx_element *element, const char *name, ufbx_element_type type); // Find any element of type `type` in `scene` by `name`. // For example if you want to find `ufbx_material` named `Mat`: // (ufbx_material*)ufbx_find_element(scene, UFBX_ELEMENT_MATERIAL, "Mat"); ufbx_abi ufbx_element *ufbx_find_element_len(const ufbx_scene *scene, ufbx_element_type type, const char *name, size_t name_len); -ufbx_inline ufbx_element *ufbx_find_element(const ufbx_scene *scene, ufbx_element_type type, const char *name) { return ufbx_find_element_len(scene, type, name, strlen(name)); } +ufbx_abi ufbx_element *ufbx_find_element(const ufbx_scene *scene, ufbx_element_type type, const char *name); // Find node in `scene` by `name` (shorthand for `ufbx_find_element(UFBX_ELEMENT_NODE)`). ufbx_abi ufbx_node *ufbx_find_node_len(const ufbx_scene *scene, const char *name, size_t name_len); -ufbx_inline ufbx_node *ufbx_find_node(const ufbx_scene *scene, const char *name) { return ufbx_find_node_len(scene, name, strlen(name)); } +ufbx_abi ufbx_node *ufbx_find_node(const ufbx_scene *scene, const char *name); // Find an animation stack in `scene` by `name` (shorthand for `ufbx_find_element(UFBX_ELEMENT_ANIM_STACK)`) ufbx_abi ufbx_anim_stack *ufbx_find_anim_stack_len(const ufbx_scene *scene, const char *name, size_t name_len); -ufbx_inline ufbx_anim_stack *ufbx_find_anim_stack(const ufbx_scene *scene, const char *name) { return ufbx_find_anim_stack_len(scene, name, strlen(name)); } +ufbx_abi ufbx_anim_stack *ufbx_find_anim_stack(const ufbx_scene *scene, const char *name); // Find a material in `scene` by `name` (shorthand for `ufbx_find_element(UFBX_ELEMENT_MATERIAL)`). ufbx_abi ufbx_material *ufbx_find_material_len(const ufbx_scene *scene, const char *name, size_t name_len); -ufbx_inline ufbx_material *ufbx_find_material(const ufbx_scene *scene, const char *name) { return ufbx_find_material_len(scene, name, strlen(name)); } +ufbx_abi ufbx_material *ufbx_find_material(const ufbx_scene *scene, const char *name); // Find a single animated property `prop` of `element` in `layer`. // Returns `NULL` if not found. ufbx_abi ufbx_anim_prop *ufbx_find_anim_prop_len(const ufbx_anim_layer *layer, const ufbx_element *element, const char *prop, size_t prop_len); -ufbx_inline ufbx_anim_prop *ufbx_find_anim_prop(const ufbx_anim_layer *layer, const ufbx_element *element, const char *prop) { return ufbx_find_anim_prop_len(layer, element, prop, strlen(prop)); } +ufbx_abi ufbx_anim_prop *ufbx_find_anim_prop(const ufbx_anim_layer *layer, const ufbx_element *element, const char *prop); // Find all animated properties of `element` in `layer`. ufbx_abi ufbx_anim_prop_list ufbx_find_anim_props(const ufbx_anim_layer *layer, const ufbx_element *element); @@ -5228,16 +5252,18 @@ ufbx_abi ufbx_matrix ufbx_get_compatible_matrix_for_normals(const ufbx_node *nod // but the rest can be uninitialized. ufbx_abi ptrdiff_t ufbx_inflate(void *dst, size_t dst_size, const ufbx_inflate_input *input, ufbx_inflate_retain *retain); -// Open a `ufbx_stream` from a file. -// Use `path_len == SIZE_MAX` for NULL terminated string. -ufbx_abi bool ufbx_open_file(ufbx_stream *stream, const char *path, size_t path_len); - // Same as `ufbx_open_file()` but compatible with the callback in `ufbx_open_file_fn`. // The `user` parameter is actually not used here. ufbx_abi bool ufbx_default_open_file(void *user, ufbx_stream *stream, const char *path, size_t path_len, const ufbx_open_file_info *info); +// Open a `ufbx_stream` from a file. +// Use `path_len == SIZE_MAX` for NULL terminated string. +ufbx_abi bool ufbx_open_file(ufbx_stream *stream, const char *path, size_t path_len, const ufbx_open_file_opts *opts, ufbx_error *error); +ufbx_unsafe ufbx_abi bool ufbx_open_file_ctx(ufbx_stream *stream, ufbx_open_file_context ctx, const char *path, size_t path_len, const ufbx_open_file_opts *opts, ufbx_error *error); + // NOTE: Uses the default ufbx allocator! ufbx_abi bool ufbx_open_memory(ufbx_stream *stream, const void *data, size_t data_size, const ufbx_open_memory_opts *opts, ufbx_error *error); +ufbx_unsafe ufbx_abi bool ufbx_open_memory_ctx(ufbx_stream *stream, ufbx_open_file_context ctx, const void *data, size_t data_size, const ufbx_open_memory_opts *opts, ufbx_error *error); // Animation evaluation @@ -5252,9 +5278,7 @@ ufbx_abi ufbx_vec3 ufbx_evaluate_anim_value_vec3(const ufbx_anim_value *anim_val // Evaluate an animated property `name` from `element` at `time`. // NOTE: If the property is not found it will have the flag `UFBX_PROP_FLAG_NOT_FOUND`. ufbx_abi ufbx_prop ufbx_evaluate_prop_len(const ufbx_anim *anim, const ufbx_element *element, const char *name, size_t name_len, double time); -ufbx_inline ufbx_prop ufbx_evaluate_prop(const ufbx_anim *anim, const ufbx_element *element, const char *name, double time) { - return ufbx_evaluate_prop_len(anim, element, name, strlen(name), time); -} +ufbx_abi ufbx_prop ufbx_evaluate_prop(const ufbx_anim *anim, const ufbx_element *element, const char *name, double time); // Evaluate all _animated_ properties of `element`. // HINT: This function returns an `ufbx_props` structure with the original properties as @@ -5351,27 +5375,19 @@ ufbx_abi ufbx_bone_pose *ufbx_get_bone_pose(const ufbx_pose *pose, const ufbx_no // Find a texture for a given material FBX property. ufbx_abi ufbx_texture *ufbx_find_prop_texture_len(const ufbx_material *material, const char *name, size_t name_len); -ufbx_inline ufbx_texture *ufbx_find_prop_texture(const ufbx_material *material, const char *name) { - return ufbx_find_prop_texture_len(material, name, strlen(name)); -} +ufbx_abi ufbx_texture *ufbx_find_prop_texture(const ufbx_material *material, const char *name); // Find a texture for a given shader property. ufbx_abi ufbx_string ufbx_find_shader_prop_len(const ufbx_shader *shader, const char *name, size_t name_len); -ufbx_inline ufbx_string ufbx_find_shader_prop(const ufbx_shader *shader, const char *name) { - return ufbx_find_shader_prop_len(shader, name, strlen(name)); -} +ufbx_abi ufbx_string ufbx_find_shader_prop(const ufbx_shader *shader, const char *name); // Map from a shader property to material property. ufbx_abi ufbx_shader_prop_binding_list ufbx_find_shader_prop_bindings_len(const ufbx_shader *shader, const char *name, size_t name_len); -ufbx_inline ufbx_shader_prop_binding_list ufbx_find_shader_prop_bindings(const ufbx_shader *shader, const char *name) { - return ufbx_find_shader_prop_bindings_len(shader, name, strlen(name)); -} +ufbx_abi ufbx_shader_prop_binding_list ufbx_find_shader_prop_bindings(const ufbx_shader *shader, const char *name); // Find an input in a shader texture. ufbx_abi ufbx_shader_texture_input *ufbx_find_shader_texture_input_len(const ufbx_shader_texture *shader, const char *name, size_t name_len); -ufbx_inline ufbx_shader_texture_input *ufbx_find_shader_texture_input(const ufbx_shader_texture *shader, const char *name) { - return ufbx_find_shader_texture_input_len(shader, name, strlen(name)); -} +ufbx_abi ufbx_shader_texture_input *ufbx_find_shader_texture_input(const ufbx_shader_texture *shader, const char *name); // Math @@ -5471,37 +5487,27 @@ ufbx_abi uint32_t ufbx_find_face_index(ufbx_mesh *mesh, size_t index); // NOTE: You need to space for `(face.num_indices - 2) * 3 - 1` indices! // HINT: Using `ufbx_mesh.max_face_triangles * 3` is always safe. ufbx_abi uint32_t ufbx_catch_triangulate_face(ufbx_panic *panic, uint32_t *indices, size_t num_indices, const ufbx_mesh *mesh, ufbx_face face); -ufbx_inline uint32_t ufbx_triangulate_face(uint32_t *indices, size_t num_indices, const ufbx_mesh *mesh, ufbx_face face) { - return ufbx_catch_triangulate_face(NULL, indices, num_indices, mesh, face); -} +ufbx_abi uint32_t ufbx_triangulate_face(uint32_t *indices, size_t num_indices, const ufbx_mesh *mesh, ufbx_face face); // Generate the half-edge representation of `mesh` to `topo[mesh->num_indices]` ufbx_abi void ufbx_catch_compute_topology(ufbx_panic *panic, const ufbx_mesh *mesh, ufbx_topo_edge *topo, size_t num_topo); -ufbx_inline void ufbx_compute_topology(const ufbx_mesh *mesh, ufbx_topo_edge *topo, size_t num_topo) { - ufbx_catch_compute_topology(NULL, mesh, topo, num_topo); -} +ufbx_abi void ufbx_compute_topology(const ufbx_mesh *mesh, ufbx_topo_edge *topo, size_t num_topo); // Get the next/previous edge around a vertex // NOTE: Does not return the half-edge on the opposite side (ie. `topo[index].twin`) // Get the next half-edge in `topo`. ufbx_abi uint32_t ufbx_catch_topo_next_vertex_edge(ufbx_panic *panic, const ufbx_topo_edge *topo, size_t num_topo, uint32_t index); -ufbx_inline uint32_t ufbx_topo_next_vertex_edge(const ufbx_topo_edge *topo, size_t num_topo, uint32_t index) { - return ufbx_catch_topo_next_vertex_edge(NULL, topo, num_topo, index); -} +ufbx_abi uint32_t ufbx_topo_next_vertex_edge(const ufbx_topo_edge *topo, size_t num_topo, uint32_t index); // Get the previous half-edge in `topo`. ufbx_abi uint32_t ufbx_catch_topo_prev_vertex_edge(ufbx_panic *panic, const ufbx_topo_edge *topo, size_t num_topo, uint32_t index); -ufbx_inline uint32_t ufbx_topo_prev_vertex_edge(const ufbx_topo_edge *topo, size_t num_topo, uint32_t index) { - return ufbx_catch_topo_prev_vertex_edge(NULL, topo, num_topo, index); -} +ufbx_abi uint32_t ufbx_topo_prev_vertex_edge(const ufbx_topo_edge *topo, size_t num_topo, uint32_t index); // Calculate a normal for a given face. // The returned normal is weighted by face area. ufbx_abi ufbx_vec3 ufbx_catch_get_weighted_face_normal(ufbx_panic *panic, const ufbx_vertex_vec3 *positions, ufbx_face face); -ufbx_inline ufbx_vec3 ufbx_get_weighted_face_normal(const ufbx_vertex_vec3 *positions, ufbx_face face) { - return ufbx_catch_get_weighted_face_normal(NULL, positions, face); -} +ufbx_abi ufbx_vec3 ufbx_get_weighted_face_normal(const ufbx_vertex_vec3 *positions, ufbx_face face); // Generate indices for normals from the topology. // Respects smoothing groups. @@ -5558,7 +5564,7 @@ ufbx_abi size_t ufbx_sample_geometry_cache_vec3(const ufbx_cache_channel *channe // Find a DOM node given a name. ufbx_abi ufbx_dom_node *ufbx_dom_find_len(const ufbx_dom_node *parent, const char *name, size_t name_len); -ufbx_inline ufbx_dom_node *ufbx_dom_find(const ufbx_dom_node *parent, const char *name) { return ufbx_dom_find_len(parent, name, strlen(name)); } +ufbx_abi ufbx_dom_node *ufbx_dom_find(const ufbx_dom_node *parent, const char *name); // Utility diff --git a/thirdparty/vulkan/patches/0003-VMA-add-vmaCalculateLazilyAllocatedBytes.patch b/thirdparty/vulkan/patches/0003-VMA-add-vmaCalculateLazilyAllocatedBytes.patch new file mode 100644 index 000000000000..4c0d90935223 --- /dev/null +++ b/thirdparty/vulkan/patches/0003-VMA-add-vmaCalculateLazilyAllocatedBytes.patch @@ -0,0 +1,55 @@ +diff --git a/thirdparty/vulkan/vk_mem_alloc.h b/thirdparty/vulkan/vk_mem_alloc.h +index ecb84094b9..50ff4ea1c2 100644 +--- a/thirdparty/vulkan/vk_mem_alloc.h ++++ b/thirdparty/vulkan/vk_mem_alloc.h +@@ -1713,6 +1713,21 @@ VMA_CALL_PRE void VMA_CALL_POST vmaCalculateStatistics( + VmaAllocator VMA_NOT_NULL allocator, + VmaTotalStatistics* VMA_NOT_NULL pStats); + ++// -- GODOT begin -- ++/** \brief Retrieves lazily allocated bytes ++ ++This function is called "calculate" not "get" because it has to traverse all ++internal data structures, so it may be quite slow. Use it for debugging purposes. ++For faster but more brief statistics suitable to be called every frame or every allocation, ++use vmaGetHeapBudgets(). ++ ++Note that when using allocator from multiple threads, returned information may immediately ++become outdated. ++*/ ++VMA_CALL_PRE uint64_t VMA_CALL_POST vmaCalculateLazilyAllocatedBytes( ++ VmaAllocator VMA_NOT_NULL allocator); ++// -- GODOT end -- ++ + /** \brief Retrieves information about current memory usage and budget for all memory heaps. + + \param allocator +@@ -14912,6 +14927,28 @@ VMA_CALL_PRE void VMA_CALL_POST vmaCalculateStatistics( + allocator->CalculateStatistics(pStats); + } + ++// -- GODOT begin -- ++VMA_CALL_PRE uint64_t VMA_CALL_POST vmaCalculateLazilyAllocatedBytes( ++ VmaAllocator allocator) ++{ ++ VMA_ASSERT(allocator); ++ VMA_DEBUG_GLOBAL_MUTEX_LOCK ++ VmaTotalStatistics stats; ++ allocator->CalculateStatistics(&stats); ++ uint64_t total_lazilily_allocated_bytes = 0; ++ for (uint32_t heapIndex = 0; heapIndex < allocator->GetMemoryHeapCount(); ++heapIndex) { ++ for (uint32_t typeIndex = 0; typeIndex < allocator->GetMemoryTypeCount(); ++typeIndex) { ++ if (allocator->MemoryTypeIndexToHeapIndex(typeIndex) == heapIndex) { ++ VkMemoryPropertyFlags flags = allocator->m_MemProps.memoryTypes[typeIndex].propertyFlags; ++ if (flags & VK_MEMORY_PROPERTY_LAZILY_ALLOCATED_BIT) ++ total_lazilily_allocated_bytes += stats.memoryType[typeIndex].statistics.allocationBytes; ++ } ++ } ++ } ++ return total_lazilily_allocated_bytes; ++} ++// -- GODOT end -- ++ + VMA_CALL_PRE void VMA_CALL_POST vmaGetHeapBudgets( + VmaAllocator allocator, + VmaBudget* pBudgets) diff --git a/thirdparty/vulkan/vk_mem_alloc.h b/thirdparty/vulkan/vk_mem_alloc.h index ecb84094b957..50ff4ea1c234 100644 --- a/thirdparty/vulkan/vk_mem_alloc.h +++ b/thirdparty/vulkan/vk_mem_alloc.h @@ -1713,6 +1713,21 @@ VMA_CALL_PRE void VMA_CALL_POST vmaCalculateStatistics( VmaAllocator VMA_NOT_NULL allocator, VmaTotalStatistics* VMA_NOT_NULL pStats); +// -- GODOT begin -- +/** \brief Retrieves lazily allocated bytes + +This function is called "calculate" not "get" because it has to traverse all +internal data structures, so it may be quite slow. Use it for debugging purposes. +For faster but more brief statistics suitable to be called every frame or every allocation, +use vmaGetHeapBudgets(). + +Note that when using allocator from multiple threads, returned information may immediately +become outdated. +*/ +VMA_CALL_PRE uint64_t VMA_CALL_POST vmaCalculateLazilyAllocatedBytes( + VmaAllocator VMA_NOT_NULL allocator); +// -- GODOT end -- + /** \brief Retrieves information about current memory usage and budget for all memory heaps. \param allocator @@ -14912,6 +14927,28 @@ VMA_CALL_PRE void VMA_CALL_POST vmaCalculateStatistics( allocator->CalculateStatistics(pStats); } +// -- GODOT begin -- +VMA_CALL_PRE uint64_t VMA_CALL_POST vmaCalculateLazilyAllocatedBytes( + VmaAllocator allocator) +{ + VMA_ASSERT(allocator); + VMA_DEBUG_GLOBAL_MUTEX_LOCK + VmaTotalStatistics stats; + allocator->CalculateStatistics(&stats); + uint64_t total_lazilily_allocated_bytes = 0; + for (uint32_t heapIndex = 0; heapIndex < allocator->GetMemoryHeapCount(); ++heapIndex) { + for (uint32_t typeIndex = 0; typeIndex < allocator->GetMemoryTypeCount(); ++typeIndex) { + if (allocator->MemoryTypeIndexToHeapIndex(typeIndex) == heapIndex) { + VkMemoryPropertyFlags flags = allocator->m_MemProps.memoryTypes[typeIndex].propertyFlags; + if (flags & VK_MEMORY_PROPERTY_LAZILY_ALLOCATED_BIT) + total_lazilily_allocated_bytes += stats.memoryType[typeIndex].statistics.allocationBytes; + } + } + } + return total_lazilily_allocated_bytes; +} +// -- GODOT end -- + VMA_CALL_PRE void VMA_CALL_POST vmaGetHeapBudgets( VmaAllocator allocator, VmaBudget* pBudgets) diff --git a/thirdparty/wayland-protocols/staging/xdg-system-bell/README b/thirdparty/wayland-protocols/staging/xdg-system-bell/README new file mode 100644 index 000000000000..a9276e16b369 --- /dev/null +++ b/thirdparty/wayland-protocols/staging/xdg-system-bell/README @@ -0,0 +1,5 @@ +system_bell protocol + +Maintainers: +Jonas Ådahl +Carlos Garnacho diff --git a/thirdparty/wayland-protocols/staging/xdg-system-bell/xdg-system-bell-v1.xml b/thirdparty/wayland-protocols/staging/xdg-system-bell/xdg-system-bell-v1.xml new file mode 100644 index 000000000000..f00508de8503 --- /dev/null +++ b/thirdparty/wayland-protocols/staging/xdg-system-bell/xdg-system-bell-v1.xml @@ -0,0 +1,58 @@ + + + + Copyright © 2016, 2023 Red Hat + + 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 (including the next + paragraph) 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. + + + + + This global interface enables clients to ring the system bell. + + Warning! The protocol described in this file is currently in the testing + phase. Backward compatible changes may be added together with the + corresponding interface version bump. Backward incompatible changes can + only be done by creating a new major version of the extension. + + + + + Notify that the object will no longer be used. + + + + + + This requests rings the system bell on behalf of a client. How ringing + the bell is implemented is up to the compositor. It may be an audible + sound, a visual feedback of some kind, or any other thing including + nothing. + + The passed surface should correspond to a toplevel like surface role, + or be null, meaning the client doesn't have a particular toplevel it + wants to associate the bell ringing with. See the xdg-shell protocol + extension for a toplevel like surface role. + + + + + diff --git a/thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml b/thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml new file mode 100644 index 000000000000..cc3271dca4d6 --- /dev/null +++ b/thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml @@ -0,0 +1,200 @@ + + + + + Copyright © 2015-2016 Red Hat Inc. + + 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 (including the next + paragraph) 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. + + + + This protocol specifies a way for making it possible to reference a surface + of a different client. With such a reference, a client can, by using the + interfaces provided by this protocol, manipulate the relationship between + its own surfaces and the surface of some other client. For example, stack + some of its own surface above the other clients surface. + + In order for a client A to get a reference of a surface of client B, client + B must first export its surface using xdg_exporter.export_toplevel. Upon + doing this, client B will receive a handle (a unique string) that it may + share with client A in some way (for example D-Bus). After client A has + received the handle from client B, it may use xdg_importer.import_toplevel + to create a reference to the surface client B just exported. See the + corresponding requests for details. + + A possible use case for this is out-of-process dialogs. For example when a + sandboxed client without file system access needs the user to select a file + on the file system, given sandbox environment support, it can export its + surface, passing the exported surface handle to an unsandboxed process that + can show a file browser dialog and stack it above the sandboxed client's + surface. + + Warning! The protocol described in this file is experimental and backward + incompatible changes may be made. Backward compatible changes may be added + together with the corresponding interface version bump. Backward + incompatible changes are done by bumping the version number in the protocol + and interface names and resetting the interface version. Once the protocol + is to be declared stable, the 'z' prefix and the version number in the + protocol and interface names are removed and the interface version number is + reset. + + + + + A global interface used for exporting surfaces that can later be imported + using xdg_importer. + + + + + Notify the compositor that the xdg_exporter object will no longer be + used. + + + + + + These errors can be emitted in response to invalid xdg_exporter + requests. + + + + + + + The export_toplevel request exports the passed surface so that it can later be + imported via xdg_importer. When called, a new xdg_exported object will + be created and xdg_exported.handle will be sent immediately. See the + corresponding interface and event for details. + + A surface may be exported multiple times, and each exported handle may + be used to create an xdg_imported multiple times. Only xdg_toplevel + equivalent surfaces may be exported, otherwise an invalid_surface + protocol error is sent. + + + + + + + + + A global interface used for importing surfaces exported by xdg_exporter. + With this interface, a client can create a reference to a surface of + another client. + + + + + Notify the compositor that the xdg_importer object will no longer be + used. + + + + + + The import_toplevel request imports a surface from any client given a handle + retrieved by exporting said surface using xdg_exporter.export_toplevel. + When called, a new xdg_imported object will be created. This new object + represents the imported surface, and the importing client can + manipulate its relationship using it. See xdg_imported for details. + + + + + + + + + An xdg_exported object represents an exported reference to a surface. The + exported surface may be referenced as long as the xdg_exported object not + destroyed. Destroying the xdg_exported invalidates any relationship the + importer may have established using xdg_imported. + + + + + Revoke the previously exported surface. This invalidates any + relationship the importer may have set up using the xdg_imported created + given the handle sent via xdg_exported.handle. + + + + + + The handle event contains the unique handle of this exported surface + reference. It may be shared with any client, which then can use it to + import the surface by calling xdg_importer.import_toplevel. A handle + may be used to import the surface multiple times. + + + + + + + + An xdg_imported object represents an imported reference to surface exported + by some client. A client can use this interface to manipulate + relationships between its own surfaces and the imported surface. + + + + + These errors can be emitted in response to invalid xdg_imported + requests. + + + + + + + Notify the compositor that it will no longer use the xdg_imported + object. Any relationship that may have been set up will at this point + be invalidated. + + + + + + Set the imported surface as the parent of some surface of the client. + The passed surface must be an xdg_toplevel equivalent, otherwise an + invalid_surface protocol error is sent. Calling this function sets up + a surface to surface relation with the same stacking and positioning + semantics as xdg_toplevel.set_parent. + + + + + + + The imported surface handle has been destroyed and any relationship set + up has been invalidated. This may happen for various reasons, for + example if the exported surface or the exported surface handle has been + destroyed, if the handle used for importing was invalid. + + + + +