Jolt: Update to 5.3.0

thirdparty/jolt_physics/Jolt/Core/Array.h

@@ -47,6 +47,83 @@ public:
 	using const_iterator = const T *;
 	using iterator = T *;
 
+	/// An iterator that traverses the array in reverse order
+	class rev_it
+	{
+	public:
+		/// Constructor
+							rev_it() = default;
+		explicit			rev_it(T *inValue)				: mValue(inValue) { }
+
+		/// Copying
+							rev_it(const rev_it &) = default;
+		rev_it &			operator = (const rev_it &) = default;
+
+		/// Comparison
+		bool				operator == (const rev_it &inRHS) const { return mValue == inRHS.mValue; }
+		bool				operator != (const rev_it &inRHS) const { return mValue != inRHS.mValue; }
+
+		/// Arithmetics
+		rev_it &			operator ++ ()					{ --mValue; return *this; }
+		rev_it				operator ++ (int)				{ return rev_it(mValue--); }
+		rev_it &			operator -- ()					{ ++mValue; return *this; }
+		rev_it				operator -- (int)				{ return rev_it(mValue++); }
+
+		rev_it				operator + (int inValue)		{ return rev_it(mValue - inValue); }
+		rev_it				operator - (int inValue)		{ return rev_it(mValue + inValue); }
+
+		rev_it &			operator += (int inValue)		{ mValue -= inValue; return *this; }
+		rev_it &			operator -= (int inValue)		{ mValue += inValue; return *this; }
+
+		/// Access
+		T &					operator * () const				{ return *mValue; }
+		T &					operator -> () const			{ return *mValue; }
+
+	private:
+		T *					mValue;
+	};
+
+	/// A const iterator that traverses the array in reverse order
+	class crev_it
+	{
+	public:
+		/// Constructor
+							crev_it() = default;
+		explicit			crev_it(const T *inValue)		: mValue(inValue) { }
+
+		/// Copying
+							crev_it(const crev_it &) = default;
+		explicit			crev_it(const rev_it &inValue)	: mValue(inValue.mValue) { }
+		crev_it &			operator = (const crev_it &) = default;
+		crev_it &			operator = (const rev_it &inRHS) { mValue = inRHS.mValue; return *this; }
+
+		/// Comparison
+		bool				operator == (const crev_it &inRHS) const { return mValue == inRHS.mValue; }
+		bool				operator != (const crev_it &inRHS) const { return mValue != inRHS.mValue; }
+
+		/// Arithmetics
+		crev_it &			operator ++ ()					{ --mValue; return *this; }
+		crev_it				operator ++ (int)				{ return crev_it(mValue--); }
+		crev_it &			operator -- ()					{ ++mValue; return *this; }
+		crev_it				operator -- (int)				{ return crev_it(mValue++); }
+
+		crev_it				operator + (int inValue)		{ return crev_it(mValue - inValue); }
+		crev_it				operator - (int inValue)		{ return crev_it(mValue + inValue); }
+
+		crev_it &			operator += (int inValue)		{ mValue -= inValue; return *this; }
+		crev_it &			operator -= (int inValue)		{ mValue += inValue; return *this; }
+
+		/// Access
+		const T &			operator * () const				{ return *mValue; }
+		const T &			operator -> () const			{ return *mValue; }
+
+	private:
+		const T *			mValue;
+	};
+
+	using reverse_iterator = rev_it;
+	using const_reverse_iterator = crev_it;
+
 private:
 	/// Move elements from one location to another
 	inline void				move(pointer inDestination, pointer inSource, size_type inCount)
@@ -388,7 +465,7 @@ public:
 	}
 
 	/// Remove one element from the array
-	void					erase(const_iterator inIter)
+	iterator				erase(const_iterator inIter)
 	{
 		size_type p = size_type(inIter - begin());
 		JPH_ASSERT(p < mSize);
@@ -396,10 +473,11 @@ public:
 		if (p + 1 < mSize)
 			move(mElements + p, mElements + p + 1, mSize - p - 1);
 		--mSize;
+		return const_cast<iterator>(inIter);
 	}
 
 	/// Remove multiple element from the array
-	void					erase(const_iterator inBegin, const_iterator inEnd)
+	iterator				erase(const_iterator inBegin, const_iterator inEnd)
 	{
 		size_type p = size_type(inBegin - begin());
 		size_type n = size_type(inEnd - inBegin);
@@ -408,6 +486,7 @@ public:
 		if (p + n < mSize)
 			move(mElements + p, mElements + p + n, mSize - p - n);
 		mSize -= n;
+		return const_cast<iterator>(inBegin);
 	}
 
 	/// Iterators
@@ -421,14 +500,34 @@ public:
 		return mElements + mSize;
 	}
 
+	inline crev_it			rbegin() const
+	{
+		return crev_it(mElements + mSize - 1);
+	}
+
+	inline crev_it			rend() const
+	{
+		return crev_it(mElements - 1);
+	}
+
 	inline const_iterator	cbegin() const
 	{
-		return mElements;
+		return begin();
 	}
 
 	inline const_iterator	cend() const
 	{
-		return mElements + mSize;
+		return end();
+	}
+
+	inline crev_it			crbegin() const
+	{
+		return rbegin();
+	}
+
+	inline crev_it			crend() const
+	{
+		return rend();
 	}
 
 	inline iterator			begin()
@@ -441,6 +540,16 @@ public:
 		return mElements + mSize;
 	}
 
+	inline rev_it			rbegin()
+	{
+		return rev_it(mElements + mSize - 1);
+	}
+
+	inline rev_it			rend()
+	{
+		return rev_it(mElements - 1);
+	}
+
 	inline const T *		data() const
 	{
 		return mElements;

thirdparty/jolt_physics/Jolt/Core/Color.h

@@ -12,7 +12,7 @@ class Color;
 using ColorArg = Color;
 
 /// Class that holds an RGBA color with 8-bits per component
-class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND Color
+class JPH_EXPORT_GCC_BUG_WORKAROUND [[nodiscard]] Color
 {
 public:
 	/// Constructors

thirdparty/jolt_physics/Jolt/Core/Core.h

@@ -6,8 +6,8 @@
 
 // Jolt library version
 #define JPH_VERSION_MAJOR 5
-#define JPH_VERSION_MINOR 2
-#define JPH_VERSION_PATCH 1
+#define JPH_VERSION_MINOR 3
+#define JPH_VERSION_PATCH 0
 
 // Determine which features the library was compiled with
 #ifdef JPH_DOUBLE_PRECISION
@@ -83,8 +83,8 @@
 	#define JPH_PLATFORM_ANDROID
 #elif defined(__linux__)
 	#define JPH_PLATFORM_LINUX
-#elif defined(__FreeBSD__)
-	#define JPH_PLATFORM_FREEBSD
+#elif defined(__FreeBSD__) || defined(__OpenBSD__) || defined(__NetBSD__)
+	#define JPH_PLATFORM_BSD
 #elif defined(__APPLE__)
 	#include <TargetConditionals.h>
 	#if defined(TARGET_OS_IPHONE) && !TARGET_OS_IPHONE
@@ -195,7 +195,11 @@
 #elif defined(JPH_PLATFORM_WASM)
 	// WebAssembly CPU architecture
 	#define JPH_CPU_WASM
-	#define JPH_CPU_ADDRESS_BITS 32
+	#if defined(__wasm64__)
+		#define JPH_CPU_ADDRESS_BITS 64
+	#else
+		#define JPH_CPU_ADDRESS_BITS 32
+	#endif
 	#define JPH_VECTOR_ALIGNMENT 16
 	#define JPH_DVECTOR_ALIGNMENT 32
 	#ifdef __wasm_simd128__
@@ -360,6 +364,7 @@
 	JPH_MSVC_SUPPRESS_WARNING(4514) /* 'X' : unreferenced inline function has been removed */	\
 	JPH_MSVC_SUPPRESS_WARNING(4710) /* 'X' : function not inlined */							\
 	JPH_MSVC_SUPPRESS_WARNING(4711) /* function 'X' selected for automatic inline expansion */	\
+	JPH_MSVC_SUPPRESS_WARNING(4714) /* function 'X' marked as __forceinline not inlined */		\
 	JPH_MSVC_SUPPRESS_WARNING(4820) /* 'X': 'Y' bytes padding added after data member 'Z' */	\
 	JPH_MSVC_SUPPRESS_WARNING(4100) /* 'X' : unreferenced formal parameter */					\
 	JPH_MSVC_SUPPRESS_WARNING(4626) /* 'X' : assignment operator was implicitly defined as deleted because a base class assignment operator is inaccessible or deleted */ \
@@ -388,9 +393,9 @@
 	// Configuration for a popular game console.
 	// This file is not distributed because it would violate an NDA.
 	// Creating one should only be a couple of minutes of work if you have the documentation for the platform
-	// (you only need to define JPH_BREAKPOINT, JPH_PLATFORM_BLUE_GET_TICKS, JPH_PLATFORM_BLUE_MUTEX*, JPH_PLATFORM_BLUE_RWLOCK* and include the right header).
+	// (you only need to define JPH_BREAKPOINT, JPH_PLATFORM_BLUE_GET_TICKS, JPH_PLATFORM_BLUE_MUTEX*, JPH_PLATFORM_BLUE_RWLOCK*, JPH_PLATFORM_BLUE_SEMAPHORE* and include the right header).
 	#include <Jolt/Core/PlatformBlue.h>
-#elif defined(JPH_PLATFORM_LINUX) || defined(JPH_PLATFORM_ANDROID) || defined(JPH_PLATFORM_MACOS) || defined(JPH_PLATFORM_IOS) || defined(JPH_PLATFORM_FREEBSD)
+#elif defined(JPH_PLATFORM_LINUX) || defined(JPH_PLATFORM_ANDROID) || defined(JPH_PLATFORM_MACOS) || defined(JPH_PLATFORM_IOS) || defined(JPH_PLATFORM_BSD)
 	#if defined(JPH_CPU_X86)
 		#define JPH_BREAKPOINT	__asm volatile ("int $0x3")
 	#elif defined(JPH_CPU_ARM) || defined(JPH_CPU_RISCV) || defined(JPH_CPU_E2K) || defined(JPH_CPU_PPC) || defined(JPH_CPU_LOONGARCH)
@@ -426,7 +431,8 @@
 	JPH_MSVC_SUPPRESS_WARNING(4514)																\
 	JPH_MSVC_SUPPRESS_WARNING(5262)																\
 	JPH_MSVC_SUPPRESS_WARNING(5264)																\
-	JPH_MSVC_SUPPRESS_WARNING(4738)
+	JPH_MSVC_SUPPRESS_WARNING(4738)																\
+	JPH_MSVC_SUPPRESS_WARNING(5045)
 
 #define JPH_SUPPRESS_WARNINGS_STD_END															\
 	JPH_SUPPRESS_WARNING_POP

thirdparty/jolt_physics/Jolt/Core/FPException.h

@@ -16,11 +16,12 @@ JPH_NAMESPACE_BEGIN
 class FPExceptionsEnable { };
 class FPExceptionDisableInvalid { };
 class FPExceptionDisableDivByZero { };
+class FPExceptionDisableOverflow { };
 
 #elif defined(JPH_USE_SSE)
 
-/// Enable floating point divide by zero exception and exceptions on invalid numbers
-class FPExceptionsEnable : public FPControlWord<0, _MM_MASK_DIV_ZERO | _MM_MASK_INVALID> { };
+/// Enable floating point divide by zero exception, overflow exceptions and exceptions on invalid numbers
+class FPExceptionsEnable : public FPControlWord<0, _MM_MASK_DIV_ZERO | _MM_MASK_INVALID | _MM_MASK_OVERFLOW> { };
 
 /// Disable invalid floating point value exceptions
 class FPExceptionDisableInvalid : public FPControlWord<_MM_MASK_INVALID, _MM_MASK_INVALID> { };
@@ -28,10 +29,13 @@ class FPExceptionDisableInvalid : public FPControlWord<_MM_MASK_INVALID, _MM_MAS
 /// Disable division by zero floating point exceptions
 class FPExceptionDisableDivByZero : public FPControlWord<_MM_MASK_DIV_ZERO, _MM_MASK_DIV_ZERO> { };
 
+/// Disable floating point overflow exceptions
+class FPExceptionDisableOverflow : public FPControlWord<_MM_MASK_OVERFLOW, _MM_MASK_OVERFLOW> { };
+
 #elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC)
 
-/// Enable floating point divide by zero exception and exceptions on invalid numbers
-class FPExceptionsEnable : public FPControlWord<0, _EM_INVALID | _EM_ZERODIVIDE> { };
+/// Enable floating point divide by zero exception, overflow exceptions and exceptions on invalid numbers
+class FPExceptionsEnable : public FPControlWord<0, _EM_INVALID | _EM_ZERODIVIDE | _EM_OVERFLOW> { };
 
 /// Disable invalid floating point value exceptions
 class FPExceptionDisableInvalid : public FPControlWord<_EM_INVALID, _EM_INVALID> { };
@@ -39,6 +43,9 @@ class FPExceptionDisableInvalid : public FPControlWord<_EM_INVALID, _EM_INVALID>
 /// Disable division by zero floating point exceptions
 class FPExceptionDisableDivByZero : public FPControlWord<_EM_ZERODIVIDE, _EM_ZERODIVIDE> { };
 
+/// Disable floating point overflow exceptions
+class FPExceptionDisableOverflow : public FPControlWord<_EM_OVERFLOW, _EM_OVERFLOW> { };
+
 #elif defined(JPH_CPU_ARM)
 
 /// Invalid operation exception bit
@@ -47,8 +54,11 @@ static constexpr uint64 FP_IOE = 1 << 8;
 /// Enable divide by zero exception bit
 static constexpr uint64 FP_DZE = 1 << 9;
 
-/// Enable floating point divide by zero exception and exceptions on invalid numbers
-class FPExceptionsEnable : public FPControlWord<FP_IOE | FP_DZE, FP_IOE | FP_DZE> { };
+/// Enable floating point overflow bit
+static constexpr uint64 FP_OFE = 1 << 10;
+
+/// Enable floating point divide by zero exception, overflow exceptions and exceptions on invalid numbers
+class FPExceptionsEnable : public FPControlWord<FP_IOE | FP_DZE | FP_OFE, FP_IOE | FP_DZE | FP_OFE> { };
 
 /// Disable invalid floating point value exceptions
 class FPExceptionDisableInvalid : public FPControlWord<0, FP_IOE> { };
@@ -56,6 +66,9 @@ class FPExceptionDisableInvalid : public FPControlWord<0, FP_IOE> { };
 /// Disable division by zero floating point exceptions
 class FPExceptionDisableDivByZero : public FPControlWord<0, FP_DZE> { };
 
+/// Disable floating point overflow exceptions
+class FPExceptionDisableOverflow : public FPControlWord<0, FP_OFE> { };
+
 #elif defined(JPH_CPU_RISCV)
 
 #error "RISC-V only implements manually checking if exceptions occurred by reading the fcsr register. It doesn't generate exceptions. JPH_FLOATING_POINT_EXCEPTIONS_ENABLED must be disabled."
@@ -76,6 +89,7 @@ class FPExceptionDisableDivByZero : public FPControlWord<0, FP_DZE> { };
 class FPExceptionsEnable { };
 class FPExceptionDisableInvalid { };
 class FPExceptionDisableDivByZero { };
+class FPExceptionDisableOverflow { };
 
 #endif
 

thirdparty/jolt_physics/Jolt/Core/HashTable.h

@@ -26,10 +26,10 @@ private:
 	class IteratorBase
 	{
 	public:
-        /// Properties
+		/// Properties
 		using difference_type = typename Table::difference_type;
-        using value_type = typename Table::value_type;
-        using iterator_category = std::forward_iterator_tag;
+		using value_type = typename Table::value_type;
+		using iterator_category = std::forward_iterator_tag;
 
 		/// Copy constructor
 							IteratorBase(const IteratorBase &inRHS) = default;
@@ -179,17 +179,9 @@ private:
 		mSize = inRHS.mSize;
 	}
 
-	/// Grow the table to the next power of 2
-	void					GrowTable()
+	/// Grow the table to a new size
+	void					GrowTable(size_type inNewMaxSize)
 	{
-		// Calculate new size
-		size_type new_max_size = max<size_type>(mMaxSize << 1, 16);
-		if (new_max_size < mMaxSize)
-		{
-			JPH_ASSERT(false, "Overflow in hash table size, can't grow!");
-			return;
-		}
-
 		// Move the old table to a temporary structure
 		size_type old_max_size = mMaxSize;
 		KeyValue *old_data = mData;
@@ -201,7 +193,7 @@ private:
 		mLoadLeft = 0;
 
 		// Allocate new table
-		AllocateTable(new_max_size);
+		AllocateTable(inNewMaxSize);
 
 		// Reset all control bytes
 		memset(mControl, cBucketEmpty, mMaxSize + 15);
@@ -252,7 +244,16 @@ protected:
 			if (num_deleted * cMaxDeletedElementsDenominator > mMaxSize * cMaxDeletedElementsNumerator)
 				rehash(0);
 			else
-				GrowTable();
+			{
+				// Grow by a power of 2
+				size_type new_max_size = max<size_type>(mMaxSize << 1, 16);
+				if (new_max_size < mMaxSize)
+				{
+					JPH_ASSERT(false, "Overflow in hash table size, can't grow!");
+					return false;
+				}
+				GrowTable(new_max_size);
+			}
 		}
 
 		// Split hash into index and control value
@@ -363,9 +364,9 @@ public:
 		using Base = IteratorBase<HashTable, iterator>;
 
 	public:
-        /// Properties
-        using reference = typename Base::value_type &;
-        using pointer = typename Base::value_type *;
+		/// Properties
+		using reference = typename Base::value_type &;
+		using pointer = typename Base::value_type *;
 
 		/// Constructors
 		explicit			iterator(HashTable *inTable) : Base(inTable) { }
@@ -400,9 +401,9 @@ public:
 		using Base = IteratorBase<const HashTable, const_iterator>;
 
 	public:
-        /// Properties
-        using reference = const typename Base::value_type &;
-        using pointer = const typename Base::value_type *;
+		/// Properties
+		using reference = const typename Base::value_type &;
+		using pointer = const typename Base::value_type *;
 
 		/// Constructors
 		explicit			const_iterator(const HashTable *inTable) : Base(inTable) { }
@@ -489,11 +490,7 @@ public:
 		if (max_size <= mMaxSize)
 			return;
 
-		// Allocate buffers
-		AllocateTable(max_size);
-
-		// Reset all control bytes
-		memset(mControl, cBucketEmpty, mMaxSize + 15);
+		GrowTable(max_size);
 	}
 
 	/// Destroy the entire hash table
@@ -523,6 +520,27 @@ public:
 		}
 	}
 
+	/// Destroy the entire hash table but keeps the memory allocated
+	void					ClearAndKeepMemory()
+	{
+		// Destruct elements
+		if constexpr (!std::is_trivially_destructible<KeyValue>())
+			if (!empty())
+				for (size_type i = 0; i < mMaxSize; ++i)
+					if (mControl[i] & cBucketUsed)
+						mData[i].~KeyValue();
+		mSize = 0;
+
+		// If there are elements that are not marked cBucketEmpty, we reset them
+		size_type max_load = sGetMaxLoad(mMaxSize);
+		if (mLoadLeft != max_load)
+		{
+			// Reset all control bytes
+			memset(mControl, cBucketEmpty, mMaxSize + 15);
+			mLoadLeft = max_load;
+		}
+	}
+
 	/// Iterator to first element
 	iterator				begin()
 	{

thirdparty/jolt_physics/Jolt/Core/Profiler.cpp

@@ -222,8 +222,331 @@ void Profiler::DumpChart(const char *inTag, const Threads &inThreads, const KeyT
 <html>
 	<head>
 		<title>Profile Chart</title>
-		<link rel="stylesheet" href="WebIncludes/profile_chart.css">
-		<script type="text/javascript" src="WebIncludes/profile_chart.js"></script>
+		<style>
+			html, body {
+				padding: 0px;
+				border: 0px;
+				margin: 0px;
+				width: 100%;
+				height: 100%;
+				overflow: hidden;
+			}
+
+			canvas {
+				position: absolute;
+				top: 10px;
+				left: 10px;
+				padding: 0px;
+				border: 0px;
+				margin: 0px;
+			}
+
+			#tooltip {
+				font: Courier New;
+				position: absolute;
+				background-color: white;
+				border: 1px;
+				border-style: solid;
+				border-color: black;
+				pointer-events: none;
+				padding: 5px;
+				font: 14px Arial;
+				visibility: hidden;
+				height: auto;
+			}
+
+			.stat {
+				color: blue;
+				text-align: right;
+			}
+		</style>
+		<script type="text/javascript">
+			var canvas;
+			var ctx;
+			var tooltip;
+			var min_scale;
+			var scale;
+			var offset_x = 0;
+			var offset_y = 0;
+			var size_y;
+			var dragging = false;
+			var previous_x = 0;
+			var previous_y = 0;
+			var bar_height = 15;
+			var line_height = bar_height + 2;
+			var thread_separation = 6;
+			var thread_font_size = 12;
+			var thread_font = thread_font_size + "px Arial";
+			var bar_font_size = 10;
+			var bar_font = bar_font_size + "px Arial";
+			var end_cycle = 0;
+
+			function drawChart()
+			{
+				ctx.clearRect(0, 0, canvas.width, canvas.height);
+
+				ctx.lineWidth = 1;
+
+				var y = offset_y;
+
+				for (var t = 0; t < threads.length; t++)
+				{
+					// Check if thread has samples
+					var thread = threads[t];
+					if (thread.start.length == 0)
+						continue;
+
+					// Draw thread name
+					y += thread_font_size;
+					ctx.font = thread_font;
+					ctx.fillStyle = "#000000";
+					ctx.fillText(thread.thread_name, 0, y);
+					y += thread_separation;
+
+					// Draw outlines for each bar of samples
+					ctx.fillStyle = "#c0c0c0";
+					for (var d = 0; d <= thread.max_depth; d++)
+						ctx.fillRect(0, y + d * line_height, canvas.width, bar_height);
+
+					// Draw samples
+					ctx.font = bar_font;
+					for (var s = 0; s < thread.start.length; s++)
+					{
+						// Cull bar
+						var rx = scale * (offset_x + thread.start[s]);
+						if (rx > canvas.width) // right of canvas
+							break;
+						var rw = scale * thread.cycles[s];
+						if (rw < 0.5) // less than half pixel, skip
+							continue;
+						if (rx + rw < 0) // left of canvas
+							continue;
+
+						// Draw bar
+						var ry = y + line_height * thread.depth[s];
+						ctx.fillStyle = thread.color[s];
+						ctx.fillRect(rx, ry, rw, bar_height);
+						ctx.strokeStyle = thread.darkened_color[s];
+						ctx.strokeRect(rx, ry, rw, bar_height);
+
+						// Get index in aggregated list
+						var a = thread.aggregator[s];
+
+						// Draw text
+						if (rw > aggregated.name_width[a])
+						{
+							ctx.fillStyle = "#000000";
+							ctx.fillText(aggregated.name[a], rx + (rw - aggregated.name_width[a]) / 2, ry + bar_height - 4);
+						}
+					}
+
+					// Next line
+					y += line_height * (1 + thread.max_depth) + thread_separation;
+				}
+
+				// Update size
+				size_y = y - offset_y;
+			}
+
+			function drawTooltip(mouse_x, mouse_y)
+			{
+				var y = offset_y;
+
+				for (var t = 0; t < threads.length; t++)
+				{
+					// Check if thread has samples
+					var thread = threads[t];
+					if (thread.start.length == 0)
+						continue;
+
+					// Thead name
+					y += thread_font_size + thread_separation;
+
+					// Draw samples
+					for (var s = 0; s < thread.start.length; s++)
+					{
+						// Cull bar
+						var rx = scale * (offset_x + thread.start[s]);
+						if (rx > mouse_x)
+							break;
+						var rw = scale * thread.cycles[s];
+						if (rx + rw < mouse_x)
+							continue;
+
+						var ry = y + line_height * thread.depth[s];
+						if (mouse_y >= ry && mouse_y < ry + bar_height)
+						{
+							// Get index into aggregated list
+							var a = thread.aggregator[s];
+
+							// Found bar, fill in tooltip
+							tooltip.style.left = (canvas.offsetLeft + mouse_x) + "px";
+							tooltip.style.top = (canvas.offsetTop + mouse_y) + "px";
+							tooltip.style.visibility = "visible";
+							tooltip.innerHTML = aggregated.name[a] + "<br>"
+								+ "<table>"
+								+ "<tr><td>Time:</td><td class=\"stat\">" + (1000000 * thread.cycles[s] / cycles_per_second).toFixed(2) + " &micro;s</td></tr>"
+								+ "<tr><td>Start:</td><td class=\"stat\">" + (1000000 * thread.start[s] / cycles_per_second).toFixed(2) + " &micro;s</td></tr>"
+								+ "<tr><td>End:</td><td class=\"stat\">" + (1000000 * (thread.start[s] + thread.cycles[s]) / cycles_per_second).toFixed(2) + " &micro;s</td></tr>"
+								+ "<tr><td>Avg. Time:</td><td class=\"stat\">" + (1000000 * aggregated.cycles_per_frame[a] / cycles_per_second / aggregated.calls[a]).toFixed(2) + " &micro;s</td></tr>"
+								+ "<tr><td>Min Time:</td><td class=\"stat\">" + (1000000 * aggregated.min_cycles[a] / cycles_per_second).toFixed(2) + " &micro;s</td></tr>"
+								+ "<tr><td>Max Time:</td><td class=\"stat\">" + (1000000 * aggregated.max_cycles[a] / cycles_per_second).toFixed(2) + " &micro;s</td></tr>"
+								+ "<tr><td>Time / Frame:</td><td class=\"stat\">" + (1000000 * aggregated.cycles_per_frame[a] / cycles_per_second).toFixed(2) + " &micro;s</td></tr>"
+								+ "<tr><td>Calls:</td><td class=\"stat\">" + aggregated.calls[a] + "</td></tr>"
+								+ "</table>";
+							return;
+						}
+					}
+
+					// Next line
+					y += line_height * (1 + thread.max_depth) + thread_separation;
+				}
+
+				// No bar found, hide tooltip
+				tooltip.style.visibility = "hidden";
+			}
+
+			function onMouseDown(evt)
+			{
+				dragging = true;
+				previous_x = evt.clientX, previous_y = evt.clientY;
+				tooltip.style.visibility = "hidden";
+			}
+
+			function onMouseUp(evt)
+			{
+				dragging = false;
+			}
+
+			function clampMotion()
+			{
+				// Clamp horizontally
+				var min_offset_x = canvas.width / scale - end_cycle;
+				if (offset_x < min_offset_x)
+					offset_x = min_offset_x;
+				if (offset_x > 0)
+					offset_x = 0;
+
+				// Clamp vertically
+				var min_offset_y = canvas.height - size_y;
+				if (offset_y < min_offset_y)
+					offset_y = min_offset_y;
+				if (offset_y > 0)
+					offset_y = 0;
+
+				// Clamp scale
+				if (scale < min_scale)
+					scale = min_scale;
+				var max_scale = 1000 * min_scale;
+				if (scale > max_scale)
+					scale = max_scale;
+			}
+
+			function onMouseMove(evt)
+			{
+				if (dragging)
+				{
+					// Calculate new offset
+					offset_x += (evt.clientX - previous_x) / scale;
+					offset_y += evt.clientY - previous_y;
+
+					clampMotion();
+
+					drawChart();
+				}
+				else
+					drawTooltip(evt.clientX - canvas.offsetLeft, evt.clientY - canvas.offsetTop);
+
+				previous_x = evt.clientX, previous_y = evt.clientY;
+			}
+
+			function onScroll(evt)
+			{
+				tooltip.style.visibility = "hidden";
+
+				var old_scale = scale;
+				if (evt.deltaY > 0)
+					scale /= 1.1;
+				else
+					scale *= 1.1;
+
+				clampMotion();
+
+				// Ensure that event under mouse stays under mouse
+				var x = previous_x - canvas.offsetLeft;
+				offset_x += x / scale - x / old_scale;
+
+				clampMotion();
+
+				drawChart();
+			}
+
+			function darkenColor(color)
+			{
+				var i = parseInt(color.slice(1), 16);
+
+				var r = i >> 16;
+				var g = (i >> 8) & 0xff;
+				var b = i & 0xff;
+
+				r = Math.round(0.8 * r);
+				g = Math.round(0.8 * g);
+				b = Math.round(0.8 * b);
+
+				i = (r << 16) + (g << 8) + b;
+
+				return "#" + i.toString(16);
+			}
+
+			function startChart()
+			{
+				// Fetch elements
+				canvas = document.getElementById('canvas');
+				ctx = canvas.getContext("2d");
+				tooltip = document.getElementById('tooltip');
+
+				// Resize canvas to fill screen
+				canvas.width = document.body.offsetWidth - 20;
+				canvas.height = document.body.offsetHeight - 20;
+
+				// Register mouse handlers
+				canvas.onmousedown = onMouseDown;
+				canvas.onmouseup = onMouseUp;
+				canvas.onmouseout = onMouseUp;
+				canvas.onmousemove = onMouseMove;
+				canvas.onwheel	= onScroll;
+
+				for (var t = 0; t < threads.length; t++)
+				{
+					var thread = threads[t];
+
+					// Calculate darkened colors
+					thread.darkened_color = new Array(thread.color.length);
+					for (var s = 0; s < thread.color.length; s++)
+						thread.darkened_color[s] = darkenColor(thread.color[s]);
+
+					// Calculate max depth and end cycle
+					thread.max_depth = 0;
+					for (var s = 0; s < thread.start.length; s++)
+					{
+						thread.max_depth = Math.max(thread.max_depth, thread.depth[s]);
+						end_cycle = Math.max(end_cycle, thread.start[s] + thread.cycles[s]);
+					}
+				}
+
+				// Calculate width of name strings
+				ctx.font = bar_font;
+				aggregated.name_width = new Array(aggregated.name.length);
+				for (var a = 0; a < aggregated.name.length; a++)
+					aggregated.name_width[a] = ctx.measureText(aggregated.name[a]).width;
+
+				// Store scale properties
+				min_scale = canvas.width / end_cycle;
+				scale = min_scale;
+
+				drawChart();
+			}
+		</script>
 	</head>
 	<body onload="startChart();">
 	<script type="text/javascript">

thirdparty/jolt_physics/Jolt/Core/STLLocalAllocator.h

@@ -43,7 +43,8 @@ public:
 	/// Constructor used when rebinding to another type. This expects the allocator to use the original memory pool from the first allocator,
 	/// but in our case we cannot use the local buffer of the original allocator as it has different size and alignment rules.
 	/// To solve this we make this allocator fall back to the heap immediately.
-	template <class T2>		STLLocalAllocator(const STLLocalAllocator<T2, N> &) : mNumElementsUsed(N) { }
+	template <class T2>
+	explicit				STLLocalAllocator(const STLLocalAllocator<T2, N> &) : mNumElementsUsed(N) { }
 
 	/// Check if inPointer is in the local buffer
 	inline bool				is_local(const_pointer inPointer) const

thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp

@@ -17,7 +17,6 @@
 #else
 	#include <windows.h>
 #endif
-
 	JPH_SUPPRESS_WARNING_POP
 #endif
 
@@ -27,6 +26,31 @@ Semaphore::Semaphore()
 {
 #ifdef JPH_PLATFORM_WINDOWS
 	mSemaphore = CreateSemaphore(nullptr, 0, INT_MAX, nullptr);
+	if (mSemaphore == nullptr)
+	{
+		Trace("Failed to create semaphore");
+		std::abort();
+	}
+#elif defined(JPH_USE_PTHREADS)
+	int ret = sem_init(&mSemaphore, 0, 0);
+	if (ret == -1)
+	{
+		Trace("Failed to create semaphore");
+		std::abort();
+	}
+#elif defined(JPH_USE_GRAND_CENTRAL_DISPATCH)
+	mSemaphore = dispatch_semaphore_create(0);
+	if (mSemaphore == nullptr)
+	{
+		Trace("Failed to create semaphore");
+		std::abort();
+	}
+#elif defined(JPH_PLATFORM_BLUE)
+	if (!JPH_PLATFORM_BLUE_SEMAPHORE_INIT(mSemaphore))
+	{
+		Trace("Failed to create semaphore");
+		std::abort();
+	}
 #endif
 }
 
@@ -34,6 +58,12 @@ Semaphore::~Semaphore()
 {
 #ifdef JPH_PLATFORM_WINDOWS
 	CloseHandle(mSemaphore);
+#elif defined(JPH_USE_PTHREADS)
+	sem_destroy(&mSemaphore);
+#elif defined(JPH_USE_GRAND_CENTRAL_DISPATCH)
+	dispatch_release(mSemaphore);
+#elif defined(JPH_PLATFORM_BLUE)
+	JPH_PLATFORM_BLUE_SEMAPHORE_DESTROY(mSemaphore);
 #endif
 }
 
@@ -41,13 +71,23 @@ void Semaphore::Release(uint inNumber)
 {
 	JPH_ASSERT(inNumber > 0);
 
-#ifdef JPH_PLATFORM_WINDOWS
-	int old_value = mCount.fetch_add(inNumber);
+#if defined(JPH_PLATFORM_WINDOWS) || defined(JPH_USE_PTHREADS) || defined(JPH_USE_GRAND_CENTRAL_DISPATCH) || defined(JPH_PLATFORM_BLUE)
+	int old_value = mCount.fetch_add(inNumber, std::memory_order_release);
 	if (old_value < 0)
 	{
 		int new_value = old_value + (int)inNumber;
 		int num_to_release = min(new_value, 0) - old_value;
+	#ifdef JPH_PLATFORM_WINDOWS
 		::ReleaseSemaphore(mSemaphore, num_to_release, nullptr);
+	#elif defined(JPH_USE_PTHREADS)
+		for (int i = 0; i < num_to_release; ++i)
+			sem_post(&mSemaphore);
+	#elif defined(JPH_USE_GRAND_CENTRAL_DISPATCH)
+		for (int i = 0; i < num_to_release; ++i)
+			dispatch_semaphore_signal(mSemaphore);
+	#elif defined(JPH_PLATFORM_BLUE)
+		JPH_PLATFORM_BLUE_SEMAPHORE_SIGNAL(mSemaphore, num_to_release);
+	#endif
 	}
 #else
 	std::lock_guard lock(mLock);
@@ -63,19 +103,31 @@ void Semaphore::Acquire(uint inNumber)
 {
 	JPH_ASSERT(inNumber > 0);
 
-#ifdef JPH_PLATFORM_WINDOWS
-	int old_value = mCount.fetch_sub(inNumber);
+#if defined(JPH_PLATFORM_WINDOWS) || defined(JPH_USE_PTHREADS) || defined(JPH_USE_GRAND_CENTRAL_DISPATCH) || defined(JPH_PLATFORM_BLUE)
+	int old_value = mCount.fetch_sub(inNumber, std::memory_order_acquire);
 	int new_value = old_value - (int)inNumber;
 	if (new_value < 0)
 	{
 		int num_to_acquire = min(old_value, 0) - new_value;
+	#ifdef JPH_PLATFORM_WINDOWS
 		for (int i = 0; i < num_to_acquire; ++i)
 			WaitForSingleObject(mSemaphore, INFINITE);
+	#elif defined(JPH_USE_PTHREADS)
+		for (int i = 0; i < num_to_acquire; ++i)
+			sem_wait(&mSemaphore);
+	#elif defined(JPH_USE_GRAND_CENTRAL_DISPATCH)
+		for (int i = 0; i < num_to_acquire; ++i)
+			dispatch_semaphore_wait(mSemaphore, DISPATCH_TIME_FOREVER);
+	#elif defined(JPH_PLATFORM_BLUE)
+		JPH_PLATFORM_BLUE_SEMAPHORE_WAIT(mSemaphore, num_to_acquire);
+	#endif
 	}
 #else
 	std::unique_lock lock(mLock);
+	mWaitVariable.wait(lock, [this, inNumber]() {
+		return mCount.load(std::memory_order_relaxed) >= int(inNumber);
+	});
 	mCount.fetch_sub(inNumber, std::memory_order_relaxed);
-	mWaitVariable.wait(lock, [this]() { return mCount >= 0; });
 #endif
 }
 

thirdparty/jolt_physics/Jolt/Core/Semaphore.h

@@ -4,47 +4,64 @@
 
 #pragma once
 
+#include <Jolt/Core/Atomics.h>
+
+// Determine which platform specific construct we'll use
 JPH_SUPPRESS_WARNINGS_STD_BEGIN
-#include <atomic>
-#include <mutex>
-#include <condition_variable>
+#ifdef JPH_PLATFORM_WINDOWS
+	// We include windows.h in the cpp file, the semaphore itself is a void pointer
+#elif defined(JPH_PLATFORM_LINUX) || defined(JPH_PLATFORM_ANDROID) || defined(JPH_PLATFORM_BSD) || defined(JPH_PLATFORM_WASM)
+	#include <semaphore.h>
+	#define JPH_USE_PTHREADS
+#elif defined(JPH_PLATFORM_MACOS) || defined(JPH_PLATFORM_IOS)
+	#include <dispatch/dispatch.h>
+	#define JPH_USE_GRAND_CENTRAL_DISPATCH
+#elif defined(JPH_PLATFORM_BLUE)
+	// Jolt/Core/PlatformBlue.h should have defined everything that is needed below
+#else
+	#include <mutex>
+	#include <condition_variable>
+#endif
 JPH_SUPPRESS_WARNINGS_STD_END
 
 JPH_NAMESPACE_BEGIN
 
-// Things we're using from STL
-using std::atomic;
-using std::mutex;
-using std::condition_variable;
-
 /// Implements a semaphore
 /// When we switch to C++20 we can use counting_semaphore to unify this
 class JPH_EXPORT Semaphore
 {
 public:
 	/// Constructor
-						Semaphore();
-						~Semaphore();
+							Semaphore();
+							~Semaphore();
 
 	/// Release the semaphore, signaling the thread waiting on the barrier that there may be work
-	void				Release(uint inNumber = 1);
+	void					Release(uint inNumber = 1);
 
 	/// Acquire the semaphore inNumber times
-	void				Acquire(uint inNumber = 1);
+	void					Acquire(uint inNumber = 1);
 
 	/// Get the current value of the semaphore
-	inline int			GetValue() const								{ return mCount.load(std::memory_order_relaxed); }
+	inline int				GetValue() const								{ return mCount.load(std::memory_order_relaxed); }
 
 private:
+#if defined(JPH_PLATFORM_WINDOWS) || defined(JPH_USE_PTHREADS) || defined(JPH_USE_GRAND_CENTRAL_DISPATCH) || defined(JPH_PLATFORM_BLUE)
 #ifdef JPH_PLATFORM_WINDOWS
-	// On windows we use a semaphore object since it is more efficient than a lock and a condition variable
-	alignas(JPH_CACHE_LINE_SIZE) atomic<int> mCount { 0 };				///< We increment mCount for every release, to acquire we decrement the count. If the count is negative we know that we are waiting on the actual semaphore.
-	void *				mSemaphore;										///< The semaphore is an expensive construct so we only acquire/release it if we know that we need to wait/have waiting threads
+	using SemaphoreType = void *;
+#elif defined(JPH_USE_PTHREADS)
+	using SemaphoreType = sem_t;
+#elif defined(JPH_USE_GRAND_CENTRAL_DISPATCH)
+	using SemaphoreType = dispatch_semaphore_t;
+#elif defined(JPH_PLATFORM_BLUE)
+	using SemaphoreType = JPH_PLATFORM_BLUE_SEMAPHORE;
+#endif
+	alignas(JPH_CACHE_LINE_SIZE) atomic<int> mCount { 0 };					///< We increment mCount for every release, to acquire we decrement the count. If the count is negative we know that we are waiting on the actual semaphore.
+	SemaphoreType			mSemaphore { };									///< The semaphore is an expensive construct so we only acquire/release it if we know that we need to wait/have waiting threads
 #else
 	// Other platforms: Emulate a semaphore using a mutex, condition variable and count
-	mutex				mLock;
-	condition_variable	mWaitVariable;
-	atomic<int>			mCount { 0 };
+	std::mutex				mLock;
+	std::condition_variable	mWaitVariable;
+	atomic<int>				mCount { 0 };
 #endif
 };
 

thirdparty/jolt_physics/Jolt/Core/StreamIn.h

@@ -18,7 +18,8 @@ public:
 	/// Read a string of bytes from the binary stream
 	virtual void		ReadBytes(void *outData, size_t inNumBytes) = 0;
 
-	/// Returns true when an attempt has been made to read past the end of the file
+	/// Returns true when an attempt has been made to read past the end of the file.
+	/// Note that this follows the convention of std::basic_ios::eof which only returns true when an attempt is made to read past the end, not when the read pointer is at the end.
 	virtual bool		IsEOF() const = 0;
 
 	/// Returns true if there was an IO failure

thirdparty/jolt_physics/Jolt/Core/TempAllocator.h

@@ -34,7 +34,7 @@ public:
 	JPH_OVERRIDE_NEW_DELETE
 
 	/// Constructs the allocator with a maximum allocatable size of inSize
-	explicit						TempAllocatorImpl(uint inSize) :
+	explicit						TempAllocatorImpl(size_t inSize) :
 		mBase(static_cast<uint8 *>(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT))),
 		mSize(inSize)
 	{
@@ -56,10 +56,10 @@ public:
 		}
 		else
 		{
-			uint new_top = mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT);
+			size_t new_top = mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT);
 			if (new_top > mSize)
 			{
-				Trace("TempAllocator: Out of memory");
+				Trace("TempAllocator: Out of memory trying to allocate %u bytes", inSize);
 				std::abort();
 			}
 			void *address = mBase + mTop;
