Jolt: Update to 5.5.0

2026-02-04 22:43:01 +01:00
parent bf95b62586
commit 998f97a2a1
70 changed files with 1041 additions and 195 deletions
@@ -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

@@ -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<Node> &inNodes) const { return mChild[inIdx] != cInvalidNodeIndex? &inNodes[mChild[inIdx]] : nullptr; }
+
 		/// Min depth of tree
 		uint				GetMinDepth(const Array<Node> &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);
@@ -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: "
@@ -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);
 		}
@@ -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;
@@ -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 <float.h>
 #include <limits.h>
 #include <string.h>
+#include <new>
 #include <utility>
 #include <cmath>
 #include <sstream>
@@ -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
@@ -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;
@@ -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): '<unnamed-enum-DISPLAYCONFIG_OUTPUT_TECHNOLOGY_OTHER>': 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 <windows.h>
 #endif
-
 	JPH_SUPPRESS_WARNING_POP
 #endif
 #ifdef JPH_PLATFORM_LINUX
@@ -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<size_t>(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<size_t>(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; } \
@@ -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);
@@ -44,7 +44,7 @@ public:
 	inline					STLAllocator(const STLAllocator<T2> &) { }

 	/// 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)
@@ -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): '<unnamed-enum-DISPLAYCONFIG_OUTPUT_TECHNOLOGY_OTHER>': 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
@@ -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<uint8 *>(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT))),
-		mSize(inSize)
+	explicit						TempAllocatorImpl(size_t inSize) : mSize(inSize)
 	{
+		if constexpr (needs_aligned_allocate)
+			mBase = static_cast<uint8 *>(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT));
+		else
+			mBase = static_cast<uint8 *>(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);
+		}
 	}
 };

@@ -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): '<unnamed-enum-DISPLAYCONFIG_OUTPUT_TECHNOLOGY_OTHER>': 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
@@ -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
@@ -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)
 		{
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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();

@@ -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
@@ -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
@@ -40,6 +40,9 @@ using BodyVector = Array<Body *>;
 using BodyIDVector = Array<BodyID>;

 /// 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
@@ -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<uint64>		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<uint64>		mCCDTicks = 0;													///< Number of ticks spent doing CCD
+		atomic<uint32>		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
@@ -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:
@@ -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.
@@ -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
@@ -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<MotionProperties::SimulationStats &>(body1.GetMotionPropertiesUnchecked()->GetSimulationStats()).mBroadPhaseTicks += num_ticks;
+	#endif
 	}

 	// Test that the root node was not swapped while finding collision pairs.
@@ -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);
 	}
@@ -6,6 +6,7 @@

 #include <Jolt/Physics/Collision/CollideShape.h>
 #include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
 #include <Jolt/Core/STLLocalAllocator.h>

 JPH_NAMESPACE_BEGIN
@@ -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;
@@ -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;
@@ -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; }
@@ -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<SubShape>;

@@ -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<Vec3> &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { }
+	explicit				ConvexHullShapeSettings(const Array<Vec3> &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { }

 	// See: ShapeSettings
 	virtual ShapeResult		Create() const override;
@@ -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);
 }

@@ -23,8 +23,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(EmptyShapeSettings)
 ShapeSettings::ShapeResult EmptyShapeSettings::Create() const
 {
 	if (mCachedResult.IsEmpty())
-		new EmptyShape(*this, mCachedResult);
-
+		Ref<Shape> shape = new EmptyShape(*this, mCachedResult);
 	return mCachedResult;
 }

@@ -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));
@@ -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]);
@@ -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.
@@ -23,7 +23,6 @@ ShapeSettings::ShapeResult MutableCompoundShapeSettings::Create() const
 	// Build a mutable compound shape
 	if (mCachedResult.IsEmpty())
 		Ref<Shape> shape = new MutableCompoundShape(*this, mCachedResult);
-
 	return mCachedResult;
 }

@@ -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
@@ -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; }
@@ -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;
 	}

@@ -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
@@ -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
@@ -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;

@@ -21,6 +21,9 @@ class DebugRenderer;
 using Constraints = Array<Ref<Constraint>>;

 /// 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:
@@ -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
 	{
@@ -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:
@@ -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)
@@ -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<uint64>		mVelocityConstraintTicks = 0;
+		atomic<uint64>		mPositionConstraintTicks = 0;
+		atomic<uint64>		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
@@ -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:
@@ -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<uint64>(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<MotionProperties::SimulationStats &>(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;
 		}
@@ -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
@@ -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
@@ -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<uint>			mSoftBodyToCollide { 0 };								///< Next soft body to take when running SoftBodyCollide jobs
 };
+/// @endcond

 JPH_NAMESPACE_END
@@ -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<uint32> 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();
@@ -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.
 	///
@@ -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
@@ -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
@@ -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
@@ -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;
@@ -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
@@ -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.
@@ -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<const ConvexShape *>(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
@@ -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 <EMotionType Type1, EMotionType Type2>
+-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 <EMotionType Type1, EMotionType Type2>
+-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<Type1, Type2>(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<Type1, Type2>(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<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
+ 			break;
+ 
+ 		case EMotionType::Kinematic:
+-			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
+ 			break;
+ 
+ 		case EMotionType::Static:
+-			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(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<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
+ 		break;
+ 
+ 	case EMotionType::Static:
+ 		JPH_ASSERT(inBody2.IsDynamic());
+-		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(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<Type1, Type2>(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<Type1, Type2>(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 <EMotionType Type1, EMotionType Type2>
+-		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 <EMotionType Type1, EMotionType Type2>
+-	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 <EMotionType Type1, EMotionType Type2>
@@ -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)
@@ -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()