From 998f97a2a13d294a00ec3ee95ed8bf2279e4cf5c Mon Sep 17 00:00:00 2001 From: Mikael Hermansson Date: Wed, 4 Feb 2026 22:43:01 +0100 Subject: [PATCH] Jolt: Update to 5.5.0 --- thirdparty/README.md | 9 +- .../Jolt/AABBTree/AABBTreeBuilder.h | 5 +- .../jolt_physics/Jolt/ConfigurationString.h | 4 +- thirdparty/jolt_physics/Jolt/Core/Array.h | 6 +- thirdparty/jolt_physics/Jolt/Core/Color.h | 14 ++ thirdparty/jolt_physics/Jolt/Core/Core.h | 42 ++-- thirdparty/jolt_physics/Jolt/Core/HashTable.h | 2 +- .../Jolt/Core/JobSystemThreadPool.cpp | 2 +- thirdparty/jolt_physics/Jolt/Core/Memory.h | 19 +- thirdparty/jolt_physics/Jolt/Core/Profiler.h | 6 +- .../jolt_physics/Jolt/Core/STLAllocator.h | 2 +- .../jolt_physics/Jolt/Core/Semaphore.cpp | 3 +- .../jolt_physics/Jolt/Core/TempAllocator.h | 33 ++- .../jolt_physics/Jolt/Core/TickCounter.cpp | 3 +- .../jolt_physics/Jolt/Core/TickCounter.h | 2 +- .../Jolt/Geometry/EPAPenetrationDepth.h | 2 +- thirdparty/jolt_physics/Jolt/Math/Vec3.inl | 8 +- thirdparty/jolt_physics/Jolt/Math/Vec4.inl | 8 +- .../jolt_physics/Jolt/Physics/Body/Body.h | 4 +- .../Jolt/Physics/Body/BodyCreationSettings.h | 4 +- .../Jolt/Physics/Body/BodyInterface.cpp | 2 +- .../Jolt/Physics/Body/BodyInterface.h | 82 +++--- .../Jolt/Physics/Body/BodyManager.cpp | 61 ++++- .../Jolt/Physics/Body/BodyManager.h | 23 ++ .../Jolt/Physics/Body/MotionProperties.h | 34 ++- .../Jolt/Physics/Character/CharacterVirtual.h | 1 + .../Physics/Collision/BroadPhase/BroadPhase.h | 3 - .../Collision/BroadPhase/BroadPhaseQuery.h | 3 + .../Physics/Collision/BroadPhase/QuadTree.cpp | 9 + .../Collision/CastSphereVsTriangles.cpp | 4 +- .../Collision/CollideShapeVsShapePerLeaf.h | 1 + .../CollideSoftBodyVerticesVsTriangles.h | 30 ++- .../Jolt/Physics/Collision/Shape/BoxShape.cpp | 12 +- .../Jolt/Physics/Collision/Shape/BoxShape.h | 4 +- .../Physics/Collision/Shape/CompoundShape.h | 2 +- .../Physics/Collision/Shape/ConvexHullShape.h | 2 +- .../Physics/Collision/Shape/CylinderShape.cpp | 12 +- .../Physics/Collision/Shape/EmptyShape.cpp | 3 +- .../Collision/Shape/HeightFieldShape.cpp | 6 +- .../Physics/Collision/Shape/MeshShape.cpp | 6 +- .../Jolt/Physics/Collision/Shape/MeshShape.h | 2 +- .../Collision/Shape/MutableCompoundShape.cpp | 1 - .../Jolt/Physics/Collision/Shape/PlaneShape.h | 4 +- .../Physics/Collision/Shape/SphereShape.h | 4 +- .../Collision/Shape/TaperedCylinderShape.cpp | 24 +- .../Jolt/Physics/Collision/TransformedShape.h | 2 +- .../Constraints/CalculateSolverSteps.h | 6 +- .../Jolt/Physics/Constraints/Constraint.h | 2 +- .../Physics/Constraints/ConstraintManager.h | 3 + .../Constraints/ContactConstraintManager.cpp | 16 ++ .../Constraints/ContactConstraintManager.h | 3 + .../Jolt/Physics/IslandBuilder.cpp | 10 + .../jolt_physics/Jolt/Physics/IslandBuilder.h | 21 +- .../Jolt/Physics/LargeIslandSplitter.h | 2 + .../Jolt/Physics/PhysicsSystem.cpp | 206 ++++++++++++--- .../jolt_physics/Jolt/Physics/PhysicsSystem.h | 13 +- .../Jolt/Physics/PhysicsUpdateContext.cpp | 2 + .../Jolt/Physics/PhysicsUpdateContext.h | 6 +- .../Jolt/Physics/Ragdoll/Ragdoll.cpp | 32 +++ .../Jolt/Physics/Ragdoll/Ragdoll.h | 5 + .../SoftBody/SoftBodyCreationSettings.h | 2 +- .../Physics/SoftBody/SoftBodySharedSettings.h | 2 +- .../Physics/SoftBody/SoftBodyUpdateContext.h | 4 + .../Physics/Vehicle/VehicleCollisionTester.h | 4 +- .../Jolt/Physics/Vehicle/VehicleEngine.h | 2 +- .../Vehicle/WheeledVehicleController.h | 2 +- ...1-backport-upstream-commit-ee3725250.patch | 53 ++++ ...2-backport-upstream-commit-bc7f1fb8c.patch | 236 ++++++++++++++++++ ...3-backport-upstream-commit-365a15367.patch | 28 +++ ...4-backport-upstream-commit-e0a6a9a16.patch | 61 +++++ 70 files changed, 1041 insertions(+), 195 deletions(-) create mode 100644 thirdparty/jolt_physics/patches/0001-backport-upstream-commit-ee3725250.patch create mode 100644 thirdparty/jolt_physics/patches/0002-backport-upstream-commit-bc7f1fb8c.patch create mode 100644 thirdparty/jolt_physics/patches/0003-backport-upstream-commit-365a15367.patch create mode 100644 thirdparty/jolt_physics/patches/0004-backport-upstream-commit-e0a6a9a16.patch diff --git a/thirdparty/README.md b/thirdparty/README.md index 684ea72edc..34bfc93851 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -510,7 +510,7 @@ Files generated from upstream source: ## jolt_physics - Upstream: https://github.com/jrouwe/JoltPhysics -- Version: 5.4.0 (036ea7b1d717b3e713ac9d8cbd47118fb9cd5d60, 2025) +- Version: 5.5.0 (23dadd0e603f1b321142d4c74df07fce85064989, 2025) - License: MIT Files extracted from upstream source: @@ -518,6 +518,13 @@ Files extracted from upstream source: - All files in `Jolt/`, except `Jolt/Jolt.cmake` and any files dependent on `ENABLE_OBJECT_STREAM`, as seen in `Jolt/Jolt.cmake` - `LICENSE` +Patches: + +- `0001-backport-upstream-commit-ee3725250.patch` (GH-115089) +- `0002-backport-upstream-commit-bc7f1fb8c.patch` (GH-115305) +- `0003-backport-upstream-commit-365a15367.patch` (GH-115305) +- `0004-backport-upstream-commit-e0a6a9a16.patch` (GH-115327) + ## libbacktrace diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h index 3b0635c11a..f272078681 100644 --- a/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h +++ b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h @@ -50,6 +50,9 @@ public: /// Check if this node has any children inline bool HasChildren() const { return mChild[0] != cInvalidNodeIndex || mChild[1] != cInvalidNodeIndex; } + /// Get child node + inline const Node * GetChild(uint inIdx, const Array &inNodes) const { return mChild[inIdx] != cInvalidNodeIndex? &inNodes[mChild[inIdx]] : nullptr; } + /// Min depth of tree uint GetMinDepth(const Array &inNodes) const; @@ -95,7 +98,7 @@ public: }; /// Constructor - AABBTreeBuilder(TriangleSplitter &inSplitter, uint inMaxTrianglesPerLeaf = 16); + explicit AABBTreeBuilder(TriangleSplitter &inSplitter, uint inMaxTrianglesPerLeaf = 16); /// Recursively build tree, returns the root node of the tree Node * Build(AABBTreeBuilderStats &outStats); diff --git a/thirdparty/jolt_physics/Jolt/ConfigurationString.h b/thirdparty/jolt_physics/Jolt/ConfigurationString.h index ef502e0db8..566f13a32e 100644 --- a/thirdparty/jolt_physics/Jolt/ConfigurationString.h +++ b/thirdparty/jolt_physics/Jolt/ConfigurationString.h @@ -32,9 +32,9 @@ inline const char *GetConfigurationString() #else #error Unknown CPU architecture #endif -#if JPH_CPU_ADDRESS_BITS == 64 +#if JPH_CPU_ARCH_BITS == 64 "64-bit " -#elif JPH_CPU_ADDRESS_BITS == 32 +#elif JPH_CPU_ARCH_BITS == 32 "32-bit " #endif "with instructions: " diff --git a/thirdparty/jolt_physics/Jolt/Core/Array.h b/thirdparty/jolt_physics/Jolt/Core/Array.h index 5f5f4e2c0b..c4a060ab5d 100644 --- a/thirdparty/jolt_physics/Jolt/Core/Array.h +++ b/thirdparty/jolt_physics/Jolt/Core/Array.h @@ -238,7 +238,7 @@ private: } /// Free memory - inline void free() + inline void deallocate() { get_allocator().deallocate(mElements, mCapacity); mElements = nullptr; @@ -251,7 +251,7 @@ private: if (mElements != nullptr) { clear(); - free(); + deallocate(); } } @@ -411,7 +411,7 @@ public: if (mElements != nullptr) { if (mSize == 0) - free(); + deallocate(); else if (mCapacity > mSize) reallocate(mSize); } diff --git a/thirdparty/jolt_physics/Jolt/Core/Color.h b/thirdparty/jolt_physics/Jolt/Core/Color.h index 7706ca85b5..4995701bc2 100644 --- a/thirdparty/jolt_physics/Jolt/Core/Color.h +++ b/thirdparty/jolt_physics/Jolt/Core/Color.h @@ -49,6 +49,20 @@ public: /// Get a visually distinct color static Color sGetDistinctColor(int inIndex); + /// Get a color value on the gradient from green through yellow to red + /// @param inValue Value in the range [0, 1], 0 = green, 0.5 = yellow, 1 = red + static Color sGreenRedGradient(float inValue) + { + if (inValue < 0.0f) + return Color::sGreen; + else if (inValue < 0.5f) + return Color(uint8(510.0f * inValue), 255, 0); + else if (inValue < 1.0f) + return Color(255, uint8(510.0f * (1.0f - inValue)), 0); + else + return Color::sRed; + } + /// Predefined colors static const Color sBlack; static const Color sDarkRed; diff --git a/thirdparty/jolt_physics/Jolt/Core/Core.h b/thirdparty/jolt_physics/Jolt/Core/Core.h index b433a77946..687c1fd95c 100644 --- a/thirdparty/jolt_physics/Jolt/Core/Core.h +++ b/thirdparty/jolt_physics/Jolt/Core/Core.h @@ -6,7 +6,7 @@ // Jolt library version #define JPH_VERSION_MAJOR 5 -#define JPH_VERSION_MINOR 4 +#define JPH_VERSION_MINOR 5 #define JPH_VERSION_PATCH 0 // Determine which features the library was compiled with @@ -121,9 +121,9 @@ // X86 CPU architecture #define JPH_CPU_X86 #if defined(__x86_64__) || defined(_M_X64) - #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_CPU_ARCH_BITS 64 #else - #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_CPU_ARCH_BITS 32 #endif #define JPH_USE_SSE #define JPH_VECTOR_ALIGNMENT 16 @@ -171,12 +171,12 @@ // ARM CPU architecture #define JPH_CPU_ARM #if defined(__aarch64__) || defined(_M_ARM64) - #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_CPU_ARCH_BITS 64 #define JPH_USE_NEON #define JPH_VECTOR_ALIGNMENT 16 #define JPH_DVECTOR_ALIGNMENT 32 #else - #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_CPU_ARCH_BITS 32 #define JPH_VECTOR_ALIGNMENT 8 // 32-bit ARM does not support aligning on the stack on 16 byte boundaries #define JPH_DVECTOR_ALIGNMENT 8 #endif @@ -184,11 +184,11 @@ // RISC-V CPU architecture #define JPH_CPU_RISCV #if __riscv_xlen == 64 - #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_CPU_ARCH_BITS 64 #define JPH_VECTOR_ALIGNMENT 16 #define JPH_DVECTOR_ALIGNMENT 32 #else - #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_CPU_ARCH_BITS 32 #define JPH_VECTOR_ALIGNMENT 16 #define JPH_DVECTOR_ALIGNMENT 8 #endif @@ -196,9 +196,9 @@ // WebAssembly CPU architecture #define JPH_CPU_WASM #if defined(__wasm64__) - #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_CPU_ARCH_BITS 64 #else - #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_CPU_ARCH_BITS 32 #endif #define JPH_VECTOR_ALIGNMENT 16 #define JPH_DVECTOR_ALIGNMENT 32 @@ -211,9 +211,9 @@ // PowerPC CPU architecture #define JPH_CPU_PPC #if defined(__powerpc64__) - #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_CPU_ARCH_BITS 64 #else - #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_CPU_ARCH_BITS 32 #endif #ifdef _BIG_ENDIAN #define JPH_CPU_BIG_ENDIAN @@ -224,16 +224,16 @@ // LoongArch CPU architecture #define JPH_CPU_LOONGARCH #if defined(__loongarch64) - #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_CPU_ARCH_BITS 64 #else - #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_CPU_ARCH_BITS 32 #endif #define JPH_VECTOR_ALIGNMENT 16 #define JPH_DVECTOR_ALIGNMENT 8 #elif defined(__e2k__) // E2K CPU architecture (MCST Elbrus 2000) #define JPH_CPU_E2K - #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_CPU_ARCH_BITS 64 #define JPH_VECTOR_ALIGNMENT 16 #define JPH_DVECTOR_ALIGNMENT 32 @@ -321,9 +321,15 @@ #else #define JPH_MSVC2019_SUPPRESS_WARNING(w) #endif +#if _MSC_VER >= 1950 +#define JPH_MSVC2026_PLUS_SUPPRESS_WARNING(w) JPH_MSVC_SUPPRESS_WARNING(w) +#else +#define JPH_MSVC2026_PLUS_SUPPRESS_WARNING(w) +#endif #else #define JPH_MSVC_SUPPRESS_WARNING(w) #define JPH_MSVC2019_SUPPRESS_WARNING(w) +#define JPH_MSVC2026_PLUS_SUPPRESS_WARNING(w) #endif // Disable common warnings triggered by Jolt when compiling with -Wall @@ -447,6 +453,7 @@ JPH_SUPPRESS_WARNINGS_STD_BEGIN #include #include #include +#include #include #include #include @@ -502,7 +509,6 @@ static_assert(sizeof(uint8) == 1, "Invalid size of uint8"); static_assert(sizeof(uint16) == 2, "Invalid size of uint16"); static_assert(sizeof(uint32) == 4, "Invalid size of uint32"); static_assert(sizeof(uint64) == 8, "Invalid size of uint64"); -static_assert(sizeof(void *) == (JPH_CPU_ADDRESS_BITS == 64? 8 : 4), "Invalid size of pointer" ); // Determine if we want extra debugging code to be active #if !defined(NDEBUG) && !defined(JPH_NO_DEBUG) @@ -528,6 +534,12 @@ static_assert(sizeof(void *) == (JPH_CPU_ADDRESS_BITS == 64? 8 : 4), "Invalid si #error Undefined #endif +// Default memory allocation alignment. +// This define can be overridden in case the user provides an Allocate function that has a different alignment than the platform default. +#ifndef JPH_DEFAULT_ALLOCATE_ALIGNMENT + #define JPH_DEFAULT_ALLOCATE_ALIGNMENT __STDCPP_DEFAULT_NEW_ALIGNMENT__ +#endif + // Cache line size (used for aligning to cache line) #ifndef JPH_CACHE_LINE_SIZE #define JPH_CACHE_LINE_SIZE 64 diff --git a/thirdparty/jolt_physics/Jolt/Core/HashTable.h b/thirdparty/jolt_physics/Jolt/Core/HashTable.h index 822cb284eb..a295a717bc 100644 --- a/thirdparty/jolt_physics/Jolt/Core/HashTable.h +++ b/thirdparty/jolt_physics/Jolt/Core/HashTable.h @@ -842,7 +842,7 @@ public: private: /// If this allocator needs to fall back to aligned allocations because the type requires it - static constexpr bool cNeedsAlignedAllocate = alignof(KeyValue) > (JPH_CPU_ADDRESS_BITS == 32? 8 : 16); + static constexpr bool cNeedsAlignedAllocate = alignof(KeyValue) > JPH_DEFAULT_ALLOCATE_ALIGNMENT; /// Max load factor is cMaxLoadFactorNumerator / cMaxLoadFactorDenominator static constexpr uint64 cMaxLoadFactorNumerator = 7; diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp index 6d4dc10f50..bf5c87e6cc 100644 --- a/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp +++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp @@ -11,6 +11,7 @@ #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. + JPH_MSVC2026_PLUS_SUPPRESS_WARNING(4865) // wingdi.h(2806,1): '': the underlying type will change from 'int' to '__int64' when '/Zc:enumTypes' is specified on the command line #ifndef WIN32_LEAN_AND_MEAN #define WIN32_LEAN_AND_MEAN #endif @@ -19,7 +20,6 @@ #else #include #endif - JPH_SUPPRESS_WARNING_POP #endif #ifdef JPH_PLATFORM_LINUX diff --git a/thirdparty/jolt_physics/Jolt/Core/Memory.h b/thirdparty/jolt_physics/Jolt/Core/Memory.h index 4f3dbc7d13..5df9781dd5 100644 --- a/thirdparty/jolt_physics/Jolt/Core/Memory.h +++ b/thirdparty/jolt_physics/Jolt/Core/Memory.h @@ -8,13 +8,20 @@ JPH_NAMESPACE_BEGIN #ifndef JPH_DISABLE_CUSTOM_ALLOCATOR -// Normal memory allocation, must be at least 8 byte aligned on 32 bit platform and 16 byte aligned on 64 bit platform +/// Normal memory allocation, must be at least 8 byte aligned on 32 bit platform and 16 byte aligned on 64 bit platform. +/// Note that you can override JPH_DEFAULT_ALLOCATE_ALIGNMENT if your allocator's alignment is different from the alignment as defined by `__STDCPP_DEFAULT_NEW_ALIGNMENT__`. using AllocateFunction = void *(*)(size_t inSize); + +/// Reallocate memory. inBlock can be nullptr in which case it must behave as a memory allocation. using ReallocateFunction = void *(*)(void *inBlock, size_t inOldSize, size_t inNewSize); + +/// Free memory. inBlock can be nullptr in which case it must do nothing. using FreeFunction = void (*)(void *inBlock); -// Aligned memory allocation +/// Aligned memory allocation. using AlignedAllocateFunction = void *(*)(size_t inSize, size_t inAlignment); + +/// Free aligned memory. inBlock can be nullptr in which case it must do nothing. using AlignedFreeFunction = void (*)(void *inBlock); // User defined allocation / free functions @@ -31,7 +38,7 @@ JPH_EXPORT void RegisterDefaultAllocator(); // It uses the non-aligned version, which on 32 bit platforms usually returns an 8 byte aligned block. // We therefore default to 16 byte aligned allocations when the regular new operator is used. // See: https://github.com/godotengine/godot/issues/105455#issuecomment-2824311547 -#if defined(JPH_COMPILER_MINGW) && JPH_CPU_ADDRESS_BITS == 32 +#if defined(JPH_COMPILER_MINGW) && JPH_CPU_ARCH_BITS == 32 #define JPH_INTERNAL_DEFAULT_ALLOCATE(size) JPH::AlignedAllocate(size, 16) #define JPH_INTERNAL_DEFAULT_FREE(pointer) JPH::AlignedFree(pointer) #else @@ -43,12 +50,16 @@ JPH_EXPORT void RegisterDefaultAllocator(); #define JPH_OVERRIDE_NEW_DELETE \ JPH_INLINE void *operator new (size_t inCount) { return JPH_INTERNAL_DEFAULT_ALLOCATE(inCount); } \ JPH_INLINE void operator delete (void *inPointer) noexcept { JPH_INTERNAL_DEFAULT_FREE(inPointer); } \ + JPH_INLINE void operator delete (void *inPointer, [[maybe_unused]] size_t inSize) noexcept { JPH_INTERNAL_DEFAULT_FREE(inPointer); } \ JPH_INLINE void *operator new[] (size_t inCount) { return JPH_INTERNAL_DEFAULT_ALLOCATE(inCount); } \ JPH_INLINE void operator delete[] (void *inPointer) noexcept { JPH_INTERNAL_DEFAULT_FREE(inPointer); } \ + JPH_INLINE void operator delete[] (void *inPointer, [[maybe_unused]] size_t inSize) noexcept{ JPH_INTERNAL_DEFAULT_FREE(inPointer); } \ JPH_INLINE void *operator new (size_t inCount, std::align_val_t inAlignment) { return JPH::AlignedAllocate(inCount, static_cast(inAlignment)); } \ - JPH_INLINE void operator delete (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } \ + JPH_INLINE void operator delete (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } \ + JPH_INLINE void operator delete (void *inPointer, [[maybe_unused]] size_t inSize, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } \ JPH_INLINE void *operator new[] (size_t inCount, std::align_val_t inAlignment) { return JPH::AlignedAllocate(inCount, static_cast(inAlignment)); } \ JPH_INLINE void operator delete[] (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } \ + JPH_INLINE void operator delete[] (void *inPointer, [[maybe_unused]] size_t inSize, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } \ JPH_INLINE void *operator new ([[maybe_unused]] size_t inCount, void *inPointer) noexcept { return inPointer; } \ JPH_INLINE void operator delete ([[maybe_unused]] void *inPointer, [[maybe_unused]] void *inPlace) noexcept { /* Do nothing */ } \ JPH_INLINE void *operator new[] ([[maybe_unused]] size_t inCount, void *inPointer) noexcept { return inPointer; } \ diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.h b/thirdparty/jolt_physics/Jolt/Core/Profiler.h index 878865313b..8926485560 100644 --- a/thirdparty/jolt_physics/Jolt/Core/Profiler.h +++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.h @@ -113,6 +113,9 @@ public: /// Remove a thread from being instrumented void RemoveThread(ProfileThread *inThread); + /// 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; + /// Singleton instance static Profiler * sInstance; @@ -167,9 +170,6 @@ private: /// 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); diff --git a/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h index f9573279ad..da328eed8e 100644 --- a/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h +++ b/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h @@ -44,7 +44,7 @@ public: 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); + static constexpr bool needs_aligned_allocate = alignof(T) > JPH_DEFAULT_ALLOCATE_ALIGNMENT; /// Allocate memory inline pointer allocate(size_type inN) diff --git a/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp b/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp index 43a054c603..ee7c31a905 100644 --- a/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp +++ b/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp @@ -9,7 +9,8 @@ #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 + JPH_MSVC2026_PLUS_SUPPRESS_WARNING(4865) // wingdi.h(2806,1): '': the underlying type will change from 'int' to '__int64' when '/Zc:enumTypes' is specified on the command line +#ifndef WIN32_LEAN_AND_MEAN #define WIN32_LEAN_AND_MEAN #endif #ifndef JPH_COMPILER_MINGW diff --git a/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h b/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h index b3d5496cf8..77b69a3b84 100644 --- a/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h +++ b/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h @@ -17,6 +17,9 @@ class JPH_EXPORT TempAllocator : public NonCopyable public: JPH_OVERRIDE_NEW_DELETE + /// If this allocator needs to fall back to aligned allocations because JPH_RVECTOR_ALIGNMENT is bigger than the platform default + static constexpr bool needs_aligned_allocate = JPH_RVECTOR_ALIGNMENT > JPH_DEFAULT_ALLOCATE_ALIGNMENT; + /// Destructor virtual ~TempAllocator() = default; @@ -34,17 +37,22 @@ public: JPH_OVERRIDE_NEW_DELETE /// Constructs the allocator with a maximum allocatable size of inSize - explicit TempAllocatorImpl(size_t inSize) : - mBase(static_cast(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT))), - mSize(inSize) + explicit TempAllocatorImpl(size_t inSize) : mSize(inSize) { + if constexpr (needs_aligned_allocate) + mBase = static_cast(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT)); + else + mBase = static_cast(JPH::Allocate(inSize)); } /// Destructor, frees the block virtual ~TempAllocatorImpl() override { JPH_ASSERT(mTop == 0); - AlignedFree(mBase); + if constexpr (needs_aligned_allocate) + AlignedFree(mBase); + else + JPH::Free(mBase); } // See: TempAllocator @@ -132,14 +140,27 @@ public: // See: TempAllocator virtual void * Allocate(uint inSize) override { - return inSize > 0? AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT) : nullptr; + if (inSize > 0) + { + if constexpr (needs_aligned_allocate) + return AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT); + else + return JPH::Allocate(inSize); + } + else + return nullptr; } // See: TempAllocator virtual void Free(void *inAddress, [[maybe_unused]] uint inSize) override { if (inAddress != nullptr) - AlignedFree(inAddress); + { + if constexpr (needs_aligned_allocate) + AlignedFree(inAddress); + else + JPH::Free(inAddress); + } } }; diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp b/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp index d08140091e..133e1cc3e3 100644 --- a/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp +++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp @@ -9,7 +9,8 @@ #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 + JPH_MSVC2026_PLUS_SUPPRESS_WARNING(4865) // wingdi.h(2806,1): '': the underlying type will change from 'int' to '__int64' when '/Zc:enumTypes' is specified on the command line +#ifndef WIN32_LEAN_AND_MEAN #define WIN32_LEAN_AND_MEAN #endif #ifndef JPH_COMPILER_MINGW diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.h b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h index 22b47c1e94..a0d427c3d1 100644 --- a/thirdparty/jolt_physics/Jolt/Core/TickCounter.h +++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h @@ -38,7 +38,7 @@ JPH_INLINE uint64 GetProcessorTickCount() asm volatile("mrs %0, cntvct_el0" : "=r" (val)); return val; #elif defined(JPH_CPU_LOONGARCH) - #if JPH_CPU_ADDRESS_BITS == 64 + #if JPH_CPU_ARCH_BITS == 64 __drdtime_t t = __rdtime_d(); return t.dvalue; #else diff --git a/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h index 3100952026..87696bb968 100644 --- a/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h +++ b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h @@ -542,7 +542,7 @@ public: 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; + outContactNormal = inDirection; // Failed to get the deepest point, use points returned by GJK and use cast direction as normal } else if (contact_normal_invalid) { diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.inl b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl index bd40c68754..ddbfd342ea 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Vec3.inl +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl @@ -871,6 +871,8 @@ uint32 Vec3::CompressUnitVector() const constexpr float cOneOverSqrt2 = 0.70710678f; constexpr uint cNumBits = 14; constexpr uint cMask = (1 << cNumBits) - 1; + constexpr uint cMaxValue = cMask - 1; // Need odd number of buckets to quantize to or else we can't encode 0 + constexpr float cScale = float(cMaxValue) / (2.0f * cOneOverSqrt2); // Store sign bit Vec3 v = *this; @@ -886,7 +888,7 @@ uint32 Vec3::CompressUnitVector() const value |= max_element << 29; // Store the other two components in a compressed format - UVec4 compressed = Vec3::sClamp((v + Vec3::sReplicate(cOneOverSqrt2)) * (float(cMask) / (2.0f * cOneOverSqrt2)) + Vec3::sReplicate(0.5f), Vec3::sZero(), Vec3::sReplicate(cMask)).ToInt(); + UVec4 compressed = Vec3::sClamp((v + Vec3::sReplicate(cOneOverSqrt2)) * cScale + Vec3::sReplicate(0.5f), Vec3::sZero(), Vec3::sReplicate(cMaxValue)).ToInt(); switch (max_element) { case 0: @@ -908,9 +910,11 @@ Vec3 Vec3::sDecompressUnitVector(uint32 inValue) constexpr float cOneOverSqrt2 = 0.70710678f; constexpr uint cNumBits = 14; constexpr uint cMask = (1u << cNumBits) - 1; + constexpr uint cMaxValue = cMask - 1; // Need odd number of buckets to quantize to or else we can't encode 0 + constexpr float cScale = 2.0f * cOneOverSqrt2 / float(cMaxValue); // Restore two components - Vec3 v = Vec3(UVec4(inValue & cMask, (inValue >> cNumBits) & cMask, 0, 0).ToFloat()) * (2.0f * cOneOverSqrt2 / float(cMask)) - Vec3(cOneOverSqrt2, cOneOverSqrt2, 0.0f); + Vec3 v = Vec3(UVec4(inValue & cMask, (inValue >> cNumBits) & cMask, 0, 0).ToFloat()) * cScale - Vec3(cOneOverSqrt2, cOneOverSqrt2, 0.0f); JPH_ASSERT(v.GetZ() == 0.0f); // Restore the highest component diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.inl b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl index 392c266864..0954fd04c7 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Vec4.inl +++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl @@ -1072,6 +1072,8 @@ uint32 Vec4::CompressUnitVector() const constexpr float cOneOverSqrt2 = 0.70710678f; constexpr uint cNumBits = 9; constexpr uint cMask = (1 << cNumBits) - 1; + constexpr uint cMaxValue = cMask - 1; // Need odd number of buckets to quantize to or else we can't encode 0 + constexpr float cScale = float(cMaxValue) / (2.0f * cOneOverSqrt2); // Store sign bit Vec4 v = *this; @@ -1087,7 +1089,7 @@ uint32 Vec4::CompressUnitVector() const value |= max_element << 29; // Store the other three components in a compressed format - UVec4 compressed = Vec4::sClamp((v + Vec4::sReplicate(cOneOverSqrt2)) * (float(cMask) / (2.0f * cOneOverSqrt2)) + Vec4::sReplicate(0.5f), Vec4::sZero(), Vec4::sReplicate(cMask)).ToInt(); + UVec4 compressed = Vec4::sClamp((v + Vec4::sReplicate(cOneOverSqrt2)) * cScale + Vec4::sReplicate(0.5f), Vec4::sZero(), Vec4::sReplicate(cMaxValue)).ToInt(); switch (max_element) { case 0: @@ -1114,9 +1116,11 @@ Vec4 Vec4::sDecompressUnitVector(uint32 inValue) constexpr float cOneOverSqrt2 = 0.70710678f; constexpr uint cNumBits = 9; constexpr uint cMask = (1u << cNumBits) - 1; + constexpr uint cMaxValue = cMask - 1; // Need odd number of buckets to quantize to or else we can't encode 0 + constexpr float cScale = 2.0f * cOneOverSqrt2 / float(cMaxValue); // Restore three components - Vec4 v = Vec4(UVec4(inValue & cMask, (inValue >> cNumBits) & cMask, (inValue >> (2 * cNumBits)) & cMask, 0).ToFloat()) * (2.0f * cOneOverSqrt2 / float(cMask)) - Vec4(cOneOverSqrt2, cOneOverSqrt2, cOneOverSqrt2, 0.0f); + Vec4 v = Vec4(UVec4(inValue & cMask, (inValue >> cNumBits) & cMask, (inValue >> (2 * cNumBits)) & cMask, 0).ToFloat()) * cScale - Vec4(cOneOverSqrt2, cOneOverSqrt2, cOneOverSqrt2, 0.0f); JPH_ASSERT(v.GetW() == 0.0f); // Restore the highest component diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h index 44d71eb999..fe5249c3b0 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h @@ -178,7 +178,7 @@ public: /// 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. + /// Add force (unit: N) at world space position 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); @@ -444,7 +444,7 @@ private: // 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer) }; -static_assert(JPH_CPU_ADDRESS_BITS != 64 || JPH_RVECTOR_ALIGNMENT < 16 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect"); +static_assert(sizeof(void *) != 8 || JPH_RVECTOR_ALIGNMENT < 16 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect"); static_assert(alignof(Body) == max(JPH_VECTOR_ALIGNMENT, JPH_RVECTOR_ALIGNMENT), "Body should properly align"); JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h index 8949b2ec3e..898b8e40e7 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h @@ -102,8 +102,8 @@ public: 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 mLinearDamping = 0.05f; ///< Linear damping: dv/dt = -c * v. c. Value should be zero or positive and is usually close to 0. + float mAngularDamping = 0.05f; ///< Angular damping: dw/dt = -c * w. c. Value should be zero or positive and 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 diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp index 4a07993798..d30bfe5797 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp @@ -139,7 +139,7 @@ void BodyInterface::AddBody(const BodyID &inBodyID, EActivation inActivationMode void BodyInterface::RemoveBody(const BodyID &inBodyID) { BodyLockWrite lock(*mBodyLockInterface, inBodyID); - if (lock.Succeeded()) + if (lock.SucceededAndIsInBroadPhase()) { const Body &body = lock.GetBody(); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h index cae78b2453..f7c3032c83 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h @@ -144,8 +144,8 @@ public: 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); + bool IsActive(const BodyID &inBodyID) const; ///< Checks if a body is currently actively simulating + void ResetSleepTimer(const BodyID &inBodyID); ///< @see Body::ResetSleepTimer ///@} /// Create a two body constraint @@ -178,8 +178,8 @@ public: ///@name Object layer of a body ///@{ - void SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer); - ObjectLayer GetObjectLayer(const BodyID &inBodyID) const; + void SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer); ///< The collision layer this body belongs to (determines if two objects can collide) + ObjectLayer GetObjectLayer(const BodyID &inBodyID) const; ///< @see Body::GetObjectLayer ///@} ///@name Position and rotation of a body @@ -189,11 +189,11 @@ public: 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; + RVec3 GetCenterOfMassPosition(const BodyID &inBodyID) const; ///< @see Body::GetCenterOfMassPosition 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; + RMat44 GetWorldTransform(const BodyID &inBodyID) const; ///< @see Body::GetWorldTransform + RMat44 GetCenterOfMassTransform(const BodyID &inBodyID) const; ///< @see Body::GetCenterOfMassTransform ///@} /// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds (will activate body if needed) @@ -203,13 +203,13 @@ public: /// 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 SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); ///< @see Body::SetLinearVelocity + Vec3 GetLinearVelocity(const BodyID &inBodyID) const; ///< @see Body::GetLinearVelocity 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 + void SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity); ///< @see Body::SetAngularVelocity + Vec3 GetAngularVelocity(const BodyID &inBodyID) const; ///< @see Body::GetAngularVelocity + 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. @see Body::GetPointVelocity /// 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$ @@ -217,86 +217,86 @@ public: ///@name Add forces to the body. Note that you should add a body to the physics system before applying forces or torques. ///@{ - 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 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 world space position inPoint. @see Body::AddForce + 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. Note that you should add a body to the physics system before applying impulses. ///@{ - 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); + void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass. @see Body::AddImpulse + void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at world space position inPoint. @see Body::AddImpulse + void AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse); ///< @see Body::AddAngularImpulse + bool ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); ///< @see Body::ApplyBuoyancyImpulse ///@} ///@name Body type ///@{ - EBodyType GetBodyType(const BodyID &inBodyID) const; + EBodyType GetBodyType(const BodyID &inBodyID) const; ///< @see Body::GetBodyType ///@} ///@name Body motion type ///@{ - void SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode); - EMotionType GetMotionType(const BodyID &inBodyID) const; + void SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode); ///< @see Body::SetMotionType + EMotionType GetMotionType(const BodyID &inBodyID) const; ///< @see Body::GetMotionType ///@} ///@name Body motion quality ///@{ - void SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality); - EMotionQuality GetMotionQuality(const BodyID &inBodyID) const; + void SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality); ///< How well it detects collisions when it has a high velocity + EMotionQuality GetMotionQuality(const BodyID &inBodyID) const; ///< @see MotionProperties::GetMotionQuality ///@} /// Get inverse inertia tensor in world space - Mat44 GetInverseInertia(const BodyID &inBodyID) const; + Mat44 GetInverseInertia(const BodyID &inBodyID) const; ///< @see Body::GetInverseInertia ///@name Restitution ///@{ - void SetRestitution(const BodyID &inBodyID, float inRestitution); - float GetRestitution(const BodyID &inBodyID) const; + void SetRestitution(const BodyID &inBodyID, float inRestitution); ///< @see Body::SetRestitution + float GetRestitution(const BodyID &inBodyID) const; ///< @see Body::GetRestitution ///@} ///@name Friction ///@{ - void SetFriction(const BodyID &inBodyID, float inFriction); - float GetFriction(const BodyID &inBodyID) const; + void SetFriction(const BodyID &inBodyID, float inFriction); ///< @see Body::SetFriction + float GetFriction(const BodyID &inBodyID) const; ///< @see Body::GetFriction ///@} ///@name Gravity factor ///@{ - void SetGravityFactor(const BodyID &inBodyID, float inGravityFactor); - float GetGravityFactor(const BodyID &inBodyID) const; + void SetGravityFactor(const BodyID &inBodyID, float inGravityFactor); ///< @see MotionProperties::SetGravityFactor + float GetGravityFactor(const BodyID &inBodyID) const; ///< @see MotionProperties::GetGravityFactor ///@} ///@name Max linear velocity ///@{ - void SetMaxLinearVelocity(const BodyID &inBodyID, float inLinearVelocity); - float GetMaxLinearVelocity(const BodyID &inBodyID) const; + void SetMaxLinearVelocity(const BodyID &inBodyID, float inLinearVelocity); ///< @see MotionProperties::SetMaxLinearVelocity + float GetMaxLinearVelocity(const BodyID &inBodyID) const; ///< @see MotionProperties::GetMaxLinearVelocity ///@} ///@name Max angular velocity ///@{ - void SetMaxAngularVelocity(const BodyID &inBodyID, float inAngularVelocity); - float GetMaxAngularVelocity(const BodyID &inBodyID) const; + void SetMaxAngularVelocity(const BodyID &inBodyID, float inAngularVelocity); ///< @see MotionProperties::SetMaxAngularVelocity + float GetMaxAngularVelocity(const BodyID &inBodyID) const; ///< @see MotionProperties::GetMaxAngularVelocity ///@} ///@name Manifold reduction ///@{ - void SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction); - bool GetUseManifoldReduction(const BodyID &inBodyID) const; + void SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction); ///< @see Body::SetUseManifoldReduction + bool GetUseManifoldReduction(const BodyID &inBodyID) const; ///< @see Body::GetUseManifoldReduction ///@} ///@name Sensor ///@{ - void SetIsSensor(const BodyID &inBodyID, bool inIsSensor); - bool IsSensor(const BodyID &inBodyID) const; + void SetIsSensor(const BodyID &inBodyID, bool inIsSensor); ///< @see Body::SetIsSensor + bool IsSensor(const BodyID &inBodyID) const; ///< @see Body::IsSensor ///@} ///@name Collision group ///@{ - void SetCollisionGroup(const BodyID &inBodyID, const CollisionGroup &inCollisionGroup); - const CollisionGroup & GetCollisionGroup(const BodyID &inBodyID) const; + void SetCollisionGroup(const BodyID &inBodyID, const CollisionGroup &inCollisionGroup); ///< @see Body::SetCollisionGroup + const CollisionGroup & GetCollisionGroup(const BodyID &inBodyID) const; ///< @see Body::GetCollisionGroup ///@} /// Get transform and shape for this body, used to perform collision detection diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp index ebf524ba05..f6c0584e19 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp @@ -47,7 +47,8 @@ JPH_NAMESPACE_BEGIN } #endif -// Helper class that combines a body and its motion properties +/// @cond INTERNAL +/// Helper class that combines a body and its motion properties class BodyWithMotionProperties : public Body { public: @@ -55,8 +56,10 @@ public: MotionProperties mMotionProperties; }; +/// @endcond -// Helper class that combines a soft body its motion properties and shape +/// @cond INTERNAL +/// Helper class that combines a soft body its motion properties and shape class SoftBodyWithMotionPropertiesAndShape : public Body { public: @@ -68,6 +71,7 @@ public: SoftBodyMotionProperties mMotionProperties; SoftBodyShape mShape; }; +/// @endcond inline void BodyManager::sDeleteBody(Body *inBody) { @@ -606,6 +610,11 @@ void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber) // Mark this body as no longer active body.mMotionProperties->mIslandIndex = Body::cInactiveIndex; + #ifdef JPH_TRACK_SIMULATION_STATS + // Reset simulation stats + body.mMotionProperties->mSimulationStats.Reset(); + #endif + // Reset velocity body.mMotionProperties->mLinearVelocity = Vec3::sZero(); body.mMotionProperties->mAngularVelocity = Vec3::sZero(); @@ -1160,4 +1169,52 @@ void BodyManager::ValidateActiveBodyBounds() } #endif // JPH_DEBUG +#ifdef JPH_TRACK_SIMULATION_STATS +void BodyManager::ResetSimulationStats() +{ + JPH_PROFILE_FUNCTION(); + + 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()]; + body->mMotionProperties->GetSimulationStats().Reset(); + } +} + +#ifdef JPH_PROFILE_ENABLED +void BodyManager::ReportSimulationStats() +{ + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + Trace("BodyID, IslandIndex, LargeIsland, BroadPhase (us), NarrowPhase (us), VelocityConstraint (us), PositionConstraint (us), UpdateBounds (us), CCD (us), NumContactConstraints, NumVelocitySteps, NumPositionSteps"); + + double us_per_tick = 1000000.0 / Profiler::sInstance->GetProcessorTicksPerSecond(); + + 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()]; + const MotionProperties *mp = body->mMotionProperties; + const MotionProperties::SimulationStats &stats = mp->GetSimulationStats(); + Trace("%u, %u, %s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %u, %u, %u", + body->GetID().GetIndex(), + mp->GetIslandIndexInternal(), + stats.mIsLargeIsland? "True" : "False", + double(stats.mBroadPhaseTicks) * us_per_tick, + double(stats.mNarrowPhaseTicks) * us_per_tick, + double(stats.mVelocityConstraintTicks) * us_per_tick, + double(stats.mPositionConstraintTicks) * us_per_tick, + double(stats.mUpdateBoundsTicks) * us_per_tick, + double(stats.mCCDTicks) * us_per_tick, + stats.mNumContactConstraints.load(memory_order_relaxed), + stats.mNumVelocitySteps, + stats.mNumPositionSteps); + } +} +#endif // JPH_PROFILE_ENABLED +#endif // JPH_TRACK_SIMULATION_STATS + JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h index 3a91b62167..9b1899843a 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h @@ -40,6 +40,9 @@ using BodyVector = Array; using BodyIDVector = Array; /// Class that contains all bodies +/// +/// WARNING: This class is an internal part of PhysicsSystem, it has no functions that can be called by users of the library. +/// Its functionality is exposed through PhysicsSystem, BodyInterface, BodyLockRead and BodyLockWrite. class JPH_EXPORT BodyManager : public NonCopyable { public: @@ -227,6 +230,16 @@ public: }; /// Draw settings + /// + /// Note that there are several debug drawing features that are not exposed through this interface since they use information + /// that is only available deep inside the simulation update and are mostly there to facilitate debugging Jolt. These options + /// use DebugRenderer::sInstance to draw. + /// + /// E.g.: + /// * To draw contact information, use ContactConstraintManager::sDrawContactManifolds. + /// * To draw when continuous collision detection is used, use PhysicsSystem::sDrawMotionQualityLinearCast. + /// * To draw what's going on in a CharacterVirtual update, use CharacterVirtual::sDrawConstraints, CharacterVirtual::sDrawWalkStairs and CharacterVirtual::sDrawStickToFloor. + /// * To draw the volume of water that interacts with a shape, use Shape::sDrawSubmergedVolumes. struct DrawSettings { bool mDrawGetSupportFunction = false; ///< Draw the GetSupport() function, used for convex collision detection @@ -289,6 +302,16 @@ public: void ValidateActiveBodyBounds(); #endif // JPH_DEBUG +#ifdef JPH_TRACK_SIMULATION_STATS + /// Resets the per body simulation stats + void ResetSimulationStats(); + +#ifdef JPH_PROFILE_ENABLED + /// Dump the per body simulation stats to the TTY + void ReportSimulationStats(); +#endif +#endif + private: /// Increment and get the sequence number of the body #ifdef JPH_COMPILER_CLANG diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h index d8a485b611..15d19f0008 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h @@ -76,11 +76,11 @@ public: 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. + /// Get linear damping: dv/dt = -c * v. c. Value should be zero or positive and 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. + /// Get angular damping: dw/dt = -c * w. c. Value should be zero or positive and is usually close to 0. inline float GetAngularDamping() const { return mAngularDamping; } void SetAngularDamping(float inAngularDamping) { JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; } @@ -186,6 +186,28 @@ public: void SetNumPositionStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); } uint GetNumPositionStepsOverride() const { return mNumPositionStepsOverride; } +#ifdef JPH_TRACK_SIMULATION_STATS + /// Stats for this body. These are average for the simulation island the body was part of. + struct SimulationStats + { + void Reset() { mBroadPhaseTicks = 0; mNarrowPhaseTicks.store(0, memory_order_relaxed); mVelocityConstraintTicks = 0; mPositionConstraintTicks = 0; mUpdateBoundsTicks = 0; mCCDTicks.store(0, memory_order_relaxed); mNumContactConstraints.store(0, memory_order_relaxed); mNumVelocitySteps = 0; mNumPositionSteps = 0; mIsLargeIsland = false; } + + uint64 mBroadPhaseTicks = 0; ///< Number of processor ticks spent doing broad phase collision detection + atomic mNarrowPhaseTicks = 0; ///< Number of ticks spent doing narrow phase collision detection + uint64 mVelocityConstraintTicks = 0; ///< Number of ticks spent solving velocity constraints + uint64 mPositionConstraintTicks = 0; ///< Number of ticks spent solving position constraints + uint64 mUpdateBoundsTicks = 0; ///< Number of ticks spent updating the broadphase and checking if the body should go to sleep + atomic mCCDTicks = 0; ///< Number of ticks spent doing CCD + atomic mNumContactConstraints = 0; ///< Number of contact constraints created for this body + uint8 mNumVelocitySteps = 0; ///< Number of velocity iterations performed + uint8 mNumPositionSteps = 0; ///< Number of position iterations performed + bool mIsLargeIsland = false; ///< If this body was part of a large island + }; + + const SimulationStats & GetSimulationStats() const { return mSimulationStats; } + SimulationStats & GetSimulationStats() { return mSimulationStats; } +#endif // JPH_TRACK_SIMULATION_STATS + //////////////////////////////////////////////////////////// // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY //////////////////////////////////////////////////////////// @@ -248,8 +270,8 @@ private: 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 mLinearDamping; ///< Linear damping: dv/dt = -c * v. Value should be zero or positive and is usually close to 0. + float mAngularDamping; ///< Angular damping: dw/dt = -c * w. Value should be zero or positive and 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 @@ -275,6 +297,10 @@ private: EBodyType mCachedBodyType; ///< Copied from Body::mBodyType and cached for asserting purposes EMotionType mCachedMotionType; ///< Copied from Body::mMotionType and cached for asserting purposes #endif + +#ifdef JPH_TRACK_SIMULATION_STATS + SimulationStats mSimulationStats; +#endif // JPH_TRACK_SIMULATION_STATS }; JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h index 4f6d63a1c2..ef70671c69 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h @@ -81,6 +81,7 @@ public: }; /// This class receives callbacks when a virtual character hits something. +/// Once created, register it on a CharacterVirtual by using the character's SetListener method. class JPH_EXPORT CharacterContactListener { public: diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h index 937f9f07f0..c4d9861a6b 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h @@ -95,9 +95,6 @@ public: /// 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. diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h index 10085e66fe..4fb70f129b 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h @@ -48,6 +48,9 @@ public: /// 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; + + /// Get the bounding box of all objects in the broadphase + virtual AABox GetBounds() const = 0; }; 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 index a70f362022..f3f5d90803 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp @@ -1452,6 +1452,10 @@ void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inAc const Body &body1 = *inBodies[b1_id.GetIndex()]; JPH_ASSERT(!body1.IsStatic()); + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 start_tick = GetProcessorTickCount(); + #endif + // Expand the bounding box by the speculative contact distance AABox bounds1 = body1.GetWorldSpaceBounds(); bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance)); @@ -1521,6 +1525,11 @@ void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inAc --top; } while (top >= 0); + + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 num_ticks = GetProcessorTickCount() - start_tick; + const_cast(body1.GetMotionPropertiesUnchecked()->GetSimulationStats()).mBroadPhaseTicks += num_ticks; + #endif } // Test that the root node was not swapped while finding collision pairs. diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp index 8e10106419..990dfa68b9 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp @@ -212,7 +212,9 @@ void CastSphereVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 // 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(); + // The distance between p and the triangle should be mRadius, but for very long casts, + // floating point accuracy can become low enough so that p is on the plane and q is zero + Vec3 contact_normal = q.NormalizedOr(back_facing? triangle_normal : -triangle_normal); 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); } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h index a4f7c95758..a081df2cb5 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h @@ -6,6 +6,7 @@ #include #include +#include #include JPH_NAMESPACE_BEGIN diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h index c91becb642..b5ef0ee10d 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h @@ -14,8 +14,9 @@ class JPH_EXPORT CollideSoftBodyVerticesVsTriangles { public: CollideSoftBodyVerticesVsTriangles(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) : - mTransform(inCenterOfMassTransform * Mat44::sScale(inScale)), - mInvTransform(mTransform.Inversed()), + mTransform(inCenterOfMassTransform), + mInvTransform(mTransform.InversedRotationTranslation()), + mScale(inScale), mNormalSign(ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f) { } @@ -28,15 +29,20 @@ public: JPH_INLINE void ProcessTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) { + // Apply the scale to the triangle + Vec3 v0 = mScale * inV0; + Vec3 v1 = mScale * inV1; + Vec3 v2 = mScale * 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); + Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(v0 - mLocalPosition, v1 - mLocalPosition, v2 - mLocalPosition, set); float dist_sq = closest_point.LengthSq(); if (dist_sq < mClosestDistanceSq) { - mV0 = inV0; - mV1 = inV1; - mV2 = inV2; + mV0 = v0; + mV1 = v1; + mV2 = v2; mClosestPoint = closest_point; mClosestDistanceSq = dist_sq; mSet = set; @@ -55,10 +61,10 @@ public: if (mSet == 0b111) { - // Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m penetration + // Closest is interior to the triangle, use plane as collision plane but don't allow more than sTriangleThickness 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)) + float penetration = triangle_normal.Dot(v0 - ioVertex.GetPosition()); + if (penetration < sTriangleThickness && ioVertex.UpdatePenetration(penetration)) ioVertex.SetCollision(Plane::sFromPointAndNormal(v0, triangle_normal), inCollidingShapeIndex); } else @@ -77,8 +83,14 @@ public: } } + /// Triangles are considered to have some thickness. This thickness extends backwards along the negative triangle normal. + /// Make this value smaller than the smallest 'wall thickness' so that the back side of the triangle doesn't protrude through the other side. + /// Make this value too small and tunneling is more likely to occur. + static inline float sTriangleThickness = 0.1f; + Mat44 mTransform; Mat44 mInvTransform; + Vec3 mScale; Vec3 mLocalPosition; Vec3 mV0, mV1, mV2; Vec3 mClosestPoint; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp index fc2281252f..2375bc3b6c 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp @@ -55,11 +55,17 @@ ShapeSettings::ShapeResult BoxShapeSettings::Create() const BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) : ConvexShape(EShapeSubType::Box, inSettings, outResult), mHalfExtent(inSettings.mHalfExtent), - mConvexRadius(inSettings.mConvexRadius) + mConvexRadius(min(inSettings.mConvexRadius, inSettings.mHalfExtent.ReduceMin())) { + // Check half extents + if (inSettings.mHalfExtent.ReduceMin() < 0.0f) + { + outResult.SetError("Invalid half extent"); + return; + } + // Check convex radius - if (inSettings.mConvexRadius < 0.0f - || inSettings.mHalfExtent.ReduceMin() < inSettings.mConvexRadius) + if (inSettings.mConvexRadius < 0.0f) { outResult.SetError("Invalid convex radius"); return; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h index 030dd2db80..01c9d27a2a 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h @@ -20,7 +20,7 @@ public: /// 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) { } + explicit BoxShapeSettings(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { } // See: ShapeSettings virtual ShapeResult Create() const override; @@ -41,7 +41,7 @@ public: /// 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); } + explicit BoxShape(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Box, inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(min(inConvexRadius, inHalfExtent.ReduceMin())) { JPH_ASSERT(inHalfExtent.ReduceMin() >= 0.0f); JPH_ASSERT(inConvexRadius >= 0.0f); } /// Get half extent of box Vec3 GetHalfExtent() const { return mHalfExtent; } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h index 033c3c5c6e..25b8c903e4 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h @@ -243,7 +243,7 @@ public: // 3 padding bytes left }; - static_assert(sizeof(SubShape) == (JPH_CPU_ADDRESS_BITS == 64? 40 : 36), "Compiler added unexpected padding"); + static_assert(sizeof(SubShape) == (sizeof(void *) == 8? 40 : 36), "Compiler added unexpected padding"); using SubShapes = Array; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h index c6b1860000..19bce7614d 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h @@ -25,7 +25,7 @@ public: /// 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) { } + explicit ConvexHullShapeSettings(const Array &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { } // See: ShapeSettings virtual ShapeResult Create() const override; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp index cb8d4ac988..826df0dd6d 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp @@ -91,15 +91,15 @@ CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResul ConvexShape(EShapeSubType::Cylinder, inSettings, outResult), mHalfHeight(inSettings.mHalfHeight), mRadius(inSettings.mRadius), - mConvexRadius(inSettings.mConvexRadius) + mConvexRadius(min(inSettings.mConvexRadius, min(inSettings.mHalfHeight, inSettings.mRadius))) { - if (inSettings.mHalfHeight < inSettings.mConvexRadius) + if (inSettings.mHalfHeight < 0.0f) { outResult.SetError("Invalid height"); return; } - if (inSettings.mRadius < inSettings.mConvexRadius) + if (inSettings.mRadius < 0.0f) { outResult.SetError("Invalid radius"); return; @@ -118,10 +118,10 @@ CylinderShape::CylinderShape(float inHalfHeight, float inRadius, float inConvexR ConvexShape(EShapeSubType::Cylinder, inMaterial), mHalfHeight(inHalfHeight), mRadius(inRadius), - mConvexRadius(inConvexRadius) + mConvexRadius(min(inConvexRadius, min(inHalfHeight, inRadius))) { - JPH_ASSERT(inHalfHeight >= inConvexRadius); - JPH_ASSERT(inRadius >= inConvexRadius); + JPH_ASSERT(inHalfHeight >= 0.0f); + JPH_ASSERT(inRadius >= 0.0f); JPH_ASSERT(inConvexRadius >= 0.0f); } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp index 8cb81f68d0..658c5a3476 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp @@ -23,8 +23,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(EmptyShapeSettings) ShapeSettings::ShapeResult EmptyShapeSettings::Create() const { if (mCachedResult.IsEmpty()) - new EmptyShape(*this, mCachedResult); - + Ref shape = new EmptyShape(*this, mCachedResult); return mCachedResult; } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp index 1d91063510..ff55f4ba28 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp @@ -2257,8 +2257,12 @@ void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, 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); + // Get distance to vertex - Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); // Clear distance for invalid bounds dist_sq = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), dist_sq, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp index 557078ada1..8297285e83 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp @@ -849,8 +849,12 @@ void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Ar 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); + // Get distance to vertex - Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, 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(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h index b5a637ed31..dc24b7a9ed 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h @@ -28,7 +28,7 @@ public: MeshShapeSettings() = default; /// Create a mesh shape. - MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList()); + explicit 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. diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp index 16155bb224..0b7d404392 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp @@ -23,7 +23,6 @@ ShapeSettings::ShapeResult MutableCompoundShapeSettings::Create() const // Build a mutable compound shape if (mCachedResult.IsEmpty()) Ref shape = new MutableCompoundShape(*this, mCachedResult); - return mCachedResult; } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h index d34054b6bd..4f65da3f1c 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h @@ -22,7 +22,7 @@ public: PlaneShapeSettings() = default; /// Create a plane shape. - PlaneShapeSettings(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = cDefaultHalfExtent) : mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { } + explicit PlaneShapeSettings(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = cDefaultHalfExtent) : mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { } // See: ShapeSettings virtual ShapeResult Create() const override; @@ -46,7 +46,7 @@ public: /// 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(); } + explicit 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 diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h index c305523396..3e432b6549 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h @@ -18,7 +18,7 @@ public: SphereShapeSettings() = default; /// Create a sphere with radius inRadius - SphereShapeSettings(float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius) { } + explicit SphereShapeSettings(float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius) { } // See: ShapeSettings virtual ShapeResult Create() const override; @@ -38,7 +38,7 @@ public: 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); } + explicit 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; } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp index bec0c47423..9222d25f1c 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp @@ -55,12 +55,12 @@ ShapeSettings::ShapeResult TaperedCylinderShapeSettings::Create() const settings.mRadius = mTopRadius; settings.mMaterial = mMaterial; settings.mConvexRadius = mConvexRadius; - new CylinderShape(settings, mCachedResult); + shape = new CylinderShape(settings, mCachedResult); } else { // Normal tapered cylinder shape - new TaperedCylinderShape(*this, mCachedResult); + shape = new TaperedCylinderShape(*this, mCachedResult); } } return mCachedResult; @@ -79,7 +79,7 @@ TaperedCylinderShape::TaperedCylinderShape(const TaperedCylinderShapeSettings &i ConvexShape(EShapeSubType::TaperedCylinder, inSettings, outResult), mTopRadius(inSettings.mTopRadius), mBottomRadius(inSettings.mBottomRadius), - mConvexRadius(inSettings.mConvexRadius) + mConvexRadius(min(inSettings.mConvexRadius, min(inSettings.mTopRadius, inSettings.mBottomRadius))) { if (mTopRadius < 0.0f) { @@ -93,27 +93,15 @@ TaperedCylinderShape::TaperedCylinderShape(const TaperedCylinderShapeSettings &i return; } - if (inSettings.mHalfHeight <= 0.0f) - { - outResult.SetError("Invalid height"); - return; - } - - if (inSettings.mConvexRadius < 0.0f) + if (mConvexRadius < 0.0f) { outResult.SetError("Invalid convex radius"); return; } - if (inSettings.mTopRadius < inSettings.mConvexRadius) + if (inSettings.mHalfHeight <= 0.0f) { - 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"); + outResult.SetError("Invalid height"); return; } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h index 21557b1e77..113ff64bc9 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h @@ -188,7 +188,7 @@ public: 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 || JPH_RVECTOR_ALIGNMENT < 16 || sizeof(TransformedShape) == JPH_IF_SINGLE_PRECISION_ELSE(64, 96), "Not properly packed"); +static_assert(sizeof(void *) != 8 || JPH_RVECTOR_ALIGNMENT < 16 || sizeof(TransformedShape) == JPH_IF_SINGLE_PRECISION_ELSE(64, 96), "Not properly packed"); static_assert(alignof(TransformedShape) == max(JPH_VECTOR_ALIGNMENT, 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 index 857eddfb8e..2c0611587f 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h @@ -8,7 +8,8 @@ JPH_NAMESPACE_BEGIN -/// Class used to calculate the total number of velocity and position steps +/// @cond INTERNAL +/// Internal class used to calculate the total number of velocity and position steps class CalculateSolverSteps { public: @@ -51,7 +52,9 @@ private: bool mApplyDefaultVelocity = false; bool mApplyDefaultPosition = false; }; +/// @endcond +/// @cond INTERNAL /// Dummy class to replace the steps calculator when we don't need the result class DummyCalculateSolverSteps { @@ -62,5 +65,6 @@ public: /* Nothing to do */ } }; +/// @endcond JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h index a30a838ca1..d5e5baedd4 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h @@ -77,7 +77,7 @@ public: /// 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. + /// Priority of the constraint when solving. Higher numbers 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; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h index e1bfb0f214..8dffcda03e 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h @@ -21,6 +21,9 @@ class DebugRenderer; using Constraints = Array>; /// A constraint manager manages all constraints of the same type +/// +/// WARNING: This class is an internal part of PhysicsSystem, it has no functions that can be called by users of the library. +/// Its functionality is exposed through PhysicsSystem and BodyInterface. class JPH_EXPORT ConstraintManager : public NonCopyable { public: diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp index 306ae64ff3..09dfab4921 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp @@ -990,6 +990,14 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA if (sDrawContactManifolds) constraint.Draw(DebugRenderer::sInstance, Color::sYellow); #endif // JPH_DEBUG_RENDERER + + #ifdef JPH_TRACK_SIMULATION_STATS + // Track new contact constraints + if (!body1->IsStatic()) + body1->GetMotionPropertiesUnchecked()->GetSimulationStats().mNumContactConstraints.fetch_add(1, memory_order_relaxed); + if (!body2->IsStatic()) + body2->GetMotionPropertiesUnchecked()->GetSimulationStats().mNumContactConstraints.fetch_add(1, memory_order_relaxed); + #endif } // Mark contact as persisted so that we won't fire OnContactRemoved callbacks @@ -1230,6 +1238,14 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i if (sDrawContactManifolds) constraint.Draw(DebugRenderer::sInstance, Color::sOrange); #endif // JPH_DEBUG_RENDERER + + #ifdef JPH_TRACK_SIMULATION_STATS + // Track new contact constraints + if constexpr (Type1 != EMotionType::Static) + inBody1.GetMotionPropertiesUnchecked()->GetSimulationStats().mNumContactConstraints.fetch_add(1, memory_order_relaxed); + if constexpr (Type2 != EMotionType::Static) + inBody2.GetMotionPropertiesUnchecked()->GetSimulationStats().mNumContactConstraints.fetch_add(1, memory_order_relaxed); + #endif } else { diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h index 73e5d749b0..dfe5fc8c38 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h @@ -24,6 +24,9 @@ JPH_NAMESPACE_BEGIN struct PhysicsSettings; class PhysicsUpdateContext; +/// A contact constraint manager manages all contacts between two bodies +/// +/// WARNING: This class is an internal part of PhysicsSystem, it has no functions that can be called by users of the library. class JPH_EXPORT ContactConstraintManager : public NonCopyable { public: diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp index c5e9f56f36..60bb930e41 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp @@ -382,6 +382,12 @@ void IslandBuilder::Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBod SortIslands(inTempAllocator); mNumPositionSteps = (uint8 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint8)); + +#ifdef JPH_TRACK_SIMULATION_STATS + mIslandStats = (IslandStats *)inTempAllocator->Allocate(mNumIslands * sizeof(IslandStats)); + for (uint32 i = 0; i < mNumIslands; ++i) + new (&mIslandStats[i]) IslandStats(); +#endif } void IslandBuilder::GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const @@ -432,6 +438,10 @@ void IslandBuilder::ResetIslands(TempAllocator *inTempAllocator) { JPH_PROFILE_FUNCTION(); +#ifdef JPH_TRACK_SIMULATION_STATS + inTempAllocator->Free(mIslandStats, mNumIslands * sizeof(IslandStats)); +#endif + inTempAllocator->Free(mNumPositionSteps, mNumIslands * sizeof(uint8)); if (mIslandsSorted != nullptr) diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h index 4c2f097d60..24eee59dc5 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h +++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h @@ -15,7 +15,7 @@ class TempAllocator; //#define JPH_VALIDATE_ISLAND_BUILDER /// Keeps track of connected bodies and builds islands for multithreaded velocity/position update -class IslandBuilder : public NonCopyable +class JPH_EXPORT IslandBuilder : public NonCopyable { public: /// Destructor @@ -54,6 +54,21 @@ public: 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]; } +#ifdef JPH_TRACK_SIMULATION_STATS + struct IslandStats + { + atomic mVelocityConstraintTicks = 0; + atomic mPositionConstraintTicks = 0; + atomic mUpdateBoundsTicks = 0; + uint8 mNumVelocitySteps = 0; + uint8 mNumPositionSteps = 0; ///< Tracking this a 2nd time since IslandBuilder::mNumPositionSteps is not filled in when there are no constraints or for large islands. + bool mIsLargeIsland = false; + }; + + /// Tracks simulation stats per island + IslandStats & GetIslandStats(uint32 inIslandIndex) { return mIslandStats[inIslandIndex]; } +#endif + /// After you're done calling the three functions above, call this function to free associated data void ResetIslands(TempAllocator *inTempAllocator); @@ -101,6 +116,10 @@ private: uint8 * mNumPositionSteps = nullptr; ///< Number of position steps for each island +#ifdef JPH_TRACK_SIMULATION_STATS + IslandStats * mIslandStats = nullptr; ///< Per island statistics +#endif + // Counters uint32 mMaxActiveBodies; ///< Maximum size of the active bodies list (see BodyManager::mActiveBodies) uint32 mNumActiveBodies = 0; ///< Number of active bodies passed to diff --git a/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h index 36f0d61401..4a7169c677 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h +++ b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h @@ -21,6 +21,8 @@ class CalculateSolverSteps; /// /// 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" +/// +/// WARNING: This class is an internal part of PhysicsSystem, it has no functions that can be called by users of the library. class LargeIslandSplitter : public NonCopyable { private: diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp index d4618a750a..7b4babfc21 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp @@ -129,6 +129,48 @@ void PhysicsSystem::RemoveStepListener(PhysicsStepListener *inListener) mStepListeners.pop_back(); } +#ifdef JPH_TRACK_SIMULATION_STATS +void PhysicsSystem::GatherIslandStats() +{ + JPH_PROFILE_FUNCTION(); + + for (uint32 island_idx = 0; island_idx < mIslandBuilder.GetNumIslands(); ++island_idx) + { + BodyID *bodies_begin, *bodies_end; + mIslandBuilder.GetBodiesInIsland(island_idx, bodies_begin, bodies_end); + uint64 num_bodies = bodies_end - bodies_begin; + + // Calculate the number of dynamic bodies + uint64 num_dynamic_bodies = 0; + for (BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id) + if (mBodyManager.GetBody(*body_id).GetMotionType() == EMotionType::Dynamic) + ++num_dynamic_bodies; + num_dynamic_bodies = max(num_dynamic_bodies, 1); // Ensure we don't divide by zero + + // Equally distribute the stats over all bodies + const IslandBuilder::IslandStats &stats = mIslandBuilder.GetIslandStats(island_idx); + uint64 num_velocity_ticks = stats.mVelocityConstraintTicks / num_dynamic_bodies; + uint64 num_position_ticks = stats.mPositionConstraintTicks / num_dynamic_bodies; + uint64 num_update_bounds_ticks = stats.mUpdateBoundsTicks / num_bodies; + + for (BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id) + { + Body &body = mBodyManager.GetBody(*body_id); + MotionProperties::SimulationStats &out_stats = body.GetMotionProperties()->GetSimulationStats(); + out_stats.mNumVelocitySteps = stats.mNumVelocitySteps; + out_stats.mNumPositionSteps = stats.mNumPositionSteps; + if (body.GetMotionType() == EMotionType::Dynamic) + { + out_stats.mVelocityConstraintTicks += num_velocity_ticks; // In case of multiple collision steps we accumulate + out_stats.mPositionConstraintTicks += num_position_ticks; + } + out_stats.mUpdateBoundsTicks += num_update_bounds_ticks; + out_stats.mIsLargeIsland = stats.mIsLargeIsland; + } + } +} +#endif // JPH_TRACK_SIMULATION_STATS + EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem) { JPH_PROFILE_FUNCTION(); @@ -162,6 +204,11 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep return EPhysicsUpdateError::None; } +#ifdef JPH_TRACK_SIMULATION_STATS + // Reset accumulated stats on the active bodies + mBodyManager.ResetSimulationStats(); +#endif + // 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; @@ -399,6 +446,11 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep mBodyManager.ValidateActiveBodyBounds(); #endif // JPH_DEBUG + #ifdef JPH_TRACK_SIMULATION_STATS + // Gather stats from the islands and distribute them over the bodies + GatherIslandStats(); + #endif + // Store the number of active bodies at the start of the step next_step->mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); @@ -580,6 +632,11 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep mBodyManager.ValidateActiveBodyBounds(); #endif // JPH_DEBUG +#ifdef JPH_TRACK_SIMULATION_STATS + // Gather stats from the islands and distribute them over the bodies + GatherIslandStats(); +#endif + // Clear the large island splitter mLargeIslandSplitter.Reset(inTempAllocator); @@ -1023,6 +1080,10 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const // If the cache hasn't handled this body pair do actual collision detection if (!pair_handled) { + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 start_ticks = GetProcessorTickCount(); + #endif + // 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); @@ -1262,6 +1323,24 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const constraint_created = collector.mConstraintCreated; } + + #ifdef JPH_TRACK_SIMULATION_STATS + // Track time spent processing collision for this body pair + uint64 num_ticks = GetProcessorTickCount() - start_ticks; + if (body1->GetMotionType() > body2->GetMotionType()) + { + // Assign all ticks to the body with the higher motion type + body1->GetMotionProperties()->GetSimulationStats().mNarrowPhaseTicks.fetch_add(num_ticks, memory_order_relaxed); + } + else + { + // When two bodies with the same motion type are involved, we give both bodies half the ticks + JPH_ASSERT(body1->GetMotionType() == body2->GetMotionType()); + num_ticks /= 2; + body1->GetMotionProperties()->GetSimulationStats().mNarrowPhaseTicks.fetch_add(num_ticks, memory_order_relaxed); + body1->GetMotionProperties()->GetSimulationStats().mNarrowPhaseTicks.fetch_add(num_ticks, memory_order_relaxed); + } + #endif } // If a contact constraint was created, we need to do some extra work @@ -1343,6 +1422,10 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, { case LargeIslandSplitter::EStatus::BatchRetrieved: { + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 start_tick = GetProcessorTickCount(); + #endif + if (first_iteration) { // Iteration 0 is used to warm start the batch (we added 1 to the number of iterations in LargeIslandSplitter::SplitIsland) @@ -1365,6 +1448,11 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, if (last_iteration) mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end); + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 num_ticks = GetProcessorTickCount() - start_tick; + mIslandBuilder.GetIslandStats(mLargeIslandSplitter.GetIslandIndex(split_island_index)).mVelocityConstraintTicks.fetch_add(num_ticks, memory_order_relaxed); + #endif + // We processed work, loop again continue; } @@ -1414,6 +1502,10 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, continue; } + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 start_tick = GetProcessorTickCount(); + #endif + // Sorting is costly but needed for a deterministic simulation, allow the user to turn this off if (mPhysicsSettings.mDeterministicSimulation) { @@ -1425,30 +1517,46 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, } // Split up large islands + #ifdef JPH_TRACK_SIMULATION_STATS + bool is_large_island = true; + #endif 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) + if (!mPhysicsSettings.mUseLargeIslandSplitter + || !mLargeIslandSplitter.SplitIsland(island_idx, mIslandBuilder, mBodyManager, mContactManager, active_constraints, steps_calculator)) { - 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; + #ifdef JPH_TRACK_SIMULATION_STATS + is_large_island = false; + #endif + + // 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); } - // Save back the lambdas in the contact cache for the warm start of the next physics update - mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end); + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 num_ticks = GetProcessorTickCount() - start_tick; + IslandBuilder::IslandStats &stats = mIslandBuilder.GetIslandStats(island_idx); + stats.mNumVelocitySteps = (uint8)steps_calculator.GetNumVelocitySteps(); + stats.mNumPositionSteps = (uint8)steps_calculator.GetNumPositionSteps(); + stats.mVelocityConstraintTicks.fetch_add(num_ticks, memory_order_relaxed); + stats.mIsLargeIsland = is_large_island; + #endif // We processed work, loop again continue; @@ -1933,11 +2041,20 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph // Create shape filter SimShapeFilterWrapper shape_filter(mSimShapeFilter, &body); + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 start_tick = GetProcessorTickCount(); + #endif + // 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::sOne(), 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); + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 num_ticks = GetProcessorTickCount() - start_tick; + const_cast(body.GetMotionPropertiesUnchecked()->GetSimulationStats()).mCCDTicks.fetch_add(num_ticks, memory_order_relaxed); + #endif + // Check if there was a hit if (ccd_body.mFractionPlusSlop < 1.0f) { @@ -2402,20 +2519,31 @@ void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, 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); + { + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 start_tick = GetProcessorTickCount(); + #endif - // 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); + // Solve the batch + ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte); + mContactManager.SolvePositionConstraints(contacts_begin, contacts_end); - // The final batch will update all bounds and check sleeping - if (final_batch) - CheckSleepAndUpdateBounds(mLargeIslandSplitter.GetIslandIndex(split_island_index), ioContext, ioStep, bodies_to_sleep); + // 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); - // We processed work, loop again - continue; + // The final batch will update all bounds and check sleeping + if (final_batch) + CheckSleepAndUpdateBounds(mLargeIslandSplitter.GetIslandIndex(split_island_index), ioContext, ioStep, bodies_to_sleep); + + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 num_ticks = GetProcessorTickCount() - start_tick; + mIslandBuilder.GetIslandStats(mLargeIslandSplitter.GetIslandIndex(split_island_index)).mPositionConstraintTicks.fetch_add(num_ticks, memory_order_relaxed); + #endif + + // We processed work, loop again + continue; + } case LargeIslandSplitter::EStatus::WaitingForBatch: break; @@ -2451,6 +2579,10 @@ void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, && num_items >= LargeIslandSplitter::cLargeIslandTreshold) continue; + #ifdef JPH_TRACK_SIMULATION_STATS + uint64 start_tick = GetProcessorTickCount(); + #endif + // Check if this island needs solving if (num_items > 0) { @@ -2465,9 +2597,21 @@ void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, } } + #ifdef JPH_TRACK_SIMULATION_STATS + // Accumulate time spent in solving position constraints + uint64 solve_position_ticks = GetProcessorTickCount(); + IslandBuilder::IslandStats &stats = mIslandBuilder.GetIslandStats(island_idx); + stats.mPositionConstraintTicks.fetch_add(solve_position_ticks - start_tick, memory_order_relaxed); + #endif + // After solving we will update all bounds and check sleeping CheckSleepAndUpdateBounds(island_idx, ioContext, ioStep, bodies_to_sleep); + #ifdef JPH_TRACK_SIMULATION_STATS + // Accumulate time spent in updating bounding box + stats.mUpdateBoundsTicks.fetch_add(GetProcessorTickCount() - solve_position_ticks, memory_order_relaxed); + #endif + // We processed work, loop again continue; } diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h index 8ba7b1e1f3..cff88d279c 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h @@ -247,7 +247,8 @@ public: /// - 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 + /// Get the bounding box of all bodies in the physics system. + /// Deprecated: Use GetBroadPhaseQuery().GetBounds() instead. AABox GetBounds() const { return mBroadPhase->GetBounds(); } #ifdef JPH_TRACK_BROADPHASE_STATS @@ -255,6 +256,11 @@ public: void ReportBroadphaseStats() { mBroadPhase->ReportStats(); } #endif // JPH_TRACK_BROADPHASE_STATS +#if defined(JPH_TRACK_SIMULATION_STATS) && defined(JPH_PROFILE_ENABLED) + /// Dump the per body simulation stats to the TTY + void ReportSimulationStats() { mBodyManager.ReportSimulationStats(); } +#endif + private: using CCDBody = PhysicsUpdateContext::Step::CCDBody; @@ -283,6 +289,11 @@ private: /// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet void TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const; +#ifdef JPH_TRACK_SIMULATION_STATS + /// Gather stats from the islands and distribute them over the bodies + void GatherIslandStats(); +#endif + using ContactAllocator = ContactConstraintManager::ContactAllocator; /// Process narrow phase for a single body pair diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp index 7c60ae6be6..4522ee49d6 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp @@ -8,6 +8,7 @@ JPH_NAMESPACE_BEGIN +/// @cond INTERNAL PhysicsUpdateContext::PhysicsUpdateContext(TempAllocator &inTempAllocator) : mTempAllocator(&inTempAllocator), mSteps(inTempAllocator) @@ -19,5 +20,6 @@ PhysicsUpdateContext::~PhysicsUpdateContext() JPH_ASSERT(mBodyPairs == nullptr); JPH_ASSERT(mActiveConstraints == nullptr); } +/// @endcond JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h index fe99c46ccc..c0987a2f73 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h @@ -19,7 +19,10 @@ class Constraint; class TempAllocator; class SoftBodyUpdateContext; -/// Information used during the Update call +/// @cond INTERNAL +/// Internal information used during the PhysicsSystem::Update call +/// +/// WARNING: This class is an internal part of PhysicsSystem, it has no functions that can be called by users of the library. class PhysicsUpdateContext : public NonCopyable { public: @@ -168,5 +171,6 @@ public: SoftBodyUpdateContext * mSoftBodyUpdateContexts = nullptr; ///< Contexts for updating soft bodies atomic mSoftBodyToCollide { 0 }; ///< Next soft body to take when running SoftBodyCollide jobs }; +/// @endcond JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp index ec9e3aa448..660eb4da9f 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp @@ -190,6 +190,38 @@ bool RagdollSettings::Stabilize() return true; } +void RagdollSettings::CalculateConstraintPriorities(uint32 inBasePriority) +{ + JPH_ASSERT(inBasePriority + (uint32)mParts.size() > inBasePriority, "Base priority is too high and will cause overflows"); + JPH_ASSERT(mSkeleton->AreJointsCorrectlyOrdered()); + + // Calculate priority for each part. Start with the base priority and increment towards the root + Array priorities; + priorities.resize(mParts.size(), inBasePriority); + for (int i = (int)mParts.size() - 1; i >= 0; --i) + { + uint32 cur_priority = inBasePriority; + int j = i; + do + { + priorities[j] = max(priorities[j], cur_priority); + cur_priority++; + + j = mSkeleton->GetJoint(j).mParentJointIndex; + } + while (j != -1); + } + + // Copy the priorities to the constraints + for (uint i = 0, n = (uint)mParts.size(); i < n; ++i) + if (mParts[i].mToParent != nullptr) + mParts[i].mToParent->mConstraintPriority = priorities[i]; + + // Use the minimum of the priorities of connected bodies for additional constraints + for (AdditionalConstraint &constraint : mAdditionalConstraints) + constraint.mConstraint->mConstraintPriority = min(priorities[constraint.mBodyIdx[0]], priorities[constraint.mBodyIdx[1]]); +} + void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices, float inMinSeparationDistance) { int joint_count = mSkeleton->GetJointCount(); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h index 8fe401080f..ff8e523212 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h @@ -27,6 +27,11 @@ public: /// @return True on success, false on failure. bool Stabilize(); + /// Initializes the constraint priorities so that constraints near the leaves of the ragdoll have a lower priority + /// than constraints near the root of the ragdoll. + /// @param inBasePriority The lowest priority that will be used in the ragdoll. + void CalculateConstraintPriorities(uint32 inBasePriority = 0); + /// 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. /// diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h index 537e1112ff..4a617c11be 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h @@ -59,7 +59,7 @@ public: 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 mLinearDamping = 0.1f; ///< Linear damping: dv/dt = -mLinearDamping * v. Value should be zero or positive and is usually close to 0. 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 diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h index 14997a4915..c12d5cac40 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h @@ -130,7 +130,7 @@ public: /// Constructor Vertex() = default; - Vertex(const Float3 &inPosition, const Float3 &inVelocity = Float3(0, 0, 0), float inInvMass = 1.0f) : mPosition(inPosition), mVelocity(inVelocity), mInvMass(inInvMass) { } + explicit 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 diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h index 2f565f5693..9b39107078 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h @@ -14,7 +14,10 @@ class SoftBodyMotionProperties; class SoftBodyContactListener; class SimShapeFilter; +/// @cond INTERNAL /// Temporary data used by the update of a soft body +/// +/// WARNING: This class is an internal part of PhysicsSystem, it has no functions that can be called by users of the library. class SoftBodyUpdateContext : public NonCopyable { public: @@ -55,5 +58,6 @@ public: 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 }; +/// @endcond JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h index 7c222756c8..5f20c23be8 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h @@ -90,7 +90,7 @@ public: /// @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)) { } + explicit 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; @@ -133,7 +133,7 @@ public: /// 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); } + explicit 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; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h index 29212ffdab..c1cc35f6dd 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h @@ -36,7 +36,7 @@ public: 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 + float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w. Value should be zero or positive and is usually close to 0. }; /// Runtime data for engine diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h index 487f60c56a..7178151f3c 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h @@ -29,7 +29,7 @@ public: 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 mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w. Value should be zero or positive and is usually close to 0. 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. diff --git a/thirdparty/jolt_physics/patches/0001-backport-upstream-commit-ee3725250.patch b/thirdparty/jolt_physics/patches/0001-backport-upstream-commit-ee3725250.patch new file mode 100644 index 0000000000..466ca09708 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0001-backport-upstream-commit-ee3725250.patch @@ -0,0 +1,53 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp +index e8a8c21459..4fb6c58c18 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp +@@ -92,11 +92,14 @@ void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 + 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); ++ result.mShape2Face.resize(3); ++ result.mShape2Face[0] = mCenterOfMassTransform2 * v0; ++ result.mShape2Face[1] = mCenterOfMassTransform2 * v1; ++ result.mShape2Face[2] = mCenterOfMassTransform2 * v2; + +- // Convert to world space +- for (Vec3 &p : result.mShape2Face) +- p = mCenterOfMassTransform2 * p; ++ // When inside out, we need to swap the triangle winding ++ if (mScaleSign < 0.0f) ++ std::swap(result.mShape2Face[1], result.mShape2Face[2]); + } + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp +index e03659454d..05c21eebfa 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp +@@ -145,6 +145,10 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, + result.mShape2Face[0] = mTransform1 * v0; + result.mShape2Face[1] = mTransform1 * v1; + result.mShape2Face[2] = mTransform1 * v2; ++ ++ // When inside out, we need to swap the triangle winding ++ if (mScaleSign2 < 0.0f) ++ std::swap(result.mShape2Face[1], result.mShape2Face[2]); + } + + // Notify the collector +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp +index 18441ea73b..566efb38ae 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp +@@ -111,6 +111,10 @@ void CollideSphereVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, + result.mShape2Face[0] = mTransform2 * (mSphereCenterIn2 + v0); + result.mShape2Face[1] = mTransform2 * (mSphereCenterIn2 + v1); + result.mShape2Face[2] = mTransform2 * (mSphereCenterIn2 + v2); ++ ++ // When inside out, we need to swap the triangle winding ++ if (mScaleSign2 < 0.0f) ++ std::swap(result.mShape2Face[1], result.mShape2Face[2]); + } + + // Notify the collector diff --git a/thirdparty/jolt_physics/patches/0002-backport-upstream-commit-bc7f1fb8c.patch b/thirdparty/jolt_physics/patches/0002-backport-upstream-commit-bc7f1fb8c.patch new file mode 100644 index 0000000000..dde2762c47 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0002-backport-upstream-commit-bc7f1fb8c.patch @@ -0,0 +1,236 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +index 7de3a4483e..972fc13583 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +@@ -46,7 +46,7 @@ void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstra + } + + 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_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, Vec3Arg inGravity, 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 +@@ -58,39 +58,19 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF + 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; ++ const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); ++ const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + + // 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; +- } ++ static_assert(false, "Static vs static makes no sense"); + float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal); + + // How much the shapes are penetrating (> 0 if penetrating, < 0 if separated) +@@ -114,11 +94,40 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF + // 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); ++ { ++ // The gravity / constant forces are 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 force. ++ // This means that our calculated restitution can be too high resulting in an increase in energy. ++ // So, when we apply restitution, we cancel the added velocity due to these forces. ++ Vec3 relative_acceleration; ++ ++ // Calculate effect of gravity ++ if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static) ++ relative_acceleration = inGravity * (mp2->GetGravityFactor() - mp1->GetGravityFactor()); ++ else if constexpr (Type1 != EMotionType::Static) ++ relative_acceleration = -inGravity * mp1->GetGravityFactor(); ++ else if constexpr (Type2 != EMotionType::Static) ++ relative_acceleration = inGravity * mp2->GetGravityFactor(); ++ else ++ static_assert(false, "Static vs static makes no sense"); ++ ++ // Calculate effect of accumulated forces ++ if constexpr (Type1 == EMotionType::Dynamic) ++ relative_acceleration -= mp1->GetAccumulatedForce() * mp1->GetInverseMass(); ++ if constexpr (Type2 == EMotionType::Dynamic) ++ relative_acceleration += mp2->GetAccumulatedForce() * mp2->GetInverseMass(); ++ ++ // We only compensate forces towards the contact normal. ++ float force_delta_velocity = min(0.0f, relative_acceleration.Dot(inWorldSpaceNormal) * inDeltaTime); ++ ++ normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - force_delta_velocity); ++ } + 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 + { +@@ -706,7 +715,7 @@ void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inC + } + + 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) ++JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2) + { + // Calculate scaled mass and inertia + Mat44 inv_i1; +@@ -737,20 +746,17 @@ JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetr + + 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); ++ wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(inDeltaTime, inGravity, 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) ++inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2) + { + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) +@@ -759,15 +765,15 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: +- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); ++ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Kinematic: +- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); ++ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Static: +- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); ++ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + default: +@@ -778,12 +784,12 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); +- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); ++ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); +- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); ++ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + default: +@@ -863,11 +869,9 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA + RMat44 transform_body1 = body1->GetCenterOfMassTransform(); + RMat44 transform_body2 = body2->GetCenterOfMassTransform(); + +- // Get time step ++ // Get time step and gravity + float delta_time = mUpdateContext->mStepDeltaTime; +- +- // Calculate value for restitution correction +- Vec3 gravity_dt = mUpdateContext->mPhysicsSystem->GetGravity() * delta_time; ++ Vec3 gravity = mUpdateContext->mPhysicsSystem->GetGravity(); + + // Copy manifolds + uint32 output_handle = ManifoldMap::cInvalidHandle; +@@ -970,7 +974,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA + 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); ++ CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity, transform_body1, transform_body2, *body1, *body2); + + // Notify island builder + mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal()); +@@ -1144,11 +1148,9 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i + // Notify island builder + mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, inBody1.GetIndexInActiveBodiesInternal(), inBody2.GetIndexInActiveBodiesInternal()); + +- // Get time step ++ // Get time step and gravity + float delta_time = mUpdateContext->mStepDeltaTime; +- +- // Calculate value for restitution correction +- float gravity_dt_dot_normal = inManifold.mWorldSpaceNormal.Dot(mUpdateContext->mPhysicsSystem->GetGravity() * delta_time); ++ Vec3 gravity = mUpdateContext->mPhysicsSystem->GetGravity(); + + // Calculate scaled mass and inertia + float inv_m1; +@@ -1222,7 +1224,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i + 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); ++ wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(delta_time, gravity, 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 +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h +index 0b5938b77e..dfe5fc8c38 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h ++++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h +@@ -427,7 +427,7 @@ private: + 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); ++ JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, Vec3Arg inGravity, 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; +@@ -485,10 +485,10 @@ public: + private: + /// 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); ++ JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, 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); ++ inline void CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, 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 diff --git a/thirdparty/jolt_physics/patches/0003-backport-upstream-commit-365a15367.patch b/thirdparty/jolt_physics/patches/0003-backport-upstream-commit-365a15367.patch new file mode 100644 index 0000000000..11690b04d9 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0003-backport-upstream-commit-365a15367.patch @@ -0,0 +1,28 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +index 972fc13583..09dfab4921 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +@@ -70,7 +70,10 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF + else if constexpr (Type2 != EMotionType::Static) + relative_velocity = mp2->GetPointVelocityCOM(r2); + else +- static_assert(false, "Static vs static makes no sense"); ++ { ++ JPH_ASSERT(false, "Static vs static makes no sense"); ++ relative_velocity = Vec3::sZero(); ++ } + float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal); + + // How much the shapes are penetrating (> 0 if penetrating, < 0 if separated) +@@ -109,7 +112,10 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF + else if constexpr (Type2 != EMotionType::Static) + relative_acceleration = inGravity * mp2->GetGravityFactor(); + else +- static_assert(false, "Static vs static makes no sense"); ++ { ++ JPH_ASSERT(false, "Static vs static makes no sense"); ++ relative_acceleration = Vec3::sZero(); ++ } + + // Calculate effect of accumulated forces + if constexpr (Type1 == EMotionType::Dynamic) diff --git a/thirdparty/jolt_physics/patches/0004-backport-upstream-commit-e0a6a9a16.patch b/thirdparty/jolt_physics/patches/0004-backport-upstream-commit-e0a6a9a16.patch new file mode 100644 index 0000000000..72788af482 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0004-backport-upstream-commit-e0a6a9a16.patch @@ -0,0 +1,61 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.h b/thirdparty/jolt_physics/Jolt/Math/Quat.h +index 6e7e3bef6c..1971e2ddf1 100644 +--- a/thirdparty/jolt_physics/Jolt/Math/Quat.h ++++ b/thirdparty/jolt_physics/Jolt/Math/Quat.h +@@ -111,6 +111,9 @@ public: + /// 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; + ++ /// Calculate angular velocity given that this quaternion represents the rotation that is reached after inDeltaTime when starting from identity rotation ++ JPH_INLINE Vec3 GetAngularVelocity(float inDeltaTime) 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); +diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.inl b/thirdparty/jolt_physics/Jolt/Math/Quat.inl +index 57e1947b30..7c160a5f23 100644 +--- a/thirdparty/jolt_physics/Jolt/Math/Quat.inl ++++ b/thirdparty/jolt_physics/Jolt/Math/Quat.inl +@@ -151,6 +151,26 @@ void Quat::GetAxisAngle(Vec3 &outAxis, float &outAngle) const + } + } + ++Vec3 Quat::GetAngularVelocity(float inDeltaTime) const ++{ ++ JPH_ASSERT(IsNormalized()); ++ ++ // w = cos(angle / 2), ensure it is positive so that we get an angle in the range [0, PI] ++ Quat w_pos = EnsureWPositive(); ++ ++ // The imaginary part of the quaternion is axis * sin(angle / 2), ++ // if the length is small use the approximation sin(x) = x to calculate angular velocity ++ Vec3 xyz = w_pos.GetXYZ(); ++ float xyz_len_sq = xyz.LengthSq(); ++ if (xyz_len_sq < 4.0e-4f) // Max error introduced is sin(0.02) - 0.02 = 7e-5 (when w is near 1 the angle becomes more inaccurate in the code below, so don't make this number too small) ++ return (2.0f / inDeltaTime) * xyz; ++ ++ // Otherwise calculate the angle from w = cos(angle / 2) and determine the axis by normalizing the imaginary part ++ // Note that it is also possible to calculate the angle through angle = 2 * atan2(|xyz|, w). This is more accurate but also 2x as expensive. ++ float angle = 2.0f * ACos(w_pos.GetW()); ++ return (xyz / (sqrt(xyz_len_sq) * inDeltaTime)) * angle; ++} ++ + Quat Quat::sFromTo(Vec3Arg inFrom, Vec3Arg inTo) + { + /* +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl +index 474ab15c18..e6caae61f5 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl ++++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl +@@ -17,10 +17,7 @@ void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRot + mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime); + + // Calculate required angular velocity +- Vec3 axis; +- float angle; +- inDeltaRotation.GetAxisAngle(axis, angle); +- mAngularVelocity = LockAngular(axis * (angle / inDeltaTime)); ++ mAngularVelocity = LockAngular(inDeltaRotation.GetAngularVelocity(inDeltaTime)); + } + + void MotionProperties::ClampLinearVelocity()