@@ -93,13 +93,13 @@ public:
 	}
 
 	/// Get the total size of the fixed buffer
-	uint							GetSize() const
+	size_t							GetSize() const
 	{
 		return mSize;
 	}
 
 	/// Get current usage in bytes of the buffer
-	uint							GetUsage() const
+	size_t							GetUsage() const
 	{
 		return mTop;
 	}
@@ -118,8 +118,8 @@ public:
 
 private:
 	uint8 *							mBase;							///< Base address of the memory block
-	uint							mSize;							///< Size of the memory block
-	uint							mTop = 0;						///< End of currently allocated area
+	size_t							mSize;							///< Size of the memory block
+	size_t							mTop = 0;						///< End of currently allocated area
 };
 
 /// Implementation of the TempAllocator that just falls back to malloc/free

thirdparty/jolt_physics/Jolt/Geometry/AABox.h

@@ -34,10 +34,11 @@ public:
 		return box;
 	}
 
-	/// Get bounding box of size 2 * FLT_MAX
+	/// Get bounding box of size FLT_MAX
 	static AABox	sBiggest()
 	{
-		return AABox(Vec3::sReplicate(-FLT_MAX), Vec3::sReplicate(FLT_MAX));
+		/// Max half extent of AABox is 0.5 * FLT_MAX so that GetSize() remains finite
+		return AABox(Vec3::sReplicate(-0.5f * FLT_MAX), Vec3::sReplicate(0.5f * FLT_MAX));
 	}
 
 	/// Comparison operators

thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h

@@ -540,9 +540,9 @@ public:
 				|| contact_normal_invalid))
 		{
 			// If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point
-			AddConvexRadius<A> add_convex_a(inA, inConvexRadiusA);
-			AddConvexRadius<B> add_convex_b(inB, inConvexRadiusB);
-			TransformedConvexObject<AddConvexRadius<A>> transformed_a(inStart, add_convex_a);
+			AddConvexRadius add_convex_a(inA, inConvexRadiusA);
+			AddConvexRadius add_convex_b(inB, inConvexRadiusB);
+			TransformedConvexObject transformed_a(inStart, add_convex_a);
 			if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB))
 				return false;
 		}

thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h

@@ -5,7 +5,6 @@
 #pragma once
 
 #include <Jolt/Core/NonCopyable.h>
-#include <Jolt/Core/FPException.h>
 #include <Jolt/Geometry/ClosestPoint.h>
 #include <Jolt/Geometry/ConvexSupport.h>
 

thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h

@@ -14,7 +14,7 @@ JPH_NAMESPACE_BEGIN
 class AABox;
 
 /// Oriented box
-class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND OrientedBox
+class JPH_EXPORT_GCC_BUG_WORKAROUND [[nodiscard]] OrientedBox
 {
 public:
 	JPH_OVERRIDE_NEW_DELETE

thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h

@@ -21,7 +21,7 @@ public:
 		mIsParallel = Vec3::sLessOrEqual(inDirection.Abs(), Vec3::sReplicate(1.0e-20f));
 
 		// Calculate 1 / direction while avoiding division by zero
-		mInvDirection = Vec3::sSelect(inDirection, Vec3::sReplicate(1.0f), mIsParallel).Reciprocal();
+		mInvDirection = Vec3::sSelect(inDirection, Vec3::sOne(), mIsParallel).Reciprocal();
 	}
 
 	Vec3			mInvDirection;					///< 1 / ray direction

thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h

@@ -15,7 +15,7 @@ JPH_INLINE float RayTriangle(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inV0
 
 	// Zero & one
 	Vec3 zero = Vec3::sZero();
-	Vec3 one = Vec3::sReplicate(1.0f);
+	Vec3 one = Vec3::sOne();
 
 	// Find vectors for two edges sharing inV0
 	Vec3 e1 = inV1 - inV0;
@@ -31,7 +31,7 @@ JPH_INLINE float RayTriangle(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inV0
 	UVec4 det_near_zero = Vec3::sLess(det.Abs(), epsilon);
 
 	// When the determinant is near zero, set it to one to avoid dividing by zero
-	det = Vec3::sSelect(det, Vec3::sReplicate(1.0f), det_near_zero);
+	det = Vec3::sSelect(det, Vec3::sOne(), det_near_zero);
 
 	// Calculate distance from inV0 to ray origin
 	Vec3 s = inOrigin - inV0;
@@ -110,7 +110,7 @@ JPH_INLINE Vec4 RayTriangle4(Vec3Arg inOrigin, Vec3Arg inDirection, Vec4Arg inV0
 	UVec4 det_near_zero = Vec4::sLess(det, epsilon);
 
 	// Set components of the determinant to 1 that are near zero to avoid dividing by zero
-	det = Vec4::sSelect(det, Vec4::sReplicate(1.0f), det_near_zero);
+	det = Vec4::sSelect(det, Vec4::sOne(), det_near_zero);
 
 	// Calculate distance from inV0 to ray origin
 	Vec4 sx = inOrigin.SplatX() - inV0X;

thirdparty/jolt_physics/Jolt/Jolt.natvis

@@ -68,7 +68,7 @@
 		<DisplayString>min=({mMin}), max=({mMax})</DisplayString>
 	</Type>
 	<Type Name="JPH::BodyID">
-		<DisplayString>{mID}</DisplayString>
+		<DisplayString>idx={mID &amp; 0x007fffff}, seq={(mID >> 23) &amp; 0xff}, in_bp={mID >> 24,d}</DisplayString>
 	</Type>
 	<Type Name="JPH::Body">
 		<DisplayString>{mDebugName}: p=({mPosition.mF32[0],g}, {mPosition.mF32[1],g}, {mPosition.mF32[2],g}), r=({mRotation.mValue.mF32[0],g}, {mRotation.mValue.mF32[1],g}, {mRotation.mValue.mF32[2],g}, {mRotation.mValue.mF32[3],g}), v=({mLinearVelocity.mF32[0],g}, {mLinearVelocity.mF32[1],g}, {mLinearVelocity.mF32[2],g}), w=({mAngularVelocity.mF32[0],g}, {mAngularVelocity.mF32[1],g}, {mAngularVelocity.mF32[2],g})</DisplayString>

thirdparty/jolt_physics/Jolt/Math/DVec3.h

@@ -50,6 +50,9 @@ public:
 	/// Vector with all zeros
 	static JPH_INLINE DVec3		sZero();
 
+	/// Vector with all ones
+	static JPH_INLINE DVec3		sOne();
+
 	/// Vectors with the principal axis
 	static JPH_INLINE DVec3		sAxisX()										{ return DVec3(1, 0, 0); }
 	static JPH_INLINE DVec3		sAxisY()										{ return DVec3(0, 1, 0); }

thirdparty/jolt_physics/Jolt/Math/DVec3.inl

@@ -147,6 +147,11 @@ DVec3 DVec3::sReplicate(double inV)
 #endif
 }
 
+DVec3 DVec3::sOne()
+{
+	return sReplicate(1.0);
+}
+
 DVec3 DVec3::sNaN()
 {
 	return sReplicate(numeric_limits<double>::quiet_NaN());
@@ -727,7 +732,7 @@ DVec3 DVec3::Abs() const
 
 DVec3 DVec3::Reciprocal() const
 {
-	return sReplicate(1.0) / mValue;
+	return sOne() / mValue;
 }
 
 DVec3 DVec3::Cross(DVec3Arg inV2) const

thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h

@@ -30,7 +30,7 @@ bool EigenValueSymmetric(const Matrix &inMatrix, Matrix &outEigVec, Vector &outE
 {
 	// This algorithm can generate infinite values, see comment below
 	FPExceptionDisableInvalid disable_invalid;
-	(void)disable_invalid;
+	JPH_UNUSED(disable_invalid);
 
 	// Maximum number of sweeps to make
 	const int cMaxSweeps = 50;

thirdparty/jolt_physics/Jolt/Math/HalfFloat.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <Jolt/Math/Vec4.h>
+#include <Jolt/Core/FPException.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -132,6 +133,9 @@ template <int RoundingMode>
 JPH_INLINE HalfFloat FromFloat(float inV)
 {
 #ifdef JPH_USE_F16C
+	FPExceptionDisableOverflow disable_overflow;
+	JPH_UNUSED(disable_overflow);
+
 	union
 	{
 		__m128i		u128;

thirdparty/jolt_physics/Jolt/Math/Math.h

@@ -9,6 +9,9 @@ JPH_NAMESPACE_BEGIN
 /// The constant \f$\pi\f$
 static constexpr float JPH_PI = 3.14159265358979323846f;
 
+/// A large floating point value which, when squared, is still much smaller than FLT_MAX
+static constexpr float cLargeFloat = 1.0e15f;
+
 /// Convert a value from degrees to radians
 JPH_INLINE constexpr float DegreesToRadians(float inV)
 {

thirdparty/jolt_physics/Jolt/Math/Vec3.h

@@ -46,6 +46,9 @@ public:
 	/// Vector with all zeros
 	static JPH_INLINE Vec3		sZero();
 
+	/// Vector with all ones
+	static JPH_INLINE Vec3		sOne();
+
 	/// Vector with all NaN's
 	static JPH_INLINE Vec3		sNaN();
 

thirdparty/jolt_physics/Jolt/Math/Vec3.inl

@@ -122,6 +122,11 @@ Vec3 Vec3::sReplicate(float inV)
 #endif
 }
 
+Vec3 Vec3::sOne()
+{
+	return sReplicate(1.0f);
+}
+
 Vec3 Vec3::sNaN()
 {
 	return sReplicate(numeric_limits<float>::quiet_NaN());
@@ -584,7 +589,7 @@ Vec3 Vec3::Abs() const
 
 Vec3 Vec3::Reciprocal() const
 {
-	return sReplicate(1.0f) / mValue;
+	return sOne() / mValue;
 }
 
 Vec3 Vec3::Cross(Vec3Arg inV2) const

thirdparty/jolt_physics/Jolt/Math/Vec4.h

@@ -38,6 +38,9 @@ public:
 	/// Vector with all zeros
 	static JPH_INLINE Vec4		sZero();
 
+	/// Vector with all ones
+	static JPH_INLINE Vec4		sOne();
+
 	/// Vector with all NaN's
 	static JPH_INLINE Vec4		sNaN();
 

thirdparty/jolt_physics/Jolt/Math/Vec4.inl

@@ -82,6 +82,11 @@ Vec4 Vec4::sReplicate(float inV)
 #endif
 }
 
+Vec4 Vec4::sOne()
+{
+	return sReplicate(1.0f);
+}
+
 Vec4 Vec4::sNaN()
 {
 	return sReplicate(numeric_limits<float>::quiet_NaN());
@@ -614,7 +619,7 @@ Vec4 Vec4::Abs() const
 
 Vec4 Vec4::Reciprocal() const
 {
-	return sReplicate(1.0f) / mValue;
+	return sOne() / mValue;
 }
 
 Vec4 Vec4::DotV(Vec4Arg inV2) const
@@ -805,7 +810,7 @@ void Vec4::SinCos(Vec4 &outSin, Vec4 &outCos) const
 
 	// Taylor expansion:
 	// Cos(x) = 1 - x^2/2! + x^4/4! - x^6/6! + x^8/8! + ... = (((x2/8!- 1/6!) * x2 + 1/4!) * x2 - 1/2!) * x2 + 1
-	Vec4 taylor_cos = ((2.443315711809948e-5f * x2 - Vec4::sReplicate(1.388731625493765e-3f)) * x2 + Vec4::sReplicate(4.166664568298827e-2f)) * x2 * x2 - 0.5f * x2 + Vec4::sReplicate(1.0f);
+	Vec4 taylor_cos = ((2.443315711809948e-5f * x2 - Vec4::sReplicate(1.388731625493765e-3f)) * x2 + Vec4::sReplicate(4.166664568298827e-2f)) * x2 * x2 - 0.5f * x2 + Vec4::sOne();
 	// Sin(x) = x - x^3/3! + x^5/5! - x^7/7! + ... = ((-x2/7! + 1/5!) * x2 - 1/3!) * x2 * x + x
 	Vec4 taylor_sin = ((-1.9515295891e-4f * x2 + Vec4::sReplicate(8.3321608736e-3f)) * x2 - Vec4::sReplicate(1.6666654611e-1f)) * x2 * x + x;
 
@@ -880,14 +885,14 @@ Vec4 Vec4::ASin() const
 	Vec4 a = Vec4::sXor(*this, asin_sign.ReinterpretAsFloat());
 
 	// ASin is not defined outside the range [-1, 1] but it often happens that a value is slightly above 1 so we just clamp here
-	a = Vec4::sMin(a, Vec4::sReplicate(1.0f));
+	a = Vec4::sMin(a, Vec4::sOne());
 
 	// When |x| <= 0.5 we use the asin approximation as is
 	Vec4 z1 = a * a;
 	Vec4 x1 = a;
 
 	// When |x| > 0.5 we use the identity asin(x) = PI / 2 - 2 * asin(sqrt((1 - x) / 2))
-	Vec4 z2 = 0.5f * (Vec4::sReplicate(1.0f) - a);
+	Vec4 z2 = 0.5f * (Vec4::sOne() - a);
 	Vec4 x2 = z2.Sqrt();
 
 	// Select which of the two situations we have
@@ -923,7 +928,7 @@ Vec4 Vec4::ATan() const
 
 	// If x > Tan(PI / 8)
 	UVec4 greater1 = Vec4::sGreater(x, Vec4::sReplicate(0.4142135623730950f));
-	Vec4 x1 = (x - Vec4::sReplicate(1.0f)) / (x + Vec4::sReplicate(1.0f));
+	Vec4 x1 = (x - Vec4::sOne()) / (x + Vec4::sOne());
 
 	// If x > Tan(3 * PI / 8)
 	UVec4 greater2 = Vec4::sGreater(x, Vec4::sReplicate(2.414213562373095f));

thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp

@@ -39,7 +39,7 @@ void Body::SetMotionType(EMotionType inMotionType)
 	if (mMotionType == inMotionType)
 		return;
 
-	JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set tot true");
+	JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set to true");
 	JPH_ASSERT(inMotionType != EMotionType::Static || !IsActive(), "Deactivate body first");
 	JPH_ASSERT(inMotionType == EMotionType::Dynamic || !IsSoftBody(), "Soft bodies can only be dynamic, you can make individual vertices kinematic by setting their inverse mass to 0");
 
@@ -96,7 +96,7 @@ void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, fl
 
 void Body::CalculateWorldSpaceBoundsInternal()
 {
-	mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
+	mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sOne());
 }
 
 void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer)
@@ -190,7 +190,7 @@ void Body::GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNorma
 	Plane surface_relative_to_body = Plane::sFromPointAndNormal(inSurfacePosition - mPosition, inSurfaceNormal);
 
 	// Calculate amount of volume that is submerged and what the center of buoyancy is
-	mShape->GetSubmergedVolume(rotation, Vec3::sReplicate(1.0f), surface_relative_to_body, outTotalVolume, outSubmergedVolume, outRelativeCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, mPosition));
+	mShape->GetSubmergedVolume(rotation, Vec3::sOne(), surface_relative_to_body, outTotalVolume, outSubmergedVolume, outRelativeCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, mPosition));
 }
 
 bool Body::ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)

thirdparty/jolt_physics/Jolt/Physics/Body/Body.h

@@ -31,7 +31,11 @@ class SoftBodyCreationSettings;
 /// The functions that get/set the position of the body all indicate if they are relative to the center of mass or to the original position in which the shape was created.
 ///
 /// The linear velocity is also velocity of the center of mass, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$.
-class alignas(JPH_RVECTOR_ALIGNMENT) JPH_EXPORT_GCC_BUG_WORKAROUND Body : public NonCopyable
+class
+#ifndef JPH_PLATFORM_DOXYGEN // Doxygen gets confused here
+	JPH_EXPORT_GCC_BUG_WORKAROUND alignas(JPH_RVECTOR_ALIGNMENT)
+#endif
+	Body : public NonCopyable
 {
 public:
 	JPH_OVERRIDE_NEW_DELETE
@@ -281,6 +285,25 @@ public:
 	/// Get world space bounding box
 	inline const AABox &	GetWorldSpaceBounds() const										{ return mBounds; }
 
+#ifdef JPH_ENABLE_ASSERTS
+	/// Validate that the cached bounding box of the body matches the actual bounding box of the body.
+	/// If this check fails then there are a number of possible causes:
+	/// 1. Shape is being modified without notifying the system of the change. E.g. if you modify a MutableCompoundShape
+	/// without calling BodyInterface::NotifyShapeChanged then there will be a mismatch between the cached bounding box
+	/// in the broad phase and the bounding box of the Shape.
+	/// 2. You are calling functions postfixed with 'Internal' which are not meant to be called by the application.
+	/// 3. If the actual bounds and cached bounds are very close, it could mean that you have a mismatch in floating
+	/// point unit state between threads. E.g. one thread has flush to zero (FTZ) or denormals are zero (DAZ) set and
+	/// the other thread does not. Or if the rounding mode differs between threads. This can cause small differences
+	/// in floating point calculations. If you are using JobSystemThreadPool you can use JobSystemThreadPool::SetThreadInitFunction
+	/// to initialize the floating point unit state.
+	inline void				ValidateCachedBounds() const
+	{
+		AABox actual_body_bounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sOne());
+		JPH_ASSERT(actual_body_bounds == mBounds, "Mismatch between cached bounding box and actual bounding box");
+	}
+#endif // JPH_ENABLE_ASSERTS
+
 	/// Access to the motion properties
 	const MotionProperties *GetMotionProperties() const										{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }
 	MotionProperties *		GetMotionProperties()											{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }

thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h

@@ -15,9 +15,10 @@ public:
 	JPH_OVERRIDE_NEW_DELETE
 
 	static constexpr uint32	cInvalidBodyID = 0xffffffff;	///< The value for an invalid body ID
-	static constexpr uint32	cBroadPhaseBit = 0x00800000;	///< This bit is used by the broadphase
+	static constexpr uint32	cBroadPhaseBit = 0x80000000;	///< This bit is used by the broadphase
 	static constexpr uint32	cMaxBodyIndex = 0x7fffff;		///< Maximum value for body index (also the maximum amount of bodies supported - 1)
 	static constexpr uint8	cMaxSequenceNumber = 0xff;		///< Maximum value for the sequence number
+	static constexpr uint	cSequenceNumberShift = 23;		///< Number of bits to shift to get the sequence number
 
 	/// Construct invalid body ID
 							BodyID() :
@@ -34,9 +35,9 @@ public:
 
 	/// Construct from index and sequence number
 	explicit				BodyID(uint32 inID, uint8 inSequenceNumber) :
-		mID((uint32(inSequenceNumber) << 24) | inID)
+		mID((uint32(inSequenceNumber) << cSequenceNumberShift) | inID)
 	{
-		JPH_ASSERT(inID < cMaxBodyIndex); // Should not use bit pattern for invalid ID and should not use the broadphase bit
+		JPH_ASSERT(inID <= cMaxBodyIndex); // Should not overlap with broadphase bit or sequence number
 	}
 
 	/// Get index in body array
@@ -51,7 +52,7 @@ public:
 	/// Functions querying the broadphase can (after acquiring a body lock) detect that the body has been removed (we assume that this won't happen more than 128 times in a row).
 	inline uint8			GetSequenceNumber() const
 	{
-		return uint8(mID >> 24);
+		return uint8(mID >> cSequenceNumberShift);
 	}
 
 	/// Returns the index and sequence number combined in an uint32

thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp

@@ -316,11 +316,11 @@ void BodyInterface::SetShape(const BodyID &inBodyID, const Shape *inShape, bool
 			{
 				BodyID id = body.GetID();
 				mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
-			}
 
-			// Optionally activate body
-			if (inActivationMode == EActivation::Activate && !body.IsStatic())
-				ActivateBodyInternal(body);
+				// Optionally activate body
+				if (inActivationMode == EActivation::Activate && !body.IsStatic())
+					ActivateBodyInternal(body);
+			}
 		}
 	}
 }
@@ -346,11 +346,11 @@ void BodyInterface::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviou
 		{
 			BodyID id = body.GetID();
 			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
-		}
 
-		// Optionally activate body
-		if (inActivationMode == EActivation::Activate && !body.IsStatic())
-			ActivateBodyInternal(body);
+			// Optionally activate body
+			if (inActivationMode == EActivation::Activate && !body.IsStatic())
+				ActivateBodyInternal(body);
+		}
 	}
 }
 
@@ -401,11 +401,11 @@ void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPo
 		{
 			BodyID id = body.GetID();
 			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
-		}
 
-		// Optionally activate body
-		if (inActivationMode == EActivation::Activate && !body.IsStatic())
-			ActivateBodyInternal(body);
+			// Optionally activate body
+			if (inActivationMode == EActivation::Activate && !body.IsStatic())
+				ActivateBodyInternal(body);
+		}
 	}
 }
 
@@ -428,11 +428,11 @@ void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RV
 			{
 				BodyID id = body.GetID();
 				mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
-			}
 
-			// Optionally activate body
-			if (inActivationMode == EActivation::Activate && !body.IsStatic())
-				ActivateBodyInternal(body);
+				// Optionally activate body
+				if (inActivationMode == EActivation::Activate && !body.IsStatic())
+					ActivateBodyInternal(body);
+			}
 		}
 	}
 }
@@ -468,11 +468,11 @@ void BodyInterface::SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EAc
 		{
 			BodyID id = body.GetID();
 			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
-		}
 
-		// Optionally activate body
-		if (inActivationMode == EActivation::Activate && !body.IsStatic())
-			ActivateBodyInternal(body);
+			// Optionally activate body
+			if (inActivationMode == EActivation::Activate && !body.IsStatic())
+				ActivateBodyInternal(body);
+		}
 	}
 }
 
@@ -509,11 +509,11 @@ void BodyInterface::SetRotation(const BodyID &inBodyID, QuatArg inRotation, EAct
 		{
 			BodyID id = body.GetID();
 			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
-		}
 
-		// Optionally activate body
-		if (inActivationMode == EActivation::Activate && !body.IsStatic())
-			ActivateBodyInternal(body);
+			// Optionally activate body
+			if (inActivationMode == EActivation::Activate && !body.IsStatic())
+				ActivateBodyInternal(body);
+		}
 	}
 }
 
@@ -990,6 +990,22 @@ bool BodyInterface::GetUseManifoldReduction(const BodyID &inBodyID) const
 		return true;
 }
 
+void BodyInterface::SetCollisionGroup(const BodyID &inBodyID, const CollisionGroup &inCollisionGroup)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		lock.GetBody().SetCollisionGroup(inCollisionGroup);
+}
+
+const CollisionGroup &BodyInterface::GetCollisionGroup(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetCollisionGroup();
+	else
+		return CollisionGroup::sInvalid;
+}
+
 TransformedShape BodyInterface::GetTransformedShape(const BodyID &inBodyID) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);

thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h

@@ -28,6 +28,7 @@ class TwoBodyConstraintSettings;
 class TwoBodyConstraint;
 class BroadPhaseLayerFilter;
 class AABox;
+class CollisionGroup;
 
 /// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations.
 /// All quantities are in world space unless otherwise specified.
@@ -273,6 +274,12 @@ public:
 	bool						GetUseManifoldReduction(const BodyID &inBodyID) const;
 	///@}
 
+	///@name Collision group
+	///@{
+	void						SetCollisionGroup(const BodyID &inBodyID, const CollisionGroup &inCollisionGroup);
+	const CollisionGroup &		GetCollisionGroup(const BodyID &inBodyID) const;
+	///@}
+
 	/// Get transform and shape for this body, used to perform collision detection
 	TransformedShape			GetTransformedShape(const BodyID &inBodyID) const;
 

thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp

@@ -1014,15 +1014,15 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 
 			// Draw the results of GetSupportFunction
 			if (inDrawSettings.mDrawGetSupportFunction)
-				body->mShape->DrawGetSupportFunction(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawSupportDirection);
+				body->mShape->DrawGetSupportFunction(inRenderer, body->GetCenterOfMassTransform(), Vec3::sOne(), color, inDrawSettings.mDrawSupportDirection);
 
 			// Draw the results of GetSupportingFace
 			if (inDrawSettings.mDrawGetSupportingFace)
-				body->mShape->DrawGetSupportingFace(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
+				body->mShape->DrawGetSupportingFace(inRenderer, body->GetCenterOfMassTransform(), Vec3::sOne());
 
 			// Draw the shape
 			if (inDrawSettings.mDrawShape)
-				body->mShape->Draw(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawShapeColor == EShapeColor::MaterialColor, inDrawSettings.mDrawShapeWireframe || is_sensor);
+				body->mShape->Draw(inRenderer, body->GetCenterOfMassTransform(), Vec3::sOne(), color, inDrawSettings.mDrawShapeColor == EShapeColor::MaterialColor, inDrawSettings.mDrawShapeWireframe || is_sensor);
 
 			// Draw bounding box
 			if (inDrawSettings.mDrawBoundingBox)
@@ -1146,9 +1146,7 @@ void BodyManager::ValidateActiveBodyBounds()
 		for (BodyID *id = mActiveBodies[type], *id_end = mActiveBodies[type] + mNumActiveBodies[type].load(memory_order_relaxed); id < id_end; ++id)
 		{
 			const Body *body = mBodies[id->GetIndex()];
-			AABox cached = body->GetWorldSpaceBounds();
-			AABox calculated = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
-			JPH_ASSERT(cached == calculated);
+			body->ValidateCachedBounds();
 		}
 }
 #endif // JPH_DEBUG

thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl

@@ -107,8 +107,8 @@ void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, floa
 
 	// Calculate local space inertia tensor (a diagonal in local space)
 	UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero());
-	Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sReplicate(1.0f), is_zero);
-	Vec3 nominator = Vec3::sSelect(Vec3::sReplicate(1.0f), Vec3::sZero(), is_zero);
+	Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sOne(), is_zero);
+	Vec3 nominator = Vec3::sSelect(Vec3::sOne(), Vec3::sZero(), is_zero);
 	Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero
 
 	// Calculate local space angular momentum

thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp

@@ -34,7 +34,7 @@ Character::Character(const CharacterSettings *inSettings, RVec3Arg inPosition, Q
 {
 	// Construct rigid body
 	BodyCreationSettings settings(mShape, inPosition, inRotation, EMotionType::Dynamic, mLayer);
-	settings.mAllowedDOFs = EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ;
+	settings.mAllowedDOFs = inSettings->mAllowedDOFs;
 	settings.mEnhancedInternalEdgeRemoval = inSettings->mEnhancedInternalEdgeRemoval;
 	settings.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
 	settings.mMassPropertiesOverride.mMass = inSettings->mMass;
@@ -75,8 +75,18 @@ void Character::CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMove
 	// Create query object layer filter
 	DefaultObjectLayerFilter object_layer_filter = mSystem->GetDefaultLayerFilter(mLayer);
 
-	// Ignore my own body
-	IgnoreSingleBodyFilter body_filter(mBodyID);
+	// Ignore sensors and my own body
+	class CharacterBodyFilter : public IgnoreSingleBodyFilter
+	{
+	public:
+		using			IgnoreSingleBodyFilter::IgnoreSingleBodyFilter;
+
+		virtual bool	ShouldCollideLocked(const Body &inBody) const override
+		{
+			return !inBody.IsSensor();
+		}
+	};
+	CharacterBodyFilter body_filter(mBodyID);
 
 	// Settings for collide shape
 	CollideShapeSettings settings;
@@ -85,7 +95,7 @@ void Character::CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMove
 	settings.mActiveEdgeMovementDirection = inMovementDirection;
 	settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces;
 
-	sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sReplicate(1.0f), inCenterOfMassTransform, settings, inBaseOffset, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter);
+	sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sOne(), inCenterOfMassTransform, settings, inBaseOffset, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter);
 }
 
 void Character::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const

thirdparty/jolt_physics/Jolt/Physics/Character/Character.h

@@ -8,6 +8,7 @@
 #include <Jolt/Physics/Collision/ObjectLayer.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
 #include <Jolt/Physics/EActivation.h>
+#include <Jolt/Physics/Body/AllowedDOFs.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -28,6 +29,9 @@ public:
 
 	/// Value to multiply gravity with for this character
 	float								mGravityFactor = 1.0f;
+
+	/// Allowed degrees of freedom for this character
+	EAllowedDOFs						mAllowedDOFs = EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ;
 };
 
 /// Runtime character object.

thirdparty/jolt_physics/Jolt/Physics/Character/CharacterID.h

@@ -0,0 +1,98 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2025 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// ID of a character. Used primarily to identify deleted characters and to sort deterministically.
+class JPH_EXPORT CharacterID
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	static constexpr uint32	cInvalidCharacterID = 0xffffffff;	///< The value for an invalid character ID
+
+	/// Construct invalid character ID
+							CharacterID() :
+		mID(cInvalidCharacterID)
+	{
+	}
+
+	/// Construct with specific value, make sure you don't use the same value twice!
+	explicit				CharacterID(uint32 inID) :
+		mID(inID)
+	{
+	}
+
+	/// Get the numeric value of the ID
+	inline uint32			GetValue() const
+	{
+		return mID;
+	}
+
+	/// Check if the ID is valid
+	inline bool				IsInvalid() const
+	{
+		return mID == cInvalidCharacterID;
+	}
+
+	/// Equals check
+	inline bool				operator == (const CharacterID &inRHS) const
+	{
+		return mID == inRHS.mID;
+	}
+
+	/// Not equals check
+	inline bool				operator != (const CharacterID &inRHS) const
+	{
+		return mID != inRHS.mID;
+	}
+
+	/// Smaller than operator, can be used for sorting characters
+	inline bool				operator < (const CharacterID &inRHS) const
+	{
+		return mID < inRHS.mID;
+	}
+
+	/// Greater than operator, can be used for sorting characters
+	inline bool				operator > (const CharacterID &inRHS) const
+	{
+		return mID > inRHS.mID;
+	}
+
+	/// Get the hash for this character ID
+	inline uint64			GetHash() const
+	{
+		return Hash<uint32>{} (mID);
+	}
+
+	/// Generate the next available character ID
+	static CharacterID		sNextCharacterID()
+	{
+		for (;;)
+		{
+			uint32 next = sNextID.fetch_add(1, std::memory_order_relaxed);
+			if (next != cInvalidCharacterID)
+				return CharacterID(next);
+		}
+	}
+
+	/// Set the next available character ID, can be used after destroying all character to prepare for a second deterministic run
+	static void				sSetNextCharacterID(uint32 inNextValue = 1)
+	{
+		sNextID.store(inNextValue, std::memory_order_relaxed);
+	}
+
+private:
+	/// Next character ID to be assigned
+	inline static atomic<uint32> sNextID = 1;
+
+	/// ID value
+	uint32					mID;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp

@@ -14,8 +14,10 @@
 #include <Jolt/Physics/Collision/Shape/ScaledShape.h>
 #include <Jolt/Physics/Collision/CollisionDispatch.h>
 #include <Jolt/Core/QuickSort.h>
+#include <Jolt/Core/ScopeExit.h>
 #include <Jolt/Geometry/ConvexSupport.h>
 #include <Jolt/Geometry/GJKClosestPoint.h>
+#include <Jolt/Geometry/RayAABox.h>
 #ifdef JPH_DEBUG_RENDERER
 	#include <Jolt/Renderer/DebugRenderer.h>
 #endif // JPH_DEBUG_RENDERER
@@ -34,25 +36,35 @@ void CharacterVsCharacterCollisionSimple::CollideCharacter(const CharacterVirtua
 	// Make shape 1 relative to inBaseOffset
 	Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44();
 
-	const Shape *shape = inCharacter->GetShape();
+	const Shape *shape1 = inCharacter->GetShape();
 	CollideShapeSettings settings = inCollideShapeSettings;
 
+	// Get bounds for character
+	AABox bounds1 = shape1->GetWorldSpaceBounds(transform1, Vec3::sOne());
+
 	// Iterate over all characters
 	for (const CharacterVirtual *c : mCharacters)
 		if (c != inCharacter
 			&& !ioCollector.ShouldEarlyOut())
 		{
-			// Collector needs to know which character we're colliding with
-			ioCollector.SetUserData(reinterpret_cast<uint64>(c));
-
 			// Make shape 2 relative to inBaseOffset
 			Mat44 transform2 = c->GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
 
 			// We need to add the padding of character 2 so that we will detect collision with its outer shell
 			settings.mMaxSeparationDistance = inCollideShapeSettings.mMaxSeparationDistance + c->GetCharacterPadding();
 
+			// Check if the bounding boxes of the characters overlap
+			const Shape *shape2 = c->GetShape();
+			AABox bounds2 = shape2->GetWorldSpaceBounds(transform2, Vec3::sOne());
+			bounds2.ExpandBy(Vec3::sReplicate(settings.mMaxSeparationDistance));
+			if (!bounds1.Overlaps(bounds2))
+				continue;
+
+			// Collector needs to know which character we're colliding with
+			ioCollector.SetUserData(reinterpret_cast<uint64>(c));
+
 			// Note that this collides against the character's shape without padding, this will be corrected for in CharacterVirtual::GetContactsAtPosition
-			CollisionDispatch::sCollideShapeVsShape(shape, c->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, SubShapeIDCreator(), SubShapeIDCreator(), settings, ioCollector);
+			CollisionDispatch::sCollideShapeVsShape(shape1, shape2, Vec3::sOne(), Vec3::sOne(), transform1, transform2, SubShapeIDCreator(), SubShapeIDCreator(), settings, ioCollector);
 		}
 
 	// Reset the user data
@@ -63,21 +75,32 @@ void CharacterVsCharacterCollisionSimple::CastCharacter(const CharacterVirtual *
 {
 	// Convert shape cast relative to inBaseOffset
 	Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44();
-	ShapeCast shape_cast(inCharacter->GetShape(), Vec3::sReplicate(1.0f), transform1, inDirection);
+	ShapeCast shape_cast(inCharacter->GetShape(), Vec3::sOne(), transform1, inDirection);
+
+	// Get world space bounds of the character in the form of center and extent
+	Vec3 origin = shape_cast.mShapeWorldBounds.GetCenter();
+	Vec3 extents = shape_cast.mShapeWorldBounds.GetExtent();
 
 	// Iterate over all characters
 	for (const CharacterVirtual *c : mCharacters)
 		if (c != inCharacter
 			&& !ioCollector.ShouldEarlyOut())
 		{
-			// Collector needs to know which character we're colliding with
-			ioCollector.SetUserData(reinterpret_cast<uint64>(c));
-
 			// Make shape 2 relative to inBaseOffset
 			Mat44 transform2 = c->GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
 
+			// Sweep bounding box of the character against the bounding box of the other character to see if they can collide
+			const Shape *shape2 = c->GetShape();
+			AABox bounds2 = shape2->GetWorldSpaceBounds(transform2, Vec3::sOne());
+			bounds2.ExpandBy(extents);
+			if (!RayAABoxHits(origin, inDirection, bounds2.mMin, bounds2.mMax))
+				continue;
+
+			// Collector needs to know which character we're colliding with
+			ioCollector.SetUserData(reinterpret_cast<uint64>(c));
+
 			// Note that this collides against the character's shape without padding, this will be corrected for in CharacterVirtual::GetFirstContactForSweep
-			CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, c->GetShape(), Vec3::sReplicate(1.0f), { }, transform2, SubShapeIDCreator(), SubShapeIDCreator(), ioCollector);
+			CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, shape2, Vec3::sOne(), { }, transform2, SubShapeIDCreator(), SubShapeIDCreator(), ioCollector);
 		}
 
 	// Reset the user data
@@ -86,6 +109,7 @@ void CharacterVsCharacterCollisionSimple::CastCharacter(const CharacterVirtual *
 
 CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) :
 	CharacterBase(inSettings, inSystem),
+	mID(inSettings->mID),
 	mBackFaceMode(inSettings->mBackFaceMode),
 	mPredictiveContactDistance(inSettings->mPredictiveContactDistance),
 	mMaxCollisionIterations(inSettings->mMaxCollisionIterations),
@@ -102,6 +126,8 @@ CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, R
 	mRotation(inRotation),
 	mUserData(inUserData)
 {
+	JPH_ASSERT(!mID.IsInvalid());
+
 	// Copy settings
 	SetMaxStrength(inSettings->mMaxStrength);
 	SetMass(inSettings->mMass);
@@ -112,7 +138,18 @@ CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, R
 		BodyCreationSettings settings(inSettings->mInnerBodyShape, GetInnerBodyPosition(), mRotation, EMotionType::Kinematic, inSettings->mInnerBodyLayer);
 		settings.mAllowSleeping = false; // Disable sleeping so that we will receive sensor callbacks
 		settings.mUserData = inUserData;
-		mInnerBodyID = inSystem->GetBodyInterface().CreateAndAddBody(settings, EActivation::Activate);
+
+		const Body *inner_body;
+		BodyInterface &bi = inSystem->GetBodyInterface();
+		if (inSettings->mInnerBodyIDOverride.IsInvalid())
+			inner_body = bi.CreateBody(settings);
+		else
+			inner_body = bi.CreateBodyWithID(inSettings->mInnerBodyIDOverride, settings);
+		if (inner_body != nullptr)
+		{
+			mInnerBodyID = inner_body->GetID();
+			bi.AddBody(mInnerBodyID, EActivation::Activate);
+		}
 	}
 }
 
@@ -192,12 +229,13 @@ void CharacterVirtual::sFillContactProperties(const CharacterVirtual *inCharacte
 	outContact.mMaterial = inCollector.GetContext()->GetMaterial(inResult.mSubShapeID2);
 }
 
-void CharacterVirtual::sFillCharacterContactProperties(Contact &outContact, CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult)
+void CharacterVirtual::sFillCharacterContactProperties(Contact &outContact, const CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult)
 {
 	outContact.mPosition = inBaseOffset + inResult.mContactPointOn2;
 	outContact.mLinearVelocity = inOtherCharacter->GetLinearVelocity();
 	outContact.mSurfaceNormal = outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero());
 	outContact.mDistance = -inResult.mPenetrationDepth;
+	outContact.mCharacterIDB = inOtherCharacter->GetID();
 	outContact.mCharacterB = inOtherCharacter;
 	outContact.mSubShapeIDB = inResult.mSubShapeID2;
 	outContact.mMotionTypeB = EMotionType::Kinematic; // Other character is kinematic, we can't directly move it
@@ -294,8 +332,8 @@ void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inRes
 		&& inResult.mPenetrationAxis.Dot(mDisplacement) > 0.0f) // Ignore penetrations that we're moving away from
 	{
 		// Test if this contact should be ignored
-		for (const IgnoredContact &c : mIgnoredContacts)
-			if (c.mBodyID == inResult.mBodyID2 && c.mSubShapeID == inResult.mSubShapeID2)
+		for (const ContactKey &c : mIgnoredContacts)
+			if (c.mBodyB == inResult.mBodyID2 && c.mSubShapeIDB == inResult.mSubShapeID2)
 				return;
 
 		Contact contact;
@@ -355,7 +393,7 @@ void CharacterVirtual::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, V
 	auto collide_shape_function = mEnhancedInternalEdgeRemoval? &NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval : &NarrowPhaseQuery::CollideShape;
 
 	// Collide shape
-	(mSystem->GetNarrowPhaseQuery().*collide_shape_function)(inShape, Vec3::sReplicate(1.0f), transform, settings, inBaseOffset, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter);
+	(mSystem->GetNarrowPhaseQuery().*collide_shape_function)(inShape, Vec3::sOne(), transform, settings, inBaseOffset, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter);
 
 	// Also collide with other characters
 	if (mCharacterVsCharacterCollision != nullptr)
@@ -417,14 +455,14 @@ void CharacterVirtual::RemoveConflictingContacts(TempContactList &ioContacts, Ig
 					if (contact1.mDistance < contact2.mDistance)
 					{
 						// Discard the 2nd contact
-						outIgnoredContacts.emplace_back(contact2.mBodyB, contact2.mSubShapeIDB);
+						outIgnoredContacts.emplace_back(contact2);
 						ioContacts.erase(ioContacts.begin() + c2);
 						c2--;
 					}
 					else
 					{
 						// Discard the first contact
-						outIgnoredContacts.emplace_back(contact1.mBodyB, contact1.mSubShapeIDB);
+						outIgnoredContacts.emplace_back(contact1);
 						ioContacts.erase(ioContacts.begin() + c1);
 						c1--;
 						break;
@@ -445,14 +483,38 @@ bool CharacterVirtual::ValidateContact(const Contact &inContact) const
 		return mListener->OnContactValidate(this, inContact.mBodyB, inContact.mSubShapeIDB);
 }
 
-void CharacterVirtual::ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings) const
+void CharacterVirtual::ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings)
 {
 	if (mListener != nullptr)
 	{
-		if (inContact.mCharacterB != nullptr)
-			mListener->OnCharacterContactAdded(this, inContact.mCharacterB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings);
+		// Check if we already know this contact
+		ListenerContacts::iterator it = mListenerContacts.find(inContact);
+		if (it != mListenerContacts.end())
+		{
+			// Max 1 contact persisted callback
+			if (++it->second.mCount == 1)
+			{
+				if (inContact.mCharacterB != nullptr)
+					mListener->OnCharacterContactPersisted(this, inContact.mCharacterB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings);
+				else
+					mListener->OnContactPersisted(this, inContact.mBodyB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings);
+				it->second.mSettings = ioSettings;
+			}
+			else
+			{
+				// Reuse the settings from the last call
+				ioSettings = it->second.mSettings;
+			}
+		}
 		else
-			mListener->OnContactAdded(this, inContact.mBodyB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings);
+		{
+			// New contact
+			if (inContact.mCharacterB != nullptr)
+				mListener->OnCharacterContactAdded(this, inContact.mCharacterB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings);
+			else
+				mListener->OnContactAdded(this, inContact.mBodyB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings);
+			mListenerContacts.insert(ListenerContacts::value_type(inContact, ioSettings));
+		}
 	}
 }
 
@@ -517,7 +579,7 @@ bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDi
 	RVec3 base_offset = start.GetTranslation();
 	ContactCastCollector collector(mSystem, this, inDisplacement, mUp, inIgnoredContacts, base_offset, contact);
 	collector.ResetEarlyOutFraction(contact.mFraction);
-	RShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement);
+	RShapeCast shape_cast(mShape, Vec3::sOne(), start, inDisplacement);
 	mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, base_offset, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter);
 
 	// Also collide with other characters
@@ -527,7 +589,7 @@ bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDi
 		mCharacterVsCharacterCollision->CastCharacter(this, start, inDisplacement, settings, base_offset, collector);
 	}
 
-	if (contact.mBodyB.IsInvalid() && contact.mCharacterB == nullptr)
+	if (contact.mBodyB.IsInvalid() && contact.mCharacterIDB.IsInvalid())
 		return false;
 
 	// Store contact
@@ -562,7 +624,7 @@ bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDi
 		AddConvexRadius add_cvx(polygon, character_padding);
 
 		// Correct fraction to hit this inflated face instead of the inner shape
-		corrected = sCorrectFractionForCharacterPadding(mShape, start.GetRotation(), inDisplacement, Vec3::sReplicate(1.0f), add_cvx, outContact.mFraction);
+		corrected = sCorrectFractionForCharacterPadding(mShape, start.GetRotation(), inDisplacement, Vec3::sOne(), add_cvx, outContact.mFraction);
 	}
 	if (!corrected)
 	{
@@ -619,7 +681,7 @@ void CharacterVirtual::DetermineConstraints(TempContactList &inContacts, float i
 	}
 }
 
-bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const
+bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime)
 {
 	Contact &contact = *ioConstraint.mContact;
 
@@ -627,6 +689,9 @@ bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstrain
 	if (!ValidateContact(contact))
 		return false;
 
+	// We collided
+	contact.mHadCollision = true;
+
 	// Send contact added event
 	CharacterContactSettings settings;
 	ContactAdded(contact, settings);
@@ -691,7 +756,7 @@ void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, f
 #ifdef JPH_DEBUG_RENDERER
 	, bool inDrawConstraints
 #endif // JPH_DEBUG_RENDERER
-	) const
+	)
 {
 	// If there are no constraints we can immediately move to our target
 	if (ioConstraints.empty())
@@ -784,21 +849,16 @@ void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, f
 			if (c->mContact->mWasDiscarded)
 				continue;
 
-			// Check if we made contact with this before
-			if (!c->mContact->mHadCollision)
+			// Handle the contact
+			if (!c->mContact->mHadCollision
+				&& !HandleContact(velocity, *c, inDeltaTime))
 			{
-				// Handle the contact
-				if (!HandleContact(velocity, *c, inDeltaTime))
-				{
-					// Constraint should be ignored, remove it from the list
-					c->mContact->mWasDiscarded = true;
+				// Constraint should be ignored, remove it from the list
+				c->mContact->mWasDiscarded = true;
 
-					// Mark it as ignored for GetFirstContactForSweep
-					ioIgnoredContacts.emplace_back(c->mContact->mBodyB, c->mContact->mSubShapeIDB);
-					continue;
-				}
-
-				c->mContact->mHadCollision = true;
+				// Mark it as ignored for GetFirstContactForSweep
+				ioIgnoredContacts.emplace_back(*c->mContact);
+				continue;
 			}
 
 			// Cancel velocity of constraint if it cannot push the character
@@ -959,8 +1019,12 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 			&& c.mDistance < mCollisionTolerance
 			&& (inSkipContactVelocityCheck || c.mSurfaceNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 1.0e-4f))
 		{
-			if (ValidateContact(c) && !c.mIsSensorB)
+			if (ValidateContact(c))
+			{
+				CharacterContactSettings dummy;
+				ContactAdded(c, dummy);
 				c.mHadCollision = true;
+			}
 			else
 				c.mWasDiscarded = true;
 		}
@@ -979,7 +1043,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 	const Contact *deepest_contact = nullptr;
 	float smallest_distance = FLT_MAX;
 	for (const Contact &c : mActiveContacts)
-		if (c.mHadCollision)
+		if (c.mHadCollision && !c.mWasDiscarded)
 		{
 			// Calculate the angle between the plane normal and the up direction
 			float cos_angle = c.mSurfaceNormal.Dot(mUp);
@@ -1127,16 +1191,20 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 
 void CharacterVirtual::StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator)
 {
+	StartTrackingContactChanges();
+
 	mActiveContacts.assign(inContacts.begin(), inContacts.end());
 
 	UpdateSupportingContact(true, inAllocator);
+
+	FinishTrackingContactChanges();
 }
 
 void CharacterVirtual::MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator
 #ifdef JPH_DEBUG_RENDERER
 	, bool inDrawConstraints
 #endif // JPH_DEBUG_RENDERER
-	) const
+	)
 {
 	JPH_DET_LOG("CharacterVirtual::MoveShape: pos: " << ioPosition << " vel: " << inVelocity << " dt: " << inDeltaTime);
 
@@ -1237,6 +1305,7 @@ Vec3 CharacterVirtual::CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocit
 	Vec3 desired_velocity = inDesiredVelocity;
 	for (const Contact &c : mActiveContacts)
 		if (c.mHadCollision
+			&& !c.mWasDiscarded
 			&& IsSlopeTooSteep(c.mSurfaceNormal))
 		{
 			// Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement.
@@ -1253,12 +1322,72 @@ Vec3 CharacterVirtual::CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocit
 	return desired_velocity;
 }
 
+void CharacterVirtual::StartTrackingContactChanges()
+{
+	// Check if we're starting for the first time
+	if (++mTrackingContactChanges > 1)
+		return;
+
+	// No need to track anything if we don't have a listener
+	JPH_ASSERT(mListenerContacts.empty());
+	if (mListener == nullptr)
+		return;
+
+	// Mark all current contacts as not seen
+	mListenerContacts.reserve(ListenerContacts::size_type(mActiveContacts.size()));
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision)
+			mListenerContacts.insert(ListenerContacts::value_type(c, ListenerContactValue()));
+}
+
+void CharacterVirtual::FinishTrackingContactChanges()
+{
+	// Check if we have to do anything
+	int count = --mTrackingContactChanges;
+	JPH_ASSERT(count >= 0, "Called FinishTrackingContactChanges more times than StartTrackingContactChanges");
+	if (count > 0)
+		return;
+
+	// No need to track anything if we don't have a listener
+	if (mListener == nullptr)
+		return;
+
+	// Since we can do multiple operations (e.g. Update followed by WalkStairs)
+	// we can end up with contacts that were marked as active to the listener but that are
+	// no longer in the active contact list. We go over all contacts and mark them again
+	// to ensure that these lists are in sync.
+	for (ListenerContacts::value_type &c : mListenerContacts)
+		c.second.mCount = 0;
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision)
+		{
+			ListenerContacts::iterator it = mListenerContacts.find(c);
+			JPH_ASSERT(it != mListenerContacts.end());
+			it->second.mCount = 1;
+		}
+
+	// Call contact removal callbacks
+	for (ListenerContacts::iterator it = mListenerContacts.begin(); it != mListenerContacts.end(); ++it)
+		if (it->second.mCount == 0)
+		{
+			const ContactKey &c = it->first;
+			if (!c.mCharacterIDB.IsInvalid())
+				mListener->OnCharacterContactRemoved(this, c.mCharacterIDB, c.mSubShapeIDB);
+			else
+				mListener->OnContactRemoved(this, c.mBodyB, c.mSubShapeIDB);
+		}
+	mListenerContacts.ClearAndKeepMemory();
+}
+
 void CharacterVirtual::Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
 {
 	// If there's no delta time, we don't need to do anything
 	if (inDeltaTime <= 0.0f)
 		return;
 
+	StartTrackingContactChanges();
+	JPH_SCOPE_EXIT([this]() { FinishTrackingContactChanges(); });
+
 	// Remember delta time for checking if we're supported by the ground
 	mLastDeltaTime = inDeltaTime;
 
@@ -1405,6 +1534,7 @@ bool CharacterVirtual::CanWalkStairs(Vec3Arg inLinearVelocity) const
 	// Check contacts for steep slopes
 	for (const Contact &c : mActiveContacts)
 		if (c.mHadCollision
+			&& !c.mWasDiscarded
 			&& c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact
 			&& IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep
 			return true;
@@ -1414,6 +1544,9 @@ bool CharacterVirtual::CanWalkStairs(Vec3Arg inLinearVelocity) const
 
 bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
 {
+	StartTrackingContactChanges();
+	JPH_SCOPE_EXIT([this]() { FinishTrackingContactChanges(); });
+
 	// Move up
 	Vec3 up = inStepUp;
 	Contact contact;
@@ -1442,6 +1575,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 	steep_slope_normals.reserve(mActiveContacts.size());
 	for (const Contact &c : mActiveContacts)
 		if (c.mHadCollision
+			&& !c.mWasDiscarded
 			&& c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact
 			&& IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep
 			steep_slope_normals.push_back(c.mSurfaceNormal);
@@ -1490,7 +1624,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 		RVec3 debug_pos = new_position + contact.mFraction * down;
 		DebugRenderer::sInstance->DrawArrow(new_position, debug_pos, Color::sWhite, 0.01f);
 		DebugRenderer::sInstance->DrawArrow(contact.mPosition, contact.mPosition + contact.mSurfaceNormal, Color::sWhite, 0.01f);
-		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sWhite, false, true);
+		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sOne(), Color::sWhite, false, true);
 	}
 #endif // JPH_DEBUG_RENDERER
 
@@ -1528,7 +1662,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 			RVec3 debug_pos = test_position + test_contact.mFraction * down;
 			DebugRenderer::sInstance->DrawArrow(test_position, debug_pos, Color::sCyan, 0.01f);
 			DebugRenderer::sInstance->DrawArrow(test_contact.mPosition, test_contact.mPosition + test_contact.mSurfaceNormal, Color::sCyan, 0.01f);
-			mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sCyan, false, true);
+			mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sOne(), Color::sCyan, false, true);
 		}
 	#endif // JPH_DEBUG_RENDERER
 
@@ -1551,6 +1685,9 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 
 bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
 {
+	StartTrackingContactChanges();
+	JPH_SCOPE_EXIT([this]() { FinishTrackingContactChanges(); });
+
 	// Try to find the floor
 	Contact contact;
 	IgnoredContactList dummy_ignored_contacts(inAllocator);
@@ -1565,7 +1702,7 @@ bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFil
 	if (sDrawStickToFloor)
 	{
 		DebugRenderer::sInstance->DrawArrow(mPosition, new_position, Color::sOrange, 0.01f);
-		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(new_position, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sOrange, false, true);
+		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(new_position, mRotation, mShape), Vec3::sOne(), Color::sOrange, false, true);
 	}
 #endif // JPH_DEBUG_RENDERER
 
@@ -1576,6 +1713,9 @@ bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFil
 
 void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
 {
+	StartTrackingContactChanges();
+	JPH_SCOPE_EXIT([this]() { FinishTrackingContactChanges(); });
+
 	// Update the velocity
 	Vec3 desired_velocity = mLinearVelocity;
 	mLinearVelocity = CancelVelocityTowardsSteepSlopes(desired_velocity);
@@ -1652,37 +1792,54 @@ void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, cons
 	}
 }
 
+void CharacterVirtual::ContactKey::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mBodyB);
+	inStream.Write(mCharacterIDB);
+	inStream.Write(mSubShapeIDB);
+}
+
+void CharacterVirtual::ContactKey::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mBodyB);
+	inStream.Read(mCharacterIDB);
+	inStream.Read(mSubShapeIDB);
+}
+
 void CharacterVirtual::Contact::SaveState(StateRecorder &inStream) const
 {
+	ContactKey::SaveState(inStream);
+
 	inStream.Write(mPosition);
 	inStream.Write(mLinearVelocity);
 	inStream.Write(mContactNormal);
 	inStream.Write(mSurfaceNormal);
 	inStream.Write(mDistance);
 	inStream.Write(mFraction);
-	inStream.Write(mBodyB);
-	inStream.Write(mSubShapeIDB);
 	inStream.Write(mMotionTypeB);
+	inStream.Write(mIsSensorB);
 	inStream.Write(mHadCollision);
 	inStream.Write(mWasDiscarded);
 	inStream.Write(mCanPushCharacter);
-	// Cannot store user data (may be a pointer) and material
+	// Cannot store pointers to character B, user data and material
 }
 
 void CharacterVirtual::Contact::RestoreState(StateRecorder &inStream)
 {
+	ContactKey::RestoreState(inStream);
+
 	inStream.Read(mPosition);
 	inStream.Read(mLinearVelocity);
 	inStream.Read(mContactNormal);
 	inStream.Read(mSurfaceNormal);
 	inStream.Read(mDistance);
 	inStream.Read(mFraction);
-	inStream.Read(mBodyB);
-	inStream.Read(mSubShapeIDB);
 	inStream.Read(mMotionTypeB);
+	inStream.Read(mIsSensorB);
 	inStream.Read(mHadCollision);
 	inStream.Read(mWasDiscarded);
 	inStream.Read(mCanPushCharacter);
+	mCharacterB = nullptr; // Cannot restore character B
 	mUserData = 0; // Cannot restore user data
 	mMaterial = PhysicsMaterial::sDefault; // Cannot restore material
 }

thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <Jolt/Physics/Character/CharacterBase.h>
+#include <Jolt/Physics/Character/CharacterID.h>
 #include <Jolt/Physics/Body/MotionType.h>
 #include <Jolt/Physics/Body/BodyFilter.h>
 #include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
@@ -23,6 +24,9 @@ class JPH_EXPORT CharacterVirtualSettings : public CharacterBaseSettings
 public:
 	JPH_OVERRIDE_NEW_DELETE
 
+	/// ID to give to this character. This is used for deterministically sorting and as an identifier to represent the character in the contact removal callback.
+	CharacterID							mID = CharacterID::sNextCharacterID();
+
 	/// Character mass (kg). Used to push down objects with gravity when the character is standing on top.
 	float								mMass = 70.0f;
 
@@ -50,6 +54,10 @@ public:
 	/// - Fast moving objects of motion quality LinearCast will not be able to pass through the CharacterVirtual in 1 time step
 	RefConst<Shape>						mInnerBodyShape;
 
+	/// For a deterministic simulation, it is important to have a deterministic body ID. When set and when mInnerBodyShape is specified,
+	/// the inner body will be created with this specified ID instead of a generated ID.
+	BodyID								mInnerBodyIDOverride;
+
 	/// Layer that the inner rigid body will be added to
 	ObjectLayer							mInnerBodyLayer = 0;
 };
@@ -84,7 +92,7 @@ public:
 	/// Same as OnContactValidate but when colliding with a CharacterVirtual
 	virtual bool						OnCharacterContactValidate(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2) { return true; }
 
-	/// Called whenever the character collides with a body.
+	/// Called whenever the character collides with a body for the first time.
 	/// @param inCharacter Character that is being solved
 	/// @param inBodyID2 Body ID of body that is being hit
 	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
@@ -93,9 +101,32 @@ public:
 	/// @param ioSettings Settings returned by the contact callback to indicate how the character should behave
 	virtual void						OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
 
+	/// Called whenever the character persists colliding with a body.
+	/// @param inCharacter Character that is being solved
+	/// @param inBodyID2 Body ID of body that is being hit
+	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
+	/// @param inContactPosition World space contact position
+	/// @param inContactNormal World space contact normal
+	/// @param ioSettings Settings returned by the contact callback to indicate how the character should behave
+	virtual void						OnContactPersisted(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+
+	/// Called whenever the character loses contact with a body.
+	/// Note that there is no guarantee that the body or its sub shape still exists at this point. The body may have been deleted since the last update.
+	/// @param inCharacter Character that is being solved
+	/// @param inBodyID2 Body ID of body that is being hit
+	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
+	virtual void						OnContactRemoved(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) { /* Default do nothing */ }
+
 	/// Same as OnContactAdded but when colliding with a CharacterVirtual
 	virtual void						OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
 
+	/// Same as OnContactPersisted but when colliding with a CharacterVirtual
+	virtual void						OnCharacterContactPersisted(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+
+	/// Same as OnContactRemoved but when colliding with a CharacterVirtual
+	/// Note that inOtherCharacterID can be the ID of a character that has been deleted. This happens if the character was in contact with this character during the last update, but has been deleted since.
+	virtual void						OnCharacterContactRemoved(const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2) { /* Default do nothing */ }
+
 	/// Called whenever a contact is being used by the solver. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces).
 	/// @param inCharacter Character that is being solved
 	/// @param inBodyID2 Body ID of body that is being hit
@@ -139,6 +170,8 @@ public:
 };
 
 /// Simple collision checker that loops over all registered characters.
+/// This is a brute force checking algorithm. If you have a lot of characters you may want to store your characters
+/// in a hierarchical structure to make this more efficient.
 /// Note that this is not thread safe, so make sure that only one CharacterVirtual is checking collision at a time.
 class JPH_EXPORT CharacterVsCharacterCollisionSimple : public CharacterVsCharacterCollision
 {
@@ -159,8 +192,9 @@ public:
 /// Runtime character object.
 /// This object usually represents the player. Contrary to the Character class it doesn't use a rigid body but moves doing collision checks only (hence the name virtual).
 /// The advantage of this is that you can determine when the character moves in the frame (usually this has to happen at a very particular point in the frame)
-/// but the downside is that other objects don't see this virtual character. In order to make this work it is recommended to pair a CharacterVirtual with a Character that
-/// moves along. This Character should be keyframed (or at least have no gravity) and move along with the CharacterVirtual so that other rigid bodies can collide with it.
+/// but the downside is that other objects don't see this virtual character. To make a CharacterVirtual visible to the simulation, you can optionally create an inner
+/// rigid body through CharacterVirtualSettings::mInnerBodyShape. A CharacterVirtual is not tracked by the PhysicsSystem so you need to update it yourself. This also means
+/// that a call to PhysicsSystem::SaveState will not save its state, you need to call CharacterVirtual::SaveState yourself.
 class JPH_EXPORT CharacterVirtual : public CharacterBase
 {
 public:
@@ -180,6 +214,9 @@ public:
 	/// Destructor
 	virtual								~CharacterVirtual() override;
 
+	/// The ID of this character
+	inline const CharacterID &			GetID() const											{ return mID; }
+
 	/// Set the contact listener
 	void								SetListener(CharacterContactListener *inListener)		{ mListener = inListener; }
 
@@ -266,6 +303,15 @@ public:
 	/// @return A new velocity vector that won't make the character move up steep slopes
 	Vec3								CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const;
 
+	/// This function is internally called by Update, WalkStairs, StickToFloor and ExtendedUpdate and is responsible for tracking if contacts are added, persisted or removed.
+	/// If you want to do multiple operations on a character (e.g. first Update then WalkStairs), you can surround the code with a StartTrackingContactChanges and FinishTrackingContactChanges pair
+	/// to only receive a single callback per contact on the CharacterContactListener. If you don't do this then you could for example receive a contact added callback during the Update and a
+	/// contact persisted callback during WalkStairs.
+	void								StartTrackingContactChanges();
+
+	/// This call triggers contact removal callbacks and is used in conjunction with StartTrackingContactChanges.
+	void								FinishTrackingContactChanges();
+
 	/// This is the main update function. It moves the character according to its current velocity (the character is similar to a kinematic body in the sense
 	/// that you set the velocity and the character will follow unless collision is blocking the way). Note it's your own responsibility to apply gravity to the character velocity!
 	/// Different surface materials (like ice) can be emulated by getting the ground material and adjusting the velocity and/or the max slope angle accordingly every frame.
@@ -383,15 +429,53 @@ public:
 	static inline bool					sDrawStickToFloor = false;								///< Draw the state of the stick to floor algorithm
 #endif
 
-	// Encapsulates a collision contact
-	struct Contact
+	/// Uniquely identifies a contact between a character and another body or character
+	class ContactKey
 	{
+	public:
+		/// Constructor
+										ContactKey() = default;
+										ContactKey(const ContactKey &inContact) = default;
+										ContactKey(const BodyID &inBodyB, const SubShapeID &inSubShapeID) : mBodyB(inBodyB), mSubShapeIDB(inSubShapeID) { }
+										ContactKey(const CharacterID &inCharacterIDB, const SubShapeID &inSubShapeID) : mCharacterIDB(inCharacterIDB), mSubShapeIDB(inSubShapeID) { }
+		ContactKey &					operator = (const ContactKey &inContact) = default;
+
+		/// Checks if two contacts refer to the same body (or virtual character)
+		inline bool						IsSameBody(const ContactKey &inOther) const				{ return mBodyB == inOther.mBodyB && mCharacterIDB == inOther.mCharacterIDB; }
+
+		/// Equality operator
+		bool							operator == (const ContactKey &inRHS) const
+		{
+			return mBodyB == inRHS.mBodyB && mCharacterIDB == inRHS.mCharacterIDB && mSubShapeIDB == inRHS.mSubShapeIDB;
+		}
+
+		bool							operator != (const ContactKey &inRHS) const
+		{
+			return !(*this == inRHS);
+		}
+
+		/// Hash of this structure
+		uint64							GetHash() const
+		{
+			static_assert(sizeof(BodyID) + sizeof(CharacterID) + sizeof(SubShapeID) == sizeof(ContactKey), "No padding expected");
+			return HashBytes(this, sizeof(ContactKey));
+		}
+
 		// Saving / restoring state for replay
 		void							SaveState(StateRecorder &inStream) const;
 		void							RestoreState(StateRecorder &inStream);
 
-		// Checks if two contacts refer to the same body (or virtual character)
-		inline bool						IsSameBody(const Contact &inOther) const				{ return mBodyB == inOther.mBodyB && mCharacterB == inOther.mCharacterB; }
+		BodyID							mBodyB;													///< ID of body we're colliding with (if not invalid)
+		CharacterID						mCharacterIDB;											///< Character we're colliding with (if not invalid)
+		SubShapeID						mSubShapeIDB;											///< Sub shape ID of body or character we're colliding with
+	};
+
+	/// Encapsulates a collision contact
+	struct Contact : public ContactKey
+	{
+		// Saving / restoring state for replay
+		void							SaveState(StateRecorder &inStream) const;
+		void							RestoreState(StateRecorder &inStream);
 
 		RVec3							mPosition;												///< Position where the character makes contact
 		Vec3							mLinearVelocity;										///< Velocity of the contact point
@@ -399,15 +483,13 @@ public:
 		Vec3							mSurfaceNormal;											///< Surface normal of the contact
 		float							mDistance;												///< Distance to the contact <= 0 means that it is an actual contact, > 0 means predictive
 		float							mFraction;												///< Fraction along the path where this contact takes place
-		BodyID							mBodyB;													///< ID of body we're colliding with (if not invalid)
-		CharacterVirtual *				mCharacterB = nullptr;									///< Character we're colliding with (if not null)
-		SubShapeID						mSubShapeIDB;											///< Sub shape ID of body we're colliding with
 		EMotionType						mMotionTypeB;											///< Motion type of B, used to determine the priority of the contact
 		bool							mIsSensorB;												///< If B is a sensor
+		const CharacterVirtual *		mCharacterB = nullptr;									///< Character we're colliding with (if not nullptr). Note that this may be a dangling pointer when accessed through GetActiveContacts(), use mCharacterIDB instead.
 		uint64							mUserData;												///< User data of B
 		const PhysicsMaterial *			mMaterial;												///< Material of B
 		bool							mHadCollision = false;									///< If the character actually collided with the contact (can be false if a predictive contact never becomes a real one)
-		bool							mWasDiscarded = false;									///< If the contact validate callback chose to discard this contact
+		bool							mWasDiscarded = false;									///< If the contact validate callback chose to discard this contact or when the body is a sensor
 		bool							mCanPushCharacter = true;								///< When true, the velocity of the contact point can push the character
 	};
 
@@ -415,9 +497,10 @@ public:
 	using ContactList = Array<Contact>;
 
 	/// Access to the internal list of contacts that the character has found.
+	/// Note that only contacts that have their mHadCollision flag set are actual contacts.
 	const ContactList &					GetActiveContacts() const								{ return mActiveContacts; }
 
-	/// Check if the character is currently in contact with or has collided with another body in the last time step
+	/// Check if the character is currently in contact with or has collided with another body in the last operation (e.g. Update or WalkStairs)
 	bool								HasCollidedWith(const BodyID &inBody) const
 	{
 		for (const CharacterVirtual::Contact &c : mActiveContacts)
@@ -426,15 +509,21 @@ public:
 		return false;
 	}
 
-	/// Check if the character is currently in contact with or has collided with another character in the last time step
-	bool								HasCollidedWith(const CharacterVirtual *inCharacter) const
+	/// Check if the character is currently in contact with or has collided with another character in the last time step (e.g. Update or WalkStairs)
+	bool								HasCollidedWith(const CharacterID &inCharacterID) const
 	{
 		for (const CharacterVirtual::Contact &c : mActiveContacts)
-			if (c.mHadCollision && c.mCharacterB == inCharacter)
+			if (c.mHadCollision && c.mCharacterIDB == inCharacterID)
 				return true;
 		return false;
 	}
 
+	/// Check if the character is currently in contact with or has collided with another character in the last time step (e.g. Update or WalkStairs)
+	bool								HasCollidedWith(const CharacterVirtual *inCharacter) const
+	{
+		return HasCollidedWith(inCharacter->GetID());
+	}
+
 private:
 	// Sorting predicate for making contact order deterministic
 	struct ContactOrderingPredicate
@@ -444,21 +533,14 @@ private:
 			if (inLHS.mBodyB != inRHS.mBodyB)
 				return inLHS.mBodyB < inRHS.mBodyB;
 
+			if (inLHS.mCharacterIDB != inRHS.mCharacterIDB)
+				return inLHS.mCharacterIDB < inRHS.mCharacterIDB;
+
 			return inLHS.mSubShapeIDB.GetValue() < inRHS.mSubShapeIDB.GetValue();
 		}
 	};
 
-	// A contact that needs to be ignored
-	struct IgnoredContact
-	{
-										IgnoredContact() = default;
-										IgnoredContact(const BodyID &inBodyID, const SubShapeID &inSubShapeID) : mBodyID(inBodyID), mSubShapeID(inSubShapeID) { }
-
-		BodyID							mBodyID;												///< ID of body we're colliding with
-		SubShapeID						mSubShapeID;											///< Sub shape of body we're colliding with
-	};
-
-	using IgnoredContactList = Array<IgnoredContact, STLTempAllocator<IgnoredContact>>;
+	using IgnoredContactList = Array<ContactKey, STLTempAllocator<ContactKey>>;
 
 	// A constraint that limits the movement of the character
 	struct Constraint
@@ -517,20 +599,20 @@ private:
 	// Helper function to convert a Jolt collision result into a contact
 	template <class taCollector>
 	inline static void					sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult);
-	inline static void					sFillCharacterContactProperties(Contact &outContact, CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult);
+	inline static void					sFillCharacterContactProperties(Contact &outContact, const CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult);
 
 	// Move the shape from ioPosition and try to displace it by inVelocity * inDeltaTime, this will try to slide the shape along the world geometry
 	void								MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator
 	#ifdef JPH_DEBUG_RENDERER
 		, bool inDrawConstraints = false
 	#endif // JPH_DEBUG_RENDERER
-		) const;
+		);
 
 	// Ask the callback if inContact is a valid contact point
 	bool								ValidateContact(const Contact &inContact) const;
 
 	// Trigger the contact callback for inContact and get the contact settings
-	void								ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings) const;
+	void								ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings);
 
 	// Tests the shape for collision around inPosition
 	void								GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
@@ -546,7 +628,7 @@ private:
 	#ifdef JPH_DEBUG_RENDERER
 		, bool inDrawConstraints = false
 	#endif // JPH_DEBUG_RENDERER
-		) const;
+		);
 
 	// Get the velocity of a body adjusted by the contact listener
 	void								GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const;
@@ -557,7 +639,7 @@ private:
 	Vec3								CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const;
 
 	// Handle contact with physics object that we're colliding against
-	bool								HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const;
+	bool								HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime);
 
 	// Does a swept test of the shape from inPosition with displacement inDisplacement, returns true if there was a collision
 	bool								GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
@@ -586,6 +668,9 @@ private:
 	// Move the inner rigid body to the current position
 	void								UpdateInnerBodyTransform();
 
+	// ID
+	CharacterID							mID;
+
 	// Our main listener for contacts
 	CharacterContactListener *			mListener = nullptr;
 
@@ -626,6 +711,22 @@ private:
 	// List of contacts that were active in the last frame
 	ContactList							mActiveContacts;
 
+	// Remembers how often we called StartTrackingContactChanges
+	int									mTrackingContactChanges = 0;
+
+	// View from a contact listener perspective on which contacts have been added/removed
+	struct ListenerContactValue
+	{
+										ListenerContactValue() = default;
+		explicit						ListenerContactValue(const CharacterContactSettings &inSettings) : mSettings(inSettings) { }
+
+		CharacterContactSettings		mSettings;
+		int								mCount = 0;
+	};
+
+	using ListenerContacts = UnorderedMap<ContactKey, ListenerContactValue>;
+	ListenerContacts					mListenerContacts;
+
 	// Remembers the delta time of the last update
 	float								mLastDeltaTime = 1.0f / 60.0f;
 

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -155,7 +155,8 @@ BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int
 {
 	JPH_PROFILE_FUNCTION();
 
-	JPH_ASSERT(inNumber > 0);
+	if (inNumber <= 0)
+		return nullptr;
 
 	const BodyVector &bodies = mBodyManager->GetBodies();
 	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
@@ -208,6 +209,12 @@ void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddSt
 {
 	JPH_PROFILE_FUNCTION();
 
+	if (inNumber <= 0)
+	{
+		JPH_ASSERT(inAddState == nullptr);
+		return;
+	}
+
 	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
 	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
 
@@ -244,6 +251,12 @@ void BroadPhaseQuadTree::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState
 {
 	JPH_PROFILE_FUNCTION();
 
+	if (inNumber <= 0)
+	{
+		JPH_ASSERT(inAddState == nullptr);
+		return;
+	}
+
 	JPH_IF_ENABLE_ASSERTS(const BodyVector &bodies = mBodyManager->GetBodies();)
 	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
 
@@ -278,11 +291,12 @@ void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber)
 {
 	JPH_PROFILE_FUNCTION();
 
+	if (inNumber <= 0)
+		return;
+
 	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
 	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
 
-	JPH_ASSERT(inNumber > 0);
-
 	BodyVector &bodies = mBodyManager->GetBodies();
 	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
 
@@ -325,7 +339,8 @@ void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber,
 {
 	JPH_PROFILE_FUNCTION();
 
-	JPH_ASSERT(inNumber > 0);
+	if (inNumber <= 0)
+		return;
 
 	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
 	if (inTakeLock)
@@ -365,7 +380,8 @@ void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber
 {
 	JPH_PROFILE_FUNCTION();
 
-	JPH_ASSERT(inNumber > 0);
+	if (inNumber <= 0)
+		return;
 
 	// First sort the bodies that actually changed layer to beginning of the array
 	const BodyVector &bodies = mBodyManager->GetBodies();

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp

@@ -15,6 +15,7 @@
 #include <Jolt/Geometry/AABox4.h>
 #include <Jolt/Geometry/RayAABox.h>
 #include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/Core/STLLocalAllocator.h>
 
 #ifdef JPH_DUMP_BROADPHASE_TREE
 JPH_SUPPRESS_WARNINGS_STD_BEGIN
@@ -57,6 +58,15 @@ void QuadTree::Node::GetChildBounds(int inChildIndex, AABox &outBounds) const
 
 void QuadTree::Node::SetChildBounds(int inChildIndex, const AABox &inBounds)
 {
+	// Bounding boxes provided to the quad tree should never be larger than cLargeFloat because this may trigger overflow exceptions
+	// e.g. when squaring the value while testing sphere overlaps
+	JPH_ASSERT(inBounds.mMin.GetX() >= -cLargeFloat && inBounds.mMin.GetX() <= cLargeFloat
+			   && inBounds.mMin.GetY() >= -cLargeFloat && inBounds.mMin.GetY() <= cLargeFloat
+			   && inBounds.mMin.GetZ() >= -cLargeFloat && inBounds.mMin.GetZ() <= cLargeFloat
+			   && inBounds.mMax.GetX() >= -cLargeFloat && inBounds.mMax.GetX() <= cLargeFloat
+			   && inBounds.mMax.GetY() >= -cLargeFloat && inBounds.mMax.GetY() <= cLargeFloat
+			   && inBounds.mMax.GetZ() >= -cLargeFloat && inBounds.mMax.GetZ() <= cLargeFloat);
+
 	// Set max first (this keeps the bounding box invalid for reading threads)
 	mBoundsMaxZ[inChildIndex] = inBounds.mMax.GetZ();
 	mBoundsMaxY[inChildIndex] = inBounds.mMax.GetY();
@@ -110,7 +120,6 @@ bool QuadTree::Node::EncapsulateChildBounds(int inChildIndex, const AABox &inBou
 // QuadTree
 ////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-const float QuadTree::cLargeFloat = 1.0e30f;
 const AABox QuadTree::cInvalidBounds(Vec3::sReplicate(cLargeFloat), Vec3::sReplicate(-cLargeFloat));
 
 void QuadTree::GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const
@@ -152,16 +161,17 @@ QuadTree::~QuadTree()
 
 	// Collect all bodies
 	Allocator::Batch free_batch;
-	NodeID node_stack[cStackSize];
-	node_stack[0] = root_node.GetNodeID();
-	JPH_ASSERT(node_stack[0].IsValid());
-	if (node_stack[0].IsNode())
+	Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack;
+	node_stack.reserve(cStackSize);
+	node_stack.push_back(root_node.GetNodeID());
+	JPH_ASSERT(node_stack.front().IsValid());
+	if (node_stack.front().IsNode())
 	{
-		int top = 0;
 		do
 		{
 			// Process node
-			NodeID node_id = node_stack[top];
+			NodeID node_id = node_stack.back();
+			node_stack.pop_back();
 			JPH_ASSERT(!node_id.IsBody());
 			uint32 node_idx = node_id.GetNodeIndex();
 			const Node &node = mAllocator->Get(node_idx);
@@ -169,17 +179,12 @@ QuadTree::~QuadTree()
 			// Recurse and get all child nodes
 			for (NodeID child_node_id : node.mChildNodeID)
 				if (child_node_id.IsValid() && child_node_id.IsNode())
-				{
-					JPH_ASSERT(top < cStackSize);
-					node_stack[top] = child_node_id;
-					top++;
-				}
+					node_stack.push_back(child_node_id);
 
 			// Mark node to be freed
 			mAllocator->AddObjectToBatch(free_batch, node_idx);
-			--top;
 		}
-		while (top >= 0);
+		while (!node_stack.empty());
 	}
 
 	// Now free all nodes
@@ -191,6 +196,30 @@ uint32 QuadTree::AllocateNode(bool inIsChanged)
 	uint32 index = mAllocator->ConstructObject(inIsChanged);
 	if (index == Allocator::cInvalidObjectIndex)
 	{
+		// If you're running out of nodes, you're most likely adding too many individual bodies to the tree.
+		// Because of the lock free nature of this tree, any individual body is added to the root of the tree.
+		// This means that if you add a lot of bodies individually, you will end up with a very deep tree and you'll be
+		// using a lot more nodes than you would if you added them in batches.
+		// Please look at BodyInterface::AddBodiesPrepare/AddBodiesFinalize.
+		//
+		// If you have created a wrapper around Jolt then a possible solution is to activate a mode during loading
+		// that queues up any bodies that need to be added. When loading is done, insert all of them as a single batch.
+		// This could be implemented as a 'start batching' / 'end batching' call to switch in and out of that mode.
+		// The rest of the code can then just use the regular 'add single body' call on your wrapper and doesn't need to know
+		// if this mode is active or not.
+		//
+		// Calling PhysicsSystem::Update or PhysicsSystem::OptimizeBroadPhase will perform maintenance
+		// on the tree and will make it efficient again. If you're not calling these functions and are adding a lot of bodies
+		// you could still be running out of nodes because the tree is not being maintained. If your application is paused,
+		// consider still calling PhysicsSystem::Update with a delta time of 0 to keep the tree in good shape.
+		//
+		// The system keeps track of a previous and a current tree, this allows for queries to continue using the old tree
+		// while the new tree is being built. If you completely clean the PhysicsSystem and rebuild it from scratch, you may
+		// want to call PhysicsSystem::OptimizeBroadPhase two times after clearing to completely get rid of any lingering nodes.
+		//
+		// The number of nodes that is allocated is related to the max number of bodies that is passed in PhysicsSystem::Init.
+		// For normal situations there are plenty of nodes available. If all else fails, you can increase the number of nodes
+		// by increasing the maximum number of bodies.
 		Trace("QuadTree: Out of nodes!");
 		std::abort();
 	}
@@ -1472,22 +1501,28 @@ void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &in
 	JPH_ASSERT(inNodeIndex != cInvalidNodeIndex);
 
 	// To avoid call overhead, create a stack in place
+	JPH_SUPPRESS_WARNING_PUSH
+	JPH_CLANG_SUPPRESS_WARNING("-Wunused-member-function") // The default constructor of StackEntry is unused when using Jolt's Array class but not when using std::vector
 	struct StackEntry
 	{
+						StackEntry() = default;
+		inline			StackEntry(uint32 inNodeIndex, uint32 inParentNodeIndex) : mNodeIndex(inNodeIndex), mParentNodeIndex(inParentNodeIndex) { }
+
 		uint32			mNodeIndex;
 		uint32			mParentNodeIndex;
 	};
-	StackEntry stack[cStackSize];
-	stack[0].mNodeIndex = inNodeIndex;
-	stack[0].mParentNodeIndex = cInvalidNodeIndex;
-	int top = 0;
+	JPH_SUPPRESS_WARNING_POP
+	Array<StackEntry, STLLocalAllocator<StackEntry, cStackSize>> stack;
+	stack.reserve(cStackSize);
+	stack.emplace_back(inNodeIndex, cInvalidNodeIndex);
 
 	uint32 num_bodies = 0;
 
 	do
 	{
 		// Copy entry from the stack
-		StackEntry cur_stack = stack[top];
+		StackEntry cur_stack = stack.back();
+		stack.pop_back();
 
 		// Validate parent
 		const Node &node = mAllocator->Get(cur_stack.mNodeIndex);
@@ -1506,10 +1541,7 @@ void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &in
 				{
 					// Child is a node, recurse
 					uint32 child_idx = child_node_id.GetNodeIndex();
-					JPH_ASSERT(top < cStackSize);
-					StackEntry &new_entry = stack[top++];
-					new_entry.mNodeIndex = child_idx;
-					new_entry.mParentNodeIndex = cur_stack.mNodeIndex;
+					stack.emplace_back(child_idx, cur_stack.mNodeIndex);
 
 					// Validate that the bounding box is bigger or equal to the bounds in the tree
 					// Bounding box could also be invalid if all children of our child were removed
@@ -1530,20 +1562,19 @@ void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &in
 					JPH_ASSERT(node_idx == cur_stack.mNodeIndex);
 					JPH_ASSERT(child_idx == i);
 
-					// Validate that the body bounds are bigger or equal to the bounds in the tree
+					// Validate that the body cached bounds still match the actual bounds
+					const Body *body = inBodies[child_node_id.GetBodyID().GetIndex()];
+					body->ValidateCachedBounds();
+
+					// Validate that the node bounds are bigger or equal to the body bounds
 					AABox body_bounds;
 					node.GetChildBounds(i, body_bounds);
-					const Body *body = inBodies[child_node_id.GetBodyID().GetIndex()];
-					AABox cached_body_bounds = body->GetWorldSpaceBounds();
-					AABox real_body_bounds = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
-					JPH_ASSERT(cached_body_bounds == real_body_bounds); // Check that cached body bounds are up to date
-					JPH_ASSERT(body_bounds.Contains(real_body_bounds));
+					JPH_ASSERT(body_bounds.Contains(body->GetWorldSpaceBounds()));
 				}
 			}
 		}
-		--top;
 	}
-	while (top >= 0);
+	while (!stack.empty());
 
 	// Check that the amount of bodies in the tree matches our counter
 	JPH_ASSERT(num_bodies == inNumExpectedBodies);
@@ -1565,14 +1596,14 @@ void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) cons
 	f << "digraph {\n";
 
 	// Iterate the entire tree
-	NodeID node_stack[cStackSize];
-	node_stack[0] = inRoot;
-	JPH_ASSERT(node_stack[0].IsValid());
-	int top = 0;
+	Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack;
+	node_stack.push_back(inRoot);
+	JPH_ASSERT(inRoot.IsValid());
 	do
 	{
 		// Check if node is a body
-		NodeID node_id = node_stack[top];
+		NodeID node_id = node_stack.back();
+		node_stack.pop_back();
 		if (node_id.IsBody())
 		{
 			// Output body
@@ -1597,9 +1628,7 @@ void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) cons
 			for (NodeID child_node_id : node.mChildNodeID)
 				if (child_node_id.IsValid())
 				{
-					JPH_ASSERT(top < cStackSize);
-					node_stack[top] = child_node_id;
-					top++;
+					node_stack.push_back(child_node_id);
 
 					// Output link
 					f << "node" << node_str << " -> ";
@@ -1610,9 +1639,8 @@ void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) cons
 					f << "\n";
 				}
 		}
-		--top;
 	}
-	while (top >= 0);
+	while (!node_stack.empty());
 
 	// Finish DOT file
 	f << "}\n";

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h

@@ -38,7 +38,7 @@ private:
 		/// Construct a node ID
 		static inline NodeID	sInvalid()							{ return NodeID(cInvalidNodeIndex); }
 		static inline NodeID	sFromBodyID(BodyID inID)			{ NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; }
-		static inline NodeID	sFromNodeIndex(uint32 inIdx)		{ NodeID node_id(inIdx | cIsNode); JPH_ASSERT(node_id.IsNode()); return node_id; }
+		static inline NodeID	sFromNodeIndex(uint32 inIdx)		{ JPH_ASSERT((inIdx & cIsNode) == 0); return NodeID(inIdx | cIsNode); }
 
 		/// Check what type of ID it is
 		inline bool				IsValid() const						{ return mID != cInvalidNodeIndex; }
@@ -261,8 +261,7 @@ public:
 
 private:
 	/// Constants
-	static const uint32			cInvalidNodeIndex = 0xffffffff;		///< Value used to indicate node index is invalid
-	static const float			cLargeFloat;						///< A large floating point number that is small enough to not cause any overflows
+	static constexpr uint32		cInvalidNodeIndex = 0xffffffff;		///< Value used to indicate node index is invalid
 	static const AABox			cInvalidBounds;						///< Invalid bounding box using cLargeFloat
 
 	/// We alternate between two trees in order to let collision queries complete in parallel to adding/removing objects to the tree

thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp

@@ -79,7 +79,8 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2,
 		mShape1ExCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, mBufferExCvxRadius, mScale1);
 
 	// Perform GJK step
-	status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + mCollideShapeSettings.mMaxSeparationDistance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
+	float max_separation_distance = mCollideShapeSettings.mMaxSeparationDistance;
+	status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + max_separation_distance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
 
 	// Check result of collision detection
 	if (status == EPAPenetrationDepth::EStatus::NotColliding)
@@ -88,12 +89,18 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2,
 	{
 		// Need to run expensive EPA algorithm
 
+		// We know we're overlapping at this point, so we can set the max separation distance to 0.
+		// Numerically it is possible that GJK finds that the shapes are overlapping but EPA finds that they're separated.
+		// In order to avoid this, we clamp the max separation distance to 1 so that we don't excessively inflate the shape,
+		// but we still inflate it enough to avoid the case where EPA misses the collision.
+		max_separation_distance = min(max_separation_distance, 1.0f);
+
 		// Get the support function
 		if (mShape1IncCvxRadius == nullptr)
 			mShape1IncCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, mBufferIncCvxRadius, mScale1);
 
 		// Add convex radius
-		AddConvexRadius<ConvexShape::Support> shape1_add_max_separation_distance(*mShape1IncCvxRadius, mCollideShapeSettings.mMaxSeparationDistance);
+		AddConvexRadius shape1_add_max_separation_distance(*mShape1IncCvxRadius, max_separation_distance);
 
 		// Perform EPA step
 		if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, triangle, mCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
@@ -101,14 +108,14 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2,
 	}
 
 	// Check if the penetration is bigger than the early out fraction
-	float penetration_depth = (point2 - point1).Length() - mCollideShapeSettings.mMaxSeparationDistance;
+	float penetration_depth = (point2 - point1).Length() - max_separation_distance;
 	if (-penetration_depth >= mCollector.GetEarlyOutFraction())
 		return;
 
 	// Correct point1 for the added separation distance
 	float penetration_axis_len = penetration_axis.Length();
 	if (penetration_axis_len > 0.0f)
-		point1 -= penetration_axis * (mCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len);
+		point1 -= penetration_axis * (max_separation_distance / penetration_axis_len);
 
 	// Check if we have enabled active edge detection
 	if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)

thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h

@@ -0,0 +1,93 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2025 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Core/STLLocalAllocator.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collide 2 shapes and returns at most 1 hit per leaf shape pairs that overlapping. This can be used when not all contacts between the shapes are needed.
+/// E.g. when testing a compound with 2 MeshShapes A and B against a compound with 2 SphereShapes C and D, then at most you'll get 4 collisions: AC, AD, BC, BD.
+/// The default CollisionDispatch::sCollideShapeVsShape function would return all intersecting triangles in A against C, all in B against C etc.
+/// @param inShape1 The first shape
+/// @param inShape2 The second shape
+/// @param inScale1 Local space scale of shape 1 (scales relative to its center of mass)
+/// @param inScale2 Local space scale of shape 2 (scales relative to its center of mass)
+/// @param inCenterOfMassTransform1 Transform to transform center of mass of shape 1 into world space
+/// @param inCenterOfMassTransform2 Transform to transform center of mass of shape 2 into world space
+/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for shape 1
+/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for shape 2
+/// @param inCollideShapeSettings Options for the CollideShape test
+/// @param ioCollector The collector that receives the results.
+/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
+/// @tparam LeafCollector The type of the collector that will be used to collect hits between leaf pairs. Must be either AnyHitCollisionCollector<CollideShapeCollector> to get any hit (cheapest) or ClosestHitCollisionCollector<CollideShapeCollector> to get the deepest hit (more expensive).
+template <class LeafCollector>
+void CollideShapeVsShapePerLeaf(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
+{
+	// Tracks information we need about a leaf shape
+	struct LeafShape
+	{
+							LeafShape() = default;
+
+							LeafShape(const AABox &inBounds, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Shape *inShape, const SubShapeIDCreator &inSubShapeIDCreator) :
+			mBounds(inBounds),
+			mCenterOfMassTransform(inCenterOfMassTransform),
+			mScale(inScale),
+			mShape(inShape),
+			mSubShapeIDCreator(inSubShapeIDCreator)
+		{
+		}
+
+		AABox				mBounds;
+		Mat44				mCenterOfMassTransform;
+		Vec3				mScale;
+		const Shape *		mShape;
+		SubShapeIDCreator	mSubShapeIDCreator;
+	};
+
+	constexpr uint cMaxLocalLeafShapes = 32;
+
+	// A collector that stores the information we need from a leaf shape in an array that is usually on the stack but can fall back to the heap if needed
+	class MyCollector : public TransformedShapeCollector
+	{
+	public:
+							MyCollector()
+		{
+			mHits.reserve(cMaxLocalLeafShapes);
+		}
+
+		void				AddHit(const TransformedShape &inShape) override
+		{
+			mHits.emplace_back(inShape.GetWorldSpaceBounds(), inShape.GetCenterOfMassTransform().ToMat44(), inShape.GetShapeScale(), inShape.mShape, inShape.mSubShapeIDCreator);
+		}
+
+		Array<LeafShape, STLLocalAllocator<LeafShape, cMaxLocalLeafShapes>> mHits;
+	};
+
+	// Get bounds of both shapes
+	AABox bounds1 = inShape1->GetWorldSpaceBounds(inCenterOfMassTransform1, inScale1);
+	AABox bounds2 = inShape2->GetWorldSpaceBounds(inCenterOfMassTransform2, inScale2);
+
+	// Get leaf shapes that overlap with the bounds of the other shape
+	MyCollector leaf_shapes1, leaf_shapes2;
+	inShape1->CollectTransformedShapes(bounds2, inCenterOfMassTransform1.GetTranslation(), inCenterOfMassTransform1.GetQuaternion(), inScale1, inSubShapeIDCreator1, leaf_shapes1, inShapeFilter);
+	inShape2->CollectTransformedShapes(bounds1, inCenterOfMassTransform2.GetTranslation(), inCenterOfMassTransform2.GetQuaternion(), inScale2, inSubShapeIDCreator2, leaf_shapes2, inShapeFilter);
+
+	// Now test each leaf shape against each other leaf
+	for (const LeafShape &leaf1 : leaf_shapes1.mHits)
+		for (const LeafShape &leaf2 : leaf_shapes2.mHits)
+			if (leaf1.mBounds.Overlaps(leaf2.mBounds))
+			{
+				// Use the leaf collector to collect max 1 hit for this pair and pass it on to ioCollector
+				LeafCollector collector;
+				CollisionDispatch::sCollideShapeVsShape(leaf1.mShape, leaf2.mShape, leaf1.mScale, leaf2.mScale, leaf1.mCenterOfMassTransform, leaf2.mCenterOfMassTransform, leaf1.mSubShapeIDCreator, leaf2.mSubShapeIDCreator, inCollideShapeSettings, collector, inShapeFilter);
+				if (collector.HadHit())
+					ioCollector.AddHit(collector.mHit);
+			}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h

@@ -66,7 +66,11 @@ public:
 	/// before AddHit is called (e.g. the user data pointer or the velocity of the body).
 	virtual void			OnBody([[maybe_unused]] const Body &inBody)		{ /* Collects nothing by default */ }
 
-	/// Set by the collision detection functions to the current TransformedShape that we're colliding against before calling the AddHit function
+	/// When running a query through the NarrowPhaseQuery class, this will be called after all AddHit calls have been made for a particular body.
+	virtual void			OnBodyEnd()										{ /* Does nothing by default */ }
+
+	/// Set by the collision detection functions to the current TransformedShape that we're colliding against before calling the AddHit function.
+	/// Note: Only valid during AddHit! For performance reasons, the pointer is not reset after leaving AddHit so the context may point to freed memory.
 	void					SetContext(const TransformedShape *inContext)	{ mContext = inContext; }
 	const TransformedShape *GetContext() const								{ return mContext; }
 

thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h

@@ -89,6 +89,91 @@ private:
 	bool				mHadHit = false;
 };
 
+/// Implementation that collects the closest / deepest hit for each body and optionally sorts them on distance
+template <class CollectorType>
+class ClosestHitPerBodyCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHits.clear();
+		mHadHit = false;
+	}
+
+	// See: CollectorType::OnBody
+	virtual void		OnBody(const Body &inBody) override
+	{
+		// Store the early out fraction so we can restore it after we've collected all hits for this body
+		mPreviousEarlyOutFraction = CollectorType::GetEarlyOutFraction();
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		float early_out = inResult.GetEarlyOutFraction();
+		if (!mHadHit || early_out < CollectorType::GetEarlyOutFraction())
+		{
+			// Update early out fraction to avoid spending work on collecting further hits for this body
+			CollectorType::UpdateEarlyOutFraction(early_out);
+
+			if (!mHadHit)
+			{
+				// First time we have a hit we append it to the array
+				mHits.push_back(inResult);
+				mHadHit = true;
+			}
+			else
+			{
+				// Closer hits will override the previous one
+				mHits.back() = inResult;
+			}
+		}
+	}
+
+	// See: CollectorType::OnBodyEnd
+	virtual void		OnBodyEnd() override
+	{
+		if (mHadHit)
+		{
+			// Reset the early out fraction to the configured value so that we will continue
+			// to collect hits at any distance for other bodies
+			JPH_ASSERT(mPreviousEarlyOutFraction != -FLT_MAX); // Check that we got a call to OnBody
+			CollectorType::ResetEarlyOutFraction(mPreviousEarlyOutFraction);
+			mHadHit = false;
+		}
+
+		// For asserting purposes we reset the stored early out fraction so we can detect that OnBody was called
+		JPH_IF_ENABLE_ASSERTS(mPreviousEarlyOutFraction = -FLT_MAX;)
+	}
+
+	/// Order hits on closest first
+	void				Sort()
+	{
+		QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); });
+	}
+
+	/// Check if any hits were collected
+	inline bool			HadHit() const
+	{
+		return !mHits.empty();
+	}
+
+	Array<ResultType>	mHits;
+
+private:
+	// Store early out fraction that was initially configured for the collector
+	float				mPreviousEarlyOutFraction = -FLT_MAX;
+
+	// Flag to indicate if we have a hit for the current body
+	bool				mHadHit = false;
+};
+
 /// Simple implementation that collects any hit
 template <class CollectorType>
 class AnyHitCollisionCollector : public CollectorType

thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp

@@ -18,6 +18,8 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CollisionGroup)
 	JPH_ADD_ATTRIBUTE(CollisionGroup, mSubGroupID)
 }
 
+const CollisionGroup CollisionGroup::sInvalid;
+
 void CollisionGroup::SaveBinaryState(StreamOut &inStream) const
 {
 	inStream.Write(mGroupID);

thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h

@@ -85,6 +85,9 @@ public:
 	/// Restore the state of this object from inStream. Does not save group filter.
 	void					RestoreBinaryState(StreamIn &inStream);
 
+	/// An invalid collision group
+	static const CollisionGroup	sInvalid;
+
 private:
 	RefConst<GroupFilter>	mGroupFilter;
 	GroupID					mGroupID = cInvalidGroup;

thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h

@@ -77,8 +77,12 @@ class InternalEdgeRemovingCollector : public CollideShapeCollector
 public:
 	/// Constructor, configures a collector to be called with all the results that do not hit internal edges
 	explicit				InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector) :
+		CollideShapeCollector(inChainedCollector),
 		mChainedCollector(inChainedCollector)
 	{
+		// Initialize arrays to full capacity to avoid needless reallocation calls
+		mVoidedFeatures.reserve(cMaxLocalVoidedFeatures);
+		mDelayedResults.reserve(cMaxLocalDelayedResults);
 	}
 
 	// See: CollideShapeCollector::Reset
@@ -221,6 +225,13 @@ public:
 		mDelayedResults.clear();
 	}
 
+	// See: CollideShapeCollector::OnBodyEnd
+	virtual void			OnBodyEnd() override
+	{
+		Flush();
+		mChainedCollector.OnBodyEnd();
+	}
+
 	/// Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges
 	static void				sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
 	{

thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp

@@ -134,8 +134,10 @@ void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPoint
 	ioContactPointsOn2 = points_to_keep_on_2;
 }
 
-void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq	, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
+void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistance, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
 {
+	JPH_ASSERT(inMaxContactDistance > 0.0f);
+
 #ifdef JPH_DEBUG_RENDERER
 	if (ContactConstraintManager::sDrawContactPoint)
 	{
@@ -165,7 +167,7 @@ void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, V
 		else if (inShape1Face.size() == 2)
 			ClipPolyVsEdge(inShape2Face, inShape1Face[0], inShape1Face[1], inPenetrationAxis, clipped_face);
 
-		// Project the points back onto the plane of shape 1 face and only keep those that are behind the plane
+		// Determine plane origin and normal for shape 1
 		Vec3 plane_origin = inShape1Face[0];
 		Vec3 plane_normal;
 		Vec3 first_edge = inShape1Face[1] - plane_origin;
@@ -180,20 +182,25 @@ void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, V
 			plane_normal = first_edge.Cross(inPenetrationAxis).Cross(first_edge);
 		}
 
-		// Check if the plane normal has any length, if not the clipped shape is so small that we'll just use the contact points
-		float plane_normal_len_sq = plane_normal.LengthSq();
-		if (plane_normal_len_sq > 0.0f)
+		// If penetration axis and plane normal are perpendicular, fall back to the contact points
+		float penetration_axis_dot_plane_normal = inPenetrationAxis.Dot(plane_normal);
+		if (penetration_axis_dot_plane_normal != 0.0f)
 		{
-			// Discard points of faces that are too far away to collide
+			float penetration_axis_len = inPenetrationAxis.Length();
+
 			for (Vec3 p2 : clipped_face)
 			{
-				float distance = (p2 - plane_origin).Dot(plane_normal); // Note should divide by length of plane_normal (unnormalized here)
-				if (distance <= 0.0f || Square(distance) < inMaxContactDistanceSq * plane_normal_len_sq) // Must be close enough to plane, note we correct for not dividing by plane normal length here
-				{
-					// Project point back on shape 1 using the normal, note we correct for not dividing by plane normal length here:
-					// p1 = p2 - (distance / sqrt(plane_normal_len_sq)) * (plane_normal / sqrt(plane_normal_len_sq));
-					Vec3 p1 = p2 - (distance / plane_normal_len_sq) * plane_normal;
+				// Project clipped face back onto the plane of face 1, we do this by solving:
+				// p1 = p2 + distance * penetration_axis / |penetration_axis|
+				// (p1 - plane_origin) . plane_normal = 0
+				// This gives us:
+				// distance = -|penetration_axis| * (p2 - plane_origin) . plane_normal / penetration_axis . plane_normal
+				float distance = (p2 - plane_origin).Dot(plane_normal) / penetration_axis_dot_plane_normal; // note left out -|penetration_axis| term
 
+				// If the point is less than inMaxContactDistance in front of the plane of face 2, add it as a contact point
+				if (distance * penetration_axis_len < inMaxContactDistance)
+				{
+					Vec3 p1 = p2 - distance * inPenetrationAxis;
 					outContactPoints1.push_back(p1);
 					outContactPoints2.push_back(p2);
 				}
@@ -213,15 +220,19 @@ void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, V
 			DebugRenderer::sInstance->DrawWirePolygon(com, inShape2Face, Color::sGreen, 0.05f);
 
 			// Draw normal
-			if (plane_normal_len_sq > 0.0f)
+			float plane_normal_len = plane_normal.Length();
+			if (plane_normal_len > 0.0f)
 			{
 				RVec3 plane_origin_ws = inCenterOfMass + plane_origin;
-				DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / sqrt(plane_normal_len_sq), Color::sYellow, 0.05f);
+				DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / plane_normal_len, Color::sYellow, 0.05f);
 			}
 
 			// Draw contact points that remain after distance check
 			for (ContactPoints::size_type p = old_size; p < outContactPoints1.size(); ++p)
+			{
 				DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints1[p], Color::sYellow, 0.1f);
+				DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints2[p], Color::sOrange, 0.1f);
+			}
 		}
 	#endif // JPH_DEBUG_RENDERER
 	}

thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h

@@ -27,7 +27,7 @@ JPH_EXPORT void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioC
 /// @param inContactPoint1 The contact point on shape 1 relative to inCenterOfMass
 /// @param inContactPoint2 The contact point on shape 2 relative to inCenterOfMass
 /// @param inPenetrationAxis The local space penetration axis in world space
-/// @param inMaxContactDistanceSq After face 2 is clipped against face 1, each remaining point on face 2 is tested against the plane of face 1. If the distance^2 on the positive side of the plane is larger than this distance, the point will be discarded as a contact point.
+/// @param inMaxContactDistance After face 2 is clipped against face 1, each remaining point on face 2 is tested against the plane of face 1. If the distance on the positive side of the plane is larger than this distance, the point will be discarded as a contact point.
 /// @param inShape1Face The supporting faces on shape 1 relative to inCenterOfMass
 /// @param inShape2Face The supporting faces on shape 2 relative to inCenterOfMass
 /// @param outContactPoints1 Returns the contact points between the two shapes for shape 1 relative to inCenterOfMass (any existing points in the output array are left as is)
@@ -35,7 +35,7 @@ JPH_EXPORT void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioC
 #ifdef JPH_DEBUG_RENDERER
 /// @param inCenterOfMass Center of mass position of body 1
 #endif
-JPH_EXPORT void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2
+JPH_EXPORT void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistance, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2
 #ifdef JPH_DEBUG_RENDERER
 	, RVec3Arg inCenterOfMass
 #endif

thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp

@@ -29,7 +29,7 @@ bool NarrowPhaseQuery::CastRay(const RRayCast &inRay, RayCastResult &ioHit, cons
 			mBodyLockInterface(inBodyLockInterface),
 			mBodyFilter(inBodyFilter)
 		{
-			UpdateEarlyOutFraction(ioHit.mFraction);
+			ResetEarlyOutFraction(ioHit.mFraction);
 		}
 
 		virtual void		AddHit(const ResultType &inResult) override
@@ -126,6 +126,10 @@ void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inR
 						// Do narrow phase collision check
 						ts.CastRay(mRay, mRayCastSettings, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
@@ -189,6 +193,10 @@ void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioC
 						// Do narrow phase collision check
 						ts.CollidePoint(mPoint, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
@@ -255,6 +263,10 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 						// Do narrow phase collision check
 						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
@@ -284,82 +296,13 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 
 void NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
 {
-	JPH_PROFILE_FUNCTION();
+	// We require these settings for internal edge removal to work
+	CollideShapeSettings settings = inCollideShapeSettings;
+	settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+	settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
 
-	class MyCollector : public CollideShapeBodyCollector
-	{
-	public:
-							MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
-			CollideShapeBodyCollector(ioCollector),
-			mShape(inShape),
-			mShapeScale(inShapeScale),
-			mCenterOfMassTransform(inCenterOfMassTransform),
-			mBaseOffset(inBaseOffset),
-			mBodyLockInterface(inBodyLockInterface),
-			mBodyFilter(inBodyFilter),
-			mShapeFilter(inShapeFilter),
-			mCollideShapeSettings(inCollideShapeSettings),
-			mCollector(ioCollector)
-		{
-			// We require these settings for internal edge removal to work
-			mCollideShapeSettings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
-			mCollideShapeSettings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
-		}
-
-		virtual void		AddHit(const ResultType &inResult) override
-		{
-			// Only test shape if it passes the body filter
-			if (mBodyFilter.ShouldCollide(inResult))
-			{
-				// Lock the body
-				BodyLockRead lock(mBodyLockInterface, inResult);
-				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
-				{
-					const Body &body = lock.GetBody();
-
-					// Check body filter again now that we've locked the body
-					if (mBodyFilter.ShouldCollideLocked(body))
-					{
-						// Collect the transformed shape
-						TransformedShape ts = body.GetTransformedShape();
-
-						// Notify collector of new body
-						mCollector.OnBody(body);
-
-						// Release the lock now, we have all the info we need in the transformed shape
-						lock.ReleaseLock();
-
-						// Do narrow phase collision check
-						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter);
-
-						// After each body, we need to flush the InternalEdgeRemovingCollector because it uses 'ts' as context and it will go out of scope at the end of this block
-						mCollector.Flush();
-
-						// Update early out fraction based on narrow phase collector
-						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
-					}
-				}
-			}
-		}
-
-		const Shape *					mShape;
-		Vec3							mShapeScale;
-		RMat44							mCenterOfMassTransform;
-		RVec3							mBaseOffset;
-		const BodyLockInterface &		mBodyLockInterface;
-		const BodyFilter &				mBodyFilter;
-		const ShapeFilter &				mShapeFilter;
-		CollideShapeSettings			mCollideShapeSettings;
-		InternalEdgeRemovingCollector	mCollector;
-	};
-
-	// Calculate bounds for shape and expand by max separation distance
-	AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale);
-	bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
-
-	// Do broadphase test
-	MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
-	mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	InternalEdgeRemovingCollector wrapper(ioCollector);
+	CollideShape(inShape, inShapeScale, inCenterOfMassTransform, settings, inBaseOffset, wrapper, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
 }
 
 void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
@@ -409,6 +352,10 @@ void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastS
 						// Do narrow phase collision check
 						ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
@@ -471,6 +418,10 @@ void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedS
 						// Do narrow phase collision check
 						ts.CollectTransformedShapes(mBox, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -92,7 +92,12 @@ MassProperties CompoundShape::GetMassProperties() const
 
 AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 {
-	if (mSubShapes.size() <= 10)
+	if (mSubShapes.empty())
+	{
+		// If there are no sub-shapes, we must return an empty box to avoid overflows in the broadphase
+		return AABox(inCenterOfMassTransform.GetTranslation(), inCenterOfMassTransform.GetTranslation());
+	}
+	else if (mSubShapes.size() <= 10)
 	{
 		AABox bounds;
 		for (const SubShape &shape : mSubShapes)

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h

@@ -36,7 +36,11 @@ public:
 		RefConst<Shape>				mShapePtr;												///< Sub shape (either this or mShape needs to be filled up)
 		Vec3						mPosition;												///< Position of the sub shape
 		Quat						mRotation;												///< Rotation of the sub shape
-		uint32						mUserData = 0;											///< User data value (can be used by the application for any purpose)
+
+		/// User data value (can be used by the application for any purpose).
+		/// Note this value can be retrieved through GetSubShape(...).mUserData, not through GetSubShapeUserData(...) as that returns Shape::GetUserData() of the leaf shape.
+		/// Use GetSubShapeIndexFromID get a shape index from a SubShapeID to pass to GetSubShape.
+		uint32						mUserData = 0;
 	};
 
 	using SubShapes = Array<SubShapeSettings>;
@@ -338,7 +342,7 @@ protected:
 	}
 
 	Vec3							mCenterOfMass { Vec3::sZero() };						///< Center of mass of the compound
-	AABox							mLocalBounds;
+	AABox							mLocalBounds { Vec3::sZero(), Vec3::sZero() };
 	SubShapes						mSubShapes;
 	float							mInnerRadius = FLT_MAX;									///< Smallest radius of GetInnerRadius() of child shapes
 

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h

@@ -30,7 +30,7 @@ public:
 	// See: ShapeSettings
 	virtual ShapeResult		Create() const override;
 
-	Array<Vec3>				mPoints;															///< Points to create the hull from
+	Array<Vec3>				mPoints;															///< Points to create the hull from. Note that these points don't need to be the vertices of the convex hull, they can contain interior points or points on faces/edges.
 	float					mMaxConvexRadius = 0.0f;											///< Convex radius as supplied by the constructor. Note that during hull creation the convex radius can be made smaller if the value is too big for the hull.
 	float					mMaxErrorConvexRadius = 0.05f;										///< Maximum distance between the shrunk hull + convex radius and the actual hull.
 	float					mHullTolerance = 1.0e-3f;											///< Points are allowed this far outside of the hull (increasing this yields a hull with less vertices). Note that the actual used value can be larger if the points of the hull are far apart.

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp

@@ -57,8 +57,9 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 	Mat44 transform_2_to_1 = inverse_transform1 * inCenterOfMassTransform2;
 
 	// Get bounding boxes
+	float max_separation_distance = inCollideShapeSettings.mMaxSeparationDistance;
 	AABox shape1_bbox = shape1->GetLocalBounds().Scaled(inScale1);
-	shape1_bbox.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+	shape1_bbox.ExpandBy(Vec3::sReplicate(max_separation_distance));
 	AABox shape2_bbox = shape2->GetLocalBounds().Scaled(inScale2);
 
 	// Check if they overlap
@@ -86,10 +87,10 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 		const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2);
 
 		// Transform shape 2 in the space of shape 1
-		TransformedConvexObject<Support> transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius);
+		TransformedConvexObject transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius);
 
 		// Perform GJK step
-		status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + inCollideShapeSettings.mMaxSeparationDistance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
+		status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + max_separation_distance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
 	}
 
 	// Check result of collision detection
@@ -105,16 +106,22 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 		{
 			// Need to run expensive EPA algorithm
 
+			// We know we're overlapping at this point, so we can set the max separation distance to 0.
+			// Numerically it is possible that GJK finds that the shapes are overlapping but EPA finds that they're separated.
+			// In order to avoid this, we clamp the max separation distance to 1 so that we don't excessively inflate the shape,
+			// but we still inflate it enough to avoid the case where EPA misses the collision.
+			max_separation_distance = min(max_separation_distance, 1.0f);
+
 			// Create support function
 			SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius;
 			const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1);
 			const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2);
 
 			// Add separation distance
-			AddConvexRadius<Support> shape1_add_max_separation_distance(*shape1_incl_cvx_radius, inCollideShapeSettings.mMaxSeparationDistance);
+			AddConvexRadius shape1_add_max_separation_distance(*shape1_incl_cvx_radius, max_separation_distance);
 
 			// Transform shape 2 in the space of shape 1
-			TransformedConvexObject<Support> transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius);
+			TransformedConvexObject transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius);
 
 			// Perform EPA step
 			if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
@@ -124,14 +131,14 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 	}
 
 	// Check if the penetration is bigger than the early out fraction
-	float penetration_depth = (point2 - point1).Length() - inCollideShapeSettings.mMaxSeparationDistance;
+	float penetration_depth = (point2 - point1).Length() - max_separation_distance;
 	if (-penetration_depth >= ioCollector.GetEarlyOutFraction())
 		return;
 
 	// Correct point1 for the added separation distance
 	float penetration_axis_len = penetration_axis.Length();
 	if (penetration_axis_len > 0.0f)
-		point1 -= penetration_axis * (inCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len);
+		point1 -= penetration_axis * (max_separation_distance / penetration_axis_len);
 
 	// Convert to world space
 	point1 = inCenterOfMassTransform1 * point1;
@@ -164,7 +171,7 @@ bool ConvexShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubSh
 
 	// Create support function
 	SupportBuffer buffer;
-	const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
+	const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
 
 	// Cast ray
 	GJKClosestPoint gjk;
@@ -234,7 +241,7 @@ void ConvexShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 	{
 		// Create support function
 		SupportBuffer buffer;
-		const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
+		const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
 
 		// Create support function for point
 		PointConvexSupport point { inPoint };
@@ -312,7 +319,7 @@ public:
 		mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)),
 		mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
 	{
-		mSupport = inShape->GetSupportFunction(ESupportMode::IncludeConvexRadius, mSupportBuffer, Vec3::sReplicate(1.0f));
+		mSupport = inShape->GetSupportFunction(ESupportMode::IncludeConvexRadius, mSupportBuffer, Vec3::sOne());
 	}
 
 	SupportBuffer		mSupportBuffer;
@@ -446,7 +453,7 @@ void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg in
 	// Get the support function with convex radius
 	SupportBuffer buffer;
 	const Support *support = GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale);
-	AddConvexRadius<Support> add_convex(*support, support->GetConvexRadius());
+	AddConvexRadius add_convex(*support, support->GetConvexRadius());
 
 	// Draw the shape
 	DebugRenderer::GeometryRef geometry = inRenderer->CreateTriangleGeometryForConvex([&add_convex](Vec3Arg inDirection) { return add_convex.GetSupport(inDirection); });
@@ -498,7 +505,7 @@ void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inC
 		SupportingFace face = ftd.first;
 
 		// Displace the face a little bit forward so it is easier to see
-		Vec3 normal = face.size() >= 3? (face[2] - face[1]).Cross(face[0] - face[1]).Normalized() : Vec3::sZero();
+		Vec3 normal = face.size() >= 3? (face[2] - face[1]).Cross(face[0] - face[1]).NormalizedOr(Vec3::sZero()) : Vec3::sZero();
 		Vec3 displacement = 0.001f * normal;
 
 		// Transform face to world space and calculate center of mass

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp

@@ -200,13 +200,14 @@ void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg in
 	float scaled_radius = scale_xz * mRadius;
 
 	float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
-	float o = sqrt(Square(x) + Square(z));
+	float xz_sq = Square(x) + Square(z);
+	float y_sq = Square(y);
 
-	// If o / |y| > scaled_radius / scaled_half_height, we're hitting the side
-	if (o * scaled_half_height > scaled_radius * abs(y))
+	// Check which component is bigger
+	if (xz_sq > y_sq)
 	{
 		// Hitting side
-		float f = -scaled_radius / o;
+		float f = -scaled_radius / sqrt(xz_sq);
 		float vx = x * f;
 		float vz = z * f;
 		outVertices.push_back(inCenterOfMassTransform * Vec3(vx, scaled_half_height, vz));
@@ -215,8 +216,21 @@ void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg in
 	else
 	{
 		// Hitting top or bottom
+
+		// When the inDirection is more than 5 degrees from vertical, align the vertices so that 1 of the vertices
+		// points towards inDirection in the XZ plane. This ensures that we always have a vertex towards max penetration depth.
+		Mat44 transform = inCenterOfMassTransform;
+		if (xz_sq > 0.00765427f * y_sq)
+		{
+			Vec4 base_x = Vec4(x, 0, z, 0) / sqrt(xz_sq);
+			Vec4 base_z = base_x.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() * Vec4(-1, 0, 1, 0);
+			transform = transform * Mat44(base_x, Vec4(0, 1, 0, 0), base_z, Vec4(0, 0, 0, 1));
+		}
+
+		// Adjust for scale and height
 		Vec3 multiplier = y < 0.0f? Vec3(scaled_radius, scaled_half_height, scaled_radius) : Vec3(-scaled_radius, -scaled_half_height, scaled_radius);
-		Mat44 transform = inCenterOfMassTransform.PreScaled(multiplier);
+		transform = transform.PreScaled(multiplier);
+
 		for (const Vec3 &v : cCylinderTopFace)
 			outVertices.push_back(transform * v);
 	}

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -201,119 +201,155 @@ uint32 HeightFieldShapeSettings::CalculateBitsPerSampleForError(float inMaxError
 
 void HeightFieldShape::CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator)
 {
+	// Limit the block size so we don't allocate more than 64K memory from the temp allocator
+	uint block_size_x = min(inSizeX, 44u);
+	uint block_size_y = min(inSizeY, 44u);
+
 	// Allocate temporary buffer for normals
-	uint normals_size = 2 * inSizeX * inSizeY * sizeof(Vec3);
+	uint normals_size = 2 * (block_size_x + 1) * (block_size_y + 1) * sizeof(Vec3);
 	Vec3 *normals = (Vec3 *)inAllocator.Allocate(normals_size);
 	JPH_SCOPE_EXIT([&inAllocator, normals, normals_size]{ inAllocator.Free(normals, normals_size); });
 
-	// Calculate triangle normals and make normals zero for triangles that are missing
-	Vec3 *out_normal = normals;
-	for (uint y = 0; y < inSizeY; ++y)
-		for (uint x = 0; x < inSizeX; ++x)
+	// Update the edges in blocks
+	for (uint block_y = 0; block_y < inSizeY; block_y += block_size_y)
+		for (uint block_x = 0; block_x < inSizeX; block_x += block_size_x)
 		{
-			// Get height on diagonal
-			const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
-			float x1y1_h = height_samples[0];
-			float x2y2_h = height_samples[inHeightsStride + 1];
-			if (x1y1_h != cNoCollisionValue && x2y2_h != cNoCollisionValue)
-			{
-				// Calculate normal for lower left triangle (e.g. T1A)
-				float x1y2_h = height_samples[inHeightsStride];
-				if (x1y2_h != cNoCollisionValue)
-				{
-					Vec3 x2y2_minus_x1y2(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
-					Vec3 x1y1_minus_x1y2(0, inHeightsScale * (x1y1_h - x1y2_h), -mScale.GetZ());
-					out_normal[0] = x2y2_minus_x1y2.Cross(x1y1_minus_x1y2).Normalized();
-				}
-				else
-					out_normal[0] = Vec3::sZero();
+			// Calculate the bottom right corner of the block
+			uint block_x_end = min(block_x + block_size_x, inSizeX);
+			uint block_y_end = min(block_y + block_size_y, inSizeY);
 
-				// Calculate normal for upper right triangle (e.g. T1B)
-				float x2y1_h = height_samples[1];
-				if (x2y1_h != cNoCollisionValue)
-				{
-					Vec3 x1y1_minus_x2y1(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y1_h), 0);
-					Vec3 x2y2_minus_x2y1(0, inHeightsScale * (x2y2_h - x2y1_h), mScale.GetZ());
-					out_normal[1] = x1y1_minus_x2y1.Cross(x2y2_minus_x2y1).Normalized();
-				}
-				else
-					out_normal[1] = Vec3::sZero();
+			// If we're not at the first block in x, we need one extra column of normals to the left
+			uint normals_x_start, normals_x_skip;
+			if (block_x > 0)
+			{
+				normals_x_start = block_x - 1;
+				normals_x_skip = 2; // We need to skip over that extra column
 			}
 			else
 			{
-				out_normal[0] = Vec3::sZero();
-				out_normal[1] = Vec3::sZero();
+				normals_x_start = 0;
+				normals_x_skip = 0;
 			}
 
-			out_normal += 2;
+			// If we're not at the last block in y, we need one extra row of normals at the bottom
+			uint normals_y_end = block_y_end < inSizeY? block_y_end + 1 : inSizeY;
+
+			// Calculate triangle normals and make normals zero for triangles that are missing
+			Vec3 *out_normal = normals;
+			for (uint y = block_y; y < normals_y_end; ++y)
+			{
+				for (uint x = normals_x_start; x < block_x_end; ++x)
+				{
+					// Get height on diagonal
+					const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
+					float x1y1_h = height_samples[0];
+					float x2y2_h = height_samples[inHeightsStride + 1];
+					if (x1y1_h != cNoCollisionValue && x2y2_h != cNoCollisionValue)
+					{
+						// Calculate normal for lower left triangle (e.g. T1A)
+						float x1y2_h = height_samples[inHeightsStride];
+						if (x1y2_h != cNoCollisionValue)
+						{
+							Vec3 x2y2_minus_x1y2(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
+							Vec3 x1y1_minus_x1y2(0, inHeightsScale * (x1y1_h - x1y2_h), -mScale.GetZ());
+							out_normal[0] = x2y2_minus_x1y2.Cross(x1y1_minus_x1y2).Normalized();
+						}
+						else
+							out_normal[0] = Vec3::sZero();
+
+						// Calculate normal for upper right triangle (e.g. T1B)
+						float x2y1_h = height_samples[1];
+						if (x2y1_h != cNoCollisionValue)
+						{
+							Vec3 x1y1_minus_x2y1(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y1_h), 0);
+							Vec3 x2y2_minus_x2y1(0, inHeightsScale * (x2y2_h - x2y1_h), mScale.GetZ());
+							out_normal[1] = x1y1_minus_x2y1.Cross(x2y2_minus_x2y1).Normalized();
+						}
+						else
+							out_normal[1] = Vec3::sZero();
+					}
+					else
+					{
+						out_normal[0] = Vec3::sZero();
+						out_normal[1] = Vec3::sZero();
+					}
+
+					out_normal += 2;
+				}
+			}
+
+			// Number of vectors to skip to get to the next row of normals
+			uint normals_pitch = 2 * (block_x_end - normals_x_start);
+
+			// Calculate active edges
+			const Vec3 *in_normal = normals;
+			uint global_bit_pos = 3 * ((inY + block_y) * (mSampleCount - 1) + (inX + block_x));
+			for (uint y = block_y; y < block_y_end; ++y)
+			{
+				in_normal += normals_x_skip; // If we have an extra column to the left, skip it here, we'll read it with in_normal[-1] below
+
+				for (uint x = block_x; x < block_x_end; ++x)
+				{
+					// Get vertex heights
+					const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
+					float x1y1_h = height_samples[0];
+					float x1y2_h = height_samples[inHeightsStride];
+					float x2y2_h = height_samples[inHeightsStride + 1];
+					bool x1y1_valid = x1y1_h != cNoCollisionValue;
+					bool x1y2_valid = x1y2_h != cNoCollisionValue;
+					bool x2y2_valid = x2y2_h != cNoCollisionValue;
+
+					// Calculate the edge flags (3 bits)
+					// See diagram in the next function for the edge numbering
+					uint16 edge_mask = 0b111;
+					uint16 edge_flags = 0;
+
+					// Edge 0
+					if (x == 0)
+						edge_mask &= 0b110; // We need normal x - 1 which we didn't calculate, don't update this edge
+					else if (x1y1_valid && x1y2_valid)
+					{
+						Vec3 edge0_direction(0, inHeightsScale * (x1y2_h - x1y1_h), mScale.GetZ());
+						if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[-1], edge0_direction, inActiveEdgeCosThresholdAngle))
+							edge_flags |= 0b001;
+					}
+
+					// Edge 1
+					if (y == inSizeY - 1)
+						edge_mask &= 0b101; // We need normal y + 1 which we didn't calculate, don't update this edge
+					else if (x1y2_valid && x2y2_valid)
+					{
+						Vec3 edge1_direction(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
+						if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[normals_pitch + 1], edge1_direction, inActiveEdgeCosThresholdAngle))
+							edge_flags |= 0b010;
+					}
+
+					// Edge 2
+					if (x1y1_valid && x2y2_valid)
+					{
+						Vec3 edge2_direction(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y2_h), -mScale.GetZ());
+						if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[1], edge2_direction, inActiveEdgeCosThresholdAngle))
+							edge_flags |= 0b100;
+					}
+
+					// Store the edge flags in the array
+					uint byte_pos = global_bit_pos >> 3;
+					uint bit_pos = global_bit_pos & 0b111;
+					JPH_ASSERT(byte_pos < mActiveEdgesSize);
+					uint8 *edge_flags_ptr = &mActiveEdges[byte_pos];
+					uint16 combined_edge_flags = uint16(edge_flags_ptr[0]) | uint16(uint16(edge_flags_ptr[1]) << 8);
+					combined_edge_flags &= ~(edge_mask << bit_pos);
+					combined_edge_flags |= edge_flags << bit_pos;
+					edge_flags_ptr[0] = uint8(combined_edge_flags);
+					edge_flags_ptr[1] = uint8(combined_edge_flags >> 8);
+
+					in_normal += 2;
+					global_bit_pos += 3;
+				}
+
+				global_bit_pos += 3 * (mSampleCount - 1 - (block_x_end - block_x));
+			}
 		}
-
-	// Calculate active edges
-	const Vec3 *in_normal = normals;
-	uint global_bit_pos = 3 * (inY * (mSampleCount - 1) + inX);
-	for (uint y = 0; y < inSizeY; ++y)
-	{
-		for (uint x = 0; x < inSizeX; ++x)
-		{
-			// Get vertex heights
-			const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
-			float x1y1_h = height_samples[0];
-			float x1y2_h = height_samples[inHeightsStride];
-			float x2y2_h = height_samples[inHeightsStride + 1];
-			bool x1y1_valid = x1y1_h != cNoCollisionValue;
-			bool x1y2_valid = x1y2_h != cNoCollisionValue;
-			bool x2y2_valid = x2y2_h != cNoCollisionValue;
-
-			// Calculate the edge flags (3 bits)
-			// See diagram in the next function for the edge numbering
-			uint16 edge_mask = 0b111;
-			uint16 edge_flags = 0;
-
-			// Edge 0
-			if (x == 0)
-				edge_mask &= 0b110; // We need normal x - 1 which we didn't calculate, don't update this edge
-			else if (x1y1_valid && x1y2_valid)
-			{
-				Vec3 edge0_direction(0, inHeightsScale * (x1y2_h - x1y1_h), mScale.GetZ());
-				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[-1], edge0_direction, inActiveEdgeCosThresholdAngle))
-					edge_flags |= 0b001;
-			}
-
-			// Edge 1
-			if (y == inSizeY - 1)
-				edge_mask &= 0b101; // We need normal y + 1 which we didn't calculate, don't update this edge
-			else if (x1y2_valid && x2y2_valid)
-			{
-				Vec3 edge1_direction(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
-				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[2 * inSizeX + 1], edge1_direction, inActiveEdgeCosThresholdAngle))
-					edge_flags |= 0b010;
-			}
-
-			// Edge 2
-			if (x1y1_valid && x2y2_valid)
-			{
-				Vec3 edge2_direction(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y2_h), -mScale.GetZ());
-				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[1], edge2_direction, inActiveEdgeCosThresholdAngle))
-					edge_flags |= 0b100;
-			}
-
-			// Store the edge flags in the array
-			uint byte_pos = global_bit_pos >> 3;
-			uint bit_pos = global_bit_pos & 0b111;
-			JPH_ASSERT(byte_pos < mActiveEdgesSize);
-			uint8 *edge_flags_ptr = &mActiveEdges[byte_pos];
-			uint16 combined_edge_flags = uint16(edge_flags_ptr[0]) | uint16(uint16(edge_flags_ptr[1]) << 8);
-			combined_edge_flags &= ~(edge_mask << bit_pos);
-			combined_edge_flags |= edge_flags << bit_pos;
-			edge_flags_ptr[0] = uint8(combined_edge_flags);
-			edge_flags_ptr[1] = uint8(combined_edge_flags >> 8);
-
-			in_normal += 2;
-			global_bit_pos += 3;
-		}
-
-		global_bit_pos += 3 * (mSampleCount - 1 - inSizeX);
-	}
 }
 
 void HeightFieldShape::CalculateActiveEdges(const HeightFieldShapeSettings &inSettings)
@@ -1710,7 +1746,7 @@ public:
 	JPH_INLINE explicit			DecodingContext(const HeightFieldShape *inShape) :
 		mShape(inShape)
 	{
-		static_assert(sizeof(sGridOffsets) / sizeof(uint) == cNumBitsXY + 1, "Offsets array is not long enough");
+		static_assert(std::size(sGridOffsets) == cNumBitsXY + 1, "Offsets array is not long enough");
 
 		// Construct root stack entry
 		mPropertiesStack[0] = 0; // level: 0, x: 0, y: 0
@@ -1869,8 +1905,8 @@ public:
 					uint32 stride = block_size_plus_1 - size_x_plus_1;
 
 					// Start range with a very large inside-out box
-					Vec3 value_min = Vec3::sReplicate(1.0e30f);
-					Vec3 value_max = Vec3::sReplicate(-1.0e30f);
+					Vec3 value_min = Vec3::sReplicate(cLargeFloat);
+					Vec3 value_max = Vec3::sReplicate(-cLargeFloat);
 
 					// Loop over the samples to determine the min and max of this block
 					for (uint32 block_y = 0; block_y < size_y_plus_1; ++block_y)

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -69,14 +69,14 @@ public:
 	/// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y).
 	/// where x and y are integers in the range x and y e [0, mSampleCount - 1].
 	Vec3							mOffset = Vec3::sZero();
-	Vec3							mScale = Vec3::sReplicate(1.0f);
+	Vec3							mScale = Vec3::sOne();
 	uint32							mSampleCount = 0;
 
 	/// Artificial minimal value of mHeightSamples, used for compression and can be used to update the terrain after creating with lower height values. If there are any lower values in mHeightSamples, this value will be ignored.
-	float							mMinHeightValue = FLT_MAX;
+	float							mMinHeightValue = cLargeFloat;
 
 	/// Artificial maximum value of mHeightSamples, used for compression and can be used to update the terrain after creating with higher height values. If there are any higher values in mHeightSamples, this value will be ignored.
-	float							mMaxHeightValue = -FLT_MAX;
+	float							mMaxHeightValue = -cLargeFloat;
 
 	/// When bigger than mMaterials.size() the internal material list will be preallocated to support this number of materials.
 	/// This avoids reallocations when calling HeightFieldShape::SetMaterials with new materials later.
@@ -349,7 +349,7 @@ private:
 	/// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y).
 	/// where x and y are integers in the range x and y e [0, mSampleCount - 1].
 	Vec3							mOffset = Vec3::sZero();
-	Vec3							mScale = Vec3::sReplicate(1.0f);
+	Vec3							mScale = Vec3::sOne();
 
 	/// Height data
 	uint32							mSampleCount = 0;							///< See HeightFieldShapeSettings::mSampleCount

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -32,6 +32,7 @@
 #include <Jolt/Geometry/Plane.h>
 #include <Jolt/Geometry/OrientedBox.h>
 #include <Jolt/TriangleSplitter/TriangleSplitterBinning.h>
+#include <Jolt/TriangleSplitter/TriangleSplitterMean.h>
 #include <Jolt/AABBTree/AABBTreeBuilder.h>
 #include <Jolt/AABBTree/AABBTreeToBuffer.h>
 #include <Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h>
@@ -55,6 +56,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MeshShapeSettings)
 	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaxTrianglesPerLeaf)
 	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mActiveEdgeCosThresholdAngle)
 	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mPerTriangleUserData)
+	JPH_ADD_ENUM_ATTRIBUTE(MeshShapeSettings, mBuildQuality)
 }
 
 // Codecs this mesh shape is using
@@ -190,12 +192,36 @@ MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult
 	sFindActiveEdges(inSettings, indexed_triangles);
 
 	// Create triangle splitter
-	TriangleSplitterBinning splitter(inSettings.mTriangleVertices, indexed_triangles);
+	union Storage
+	{
+									Storage() { }
+									~Storage() { }
+
+		TriangleSplitterBinning		mBinning;
+		TriangleSplitterMean		mMean;
+	};
+	Storage storage;
+	TriangleSplitter *splitter = nullptr;
+	switch (inSettings.mBuildQuality)
+	{
+	case MeshShapeSettings::EBuildQuality::FavorRuntimePerformance:
+		splitter = new (&storage.mBinning) TriangleSplitterBinning(inSettings.mTriangleVertices, indexed_triangles);
+		break;
+
+	case MeshShapeSettings::EBuildQuality::FavorBuildSpeed:
+		splitter = new (&storage.mMean) TriangleSplitterMean(inSettings.mTriangleVertices, indexed_triangles);
+		break;
+
+	default:
+		JPH_ASSERT(false);
+		break;
+	}
 
 	// Build tree
-	AABBTreeBuilder builder(splitter, inSettings.mMaxTrianglesPerLeaf);
+	AABBTreeBuilder builder(*splitter, inSettings.mMaxTrianglesPerLeaf);
 	AABBTreeBuilderStats builder_stats;
 	const AABBTreeBuilder::Node *root = builder.Build(builder_stats);
+	splitter->~TriangleSplitter();
 
 	// Convert to buffer
 	AABBTreeToBuffer<TriangleCodec, NodeCodec> buffer;
@@ -221,6 +247,14 @@ MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult
 
 void MeshShape::sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices)
 {
+	// Check if we're requested to make all edges active
+	if (inSettings.mActiveEdgeCosThresholdAngle < 0.0f)
+	{
+		for (IndexedTriangle &triangle : ioIndices)
+			triangle.mMaterialIndex |= 0b111 << FLAGS_ACTIVE_EGDE_SHIFT;
+		return;
+	}
+
 	// A struct to hold the two vertex indices of an edge
 	struct Edge
 	{
@@ -349,7 +383,7 @@ MassProperties MeshShape::GetMassProperties() const
 	// creating a Body:
 	//
 	// BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
-	// BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sReplicate(1.0f), 1000.0f);
+	// BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sOne(), 1000.0f);
 	//
 	// Note that for a mesh shape to simulate properly, it is best if the mesh is manifold
 	// (i.e. closed, all edges shared by only two triangles, consistent winding order).

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h

@@ -58,6 +58,8 @@ public:
 	/// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
 	/// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly).
 	/// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees).
+	/// Negative values will make all edges active and causes EActiveEdgeMode::CollideOnlyWithActive to behave as EActiveEdgeMode::CollideWithAll.
+	/// This speeds up the build process but will require all bodies that can interact with the mesh to use BodyCreationSettings::mEnhancedInternalEdgeRemoval = true.
 	float							mActiveEdgeCosThresholdAngle = 0.996195f;					// cos(5 degrees)
 
 	/// When true, we store the user data coming from Triangle::mUserData or IndexedTriangle::mUserData in the mesh shape.
@@ -65,6 +67,15 @@ public:
 	/// Can be retrieved using MeshShape::GetTriangleUserData.
 	/// Turning this on increases the memory used by the MeshShape by roughly 25%.
 	bool							mPerTriangleUserData = false;
+
+	enum class EBuildQuality
+	{
+		FavorRuntimePerformance,																///< Favor runtime performance, takes more time to build the MeshShape but performs better
+		FavorBuildSpeed,																		///< Favor build speed, build the tree faster but the MeshShape will be slower
+	};
+
+	/// Determines the quality of the tree building process.
+	EBuildQuality					mBuildQuality = EBuildQuality::FavorRuntimePerformance;
 };
 
 /// A mesh shape, consisting of triangles. Mesh shapes are mostly used for static geometry.

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp

@@ -142,8 +142,8 @@ void MutableCompoundShape::CalculateLocalBounds()
 	}
 	else
 	{
-		// There are no subshapes, set the bounding box to invalid
-		mLocalBounds.SetEmpty();
+		// There are no subshapes, make the bounding box empty
+		mLocalBounds.mMin = mLocalBounds.mMax = Vec3::sZero();
 	}
 
 	// Cache the inner radius as it can take a while to recursively iterate over all sub shapes
@@ -181,7 +181,7 @@ void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumbe
 				Mat44 transform = Mat44::sRotationTranslation(sub_shape.GetRotation(), sub_shape.GetPositionCOM());
 
 				// Get the bounding box
-				sub_shape_bounds = sub_shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f));
+				sub_shape_bounds = sub_shape.mShape->GetWorldSpaceBounds(transform, Vec3::sOne());
 			}
 
 			// Put the bounds as columns in a matrix
@@ -206,29 +206,37 @@ void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumbe
 	CalculateLocalBounds();
 }
 
-uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData)
+uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData, uint inIndex)
 {
 	SubShape sub_shape;
 	sub_shape.mShape = inShape;
 	sub_shape.mUserData = inUserData;
 	sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass);
-	mSubShapes.push_back(sub_shape);
-	uint shape_idx = (uint)mSubShapes.size() - 1;
 
-	CalculateSubShapeBounds(shape_idx, 1);
-
-	return shape_idx;
+	if (inIndex >= mSubShapes.size())
+	{
+		uint shape_idx = uint(mSubShapes.size());
+		mSubShapes.push_back(sub_shape);
+		CalculateSubShapeBounds(shape_idx, 1);
+		return shape_idx;
+	}
+	else
+	{
+		mSubShapes.insert(mSubShapes.begin() + inIndex, sub_shape);
+		CalculateSubShapeBounds(inIndex, uint(mSubShapes.size()) - inIndex);
+		return inIndex;
+	}
 }
 
 void MutableCompoundShape::RemoveShape(uint inIndex)
 {
 	mSubShapes.erase(mSubShapes.begin() + inIndex);
 
+	// We always need to recalculate the bounds of the sub shapes as we test blocks
+	// of 4 sub shapes at a time and removed shapes get their bounds updated
+	// to repeat the bounds of the previous sub shape
 	uint num_bounds = (uint)mSubShapes.size() - inIndex;
-	if (num_bounds > 0)
-		CalculateSubShapeBounds(inIndex, num_bounds);
-	else
-		CalculateLocalBounds();
+	CalculateSubShapeBounds(inIndex, num_bounds);
 }
 
 void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation)

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h

@@ -27,6 +27,8 @@ public:
 /// Note: If you're using MutableCompoundShape and are querying data while modifying the shape you'll have a race condition.
 /// In this case it is best to create a new MutableCompoundShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape.
 /// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes.
+///
+/// When you modify a MutableCompoundShape, beware that the SubShapeIDs of all other shapes can change. So be careful when storing SubShapeIDs.
 class JPH_EXPORT MutableCompoundShape final : public CompoundShape
 {
 public:
@@ -66,8 +68,13 @@ public:
 
 	/// Adding a new shape.
 	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inPosition The position of the new shape
+	/// @param inRotation The orientation of the new shape
+	/// @param inShape The shape to add
+	/// @param inUserData User data that will be stored with the shape and can be retrieved using GetCompoundUserData
+	/// @param inIndex Index where to insert the shape, UINT_MAX to add to the end
 	/// @return The index of the newly added shape
-	uint							AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0);
+	uint							AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0, uint inIndex = UINT_MAX);
 
 	/// Remove a shape by index.
 	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h

@@ -71,7 +71,7 @@ public:
 	virtual MassProperties			GetMassProperties() const override;
 
 	// See Shape::GetMaterial
-	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override	{ JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; }
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override	{ JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return GetMaterial(); }
 
 	// See Shape::GetSurfaceNormal
 	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mPlane.GetNormal(); }
@@ -114,6 +114,10 @@ public:
 	// See Shape::GetVolume
 	virtual float					GetVolume() const override									{ return 0; }
 
+	/// Material of the shape
+	void							SetMaterial(const PhysicsMaterial *inMaterial)				{ mMaterial = inMaterial; }
+	const PhysicsMaterial *			GetMaterial() const											{ return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; }
+
 	// Register shape functions with the registry
 	static void						sRegister();
 

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h

@@ -18,7 +18,7 @@ namespace ScaleHelpers
 	static constexpr float	cScaleToleranceSq = 1.0e-8f;
 
 	/// Test if a scale is identity
-	inline bool				IsNotScaled(Vec3Arg inScale)									{ return inScale.IsClose(Vec3::sReplicate(1.0f), cScaleToleranceSq); }
+	inline bool				IsNotScaled(Vec3Arg inScale)									{ return inScale.IsClose(Vec3::sOne(), cScaleToleranceSq); }
 
 	/// Test if a scale is uniform
 	inline bool				IsUniformScale(Vec3Arg inScale)									{ return inScale.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>().IsClose(inScale, cScaleToleranceSq); }

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp

@@ -233,7 +233,7 @@ Vec3 Shape::MakeScaleValid(Vec3Arg inScale) const
 
 Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const
 {
-	const Vec3 unit_scale = Vec3::sReplicate(1.0f);
+	const Vec3 unit_scale = Vec3::sOne();
 
 	if (inScale.IsNearZero())
 	{

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp

@@ -303,7 +303,7 @@ void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3
 void SphereShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
 {
 	float scaled_radius = GetScaledRadius(inScale);
-	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, Vec3::sReplicate(1.0f), Mat44::sScale(scaled_radius), sUnitSphereTriangles.data(), sUnitSphereTriangles.size(), GetMaterial());
+	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, Vec3::sOne(), Mat44::sScale(scaled_radius), sUnitSphereTriangles.data(), sUnitSphereTriangles.size(), GetMaterial());
 }
 
 int SphereShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp

@@ -234,7 +234,7 @@ StaticCompoundShape::StaticCompoundShape(const StaticCompoundShapeSettings &inSe
 
 		// Transform the shape's bounds into our local space
 		Mat44 transform = Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM());
-		AABox shape_bounds = shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f));
+		AABox shape_bounds = shape.mShape->GetWorldSpaceBounds(transform, Vec3::sOne());
 
 		// Store bounds and body index for tree construction
 		bounds[i] = shape_bounds;

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp

@@ -376,7 +376,7 @@ void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMa
 	if (mGeometry == nullptr)
 	{
 		SupportBuffer buffer;
-		const Support *support = GetSupportFunction(ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
+		const Support *support = GetSupportFunction(ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
 		mGeometry = inRenderer->CreateTriangleGeometryForConvex([support](Vec3Arg inDirection) { return support->GetSupport(inDirection); });
 	}
 

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp

@@ -265,24 +265,40 @@ void TaperedCylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec
 		outVertices.push_back(inCenterOfMassTransform * (normal_xz * top_radius + Vec3(0, top, 0)));
 		outVertices.push_back(inCenterOfMassTransform * (normal_xz * bottom_radius + Vec3(0, bottom, 0)));
 	}
-	else if (inDirection.GetY() < 0.0f)
-	{
-		// Top of the cylinder
-		if (top_radius > cMinRadius)
-		{
-			Vec3 top_3d(0, top, 0);
-			for (Vec3 v : cTaperedCylinderFace)
-				outVertices.push_back(inCenterOfMassTransform * (top_radius * v + top_3d));
-		}
-	}
 	else
 	{
-		// Bottom of the cylinder
-		if (bottom_radius > cMinRadius)
+		// When the inDirection is more than 5 degrees from vertical, align the vertices so that 1 of the vertices
+		// points towards inDirection in the XZ plane. This ensures that we always have a vertex towards max penetration depth.
+		Mat44 transform = inCenterOfMassTransform;
+		Vec4 base_x = Vec4(inDirection.GetX(), 0, inDirection.GetZ(), 0);
+		float xz_sq = base_x.LengthSq();
+		float y_sq = Square(inDirection.GetY());
+		if (xz_sq > 0.00765427f * y_sq)
 		{
-			Vec3 bottom_3d(0, bottom, 0);
-			for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v)
-				outVertices.push_back(inCenterOfMassTransform * (bottom_radius * *v + bottom_3d));
+			base_x /= sqrt(xz_sq);
+			Vec4 base_z = base_x.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() * Vec4(-1, 0, 1, 0);
+			transform = transform * Mat44(base_x, Vec4(0, 1, 0, 0), base_z, Vec4(0, 0, 0, 1));
+		}
+
+		if (inDirection.GetY() < 0.0f)
+		{
+			// Top of the cylinder
+			if (top_radius > cMinRadius)
+			{
+				Vec3 top_3d(0, top, 0);
+				for (Vec3 v : cTaperedCylinderFace)
+					outVertices.push_back(transform * (top_radius * v + top_3d));
+			}
+		}
+		else
+		{
+			// Bottom of the cylinder
+			if (bottom_radius > cMinRadius)
+			{
+				Vec3 bottom_3d(0, bottom, 0);
+				for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v)
+					outVertices.push_back(transform * (bottom_radius * *v + bottom_3d));
+			}
 		}
 	}
 }

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp

@@ -188,7 +188,7 @@ MassProperties TriangleShape::GetMassProperties() const
 	// creating a Body:
 	//
 	// BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
-	// BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sReplicate(1.0f), 1000.0f);
+	// BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sOne(), 1000.0f);
 	//
 	// Note that this makes the triangle shape behave the same as a mesh shape with a single triangle.
 	// In practice there is very little use for a dynamic triangle shape as back side collisions will be ignored
@@ -413,6 +413,9 @@ void TriangleShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle);
 		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle);
+
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCollideShape);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCastShape);
 	}
 
 	// Specialized collision functions

thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -249,9 +249,14 @@ void ContactConstraintManager::CachedBodyPair::RestoreState(StateRecorder &inStr
 
 void ContactConstraintManager::ManifoldCache::Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize)
 {
-	mAllocator.Init(inMaxBodyPairs * sizeof(BodyPairMap::KeyValue) + inCachedManifoldsSize);
+	uint max_body_pairs = min(inMaxBodyPairs, cMaxBodyPairsLimit);
+	JPH_ASSERT(max_body_pairs == inMaxBodyPairs, "Cannot support this many body pairs!");
+	JPH_ASSERT(inMaxContactConstraints <= cMaxContactConstraintsLimit); // Should have been enforced by caller
+
+	mAllocator.Init(uint(min(uint64(max_body_pairs) * sizeof(BodyPairMap::KeyValue) + inCachedManifoldsSize, uint64(~uint(0)))));
+
 	mCachedManifolds.Init(GetNextPowerOf2(inMaxContactConstraints));
-	mCachedBodyPairs.Init(GetNextPowerOf2(inMaxBodyPairs));
+	mCachedBodyPairs.Init(GetNextPowerOf2(max_body_pairs));
 }
 
 void ContactConstraintManager::ManifoldCache::Clear()
@@ -676,14 +681,18 @@ ContactConstraintManager::~ContactConstraintManager()
 
 void ContactConstraintManager::Init(uint inMaxBodyPairs, uint inMaxContactConstraints)
 {
-	mMaxConstraints = inMaxContactConstraints;
+	// Limit the number of constraints so that the allocation size fits in an unsigned integer
+	mMaxConstraints = min(inMaxContactConstraints, cMaxContactConstraintsLimit);
+	JPH_ASSERT(mMaxConstraints == inMaxContactConstraints, "Cannot support this many contact constraints!");
 
 	// Calculate worst case cache usage
-	uint cached_manifolds_size = inMaxContactConstraints * (sizeof(CachedManifold) + (MaxContactPoints - 1) * sizeof(CachedContactPoint));
+	constexpr uint cMaxManifoldSizePerConstraint = sizeof(CachedManifold) + (MaxContactPoints - 1) * sizeof(CachedContactPoint);
+	static_assert(cMaxManifoldSizePerConstraint < sizeof(ContactConstraint)); // If not true, then the next line can overflow
+	uint cached_manifolds_size = mMaxConstraints * cMaxManifoldSizePerConstraint;
 
 	// Init the caches
-	mCache[0].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size);
-	mCache[1].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size);
+	mCache[0].Init(inMaxBodyPairs, mMaxConstraints, cached_manifolds_size);
+	mCache[1].Init(inMaxBodyPairs, mMaxConstraints, cached_manifolds_size);
 }
 
 void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inContext)

thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -472,6 +472,14 @@ private:
 		WorldContactPoints		mContactPoints;
 	};
 
+public:
+	/// The maximum value that can be passed to Init for inMaxContactConstraints. Note you should really use a lower value, using this value will cost a lot of memory!
+	static constexpr uint		cMaxContactConstraintsLimit = ~uint(0) / sizeof(ContactConstraint);
+
+	/// The maximum value that can be passed to Init for inMaxBodyPairs. Note you should really use a lower value, using this value will cost a lot of memory!
+	static constexpr uint		cMaxBodyPairsLimit = ~uint(0) / sizeof(BodyPairMap::KeyValue);
+
+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);

thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -140,7 +140,7 @@ void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
 	JPH_ASSERT(inLimitsMax >= 0.0f && inLimitsMax <= JPH_PI);
 	mLimitsMin = inLimitsMin;
 	mLimitsMax = inLimitsMax;
-	mHasLimits = mLimitsMin > -JPH_PI && mLimitsMax < JPH_PI;
+	mHasLimits = mLimitsMin > -JPH_PI || mLimitsMax < JPH_PI;
 }
 
 void HingeConstraint::CalculateA1AndTheta()

thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp

@@ -42,7 +42,7 @@ void PhysicsScene::AddSoftBody(const SoftBodyCreationSettings &inSoftBody)
 
 bool PhysicsScene::FixInvalidScales()
 {
-	const Vec3 unit_scale = Vec3::sReplicate(1.0f);
+	const Vec3 unit_scale = Vec3::sOne();
 
 	bool success = true;
 	for (BodyCreationSettings &b : mBodies)

thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h

@@ -55,8 +55,8 @@ struct PhysicsSettings
 	/// Fraction of its inner radius a body may penetrate another body for the LinearCast motion quality
 	float		mLinearCastMaxPenetration = 0.25f;
 
-	/// Max squared distance to use to determine if two points are on the same plane for determining the contact manifold between two shape faces (unit: meter^2)
-	float		mManifoldToleranceSq = 1.0e-6f;
+	/// Max distance to use to determine if two points are on the same plane for determining the contact manifold between two shape faces (unit: meter)
+	float		mManifoldTolerance = 1.0e-3f;
 
 	/// Maximum distance to correct in a single iteration when solving position constraints (unit: meters)
 	float		mMaxPenetrationDistance = 0.2f;

thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp

@@ -77,13 +77,15 @@ PhysicsSystem::~PhysicsSystem()
 
 void PhysicsSystem::Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter)
 {
-	JPH_ASSERT(inMaxBodies <= BodyID::cMaxBodyIndex, "Cannot support this many bodies");
+	// Clamp max bodies
+	uint max_bodies = min(inMaxBodies, cMaxBodiesLimit);
+	JPH_ASSERT(max_bodies == inMaxBodies, "Cannot support this many bodies!");
 
 	mObjectVsBroadPhaseLayerFilter = &inObjectVsBroadPhaseLayerFilter;
 	mObjectLayerPairFilter = &inObjectLayerPairFilter;
 
 	// Initialize body manager
-	mBodyManager.Init(inMaxBodies, inNumBodyMutexes, inBroadPhaseLayerInterface);
+	mBodyManager.Init(max_bodies, inNumBodyMutexes, inBroadPhaseLayerInterface);
 
 	// Create broadphase
 	mBroadPhase = new BROAD_PHASE();
@@ -93,7 +95,7 @@ void PhysicsSystem::Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBody
 	mContactManager.Init(inMaxBodyPairs, inMaxContactConstraints);
 
 	// Init islands builder
-	mIslandBuilder.Init(inMaxBodies);
+	mIslandBuilder.Init(max_bodies);
 
 	// Initialize body interface
 	mBodyInterfaceLocking.Init(mBodyLockInterfaceLocking, mBodyManager, *mBroadPhase);
@@ -139,10 +141,10 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 	// Sync point for the broadphase. This will allow it to do clean up operations without having any mutexes locked yet.
 	mBroadPhase->FrameSync();
 
-	// If there are no active bodies or there's no time delta
+	// If there are no active bodies (and no step listener to wake them up) or there's no time delta
 	uint32 num_active_rigid_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
 	uint32 num_active_soft_bodies = mBodyManager.GetNumActiveBodies(EBodyType::SoftBody);
-	if ((num_active_rigid_bodies == 0 && num_active_soft_bodies == 0) || inDeltaTime <= 0.0f)
+	if ((num_active_rigid_bodies == 0 && num_active_soft_bodies == 0 && mStepListeners.empty()) || inDeltaTime <= 0.0f)
 	{
 		mBodyManager.LockAllBodies();
 
@@ -152,8 +154,9 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 		mBroadPhase->UpdateFinalize(update_state);
 		mBroadPhase->UnlockModifications();
 
-		// Call contact removal callbacks from contacts that existed in the previous update
-		mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0);
+		// If time has passed, call contact removal callbacks from contacts that existed in the previous update
+		if (inDeltaTime > 0.0f)
+			mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0);
 
 		mBodyManager.UnlockAllBodies();
 		return EPhysicsUpdateError::None;
@@ -844,6 +847,9 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 	// (always start looking at results from the next job)
 	int read_queue_idx = (inJobIndex + 1) % ioStep->mBodyPairQueues.size();
 
+	// Allocate space to temporarily store a batch of active bodies
+	BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(cActiveBodiesBatchSize * sizeof(BodyID));
+
 	for (;;)
 	{
 		// Check if there are active bodies to be processed
@@ -895,7 +901,6 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 
 				// Copy active bodies to temporary array, broadphase will reorder them
 				uint32 batch_size = active_bodies_read_idx_end - active_bodies_read_idx;
-				BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(batch_size * sizeof(BodyID));
 				memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody) + active_bodies_read_idx, batch_size * sizeof(BodyID));
 
 				// Find pairs in the broadphase
@@ -960,6 +965,23 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 	}
 }
 
+void PhysicsSystem::sDefaultSimCollideBodyVsBody(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	SubShapeIDCreator part1, part2;
+
+	if (inBody1.GetEnhancedInternalEdgeRemovalWithBody(inBody2))
+	{
+		// Collide with enhanced internal edge removal
+		ioCollideShapeSettings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+		InternalEdgeRemovingCollector::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+	else
+	{
+		// Regular collide
+		CollisionDispatch::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+}
+
 void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair)
 {
 	JPH_PROFILE_FUNCTION();
@@ -1003,13 +1025,10 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 		if (body_pair_handle == nullptr)
 			return; // Out of cache space
 
-		// If we want enhanced active edge detection for this body pair
-		bool enhanced_active_edges = body1->GetEnhancedInternalEdgeRemovalWithBody(*body2);
-
 		// Create the query settings
 		CollideShapeSettings settings;
 		settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
-		settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges && !enhanced_active_edges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
+		settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
 		settings.mMaxSeparationDistance = body1->IsSensor() || body2->IsSensor()? 0.0f : mPhysicsSettings.mSpeculativeContactDistance;
 		settings.mActiveEdgeMovementDirection = body1->GetLinearVelocity() - body2->GetLinearVelocity();
 
@@ -1121,7 +1140,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 
 					// Determine contact points
 					const PhysicsSettings &settings = mSystem->mPhysicsSettings;
-					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition()));
+					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, settings.mSpeculativeContactDistance + settings.mManifoldTolerance, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition()));
 
 					// Prune if we have more than 32 points (this means we could run out of space in the next iteration)
 					if (manifold->mRelativeContactPointsOn1.size() > 32)
@@ -1137,9 +1156,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 			ReductionCollideShapeCollector collector(this, body1, body2);
 
 			// Perform collision detection between the two shapes
-			SubShapeIDCreator part1, part2;
-			auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape;
-			f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, shape_filter);
+			mSimCollideBodyVsBody(*body1, *body2, transform1, transform2, settings, collector, shape_filter);
 
 			// Add the contacts
 			for (ContactManifold &manifold : collector.mManifolds)
@@ -1207,7 +1224,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 					ContactManifold manifold;
 					manifold.mBaseOffset = mBody1->GetCenterOfMassPosition();
 					const PhysicsSettings &settings = mSystem->mPhysicsSettings;
-					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
+					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, settings.mSpeculativeContactDistance + settings.mManifoldTolerance, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
 
 					// Calculate normal
 					manifold.mWorldSpaceNormal = inResult.mPenetrationAxis.Normalized();
@@ -1238,9 +1255,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 			NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle);
 
 			// Perform collision detection between the two shapes
-			SubShapeIDCreator part1, part2;
-			auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape;
-			f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, shape_filter);
+			mSimCollideBodyVsBody(*body1, *body2, transform1, transform2, settings, collector, shape_filter);
 
 			constraint_created = collector.mConstraintCreated;
 		}
@@ -1709,9 +1724,9 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 		if (sDrawMotionQualityLinearCast)
 		{
 			RMat44 com = body.GetCenterOfMassTransform();
-			body.GetShape()->Draw(DebugRenderer::sInstance, com, Vec3::sReplicate(1.0f), Color::sGreen, false, true);
+			body.GetShape()->Draw(DebugRenderer::sInstance, com, Vec3::sOne(), Color::sGreen, false, true);
 			DebugRenderer::sInstance->DrawArrow(com.GetTranslation(), com.GetTranslation() + ccd_body.mDeltaPosition, Color::sGreen, 0.1f);
-			body.GetShape()->Draw(DebugRenderer::sInstance, com.PostTranslated(ccd_body.mDeltaPosition), Vec3::sReplicate(1.0f), Color::sRed, false, true);
+			body.GetShape()->Draw(DebugRenderer::sInstance, com.PostTranslated(ccd_body.mDeltaPosition), Vec3::sOne(), Color::sRed, false, true);
 		}
 	#endif // JPH_DEBUG_RENDERER
 
@@ -1917,7 +1932,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 		SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper();
 
 		// Check if we collide with any other body. Note that we use the non-locking interface as we know the broadphase cannot be modified at this point.
-		RShapeCast shape_cast(body.GetShape(), Vec3::sReplicate(1.0f), body.GetCenterOfMassTransform(), ccd_body.mDeltaPosition);
+		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);
 
@@ -1929,7 +1944,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 			// Determine contact manifold
 			ContactManifold manifold;
 			manifold.mBaseOffset = shape_cast.mCenterOfMassStart.GetTranslation();
-			ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldToleranceSq, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
+			ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldTolerance, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
 			manifold.mSubShapeID1 = cast_shape_result.mSubShapeID1;
 			manifold.mSubShapeID2 = cast_shape_result.mSubShapeID2;
 			manifold.mPenetrationDepth = cast_shape_result.mPenetrationDepth;
@@ -2184,11 +2199,11 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 					{
 						// Draw the collision location
 						RMat44 collision_transform = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFraction * ccd_body->mDeltaPosition);
-						body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform, Vec3::sReplicate(1.0f), Color::sYellow, false, true);
+						body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform, Vec3::sOne(), Color::sYellow, false, true);
 
 						// Draw the collision location + slop
 						RMat44 collision_transform_plus_slop = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFractionPlusSlop * ccd_body->mDeltaPosition);
-						body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform_plus_slop, Vec3::sReplicate(1.0f), Color::sOrange, false, true);
+						body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform_plus_slop, Vec3::sOne(), Color::sOrange, false, true);
 
 						// Draw contact normal
 						DebugRenderer::sInstance->DrawArrow(ccd_body->mContactPointOn2, ccd_body->mContactPointOn2 - ccd_body->mContactNormal, Color::sYellow, 0.1f);

thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h

@@ -35,6 +35,19 @@ public:
 								PhysicsSystem()												: mContactManager(mPhysicsSettings) JPH_IF_ENABLE_ASSERTS(, mConstraintManager(&mBodyManager)) { }
 								~PhysicsSystem();
 
+	/// The maximum value that can be passed to Init for inMaxBodies.
+	static constexpr uint		cMaxBodiesLimit = BodyID::cMaxBodyIndex + 1;
+
+	/// The maximum value that can be passed to Init for inMaxBodyPairs.
+	/// Note you should really use a lower value, using this value will cost a lot of memory!
+	/// On a 32 bit platform, you'll run out of memory way before you reach this limit.
+	static constexpr uint		cMaxBodyPairsLimit = ContactConstraintManager::cMaxBodyPairsLimit;
+
+	/// The maximum value that can be passed to Init for inMaxContactConstraints.
+	/// Note you should really use a lower value, using this value will cost a lot of memory!
+	/// On a 32 bit platform, you'll run out of memory way before you reach this limit.
+	static constexpr uint		cMaxContactConstraintsLimit = ContactConstraintManager::cMaxContactConstraintsLimit;
+
 	/// Initialize the system.
 	/// @param inMaxBodies Maximum number of bodies to support.
 	/// @param inNumBodyMutexes Number of body mutexes to use. Should be a power of 2 in the range [1, 64], use 0 to auto detect.
@@ -76,6 +89,24 @@ public:
 	void						SetSimShapeFilter(const SimShapeFilter *inShapeFilter)		{ mSimShapeFilter = inShapeFilter; }
 	const SimShapeFilter *		GetSimShapeFilter() const									{ return mSimShapeFilter; }
 
+	/// Advanced use only: This function is similar to CollisionDispatch::sCollideShapeVsShape but only used to collide bodies during simulation.
+	/// inBody1 The first body to collide.
+	/// inBody2 The second body to collide.
+	/// inCenterOfMassTransform1 The center of mass transform of the first body (note this will not be the actual world space position of the body, it will be made relative to some position so we can drop down to single precision).
+	/// inCenterOfMassTransform2 The center of mass transform of the second body.
+	/// ioCollideShapeSettings Settings that control the collision detection. Note that the implementation can freely overwrite the shape settings if needed, the caller provides a temporary that will not be used after the function returns.
+	/// ioCollector The collector that will receive the contact points.
+	/// inShapeFilter The shape filter that can be used to exclude shapes from colliding with each other.
+	using SimCollideBodyVsBody = std::function<void(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)>;
+
+	/// Advanced use only: Set the function that will be used to collide two bodies during simulation.
+	/// This function is expected to eventually call CollideShapeCollector::AddHit all contact points between the shapes of body 1 and 2 in their given transforms.
+	void						SetSimCollideBodyVsBody(const SimCollideBodyVsBody &inBodyVsBody) { mSimCollideBodyVsBody = inBodyVsBody; }
+	const SimCollideBodyVsBody &GetSimCollideBodyVsBody() const								{ return mSimCollideBodyVsBody; }
+
+	/// Advanced use only: Default function that is used to collide two bodies during simulation.
+	static void					sDefaultSimCollideBodyVsBody(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+
 	/// Control the main constants of the physics simulation
 	void						SetPhysicsSettings(const PhysicsSettings &inSettings)		{ mPhysicsSettings = inSettings; }
 	const PhysicsSettings &		GetPhysicsSettings() const									{ return mPhysicsSettings; }
@@ -308,6 +339,9 @@ private:
 	/// The shape filter that is used to filter out sub shapes during simulation
 	const SimShapeFilter *		mSimShapeFilter = nullptr;
 
+	/// The collision function that is used to collide two shapes during simulation
+	SimCollideBodyVsBody		mSimCollideBodyVsBody = &sDefaultSimCollideBodyVsBody;
+
 	/// Simulation settings
 	PhysicsSettings				mPhysicsSettings;
 

thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -106,7 +106,7 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 	JPH_PROFILE_FUNCTION();
 
 	// Reset flag prior to collision detection
-	mNeedContactCallback = false;
+	mNeedContactCallback.store(false, memory_order_relaxed);
 
 	struct Collector : public CollideShapeBodyCollector
 	{
@@ -170,7 +170,7 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 						Array<LeafShape>	mHits;
 					};
 					LeafShapeCollector collector;
-					body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sReplicate(1.0f), SubShapeIDCreator(), collector, mShapeFilter);
+					body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sOne(), SubShapeIDCreator(), collector, mShapeFilter);
 					if (collector.mHits.empty())
 						return;
 
@@ -236,6 +236,7 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 	DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(layer);
 	DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(layer);
 	inSystem.GetBroadPhaseQuery().CollideAABox(world_bounds, collector, broadphase_layer_filter, object_layer_filter);
+	mNumSensors = uint(mCollidingSensors.size()); // Workaround for TSAN false positive: store mCollidingSensors.size() in a separate variable.
 }
 
 void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices)
@@ -269,7 +270,7 @@ void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSens
 
 	// We need a contact callback if one of the sensors collided
 	if (ioSensor.mHasContact)
-		mNeedContactCallback = true;
+		mNeedContactCallback.store(true, memory_order_relaxed);
 }
 
 void SoftBodyMotionProperties::ApplyPressure(const SoftBodyUpdateContext &inContext)
@@ -315,7 +316,7 @@ void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &i
 
 	// Integrate
 	Vec3 sub_step_gravity = inContext.mGravity * dt;
-	Vec3 sub_step_impulse = GetAccumulatedForce() * dt;
+	Vec3 sub_step_impulse = GetAccumulatedForce() * dt / max(float(mVertices.size()), 1.0f);
 	for (Vertex &v : mVertices)
 		if (v.mInvMass > 0.0f)
 		{
@@ -621,7 +622,7 @@ void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(cons
 					v.mHasContact = true;
 
 					// We need a contact callback if one of the vertices collided
-					mNeedContactCallback = true;
+					mNeedContactCallback.store(true, memory_order_relaxed);
 
 					// Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position)
 					CollidingShape &cs = mCollidingShapes[v.mCollidingShapeIndex];
@@ -720,7 +721,7 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont
 	JPH_PROFILE_FUNCTION();
 
 	// Contact callback
-	if (mNeedContactCallback && ioContext.mContactListener != nullptr)
+	if (mNeedContactCallback.load(memory_order_relaxed) && ioContext.mContactListener != nullptr)
 	{
 		// Remove non-colliding sensors from the list
 		for (int i = int(mCollidingSensors.size()) - 1; i >= 0; --i)
@@ -765,7 +766,7 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont
 
 	// Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space
 	float num_vertices_divider = float(max(int(mVertices.size()), 1));
-	SetLinearVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(linear_velocity / num_vertices_divider));
+	SetLinearVelocityClamped(ioContext.mCenterOfMassTransform.Multiply3x3(linear_velocity / num_vertices_divider));
 	SetAngularVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(angular_velocity / num_vertices_divider));
 
 	if (mUpdatePosition)
@@ -877,7 +878,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCol
 			// Process collision planes
 			uint num_vertices_to_process = min(SoftBodyUpdateContext::cVertexCollisionBatch, num_vertices - next_vertex);
 			DetermineCollisionPlanes(next_vertex, num_vertices_to_process);
-			uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_release) + num_vertices_to_process;
+			uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_acq_rel) + num_vertices_to_process;
 			if (vertices_processed >= num_vertices)
 			{
 				// Determine next state
@@ -896,19 +897,18 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCol
 SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext)
 {
 	// Do a relaxed read to see if there are more sensors to process
-	uint num_sensors = (uint)mCollidingSensors.size();
-	if (ioContext.mNextSensorIndex.load(memory_order_relaxed) < num_sensors)
+	if (ioContext.mNextSensorIndex.load(memory_order_relaxed) < mNumSensors)
 	{
 		// Fetch next sensor to process
 		uint sensor_index = ioContext.mNextSensorIndex.fetch_add(1, memory_order_acquire);
-		if (sensor_index < num_sensors)
+		if (sensor_index < mNumSensors)
 		{
 			// Process this sensor
 			DetermineSensorCollisions(mCollidingSensors[sensor_index]);
 
 			// Determine next state
-			uint sensors_processed = ioContext.mNumSensorsProcessed.fetch_add(1, memory_order_release) + 1;
-			if (sensors_processed >= num_sensors)
+			uint sensors_processed = ioContext.mNumSensorsProcessed.fetch_add(1, memory_order_acq_rel) + 1;
+			if (sensors_processed >= mNumSensors)
 				StartFirstIteration(ioContext);
 			return EStatus::DidWork;
 		}
@@ -961,7 +961,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstra
 				ProcessGroup(ioContext, next_group);
 
 				// Increment total number of groups processed
-				num_groups_processed = ioContext.mNumConstraintGroupsProcessed.fetch_add(1, memory_order_relaxed) + 1;
+				num_groups_processed = ioContext.mNumConstraintGroupsProcessed.fetch_add(1, memory_order_acq_rel) + 1;
 			}
 
 			if (num_groups_processed >= num_groups)
@@ -981,7 +981,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstra
 					StartNextIteration(ioContext);
 
 					// Reset group logic
-					ioContext.mNumConstraintGroupsProcessed.store(0, memory_order_relaxed);
+					ioContext.mNumConstraintGroupsProcessed.store(0, memory_order_release);
 					ioContext.mNextConstraintGroup.store(0, memory_order_release);
 				}
 				else
@@ -1002,7 +1002,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstra
 
 SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
 {
-	switch (ioContext.mState.load(memory_order_relaxed))
+	switch (ioContext.mState.load(memory_order_acquire))
 	{
 	case SoftBodyUpdateContext::EState::DetermineCollisionPlanes:
 		return ParallelDetermineCollisionPlanes(ioContext);

thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h

@@ -286,10 +286,11 @@ private:
 	AABox								mLocalBounds;								///< Bounding box of all vertices
 	AABox								mLocalPredictedBounds;						///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time
 	uint32								mNumIterations;								///< Number of solver iterations
+	uint								mNumSensors;								///< Workaround for TSAN false positive: store mCollidingSensors.size() in a separate variable.
 	float								mPressure;									///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure
 	float								mSkinnedMaxDistanceMultiplier = 1.0f;		///< Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints
 	bool								mUpdatePosition;							///< Update the position of the body while simulating (set to false for something that is attached to the static world)
-	bool								mNeedContactCallback = false;						///< True if the soft body has collided with anything in the last update
+	atomic<bool>						mNeedContactCallback = false;				///< True if the soft body has collided with anything in the last update
 	bool								mEnableSkinConstraints = true;				///< If skin constraints are enabled
 	bool								mSkinStatePreviousPositionValid = false;	///< True if the skinning was updated in the last update so that the previous position of the skin state is valid
 };

thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp

@@ -328,6 +328,9 @@ void SoftBodyShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::SoftBody, sCollideConvexVsSoftBody);
 		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::SoftBody, sCastConvexVsSoftBody);
+
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::SoftBody, s, CollisionDispatch::sReversedCollideShape);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::SoftBody, s, CollisionDispatch::sReversedCastShape);
 	}
 
 	// Specialized collision functions

thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -541,6 +541,10 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 			if (group_idx[i] == -1)
 				bounds.Encapsulate(Vec3(mVertices[i].mPosition));
 
+		// If the bounds are invalid, it means that there were no ungrouped vertices
+		if (!bounds.IsValid())
+			break;
+
 		// Determine longest and shortest axis
 		Vec3 bounds_size = bounds.GetSize();
 		uint max_axis = bounds_size.GetHighestComponentIndex();
@@ -1029,4 +1033,147 @@ SoftBodySharedSettings::SettingsResult SoftBodySharedSettings::sRestoreWithMater
 	return result;
 }
 
+Ref<SoftBodySharedSettings> SoftBodySharedSettings::sCreateCube(uint inGridSize, float inGridSpacing)
+{
+	const Vec3 cOffset = Vec3::sReplicate(-0.5f * inGridSpacing * (inGridSize - 1));
+
+	// Create settings
+	SoftBodySharedSettings *settings = new SoftBodySharedSettings;
+	for (uint z = 0; z < inGridSize; ++z)
+		for (uint y = 0; y < inGridSize; ++y)
+			for (uint x = 0; x < inGridSize; ++x)
+			{
+				SoftBodySharedSettings::Vertex v;
+				(cOffset + Vec3::sReplicate(inGridSpacing) * Vec3(float(x), float(y), float(z))).StoreFloat3(&v.mPosition);
+				settings->mVertices.push_back(v);
+			}
+
+	// Function to get the vertex index of a point on the cube
+	auto vertex_index = [inGridSize](uint inX, uint inY, uint inZ)
+	{
+		return inX + inY * inGridSize + inZ * inGridSize * inGridSize;
+	};
+
+	// Create edges
+	for (uint z = 0; z < inGridSize; ++z)
+		for (uint y = 0; y < inGridSize; ++y)
+			for (uint x = 0; x < inGridSize; ++x)
+			{
+				SoftBodySharedSettings::Edge e;
+				e.mVertex[0] = vertex_index(x, y, z);
+				if (x < inGridSize - 1)
+				{
+					e.mVertex[1] = vertex_index(x + 1, y, z);
+					settings->mEdgeConstraints.push_back(e);
+				}
+				if (y < inGridSize - 1)
+				{
+					e.mVertex[1] = vertex_index(x, y + 1, z);
+					settings->mEdgeConstraints.push_back(e);
+				}
+				if (z < inGridSize - 1)
+				{
+					e.mVertex[1] = vertex_index(x, y, z + 1);
+					settings->mEdgeConstraints.push_back(e);
+				}
+			}
+	settings->CalculateEdgeLengths();
+
+	// Tetrahedrons to fill a cube
+	const int tetra_indices[6][4][3] = {
+		{ {0, 0, 0}, {0, 1, 1}, {0, 0, 1}, {1, 1, 1} },
+		{ {0, 0, 0}, {0, 1, 0}, {0, 1, 1}, {1, 1, 1} },
+		{ {0, 0, 0}, {0, 0, 1}, {1, 0, 1}, {1, 1, 1} },
+		{ {0, 0, 0}, {1, 0, 1}, {1, 0, 0}, {1, 1, 1} },
+		{ {0, 0, 0}, {1, 1, 0}, {0, 1, 0}, {1, 1, 1} },
+		{ {0, 0, 0}, {1, 0, 0}, {1, 1, 0}, {1, 1, 1} }
+	};
+
+	// Create volume constraints
+	for (uint z = 0; z < inGridSize - 1; ++z)
+		for (uint y = 0; y < inGridSize - 1; ++y)
+			for (uint x = 0; x < inGridSize - 1; ++x)
+				for (uint t = 0; t < 6; ++t)
+				{
+					SoftBodySharedSettings::Volume v;
+					for (uint i = 0; i < 4; ++i)
+						v.mVertex[i] = vertex_index(x + tetra_indices[t][i][0], y + tetra_indices[t][i][1], z + tetra_indices[t][i][2]);
+					settings->mVolumeConstraints.push_back(v);
+				}
+
+	settings->CalculateVolumeConstraintVolumes();
+
+	// Create faces
+	for (uint y = 0; y < inGridSize - 1; ++y)
+		for (uint x = 0; x < inGridSize - 1; ++x)
+		{
+			SoftBodySharedSettings::Face f;
+
+			// Face 1
+			f.mVertex[0] = vertex_index(x, y, 0);
+			f.mVertex[1] = vertex_index(x, y + 1, 0);
+			f.mVertex[2] = vertex_index(x + 1, y + 1, 0);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, y + 1, 0);
+			f.mVertex[2] = vertex_index(x + 1, y, 0);
+			settings->AddFace(f);
+
+			// Face 2
+			f.mVertex[0] = vertex_index(x, y, inGridSize - 1);
+			f.mVertex[1] = vertex_index(x + 1, y + 1, inGridSize - 1);
+			f.mVertex[2] = vertex_index(x, y + 1, inGridSize - 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, y, inGridSize - 1);
+			f.mVertex[2] = vertex_index(x + 1, y + 1, inGridSize - 1);
+			settings->AddFace(f);
+
+			// Face 3
+			f.mVertex[0] = vertex_index(x, 0, y);
+			f.mVertex[1] = vertex_index(x + 1, 0, y + 1);
+			f.mVertex[2] = vertex_index(x, 0, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, 0, y);
+			f.mVertex[2] = vertex_index(x + 1, 0, y + 1);
+			settings->AddFace(f);
+
+			// Face 4
+			f.mVertex[0] = vertex_index(x, inGridSize - 1, y);
+			f.mVertex[1] = vertex_index(x, inGridSize - 1, y + 1);
+			f.mVertex[2] = vertex_index(x + 1, inGridSize - 1, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, inGridSize - 1, y + 1);
+			f.mVertex[2] = vertex_index(x + 1, inGridSize - 1, y);
+			settings->AddFace(f);
+
+			// Face 5
+			f.mVertex[0] = vertex_index(0, x, y);
+			f.mVertex[1] = vertex_index(0, x, y + 1);
+			f.mVertex[2] = vertex_index(0, x + 1, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(0, x + 1, y + 1);
+			f.mVertex[2] = vertex_index(0, x + 1, y);
+			settings->AddFace(f);
+
+			// Face 6
+			f.mVertex[0] = vertex_index(inGridSize - 1, x, y);
+			f.mVertex[1] = vertex_index(inGridSize - 1, x + 1, y + 1);
+			f.mVertex[2] = vertex_index(inGridSize - 1, x, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(inGridSize - 1, x + 1, y);
+			f.mVertex[2] = vertex_index(inGridSize - 1, x + 1, y + 1);
+			settings->AddFace(f);
+		}
+
+	// Optimize the settings
+	settings->Optimize();
+
+	return settings;
+}
+
 JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h

@@ -111,6 +111,12 @@ public:
 	/// Restore a shape and materials. Pass in an empty map in ioSettingsMap / ioMaterialMap or reuse the same map while reading multiple settings objects from the same stream in order to restore duplicates.
 	static SettingsResult sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap);
 
+	/// Create a cube. This can be used to create a simple soft body for testing purposes.
+	/// It will contain edge constraints, volume constraints and faces.
+	/// @param inGridSize Number of points along each axis
+	/// @param inGridSpacing Distance between points
+	static Ref<SoftBodySharedSettings> sCreateCube(uint inGridSize, float inGridSpacing);
+
 	/// A vertex is a particle, the data in this structure is only used during creation of the soft body and not during simulation
 	struct JPH_EXPORT Vertex
 	{

thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h

@@ -28,4 +28,6 @@ public:
 	float					mStiffness = 1000.0f;						///< Stiffness (spring constant in N/m) of anti rollbar, can be 0 to disable the anti-rollbar
 };
 
+using VehicleAntiRollBars = Array<VehicleAntiRollBar>;
+
 JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp

@@ -139,7 +139,7 @@ bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, c
 	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
 	float wheel_radius = wheel_settings->mRadius;
 	float shape_cast_length = wheel_settings->mSuspensionMaxLength + wheel_radius - mRadius;
-	RShapeCast shape_cast(&sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length);
+	RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length);
 
 	ShapeCastSettings settings;
 	settings.mUseShrunkenShapeAndConvexRadius = true;
@@ -261,7 +261,7 @@ bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem,
 	CylinderShape cylinder(wheel_half_width, wheel_settings->mRadius, min(wheel_half_width, wheel_settings->mRadius) * mConvexRadiusFraction);
 	cylinder.SetEmbedded();
 
-	RShapeCast shape_cast(&cylinder, Vec3::sReplicate(1.0f), shape_cast_start, inDirection * max_suspension_length);
+	RShapeCast shape_cast(&cylinder, Vec3::sOne(), shape_cast_start, inDirection * max_suspension_length);
 
 	ShapeCastSettings settings;
 	settings.mUseShrunkenShapeAndConvexRadius = true;

thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -80,7 +80,8 @@ VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstrain
 	mBody(&inVehicleBody),
 	mForward(inSettings.mForward),
 	mUp(inSettings.mUp),
-	mWorldUp(inSettings.mUp)
+	mWorldUp(inSettings.mUp),
+	mAntiRollBars(inSettings.mAntiRollBars)
 {
 	// Check sanity of incoming settings
 	JPH_ASSERT(inSettings.mUp.IsNormalized());
@@ -90,15 +91,6 @@ VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstrain
 	// Store max pitch/roll angle
 	SetMaxPitchRollAngle(inSettings.mMaxPitchRollAngle);
 
-	// Copy anti-rollbar settings
-	mAntiRollBars.resize(inSettings.mAntiRollBars.size());
-	for (uint i = 0; i < mAntiRollBars.size(); ++i)
-	{
-		const VehicleAntiRollBar &r = inSettings.mAntiRollBars[i];
-		mAntiRollBars[i] = r;
-		JPH_ASSERT(r.mStiffness >= 0.0f);
-	}
-
 	// Construct our controller class
 	mController = inSettings.mController->ConstructController(*this);
 
@@ -283,6 +275,8 @@ void VehicleConstraint::OnStep(const PhysicsStepListenerContext &inContext)
 	// Calculate anti-rollbar impulses
 	for (const VehicleAntiRollBar &r : mAntiRollBars)
 	{
+		JPH_ASSERT(r.mStiffness >= 0.0f);
+
 		Wheel *lw = mWheels[r.mLeftWheel];
 		Wheel *rw = mWheels[r.mRightWheel];
 
@@ -309,16 +303,19 @@ void VehicleConstraint::OnStep(const PhysicsStepListenerContext &inContext)
 		mPostStepCallback(*this, inContext);
 
 	// If the wheels are rotating, we don't want to go to sleep yet
-	bool allow_sleep = mController->AllowSleep();
-	if (allow_sleep)
-		for (const Wheel *w : mWheels)
-			if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f))
-			{
-				allow_sleep = false;
-				break;
-			}
-	if (mBody->GetAllowSleeping() != allow_sleep)
-		mBody->SetAllowSleeping(allow_sleep);
+	if (mBody->GetAllowSleeping())
+	{
+		bool allow_sleep = mController->AllowSleep();
+		if (allow_sleep)
+			for (const Wheel *w : mWheels)
+				if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f))
+				{
+					allow_sleep = false;
+					break;
+				}
+		if (!allow_sleep)
+			mBody->ResetSleepTimer();
+	}
 
 	// Increment step counter
 	++mCurrentStep;

thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h

@@ -32,7 +32,7 @@ public:
 	Vec3						mForward { 0, 0, 1 };						///< Vector indicating forward direction of the vehicle (in local space to the body)
 	float						mMaxPitchRollAngle = JPH_PI;				///< Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off.
 	Array<Ref<WheelSettings>>	mWheels;									///< List of wheels and their properties
-	Array<VehicleAntiRollBar>	mAntiRollBars;								///< List of anti rollbars and their properties
+	VehicleAntiRollBars			mAntiRollBars;								///< List of anti rollbars and their properties
 	Ref<VehicleControllerSettings> mController;								///< Defines how the vehicle can accelerate / decelerate
 
 protected:
@@ -161,6 +161,10 @@ public:
 	/// @param inWheelUp Unit vector that indicates up in model space of the wheel
 	RMat44						GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const;
 
+	/// Access to the vehicle's anti roll bars
+	const VehicleAntiRollBars &	GetAntiRollBars() const						{ return mAntiRollBars; }
+	VehicleAntiRollBars &		GetAntiRollBars()							{ return mAntiRollBars; }
+
 	/// Number of simulation steps between wheel collision tests when the vehicle is active. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc.
 	/// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion.
 	/// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle.
@@ -214,7 +218,7 @@ private:
 	Vec3						mUp;										///< Local space up vector for the vehicle
 	Vec3						mWorldUp;									///< Vector indicating the world space up direction (used to limit vehicle pitch/roll)
 	Wheels						mWheels;									///< Wheel states of the vehicle
-	Array<VehicleAntiRollBar>	mAntiRollBars;								///< Anti rollbars of the vehicle
+	VehicleAntiRollBars			mAntiRollBars;								///< Anti rollbars of the vehicle
 	VehicleController *			mController;								///< Controls the acceleration / deceleration of the vehicle
 	bool						mIsActive = false;							///< If this constraint is active
 	uint						mNumStepsBetweenCollisionTestActive = 1;	///< Number of simulation steps between wheel collision tests when the vehicle is active

thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h

@@ -37,7 +37,7 @@ public:
 };
 
 /// Runtime data for interface that controls acceleration / deceleration of the vehicle
-class JPH_EXPORT VehicleController : public RefTarget<VehicleController>, public NonCopyable
+class JPH_EXPORT VehicleController : public NonCopyable
 {
 public:
 	JPH_OVERRIDE_NEW_DELETE

thirdparty/jolt_physics/Jolt/RegisterTypes.cpp

@@ -45,6 +45,10 @@ JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPath)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPathHermite)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, VehicleConstraintSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, WheeledVehicleControllerSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, WheelSettingsWV)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, TrackedVehicleControllerSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, WheelSettingsTV)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, MotorcycleControllerSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RackAndPinionConstraintSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GearConstraintSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PulleyConstraintSettings)
@@ -156,6 +160,10 @@ void RegisterTypesInternal(uint64 inVersionID)
 		JPH_RTTI(PathConstraintSettings),
 		JPH_RTTI(VehicleConstraintSettings),
 		JPH_RTTI(WheeledVehicleControllerSettings),
+		JPH_RTTI(WheelSettingsWV),
+		JPH_RTTI(TrackedVehicleControllerSettings),
+		JPH_RTTI(WheelSettingsTV),
+		JPH_RTTI(MotorcycleControllerSettings),
 		JPH_RTTI(PathConstraintPath),
 		JPH_RTTI(PathConstraintPathHermite),
 		JPH_RTTI(RackAndPinionConstraintSettings),

thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp

@@ -18,7 +18,7 @@ DebugRenderer *DebugRenderer::sInstance = nullptr;
 static const int sMaxLevel = 4;
 
 // Distance for each LOD level, these are tweaked for an object of approx. size 1. Use the lod scale to scale these distances.
-static const float sLODDistanceForLevel[] = { 5.0f, 10.0f, 40.0f, FLT_MAX };
+static const float sLODDistanceForLevel[] = { 5.0f, 10.0f, 40.0f, cLargeFloat };
 
 DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor)
 {

thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h

@@ -218,7 +218,7 @@ public:
 
 		/// Constructor
 							Geometry(const AABox &inBounds) : mBounds(inBounds) { }
-							Geometry(const Batch &inBatch, const AABox &inBounds) : mBounds(inBounds) { mLODs.push_back({ inBatch, FLT_MAX }); }
+							Geometry(const Batch &inBatch, const AABox &inBounds) : mBounds(inBounds) { mLODs.push_back({ inBatch, cLargeFloat }); }
 
 		/// Determine which LOD to render
 		/// @param inCameraPosition Current position of the camera

thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h

@@ -1,27 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/Geometry/IndexedTriangle.h>
-#include <Jolt/Core/NonCopyable.h>
-
-JPH_NAMESPACE_BEGIN
-
-/// A class that groups triangles in batches of N (according to closeness)
-class JPH_EXPORT TriangleGrouper : public NonCopyable
-{
-public:
-	/// Virtual destructor
-	virtual					~TriangleGrouper() = default;
-
-	/// Group a batch of indexed triangles
-	/// @param inVertices The list of vertices
-	/// @param inTriangles The list of indexed triangles (indexes into inVertices)
-	/// @param inGroupSize How big each group should be
-	/// @param outGroupedTriangleIndices An ordered list of indices (indexing into inTriangles), contains groups of inGroupSize large worth of indices to triangles that are grouped together. If the triangle count is not an exact multiple of inGroupSize the last batch will be smaller.
-	virtual void			Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices) = 0;
-};
-
-JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp

@@ -1,95 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h>
-#include <Jolt/Geometry/MortonCode.h>
-#include <Jolt/Core/QuickSort.h>
-
-JPH_NAMESPACE_BEGIN
-
-void TriangleGrouperClosestCentroid::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices)
-{
-	const uint triangle_count = (uint)inTriangles.size();
-	const uint num_batches = (triangle_count + inGroupSize - 1) / inGroupSize;
-
-	Array<Vec3> centroids;
-	centroids.resize(triangle_count);
-
-	outGroupedTriangleIndices.resize(triangle_count);
-
-	for (uint t = 0; t < triangle_count; ++t)
-	{
-		// Store centroid
-		centroids[t] = inTriangles[t].GetCentroid(inVertices);
-
-		// Initialize sort table
-		outGroupedTriangleIndices[t] = t;
-	}
-
-	Array<uint>::const_iterator triangles_end = outGroupedTriangleIndices.end();
-
-	// Sort per batch
-	for (uint b = 0; b < num_batches - 1; ++b)
-	{
-		// Get iterators
-		Array<uint>::iterator batch_begin = outGroupedTriangleIndices.begin() + b * inGroupSize;
-		Array<uint>::iterator batch_end = batch_begin + inGroupSize;
-		Array<uint>::iterator batch_begin_plus_1 = batch_begin + 1;
-		Array<uint>::iterator batch_end_minus_1 = batch_end - 1;
-
-		// Find triangle with centroid with lowest X coordinate
-		Array<uint>::iterator lowest_iter = batch_begin;
-		float lowest_val = centroids[*lowest_iter].GetX();
-		for (Array<uint>::iterator other = batch_begin; other != triangles_end; ++other)
-		{
-			float val = centroids[*other].GetX();
-			if (val < lowest_val)
-			{
-				lowest_iter = other;
-				lowest_val = val;
-			}
-		}
-
-		// Make this triangle the first in a new batch
-		std::swap(*batch_begin, *lowest_iter);
-		Vec3 first_centroid = centroids[*batch_begin];
-
-		// Sort remaining triangles in batch on distance to first triangle
-		QuickSort(batch_begin_plus_1, batch_end,
-			[&first_centroid, &centroids](uint inLHS, uint inRHS)
-			{
-				return (centroids[inLHS] - first_centroid).LengthSq() < (centroids[inRHS] - first_centroid).LengthSq();
-			});
-
-		// Loop over remaining triangles
-		float furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq();
-		for (Array<uint>::iterator other = batch_end; other != triangles_end; ++other)
-		{
-			// Check if this triangle is closer than the furthest triangle in the batch
-			float dist = (centroids[*other] - first_centroid).LengthSq();
-			if (dist < furthest_dist)
-			{
-				// Replace furthest triangle
-				uint other_val = *other;
-				*other = *batch_end_minus_1;
-
-				// Find first element that is bigger than this one and insert the current item before it
-				Array<uint>::iterator upper = std::upper_bound(batch_begin_plus_1, batch_end, dist,
-					[&first_centroid, &centroids](float inLHS, uint inRHS)
-					{
-						return inLHS < (centroids[inRHS] - first_centroid).LengthSq();
-					});
-				std::copy_backward(upper, batch_end_minus_1, batch_end);
-				*upper = other_val;
-
-				// Calculate new furthest distance
-				furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq();
-			}
-		}
-	}
-}
-
-JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h

@@ -1,21 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/TriangleGrouper/TriangleGrouper.h>
-
-JPH_NAMESPACE_BEGIN
-
-/// A class that groups triangles in batches of N.
-/// Starts with centroid with lowest X coordinate and finds N closest centroids, this repeats until all groups have been found.
-/// Time complexity: O(N^2)
-class JPH_EXPORT TriangleGrouperClosestCentroid : public TriangleGrouper
-{
-public:
-	// See: TriangleGrouper::Group
-	virtual void			Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices) override;
-};
-
-JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp

@@ -1,49 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/TriangleGrouper/TriangleGrouperMorton.h>
-#include <Jolt/Geometry/MortonCode.h>
-#include <Jolt/Core/QuickSort.h>
-
-JPH_NAMESPACE_BEGIN
-
-void TriangleGrouperMorton::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices)
-{
-	const uint triangle_count = (uint)inTriangles.size();
-
-	Array<Vec3> centroids;
-	centroids.resize(triangle_count);
-
-	outGroupedTriangleIndices.resize(triangle_count);
-
-	for (uint t = 0; t < triangle_count; ++t)
-	{
-		// Store centroid
-		centroids[t] = inTriangles[t].GetCentroid(inVertices);
-
-		// Initialize sort table
-		outGroupedTriangleIndices[t] = t;
-	}
-
-	// Get bounding box of all centroids
-	AABox centroid_bounds;
-	for (uint t = 0; t < triangle_count; ++t)
-		centroid_bounds.Encapsulate(centroids[t]);
-
-	// Make sure box is not degenerate
-	centroid_bounds.EnsureMinimalEdgeLength(1.0e-5f);
-
-	// Calculate morton code for each centroid
-	Array<uint32> morton_codes;
-	morton_codes.resize(triangle_count);
-	for (uint t = 0; t < triangle_count; ++t)
-		morton_codes[t] = MortonCode::sGetMortonCode(centroids[t], centroid_bounds);
-
-	// Sort triangles based on morton code
-	QuickSort(outGroupedTriangleIndices.begin(), outGroupedTriangleIndices.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; });
-}
-
-JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h

@@ -1,20 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/TriangleGrouper/TriangleGrouper.h>
-
-JPH_NAMESPACE_BEGIN
-
-/// A class that groups triangles in batches of N according to morton code of centroid.
-/// Time complexity: O(N log(N))
-class JPH_EXPORT TriangleGrouperMorton : public TriangleGrouper
-{
-public:
-	// See: TriangleGrouper::Group
-	virtual void			Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices) override;
-};
-
-JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp

@@ -13,7 +13,7 @@ TriangleSplitter::TriangleSplitter(const VertexList &inVertices, const IndexedTr
 	mTriangles(inTriangles)
 {
 	mSortedTriangleIdx.resize(inTriangles.size());
-	mCentroids.resize(inTriangles.size());
+	mCentroids.resize(inTriangles.size() + 1); // Add 1 so we can load with Vec3::sLoadFloat3Unsafe
 
 	for (uint t = 0; t < inTriangles.size(); ++t)
 	{
@@ -23,44 +23,50 @@ TriangleSplitter::TriangleSplitter(const VertexList &inVertices, const IndexedTr
 		// Calculate centroid
 		inTriangles[t].GetCentroid(inVertices).StoreFloat3(&mCentroids[t]);
 	}
+
+	// Make sure Vec3::sLoatFloat3Unsafe doesn't read uninitialized data
+	mCentroids.back() = Float3(0, 0, 0);
 }
 
 bool TriangleSplitter::SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight)
 {
 	// Divide triangles
-	uint start = inTriangles.mBegin, end = inTriangles.mEnd;
+	uint *start = mSortedTriangleIdx.data() + inTriangles.mBegin;
+	uint *end = mSortedTriangleIdx.data() + inTriangles.mEnd;
 	while (start < end)
 	{
 		// Search for first element that is on the right hand side of the split plane
-		while (start < end && mCentroids[mSortedTriangleIdx[start]][inDimension] < inSplit)
+		while (start < end && mCentroids[*start][inDimension] < inSplit)
 			++start;
 
 		// Search for the first element that is on the left hand side of the split plane
-		while (start < end && mCentroids[mSortedTriangleIdx[end - 1]][inDimension] >= inSplit)
+		while (start < end && mCentroids[*(end - 1)][inDimension] >= inSplit)
 			--end;
 
 		if (start < end)
 		{
 			// Swap the two elements
-			std::swap(mSortedTriangleIdx[start], mSortedTriangleIdx[end - 1]);
-			++start;
 			--end;
+			std::swap(*start, *end);
+			++start;
 		}
 	}
 	JPH_ASSERT(start == end);
 
+	uint start_idx = uint(start - mSortedTriangleIdx.data());
+
 #ifdef JPH_ENABLE_ASSERTS
 	// Validate division algorithm
-	JPH_ASSERT(inTriangles.mBegin <= start);
-	JPH_ASSERT(start <= inTriangles.mEnd);
-	for (uint i = inTriangles.mBegin; i < start; ++i)
+	JPH_ASSERT(inTriangles.mBegin <= start_idx);
+	JPH_ASSERT(start_idx <= inTriangles.mEnd);
+	for (uint i = inTriangles.mBegin; i < start_idx; ++i)
 		JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] < inSplit);
-	for (uint i = start; i < inTriangles.mEnd; ++i)
+	for (uint i = start_idx; i < inTriangles.mEnd; ++i)
 		JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] >= inSplit);
 #endif
 
-	outLeft = Range(inTriangles.mBegin, start);
-	outRight = Range(start, inTriangles.mEnd);
+	outLeft = Range(inTriangles.mBegin, start_idx);
+	outRight = Range(start_idx, inTriangles.mEnd);
 	return outLeft.Count() > 0 && outRight.Count() > 0;
 }
 

thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp

@@ -19,10 +19,13 @@ TriangleSplitterBinning::TriangleSplitterBinning(const VertexList &inVertices, c
 
 bool TriangleSplitterBinning::Split(const Range &inTriangles, Range &outLeft, Range &outRight)
 {
+	const uint *begin = mSortedTriangleIdx.data() + inTriangles.mBegin;
+	const uint *end = mSortedTriangleIdx.data() + inTriangles.mEnd;
+
 	// Calculate bounds for this range
 	AABox centroid_bounds;
-	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t)
-		centroid_bounds.Encapsulate(Vec3(mCentroids[mSortedTriangleIdx[t]]));
+	for (const uint *t = begin; t < end; ++t)
+		centroid_bounds.Encapsulate(Vec3::sLoadFloat3Unsafe(mCentroids[*t]));
 
 	// Convert bounds to min coordinate and size
 	// Prevent division by zero if one of the dimensions is zero
@@ -57,11 +60,11 @@ bool TriangleSplitterBinning::Split(const Range &inTriangles, Range &outLeft, Ra
 	}
 
 	// Bin all triangles in all dimensions at once
-	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t)
+	for (const uint *t = begin; t < end; ++t)
 	{
-		Vec3 centroid_pos(mCentroids[mSortedTriangleIdx[t]]);
+		Vec3 centroid_pos = Vec3::sLoadFloat3Unsafe(mCentroids[*t]);
 
-		AABox triangle_bounds = AABox::sFromTriangle(mVertices, GetTriangle(t));
+		AABox triangle_bounds = AABox::sFromTriangle(mVertices, mTriangles[*t]);
 
 		Vec3 bin_no_f = (centroid_pos - bounds_min) / bounds_size * float(num_bins);
 		UVec4 bin_no = UVec4::sMin(bin_no_f.ToInt(), UVec4::sReplicate(num_bins - 1));

thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp

@@ -1,170 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h>
-#include <Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h>
-
-JPH_NAMESPACE_BEGIN
-
-TriangleSplitterFixedLeafSize::TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) :
-	TriangleSplitter(inVertices, inTriangles),
-	mLeafSize(inLeafSize),
-	mMinNumBins(inMinNumBins),
-	mMaxNumBins(inMaxNumBins),
-	mNumTrianglesPerBin(inNumTrianglesPerBin)
-{
-	// Group the triangles
-	TriangleGrouperClosestCentroid grouper;
-	grouper.Group(inVertices, inTriangles, mLeafSize, mSortedTriangleIdx);
-
-	// Pad triangles so that we have a multiple of mLeafSize
-	const uint num_triangles = (uint)inTriangles.size();
-	const uint num_groups = (num_triangles + mLeafSize - 1) / mLeafSize;
-	const uint last_triangle_idx = mSortedTriangleIdx.back();
-	for (uint t = num_triangles, t_end = num_groups * mLeafSize; t < t_end; ++t)
-		mSortedTriangleIdx.push_back(last_triangle_idx);
-}
-
-Vec3 TriangleSplitterFixedLeafSize::GetCentroidForGroup(uint inFirstTriangleInGroup)
-{
-	JPH_ASSERT(inFirstTriangleInGroup % mLeafSize == 0);
-	AABox box;
-	for (uint g = 0; g < mLeafSize; ++g)
-		box.Encapsulate(mVertices, GetTriangle(inFirstTriangleInGroup + g));
-	return box.GetCenter();
-}
-
-bool TriangleSplitterFixedLeafSize::Split(const Range &inTriangles, Range &outLeft, Range &outRight)
-{
-	// Cannot split anything smaller than leaf size
-	JPH_ASSERT(inTriangles.Count() > mLeafSize);
-	JPH_ASSERT(inTriangles.Count() % mLeafSize == 0);
-
-	// Calculate bounds for this range
-	AABox centroid_bounds;
-	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize)
-		centroid_bounds.Encapsulate(GetCentroidForGroup(t));
-
-	float best_cp = FLT_MAX;
-	uint best_dim = 0xffffffff;
-	float best_split = 0;
-
-	// Bin in all dimensions
-	uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins);
-	Array<Bin> bins(num_bins);
-	for (uint dim = 0; dim < 3; ++dim)
-	{
-		float bounds_min = centroid_bounds.mMin[dim];
-		float bounds_size = centroid_bounds.mMax[dim] - bounds_min;
-
-		// Skip axis if too small
-		if (bounds_size < 1.0e-5f)
-			continue;
-
-		// Initialize bins
-		for (uint b = 0; b < num_bins; ++b)
-		{
-			Bin &bin = bins[b];
-			bin.mBounds.SetEmpty();
-			bin.mMinCentroid = bounds_min + bounds_size * (b + 1) / num_bins;
-			bin.mNumTriangles = 0;
-		}
-
-		// Bin all triangles
-		for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize)
-		{
-			// Calculate average centroid for group
-			float centroid_pos = GetCentroidForGroup(t)[dim];
-
-			// Select bin
-			uint bin_no = min(uint((centroid_pos - bounds_min) / bounds_size * num_bins), num_bins - 1);
-			Bin &bin = bins[bin_no];
-
-			// Put all triangles of group in same bin
-			for (uint g = 0; g < mLeafSize; ++g)
-				bin.mBounds.Encapsulate(mVertices, GetTriangle(t + g));
-			bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos);
-			bin.mNumTriangles += mLeafSize;
-		}
-
-		// Calculate totals left to right
-		AABox prev_bounds;
-		int prev_triangles = 0;
-		for (uint b = 0; b < num_bins; ++b)
-		{
-			Bin &bin = bins[b];
-			bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin
-			bin.mNumTrianglesAccumulatedLeft = prev_triangles;
-			prev_bounds.Encapsulate(bin.mBounds);
-			prev_triangles += bin.mNumTriangles;
-		}
-
-		// Calculate totals right to left
-		prev_bounds.SetEmpty();
-		prev_triangles = 0;
-		for (int b = num_bins - 1; b >= 0; --b)
-		{
-			Bin &bin = bins[b];
-			prev_bounds.Encapsulate(bin.mBounds);
-			prev_triangles += bin.mNumTriangles;
-			bin.mBoundsAccumulatedRight = prev_bounds;
-			bin.mNumTrianglesAccumulatedRight = prev_triangles;
-		}
-
-		// Get best splitting plane
-		for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side
-		{
-			// Calculate surface area heuristic and see if it is better than the current best
-			const Bin &bin = bins[b];
-			float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight;
-			if (cp < best_cp)
-			{
-				best_cp = cp;
-				best_dim = dim;
-				best_split = bin.mMinCentroid;
-			}
-		}
-	}
-
-	// No split found?
-	if (best_dim == 0xffffffff)
-		return false;
-
-	// Divide triangles
-	uint start = inTriangles.mBegin, end = inTriangles.mEnd;
-	while (start < end)
-	{
-		// Search for first element that is on the right hand side of the split plane
-		while (start < end && GetCentroidForGroup(start)[best_dim] < best_split)
-			start += mLeafSize;
-
-		// Search for the first element that is on the left hand side of the split plane
-		while (start < end && GetCentroidForGroup(end - mLeafSize)[best_dim] >= best_split)
-			end -= mLeafSize;
-
-		if (start < end)
-		{
-			// Swap the two elements
-			for (uint g = 0; g < mLeafSize; ++g)
-				std::swap(mSortedTriangleIdx[start + g], mSortedTriangleIdx[end - mLeafSize + g]);
-			start += mLeafSize;
-			end -= mLeafSize;
-		}
-	}
-	JPH_ASSERT(start == end);
-
-	// No suitable split found, doing random split in half
-	if (start == inTriangles.mBegin || start == inTriangles.mEnd)
-		start = inTriangles.mBegin + (inTriangles.Count() / mLeafSize + 1) / 2 * mLeafSize;
-
-	outLeft = Range(inTriangles.mBegin, start);
-	outRight = Range(start, inTriangles.mEnd);
-	JPH_ASSERT(outLeft.mEnd > outLeft.mBegin && outRight.mEnd > outRight.mBegin);
-	JPH_ASSERT(outLeft.Count() % mLeafSize == 0 && outRight.Count() % mLeafSize == 0);
-	return true;
-}
-
-JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h

@@ -1,55 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/TriangleSplitter/TriangleSplitter.h>
-#include <Jolt/Geometry/AABox.h>
-
-JPH_NAMESPACE_BEGIN
-
-/// Same as TriangleSplitterBinning, but ensuring that leaves have a fixed amount of triangles
-/// The resulting tree should be suitable for processing on GPU where we want all threads to process an equal amount of triangles
-class JPH_EXPORT TriangleSplitterFixedLeafSize : public TriangleSplitter
-{
-public:
-	/// Constructor
-							TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6);
-
-	// See TriangleSplitter::GetStats
-	virtual void			GetStats(Stats &outStats) const override
-	{
-		outStats.mSplitterName = "TriangleSplitterFixedLeafSize";
-		outStats.mLeafSize = mLeafSize;
-	}
-
-	// See TriangleSplitter::Split
-	virtual bool			Split(const Range &inTriangles, Range &outLeft, Range &outRight) override;
-
-private:
-	/// Get centroid for group
-	Vec3					GetCentroidForGroup(uint inFirstTriangleInGroup);
-
-	// Configuration
-	const uint				mLeafSize;
-	const uint				mMinNumBins;
-	const uint				mMaxNumBins;
-	const uint				mNumTrianglesPerBin;
-
-	struct Bin
-	{
-		// Properties of this bin
-		AABox				mBounds;
-		float				mMinCentroid;
-		uint				mNumTriangles;
-
-		// Accumulated data from left most / right most bin to current (including this bin)
-		AABox				mBoundsAccumulatedLeft;
-		AABox				mBoundsAccumulatedRight;
-		uint				mNumTrianglesAccumulatedLeft;
-		uint				mNumTrianglesAccumulatedRight;
-	};
-};
-
-JPH_NAMESPACE_END