Ajout de Jolt Physics + 1ere version des factory entitecomposants - camera, transform, rigidbody, collider, renderer

lib/All/JoltPhysics/UnitTests/Core/ArrayTest.cpp

@@ -0,0 +1,726 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+
+TEST_SUITE("ArrayTest")
+{
+	// A test class that is non-trivially copyable to test if the Array class correctly constructs/destructs/copies and moves elements.
+	class NonTriv
+	{
+	public:
+							NonTriv() : mValue(0)										{ ++sNumConstructors; }
+		explicit			NonTriv(int inValue) : mValue(inValue)						{ ++sNumConstructors; }
+							NonTriv(const NonTriv &inValue) : mValue(inValue.mValue)	{ ++sNumCopyConstructors; }
+							NonTriv(NonTriv &&inValue) : mValue(inValue.mValue)			{ inValue.mValue = 0; ++sNumMoveConstructors; }
+							~NonTriv()													{ ++sNumDestructors; }
+
+		NonTriv &			operator = (const NonTriv &inRHS)							{ mValue = inRHS.mValue; return *this; }
+
+		bool				operator == (const NonTriv &inRHS) const					{ return mValue == inRHS.mValue; }
+		bool				operator != (const NonTriv &inRHS) const					{ return mValue != inRHS.mValue; }
+
+		int					Value() const												{ return mValue; }
+
+		static void			sReset()													{ sNumConstructors = 0; sNumCopyConstructors = 0; sNumMoveConstructors = 0; sNumDestructors = 0; }
+
+		static inline int	sNumConstructors = 0;
+		static inline int	sNumCopyConstructors = 0;
+		static inline int	sNumMoveConstructors = 0;
+		static inline int	sNumDestructors = 0;
+
+		int					mValue;
+	};
+
+	TEST_CASE("TestConstructLength")
+	{
+		Array<int> arr(55);
+		CHECK(arr.size() == 55);
+	}
+
+	TEST_CASE("TestConstructLengthNonTriv")
+	{
+		NonTriv::sReset();
+		Array<NonTriv> arr(55);
+		CHECK(arr.size() == 55);
+		CHECK(NonTriv::sNumConstructors == 55);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+	}
+
+	TEST_CASE("TestConstructValue")
+	{
+		Array<int> arr(5, 55);
+		CHECK(arr.size() == 5);
+		for (int i = 0; i < 5; ++i)
+			CHECK(arr[i] == 55);
+	}
+
+	TEST_CASE("TestConstructValueNonTriv")
+	{
+		NonTriv v(55);
+		NonTriv::sReset();
+		Array<NonTriv> arr(5, v);
+		CHECK(arr.size() == 5);
+		for (int i = 0; i < 5; ++i)
+			CHECK(arr[i].Value() == 55);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 5);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+	}
+
+	TEST_CASE("TestConstructIterator")
+	{
+		int values[] = { 1, 2, 3 };
+
+		Array<int> arr(values, values + 3);
+		CHECK(arr.size() == 3);
+		CHECK(arr[0] == 1);
+		CHECK(arr[1] == 2);
+		CHECK(arr[2] == 3);
+	}
+
+	TEST_CASE("TestConstructIteratorNonTriv")
+	{
+		NonTriv values[] = { NonTriv(1), NonTriv(2), NonTriv(3) };
+
+		NonTriv::sReset();
+		Array<NonTriv> arr(values, values + 3);
+		CHECK(arr.size() == 3);
+		CHECK(arr[0].Value() == 1);
+		CHECK(arr[1].Value() == 2);
+		CHECK(arr[2].Value() == 3);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 3);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+	}
+
+	TEST_CASE("TestConstructInitializerList")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		CHECK(arr.size() == 3);
+		CHECK(arr[0] == 1);
+		CHECK(arr[1] == 2);
+		CHECK(arr[2] == 3);
+	}
+
+	TEST_CASE("TestConstructInitializerListNonTriv")
+	{
+		NonTriv::sReset();
+		Array<NonTriv> arr({ NonTriv(1), NonTriv(2), NonTriv(3) });
+		CHECK(arr.size() == 3);
+		CHECK(arr[0].Value() == 1);
+		CHECK(arr[1].Value() == 2);
+		CHECK(arr[2].Value() == 3);
+		CHECK(NonTriv::sNumConstructors == 3); // For the initializer list
+		CHECK(NonTriv::sNumCopyConstructors == 3); // Initializing the array
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 3); // For the initializer list
+	}
+
+	TEST_CASE("TestConstructFromArray")
+	{
+		Array<int> arr = { 1, 2, 3 };
+		Array<int> arr2(arr);
+		CHECK(arr2.size() == 3);
+		CHECK(arr2[0] == 1);
+		CHECK(arr2[1] == 2);
+		CHECK(arr2[2] == 3);
+	}
+
+	TEST_CASE("TestConstructFromArrayNonTriv")
+	{
+		Array<NonTriv> arr = { NonTriv(1), NonTriv(2), NonTriv(3) };
+		NonTriv::sReset();
+		Array<NonTriv> arr2(arr);
+		CHECK(arr2.size() == 3);
+		CHECK(arr2[0].Value() == 1);
+		CHECK(arr2[1].Value() == 2);
+		CHECK(arr2[2].Value() == 3);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 3);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+	}
+
+	TEST_CASE("TestMoveFromArray")
+	{
+		Array<int> arr = { 1, 2, 3 };
+		Array<int> arr2(std::move(arr));
+		CHECK(arr2.size() == 3);
+		CHECK(arr2[0] == 1);
+		CHECK(arr2[1] == 2);
+		CHECK(arr2[2] == 3);
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 0);
+	}
+
+	TEST_CASE("TestMoveFromArrayNonTriv")
+	{
+		Array<NonTriv> arr = { NonTriv(1), NonTriv(2), NonTriv(3) };
+		NonTriv::sReset();
+		Array<NonTriv> arr2(std::move(arr)); // This just updates the mElements pointer so should not call any constructors/destructors etc.
+		CHECK(arr2.size() == 3);
+		CHECK(arr2[0].Value() == 1);
+		CHECK(arr2[1].Value() == 2);
+		CHECK(arr2[2].Value() == 3);
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 0);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+	}
+
+	TEST_CASE("TestClear")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		CHECK(arr.size() == 3);
+		arr.clear();
+		CHECK(arr.size() == 0);
+	}
+
+	TEST_CASE("TestClearNonTriv")
+	{
+		NonTriv::sReset();
+		Array<NonTriv> arr({ NonTriv(1), NonTriv(2), NonTriv(3) });
+		CHECK(arr.size() == 3);
+		CHECK(NonTriv::sNumConstructors == 3); // For initializer list
+		CHECK(NonTriv::sNumCopyConstructors == 3); // To move into array
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 3); // For initializer list
+		NonTriv::sReset();
+		arr.clear();
+		CHECK(arr.size() == 0);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 3);
+	}
+
+	TEST_CASE("TestPushBack")
+	{
+		Array<int> arr;
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 0);
+
+		arr.push_back(1);
+		CHECK(arr.size() == 1);
+		CHECK(arr[0] == 1);
+
+		arr.push_back(2);
+		CHECK(arr.size() == 2);
+		CHECK(arr[0] == 1);
+		CHECK(arr[1] == 2);
+
+		arr.pop_back();
+		CHECK(arr.size() == 1);
+
+		arr.pop_back();
+		CHECK(arr.size() == 0);
+		CHECK(arr.empty());
+	}
+
+	TEST_CASE("TestPushBackNonTriv")
+	{
+		NonTriv v1(1);
+		NonTriv v2(2);
+
+		NonTriv::sReset();
+		Array<NonTriv> arr;
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 0);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+
+		NonTriv::sReset();
+		arr.push_back(v1);
+		CHECK(arr.size() == 1);
+		CHECK(arr[0].Value() == 1);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 1);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+
+		NonTriv::sReset();
+		arr.push_back(v2);
+		CHECK(arr.size() == 2);
+		CHECK(arr[0].Value() == 1);
+		CHECK(arr[1].Value() == 2);
+#ifndef JPH_USE_STD_VECTOR
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 1);
+		CHECK(NonTriv::sNumMoveConstructors == 1); // Array resizes from 1 to 2
+		CHECK(NonTriv::sNumDestructors == 1); // Array resizes from 1 to 2
+#endif // JPH_USE_STD_VECTOR
+
+		NonTriv::sReset();
+		arr.pop_back();
+		CHECK(arr.size() == 1);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 1);
+
+		NonTriv::sReset();
+		arr.pop_back();
+		CHECK(arr.size() == 0);
+		CHECK(arr.empty());
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 1);
+	}
+
+	TEST_CASE("TestPushBackMove")
+	{
+		Array<Array<int>> arr;
+		Array<int> arr2 = { 1, 2, 3 };
+		arr.push_back(std::move(arr2));
+		CHECK(arr2.size() == 0);
+		CHECK(arr[0] == Array<int>({ 1, 2, 3 }));
+	}
+
+	TEST_CASE("TestEmplaceBack")
+	{
+		struct Test
+		{
+			Test(int inA, int inB) : mA(inA), mB(inB) { }
+
+			int mA;
+			int mB;
+		};
+
+		Array<Test> arr;
+		arr.emplace_back(1, 2);
+		CHECK(arr.size() == 1);
+		CHECK(arr[0].mA == 1);
+		CHECK(arr[0].mB == 2);
+	}
+
+	TEST_CASE("TestReserve")
+	{
+		Array<int> arr;
+		CHECK(arr.capacity() == 0);
+
+		arr.reserve(123);
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 123);
+
+		arr.reserve(456);
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 456);
+	}
+
+	TEST_CASE("TestReserveNonTriv")
+	{
+		NonTriv::sReset();
+
+		Array<NonTriv> arr;
+		CHECK(arr.capacity() == 0);
+
+		arr.reserve(123);
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 123);
+
+		arr.reserve(456);
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 456);
+
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+	}
+
+	TEST_CASE("TestResize")
+	{
+		Array<int> arr;
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 0);
+		CHECK(arr.data() == nullptr);
+
+		arr.resize(0);
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 0);
+		CHECK(arr.data() == nullptr);
+
+		arr.resize(123);
+		CHECK(arr.size() == 123);
+		CHECK(arr.capacity() == 123);
+		for (int i = 0; i < 123; ++i)
+			arr[i] = i;
+
+		arr.resize(456);
+		CHECK(arr.size() == 456);
+		CHECK(arr.capacity() == 456);
+		for (int i = 0; i < 123; ++i)
+			CHECK(arr[i] == i);
+
+		arr.resize(10);
+		CHECK(arr.size() == 10);
+		CHECK(arr.capacity() >= 10);
+	}
+
+	TEST_CASE("TestResizeNonTriv")
+	{
+		NonTriv::sReset();
+		Array<NonTriv> arr;
+		CHECK(arr.size() == 0);
+		CHECK(arr.capacity() == 0);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+
+		NonTriv::sReset();
+		arr.resize(123);
+		CHECK(arr.size() == 123);
+		CHECK(arr.capacity() == 123);
+		CHECK(NonTriv::sNumConstructors == 123);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+		for (int i = 0; i < 123; ++i)
+			arr[i] = NonTriv(i);
+
+		NonTriv::sReset();
+		arr.resize(456);
+		CHECK(arr.size() == 456);
+		CHECK(arr.capacity() == 456);
+		for (int i = 0; i < 123; ++i)
+			CHECK(arr[i].Value() == i);
+#ifndef JPH_USE_STD_VECTOR
+		CHECK(NonTriv::sNumConstructors == 456 - 123);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 123);
+		CHECK(NonTriv::sNumDestructors == 123); // Switched to a new block, all old elements are destroyed after being moved
+#endif // JPH_USE_STD_VECTOR
+
+		NonTriv::sReset();
+		arr.resize(10);
+		CHECK(arr.size() == 10);
+		CHECK(arr.capacity() >= 10);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 456 - 10);
+	}
+
+	TEST_CASE("TestResizeWithValue")
+	{
+		Array<int> arr;
+		arr.resize(10, 55);
+		CHECK(arr.size() == 10);
+		CHECK(arr.capacity() == 10);
+		for (int i = 0; i < 10; ++i)
+			CHECK(arr[i] == 55);
+	}
+
+	TEST_CASE("TestResizeWithValueNonTriv")
+	{
+		NonTriv v(55);
+		Array<NonTriv> arr;
+		NonTriv::sReset();
+		arr.resize(10, v);
+		CHECK(arr.size() == 10);
+		CHECK(arr.capacity() == 10);
+		for (int i = 0; i < 10; ++i)
+			CHECK(arr[i].Value() == 55);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 10);
+		CHECK(NonTriv::sNumMoveConstructors == 0);
+		CHECK(NonTriv::sNumDestructors == 0);
+	}
+
+#ifndef JPH_USE_STD_VECTOR // std::vector can choose to not shrink the array when calling shrink_to_fit so we can't test this
+	TEST_CASE("TestShrinkToFit")
+	{
+		Array<int> arr;
+		for (int i = 0; i < 5; ++i)
+			arr.push_back(i);
+		CHECK(arr.capacity() > 5);
+		CHECK(arr.size() == 5);
+
+		arr.shrink_to_fit();
+		CHECK(arr.capacity() == 5);
+		CHECK(arr.size() == 5);
+		for (int i = 0; i < 5; ++i)
+			CHECK(arr[i] == i);
+
+		arr.clear();
+		CHECK(arr.capacity() == 5);
+		CHECK(arr.size() == 0);
+
+		arr.shrink_to_fit();
+		CHECK(arr.capacity() == 0);
+		CHECK(arr.data() == nullptr);
+	}
+
+	TEST_CASE("TestShrinkToFitNonTriv")
+	{
+		Array<NonTriv> arr;
+		for (int i = 0; i < 5; ++i)
+			arr.push_back(NonTriv(i));
+		CHECK(arr.capacity() > 5);
+		CHECK(arr.size() == 5);
+
+		NonTriv::sReset();
+		arr.shrink_to_fit();
+		CHECK(arr.capacity() == 5);
+		CHECK(arr.size() == 5);
+		for (int i = 0; i < 5; ++i)
+			CHECK(arr[i].Value() == i);
+		CHECK(NonTriv::sNumConstructors == 0);
+		CHECK(NonTriv::sNumCopyConstructors == 0);
+		CHECK(NonTriv::sNumMoveConstructors == 5);
+		CHECK(NonTriv::sNumDestructors == 5); // Switched to a new block, all old elements are destroyed after being moved
+	}
+#endif // JPH_USE_STD_VECTOR
+
+	TEST_CASE("TestAssignIterator")
+	{
+		int values[] = { 1, 2, 3 };
+
+		Array<int> arr({ 4, 5, 6 });
+		arr.assign(values, values + 3);
+		CHECK(arr.size() == 3);
+		CHECK(arr[0] == 1);
+		CHECK(arr[1] == 2);
+		CHECK(arr[2] == 3);
+	}
+
+	TEST_CASE("TestAssignInitializerList")
+	{
+		Array<int> arr({ 4, 5, 6 });
+		arr.assign({ 1, 2, 3 });
+		CHECK(arr.size() == 3);
+		CHECK(arr[0] == 1);
+		CHECK(arr[1] == 2);
+		CHECK(arr[2] == 3);
+	}
+
+	TEST_CASE("TestSwap")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		Array<int> arr2({ 4, 5, 6 });
+		arr.swap(arr2);
+		CHECK(arr == Array<int>({ 4, 5, 6 }));
+		CHECK(arr2 == Array<int>({ 1, 2, 3 }));
+	}
+
+	TEST_CASE("TestInsertBegin")
+	{
+		Array<int> arr = { 1, 2, 3 };
+		arr.insert(arr.begin(), 4);
+		CHECK(arr == Array<int>({ 4, 1, 2, 3 }));
+	}
+
+	TEST_CASE("TestInsertMid")
+	{
+		Array<int> arr = { 1, 2, 3 };
+		arr.insert(arr.begin() + 1, 4);
+		CHECK(arr == Array<int>({ 1, 4, 2, 3 }));
+	}
+
+	TEST_CASE("TestInsertEnd")
+	{
+		Array<int> arr = { 1, 2, 3 };
+		arr.insert(arr.begin() + 3, 4);
+		CHECK(arr == Array<int>({ 1, 2, 3, 4 }));
+	}
+
+	TEST_CASE("TestInsertMultipleBegin")
+	{
+		Array<int> values_to_insert = { 4, 5, 6, 7 };
+		Array<int> arr = { 1, 2, 3 };
+		arr.insert(arr.begin(), values_to_insert.begin(), values_to_insert.end());
+		CHECK(arr == Array<int>({ 4, 5, 6, 7, 1, 2, 3 }));
+	}
+
+	TEST_CASE("TestInsertMultipleMid")
+	{
+		Array<int> values_to_insert = { 4, 5, 6, 7 };
+		Array<int> arr = { 1, 2, 3 };
+		arr.insert(arr.begin() + 1, values_to_insert.begin(), values_to_insert.end());
+		CHECK(arr == Array<int>({ 1, 4, 5, 6, 7, 2, 3 }));
+	}
+
+	TEST_CASE("TestInsertMultipleEnd")
+	{
+		Array<int> values_to_insert = { 4, 5, 6, 7 };
+		Array<int> arr = { 1, 2, 3 };
+		arr.insert(arr.begin() + 3, values_to_insert.begin(), values_to_insert.end());
+		CHECK(arr == Array<int>({ 1, 2, 3, 4, 5, 6, 7 }));
+	}
+
+	TEST_CASE("TestFrontBack")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		CHECK(arr.front() == 1);
+		CHECK(arr.back() == 3);
+	}
+
+	TEST_CASE("TestAssign")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		Array<int> arr2({ 4, 5, 6 });
+		arr = arr2;
+		CHECK(arr == Array<int>({ 4, 5, 6 }));
+		Array<int> &arr3 = arr; // Avoid compiler warning
+		arr = arr3;
+		CHECK(arr == Array<int>({ 4, 5, 6 }));
+		arr = { 7, 8, 9 };
+		CHECK(arr == Array<int>({ 7, 8, 9 }));
+	}
+
+	TEST_CASE("TestAssignMove")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		Array<int> arr2({ 4, 5, 6 });
+		arr = std::move(arr2);
+		CHECK(arr == Array<int>({ 4, 5, 6 }));
+		CHECK(arr2.empty());
+	}
+
+	TEST_CASE("TestEraseBegin")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		arr.erase(arr.begin());
+		CHECK(arr == Array<int>({ 2, 3 }));
+	}
+
+	TEST_CASE("TestEraseMid")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		arr.erase(arr.begin() + 1);
+		CHECK(arr == Array<int>({ 1, 3 }));
+	}
+
+	TEST_CASE("TestEraseEnd")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		arr.erase(arr.begin() + 2);
+		CHECK(arr == Array<int>({ 1, 2 }));
+	}
+
+	TEST_CASE("TestEraseMultipleBegin")
+	{
+		Array<int> arr({ 1, 2, 3, 4, 5 });
+		arr.erase(arr.begin(), arr.begin() + 2);
+		CHECK(arr == Array<int>({ 3, 4, 5 }));
+	}
+
+	TEST_CASE("TestEraseMultipleMid")
+	{
+		Array<int> arr({ 1, 2, 3, 4, 5 });
+		arr.erase(arr.begin() + 2, arr.begin() + 4);
+		CHECK(arr == Array<int>({ 1, 2, 5 }));
+	}
+
+	TEST_CASE("TestEraseMultipleEnd")
+	{
+		Array<int> arr({ 1, 2, 3, 4, 5 });
+		arr.erase(arr.begin() + 3, arr.begin() + 5);
+		CHECK(arr == Array<int>({ 1, 2, 3 }));
+	}
+
+	TEST_CASE("TestEquals")
+	{
+		Array<int> arr({ 1, 2, 3 });
+		Array<int> arr2({ 4, 5, 6 });
+		CHECK(arr == arr);
+		CHECK(!(arr == arr2));
+		CHECK(!(arr != arr));
+		CHECK(arr != arr2);
+	}
+
+	TEST_CASE("TestReverseIterator")
+	{
+		Array<int> arr({ 1, 2, 3, 4, 5, 6 });
+		Array<int>::reverse_iterator r = arr.rbegin();
+		CHECK(*r == 6);
+
+		int v = *(++r);
+		CHECK(v == 5);
+		CHECK(*r == 5);
+
+		v = *(--r);
+		CHECK(v == 6);
+		CHECK(*r == 6);
+
+		v = *(r++);
+		CHECK(v == 6);
+		CHECK(*r == 5);
+
+		v = *(r--);
+		CHECK(v == 5);
+		CHECK(*r == 6);
+
+		v = *(r += 2);
+		CHECK(v == 4);
+		CHECK(*r == 4);
+
+		v = *(r -= 2);
+		CHECK(v == 6);
+		CHECK(*r == 6);
+
+		CHECK(r == arr.rbegin());
+		CHECK(r != arr.rend());
+
+		r += 6;
+		CHECK(r == arr.rend());
+
+		CHECK(*(arr.rbegin() + 3) == 3);
+		CHECK(*(arr.rend() - 3) == 3);
+	}
+
+	TEST_CASE("TestConstReverseIterator")
+	{
+		const Array<int> arr({ 1, 2, 3, 4, 5, 6 });
+		Array<int>::const_reverse_iterator r = arr.rbegin();
+		CHECK(*r == 6);
+
+		int v = *(++r);
+		CHECK(v == 5);
+		CHECK(*r == 5);
+
+		v = *(--r);
+		CHECK(v == 6);
+		CHECK(*r == 6);
+
+		v = *(r++);
+		CHECK(v == 6);
+		CHECK(*r == 5);
+
+		v = *(r--);
+		CHECK(v == 5);
+		CHECK(*r == 6);
+
+		v = *(r += 2);
+		CHECK(v == 4);
+		CHECK(*r == 4);
+
+		v = *(r -= 2);
+		CHECK(v == 6);
+		CHECK(*r == 6);
+
+		CHECK(r == arr.rbegin());
+		CHECK(r == arr.crbegin());
+		CHECK(r != arr.rend());
+		CHECK(r != arr.crend());
+
+		r += 6;
+		CHECK(r == arr.rend());
+		CHECK(r == arr.crend());
+
+		CHECK(*(arr.rbegin() + 3) == 3);
+		CHECK(*(arr.rend() - 3) == 3);
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/BinaryHeapTest.cpp

@@ -0,0 +1,47 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/BinaryHeap.h>
+
+TEST_SUITE("BinaryHeapTest")
+{
+	TEST_CASE("TestBinaryHeap")
+	{
+		// Add some numbers
+		Array<int> array;
+		array.reserve(1100);
+		for (int i = 0; i < 1000; ++i)
+			array.push_back(i);
+
+		// Ensure we have some duplicates
+		for (int i = 0; i < 1000; i += 10)
+			array.push_back(i);
+
+		// Shuffle the array
+		UnitTestRandom random(123);
+		std::shuffle(array.begin(), array.end(), random);
+
+		// Add the numbers to the heap
+		Array<int> heap;
+		for (int i : array)
+		{
+			heap.push_back(i);
+			BinaryHeapPush(heap.begin(), heap.end(), std::less<int> { });
+		}
+
+		// Check that the heap is sorted
+		int last = INT_MAX;
+		int seen[1000] { 0 };
+		while (!heap.empty())
+		{
+			BinaryHeapPop(heap.begin(), heap.end(), std::less<int> { });
+			int current = heap.back();
+			CHECK(++seen[current] <= (current % 10 == 0? 2 : 1));
+			heap.pop_back();
+			CHECK(current <= last);
+			last = current;
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/FPFlushDenormalsTest.cpp

@@ -0,0 +1,44 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/FPFlushDenormals.h>
+#include <atomic>
+
+#if !defined(JPH_CPU_WASM) && !defined(JPH_CPU_RISCV) && !defined(JPH_CPU_PPC) && !defined(JPH_CPU_LOONGARCH)
+
+// Implemented as a global atomic so the compiler can't optimize it to a constant
+extern atomic<float> TestFltMin;
+atomic<float> TestFltMin = FLT_MIN;
+
+TEST_SUITE("FlushDenormalsTests")
+{
+	TEST_CASE("TestFlushDenormals")
+	{
+		// By default flush denormals should be off
+		{
+			float value = TestFltMin * 0.1f;
+			CHECK(value > 0.0f);
+		}
+
+		// Turn flush denormal on
+		{
+			FPFlushDenormals flush_denormals;
+
+			float value = TestFltMin * 0.1f;
+			CHECK(value == 0.0f);
+		}
+
+		// Check if state was properly restored
+		{
+			float value = TestFltMin * 0.1f;
+			CHECK(value > 0.0f);
+		}
+
+		// Update TestFltMin to prevent the compiler from optimizing away TestFltMin and replace all calculations above with 0
+		TestFltMin = 1.0f;
+	}
+}
+
+#endif // JPH_CPU_WASM

lib/All/JoltPhysics/UnitTests/Core/HashCombineTest.cpp

@@ -0,0 +1,51 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/HashCombine.h>
+
+TEST_SUITE("HashCombineTest")
+{
+	TEST_CASE("TestHashBytes")
+	{
+		CHECK(HashBytes("This is a test", 14) == 2733878766136413408UL);
+	}
+
+	TEST_CASE("TestHashString")
+	{
+		CHECK(HashString("This is a test") == 2733878766136413408UL);
+	}
+
+	TEST_CASE("TestHashStruct")
+	{
+		const char *char_test = "This is a test";
+		CHECK(Hash<const char *> { } (char_test) == 2733878766136413408UL);
+
+		std::string_view str_view_test = "This is a test";
+		CHECK(Hash<std::string_view> { } (str_view_test) == 2733878766136413408UL);
+
+		String str_test = "This is a test";
+		CHECK(Hash<String> { } (str_test) == 2733878766136413408UL);
+	}
+
+	TEST_CASE("TestHashCombine")
+	{
+		int val1 = 0;
+		uint64 val1_hash = Hash<int> { } (val1);
+		int val2 = 1;
+		uint64 val2_hash = Hash<int> { } (val2);
+
+		// Check non-commutative
+		uint64 seed1 = val1_hash;
+		HashCombine(seed1, val2);
+		uint64 seed2 = val2_hash;
+		HashCombine(seed2, val1);
+		CHECK(seed1 != seed2);
+
+		// Check that adding a 0 changes the hash
+		uint64 seed3 = val1_hash;
+		HashCombine(seed3, val1);
+		CHECK(seed3 != val1_hash);
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/InsertionSortTest.cpp

@@ -0,0 +1,96 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+
+#include <Jolt/Core/InsertionSort.h>
+
+TEST_SUITE("InsertionSortTest")
+{
+	TEST_CASE("TestOrderedArray")
+	{
+		Array<int> array;
+		for (int i = 0; i < 10; i++)
+			array.push_back(i);
+
+		InsertionSort(array.begin(), array.end());
+
+		for (int i = 0; i < 10; i++)
+			CHECK(array[i] == i);
+	}
+
+	TEST_CASE("TestOrderedArrayComparator")
+	{
+		Array<int> array;
+		for (int i = 0; i < 100; i++)
+			array.push_back(i);
+
+		InsertionSort(array.begin(), array.end(), greater<int> {});
+
+		for (int i = 0; i < 100; i++)
+			CHECK(array[i] == 99 - i);
+	}
+
+	TEST_CASE("TestReversedArray")
+	{
+		Array<int> array;
+		for (int i = 0; i < 10; i++)
+			array.push_back(9 - i);
+
+		InsertionSort(array.begin(), array.end());
+
+		for (int i = 0; i < 10; i++)
+			CHECK(array[i] == i);
+	}
+
+	TEST_CASE("TestRandomArray")
+	{
+		UnitTestRandom random;
+
+		Array<UnitTestRandom::result_type> array;
+		for (int i = 0; i < 100; i++)
+		{
+			UnitTestRandom::result_type value = random();
+
+			// Insert value at beginning
+			array.insert(array.begin(), value);
+
+			// Insert value at end
+			array.push_back(value);
+		}
+
+		InsertionSort(array.begin(), array.end());
+
+		for (Array<int>::size_type i = 0; i < array.size() - 2; i += 2)
+		{
+			// We inserted the same value twice so these elements should be the same
+			CHECK(array[i] == array[i + 1]);
+
+			// The next element should be bigger or equal
+			CHECK(array[i] <= array[i + 2]);
+		}
+	}
+
+	TEST_CASE("TestEmptyArray")
+	{
+		Array<int> array;
+		InsertionSort(array.begin(), array.end());
+		CHECK(array.empty());
+	}
+
+	TEST_CASE("Test1ElementArray")
+	{
+		Array<int> array { 1 };
+		InsertionSort(array.begin(), array.end());
+		CHECK(array[0] == 1);
+	}
+
+	TEST_CASE("Test2ElementArray")
+	{
+		Array<int> array { 2, 1 };
+		InsertionSort(array.begin(), array.end());
+		CHECK(array[0] == 1);
+		CHECK(array[1] == 2);
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/JobSystemTest.cpp

@@ -0,0 +1,91 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/JobSystemThreadPool.h>
+
+TEST_SUITE("JobSystemTest")
+{
+	TEST_CASE("TestJobSystemRunJobs")
+	{
+		// Create job system
+		const int cMaxJobs = 128;
+		const int cMaxBarriers = 10;
+		const int cMaxThreads = 10;
+		JobSystemThreadPool system(cMaxJobs, cMaxBarriers, cMaxThreads);
+
+		// Create array of zeros
+		atomic<uint32> values[cMaxJobs];
+		for (int i = 0; i < cMaxJobs; ++i)
+			values[i] = 0;
+
+		// Create a barrier
+		JobSystem::Barrier *barrier = system.CreateBarrier();
+
+		// Create jobs that will increment all values
+		for (int i = 0; i < cMaxJobs; ++i)
+		{
+			JobHandle handle = system.CreateJob("JobTest", Color::sRed, [&values, i] { values[i]++; });
+			barrier->AddJob(handle);
+		}
+
+		// Wait for the barrier to complete
+		system.WaitForJobs(barrier);
+
+		// Destroy our barrier
+		system.DestroyBarrier(barrier);
+
+		// Test all values are 1
+		for (int i = 0; i < cMaxJobs; ++i)
+			CHECK(values[i] == 1);
+	}
+
+	TEST_CASE("TestJobSystemRunChain")
+	{
+		// Create job system
+		const int cMaxJobs = 128;
+		const int cMaxBarriers = 10;
+		JobSystemThreadPool system(cMaxJobs, cMaxBarriers);
+
+		// Create a barrier
+		JobSystem::Barrier *barrier = system.CreateBarrier();
+
+		// Counter that keeps track of order in which jobs ran
+		atomic<uint32> counter = 1;
+
+		// Create array of zeros
+		atomic<uint32> values[cMaxJobs];
+		for (int i = 0; i < cMaxJobs; ++i)
+			values[i] = 0;
+
+		// Create jobs that will set sequence number
+		JobHandle handles[cMaxJobs];
+		for (int i = 0; i < cMaxJobs; ++i)
+		{
+			handles[i] = system.CreateJob("JobTestChain", Color::sRed, [&values, &counter, &handles, i] {
+				// Set sequence number
+				values[i] = counter++;
+
+				// Start previous job
+				if (i > 0)
+					handles[i - 1].RemoveDependency();
+			}, 1);
+
+			barrier->AddJob(handles[i]);
+		}
+
+		// Start the last job
+		handles[cMaxJobs - 1].RemoveDependency();
+
+		// Wait for the barrier to complete
+		system.WaitForJobs(barrier);
+
+		// Destroy our barrier
+		system.DestroyBarrier(barrier);
+
+		// Test jobs were executed in reverse order
+		for (int i = cMaxJobs - 1; i >= 0; --i)
+			CHECK(values[i] == cMaxJobs - i);
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/LinearCurveTest.cpp

@@ -0,0 +1,57 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/LinearCurve.h>
+
+TEST_SUITE("LinearCurveTest")
+{
+	TEST_CASE("Test0PointCurve")
+	{
+		LinearCurve curve;
+
+		CHECK(curve.GetValue(1.0f) == 0.0f);
+	}
+
+	TEST_CASE("Test1PointCurve")
+	{
+		LinearCurve curve;
+		curve.AddPoint(1.0f, 20.0f);
+
+		CHECK(curve.GetValue(0.9f) == 20.0f);
+		CHECK(curve.GetValue(1.0f) == 20.0f);
+		CHECK(curve.GetValue(1.1f) == 20.0f);
+	}
+
+	TEST_CASE("Test2PointCurve")
+	{
+		LinearCurve curve;
+		curve.AddPoint(-1.0f, 40.0f);
+		curve.AddPoint(-3.0f, 20.0f);
+		curve.Sort();
+
+		CHECK_APPROX_EQUAL(curve.GetValue(-3.1f), 20.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(-3.0f), 20.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(-2.0f), 30.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(-1.0f), 40.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(-0.9f), 40.0f);
+	}
+
+	TEST_CASE("Test3PointCurve")
+	{
+		LinearCurve curve;
+		curve.AddPoint(1.0f, 20.0f);
+		curve.AddPoint(5.0f, 60.0f);
+		curve.AddPoint(3.0f, 40.0f);
+		curve.Sort();
+
+		CHECK_APPROX_EQUAL(curve.GetValue(0.9f), 20.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(1.0f), 20.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(2.0f), 30.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(3.0f), 40.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(4.0f), 50.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(5.0f), 60.0f);
+		CHECK_APPROX_EQUAL(curve.GetValue(5.1f), 60.0f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/PreciseMathTest.cpp

@@ -0,0 +1,29 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <atomic>
+
+// Implemented as a global atomic so the compiler can't optimize it to a constant
+extern atomic<float> One, OneTenth, Ten;
+atomic<float> One = 1.0f, OneTenth = 0.1f /* Actually: 0.100000001f */, Ten = 10.0f;
+
+JPH_PRECISE_MATH_ON
+
+TEST_SUITE("PreciseMathTest")
+{
+	TEST_CASE("CheckNoFMA")
+	{
+		// Should not use the FMA instruction and end up being zero
+		// If FMA is active, a * b will not be rounded to 1.0f so the result will be a small positive number
+		float a = OneTenth, b = Ten, c = One;
+		float result = a * b - c;
+		CHECK(result == 0.0f);
+
+		// Compiler should not optimize these variables away
+		One = OneTenth = Ten = 2.0f;
+	}
+}
+
+JPH_PRECISE_MATH_OFF

lib/All/JoltPhysics/UnitTests/Core/QuickSortTest.cpp

@@ -0,0 +1,96 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+
+#include <Jolt/Core/QuickSort.h>
+
+TEST_SUITE("QuickSortTest")
+{
+	TEST_CASE("TestOrderedArray")
+	{
+		Array<int> array;
+		for (int i = 0; i < 100; i++)
+			array.push_back(i);
+
+		QuickSort(array.begin(), array.end());
+
+		for (int i = 0; i < 100; i++)
+			CHECK(array[i] == i);
+	}
+
+	TEST_CASE("TestOrderedArrayComparator")
+	{
+		Array<int> array;
+		for (int i = 0; i < 100; i++)
+			array.push_back(i);
+
+		QuickSort(array.begin(), array.end(), greater<int> {});
+
+		for (int i = 0; i < 100; i++)
+			CHECK(array[i] == 99 - i);
+	}
+
+	TEST_CASE("TestReversedArray")
+	{
+		Array<int> array;
+		for (int i = 0; i < 100; i++)
+			array.push_back(99 - i);
+
+		QuickSort(array.begin(), array.end());
+
+		for (int i = 0; i < 100; i++)
+			CHECK(array[i] == i);
+	}
+
+	TEST_CASE("TestRandomArray")
+	{
+		UnitTestRandom random;
+
+		Array<UnitTestRandom::result_type> array;
+		for (int i = 0; i < 1000; i++)
+		{
+			UnitTestRandom::result_type value = random();
+
+			// Insert value at beginning
+			array.insert(array.begin(), value);
+
+			// Insert value at end
+			array.push_back(value);
+		}
+
+		QuickSort(array.begin(), array.end());
+
+		for (Array<int>::size_type i = 0; i < array.size() - 2; i += 2)
+		{
+			// We inserted the same value twice so these elements should be the same
+			CHECK(array[i] == array[i + 1]);
+
+			// The next element should be bigger or equal
+			CHECK(array[i] <= array[i + 2]);
+		}
+	}
+
+	TEST_CASE("TestEmptyArray")
+	{
+		Array<int> array;
+		QuickSort(array.begin(), array.end());
+		CHECK(array.empty());
+	}
+
+	TEST_CASE("Test1ElementArray")
+	{
+		Array<int> array { 1 };
+		QuickSort(array.begin(), array.end());
+		CHECK(array[0] == 1);
+	}
+
+	TEST_CASE("Test2ElementArray")
+	{
+		Array<int> array { 2, 1 };
+		QuickSort(array.begin(), array.end());
+		CHECK(array[0] == 1);
+		CHECK(array[1] == 2);
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/STLLocalAllocatorTest.cpp

@@ -0,0 +1,215 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2025 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+
+#include <Jolt/Core/STLLocalAllocator.h>
+
+TEST_SUITE("STLLocalAllocatorTest")
+{
+	/// The number of elements in the local buffer
+	static constexpr size_t N = 20;
+
+#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+	template <class ArrayType>
+	static bool sIsLocal(ArrayType &inArray)
+	{
+	#ifdef JPH_USE_STD_VECTOR
+		// Check that the data pointer is within the array.
+		// Note that when using std::vector we cannot use get_allocator as that makes a copy of the allocator internally
+		// and we've disabled the copy constructor since our allocator cannot be copied.
+		const uint8 *data = reinterpret_cast<const uint8 *>(inArray.data());
+		const uint8 *array = reinterpret_cast<const uint8 *>(&inArray);
+		return data >= array && data < array + sizeof(inArray);
+	#else
+		return inArray.get_allocator().is_local(inArray.data());
+	#endif
+	}
+#endif
+
+	template <class ArrayType, bool NonTrivial>
+	static void sTestArray()
+	{
+		// Allocate so that we will run out of local memory and reallocate from heap at least once
+		ArrayType arr;
+		for (int i = 0; i < 64; ++i)
+			arr.push_back(i);
+		CHECK(arr.size() == 64);
+		for (int i = 0; i < 64; ++i)
+		{
+			CHECK(arr[i] == i);
+		#if !defined(JPH_USE_STD_VECTOR) && !defined(JPH_DISABLE_CUSTOM_ALLOCATOR)
+			// We only have to move elements once we run out of the local buffer, this happens as we resize
+			// from 16 to 32 elements, we'll reallocate again at 32 and 64
+			if constexpr (NonTrivial)
+				CHECK(arr[i].GetNonTriv() == (i < 16? 3 : (i < 32? 2 : 1)));
+		#endif
+		}
+		CHECK(IsAligned(arr.data(), alignof(typename ArrayType::value_type)));
+	#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+		CHECK(!sIsLocal(arr));
+	#endif
+
+		// Check that we can copy the array to another array
+		ArrayType arr2;
+		arr2 = arr;
+		for (int i = 0; i < 64; ++i)
+		{
+			CHECK(arr2[i] == i);
+			if constexpr (NonTrivial)
+				CHECK(arr2[i].GetNonTriv() == -999);
+		}
+		CHECK(IsAligned(arr2.data(), alignof(typename ArrayType::value_type)));
+	#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+		CHECK(!sIsLocal(arr2));
+	#endif
+
+		// Clear the array
+		arr.clear();
+		arr.shrink_to_fit();
+		CHECK(arr.size() == 0);
+	#ifndef JPH_USE_STD_VECTOR // Some implementations of std::vector ignore shrink_to_fit
+		CHECK(arr.capacity() == 0);
+		CHECK(arr.data() == nullptr);
+	#endif
+
+		// Allocate so we stay within the local buffer
+		for (int i = 0; i < 10; ++i)
+			arr.push_back(i);
+		CHECK(arr.size() == 10);
+		for (int i = 0; i < 10; ++i)
+		{
+			CHECK(arr[i] == i);
+		#if !defined(JPH_USE_STD_VECTOR) && !defined(JPH_DISABLE_CUSTOM_ALLOCATOR)
+			// We never need to move elements as they stay within the local buffer
+			if constexpr (NonTrivial)
+				CHECK(arr[i].GetNonTriv() == 1);
+		#endif
+		}
+		CHECK(IsAligned(arr.data(), alignof(typename ArrayType::value_type)));
+	#if !defined(JPH_USE_STD_VECTOR) && !defined(JPH_DISABLE_CUSTOM_ALLOCATOR) // Doesn't work with std::vector since it doesn't use the reallocate function and runs out of space
+		CHECK(sIsLocal(arr));
+	#endif
+
+		// Check that we can copy the array to the local buffer
+		ArrayType arr3;
+		arr3 = arr;
+		CHECK(arr3.size() == 10);
+		for (int i = 0; i < 10; ++i)
+		{
+			CHECK(arr3[i] == i);
+			if constexpr (NonTrivial)
+				CHECK(arr3[i].GetNonTriv() == -999);
+		}
+		CHECK(IsAligned(arr3.data(), alignof(typename ArrayType::value_type)));
+	#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+		CHECK(sIsLocal(arr3));
+	#endif
+
+		// Check that if we reserve the memory, that we can fully fill the array in local memory
+		ArrayType arr4;
+		arr4.reserve(N);
+		for (int i = 0; i < int(N); ++i)
+			arr4.push_back(i);
+		CHECK(arr4.size() == N);
+		CHECK(arr4.capacity() == N);
+		for (int i = 0; i < int(N); ++i)
+		{
+			CHECK(arr4[i] == i);
+			if constexpr (NonTrivial)
+				CHECK(arr4[i].GetNonTriv() == 1);
+		}
+		CHECK(IsAligned(arr4.data(), alignof(typename ArrayType::value_type)));
+	#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+		CHECK(sIsLocal(arr4));
+	#endif
+	}
+
+	TEST_CASE("TestAllocation")
+	{
+		using Allocator = STLLocalAllocator<int, N>;
+		using ArrayType = Array<int, Allocator>;
+	#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+		static_assert(AllocatorHasReallocate<Allocator>::sValue);
+	#endif
+
+		sTestArray<ArrayType, false>();
+	}
+
+	TEST_CASE("TestAllocationAligned")
+	{
+		// Force the need for an aligned allocation
+		struct alignas(64) Aligned
+		{
+						Aligned(int inValue)	: mValue(inValue) { }
+			operator	int() const				{ return mValue; }
+
+		private:
+			int			mValue;
+		};
+		static_assert(std::is_trivially_copyable<Aligned>());
+
+		using Allocator = STLLocalAllocator<Aligned, N>;
+		using ArrayType = Array<Aligned, Allocator>;
+	#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+		static_assert(AllocatorHasReallocate<Allocator>::sValue);
+	#endif
+
+		sTestArray<ArrayType, false>();
+	}
+
+	TEST_CASE("TestAllocationNonTrivial")
+	{
+		// Force non trivial copy constructor
+		struct NonTriv
+		{
+						NonTriv(int inValue)				: mValue(inValue) { }
+						NonTriv(const NonTriv &inRHS)		: mValue(inRHS.mValue), mMakeNonTriv(-999) { }
+						NonTriv(NonTriv &&inRHS)			: mValue(inRHS.mValue), mMakeNonTriv(inRHS.mMakeNonTriv + 1) { }
+			NonTriv &	operator = (const NonTriv &inRHS)	{ mValue = inRHS.mValue; mMakeNonTriv = -9999; return *this; }
+			operator	int() const							{ return mValue; }
+			int			GetNonTriv() const					{ return mMakeNonTriv; }
+
+		private:
+			int			mValue;
+			int			mMakeNonTriv = 0;
+		};
+		static_assert(!std::is_trivially_copyable<NonTriv>());
+
+		using Allocator = STLLocalAllocator<NonTriv, N>;
+		using ArrayType = Array<NonTriv, Allocator>;
+	#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+		static_assert(AllocatorHasReallocate<Allocator>::sValue);
+	#endif
+
+		sTestArray<ArrayType, true>();
+	}
+
+	TEST_CASE("TestAllocationAlignedNonTrivial")
+	{
+		// Force non trivial copy constructor
+		struct alignas(64) AlNonTriv
+		{
+						AlNonTriv(int inValue)				: mValue(inValue) { }
+						AlNonTriv(const AlNonTriv &inRHS)	: mValue(inRHS.mValue), mMakeNonTriv(-999) { }
+						AlNonTriv(AlNonTriv &&inRHS)		: mValue(inRHS.mValue), mMakeNonTriv(inRHS.mMakeNonTriv + 1) { }
+			AlNonTriv &	operator = (const AlNonTriv &inRHS) { mValue = inRHS.mValue; mMakeNonTriv = -9999; return *this; }
+			operator	int() const							{ return mValue; }
+			int			GetNonTriv() const					{ return mMakeNonTriv; }
+
+		private:
+			int			mValue;
+			int			mMakeNonTriv = 0;
+		};
+		static_assert(!std::is_trivially_copyable<AlNonTriv>());
+
+		using Allocator = STLLocalAllocator<AlNonTriv, N>;
+		using ArrayType = Array<AlNonTriv, Allocator>;
+	#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+		static_assert(AllocatorHasReallocate<Allocator>::sValue);
+	#endif
+
+		sTestArray<ArrayType, true>();
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/ScopeExitTest.cpp

@@ -0,0 +1,46 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/ScopeExit.h>
+
+TEST_SUITE("ScopeExitTest")
+{
+	TEST_CASE("TestScopeExitOrder")
+	{
+		int value = 0;
+		{
+			// Last created should be first destroyed
+			JPH_SCOPE_EXIT([&value]{ CHECK(value == 1); value = 2; });
+			JPH_SCOPE_EXIT([&value]{ CHECK(value == 0); value = 1; });
+			CHECK(value == 0);
+		}
+		CHECK(value == 2);
+	}
+
+	TEST_CASE("TestScopeExitRelease")
+	{
+		int value = 0;
+		{
+			ScopeExit scope_exit([&value]{ value++; });
+			CHECK(value == 0);
+			// Don't call the exit function anymore
+			scope_exit.Release();
+		}
+		CHECK(value == 0);
+	}
+
+	TEST_CASE("TestScopeExitInvoke")
+	{
+		int value = 0;
+		{
+			ScopeExit scope_exit([&value]{ value++; });
+			CHECK(value == 0);
+			scope_exit.Invoke();
+			CHECK(value == 1);
+			// Should not call again on exit
+		}
+		CHECK(value == 1);
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/StringToolsTest.cpp

@@ -0,0 +1,93 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/StringTools.h>
+
+TEST_SUITE("StringToolsTest")
+{
+	TEST_CASE("TestStringFormat")
+	{
+		CHECK(StringFormat("Test: %d", 1234) == "Test: 1234");
+	}
+
+	TEST_CASE("TestConvertToString")
+	{
+		CHECK(ConvertToString(1234) == "1234");
+		CHECK(ConvertToString(-1) == "-1");
+		CHECK(ConvertToString(0x7fffffffffffffffUL) == "9223372036854775807");
+	}
+
+	TEST_CASE("StringReplace")
+	{
+		JPH::String value = "Hello this si si a test";
+		StringReplace(value, "si", "is");
+		CHECK(value == "Hello this is is a test");
+		StringReplace(value, "is is", "is");
+		CHECK(value == "Hello this is a test");
+		StringReplace(value, "Hello", "Bye");
+		CHECK(value == "Bye this is a test");
+		StringReplace(value, "a test", "complete");
+		CHECK(value == "Bye this is complete");
+	}
+
+	TEST_CASE("StringToVector")
+	{
+		Array<JPH::String> value;
+		StringToVector("", value);
+		CHECK(value.empty());
+
+		StringToVector("a,b,c", value);
+		CHECK(value[0] == "a");
+		CHECK(value[1] == "b");
+		CHECK(value[2] == "c");
+
+		StringToVector("a,.b,.c,", value, ".");
+		CHECK(value[0] == "a,");
+		CHECK(value[1] == "b,");
+		CHECK(value[2] == "c,");
+	}
+
+	TEST_CASE("VectorToString")
+	{
+		Array<JPH::String> input;
+		JPH::String value;
+		VectorToString(input, value);
+		CHECK(value.empty());
+
+		input = { "a", "b", "c" };
+		VectorToString(input, value);
+		CHECK(value == "a,b,c");
+
+		VectorToString(input, value, ", ");
+		CHECK(value == "a, b, c");
+	}
+
+	TEST_CASE("ToLower")
+	{
+		CHECK(ToLower("123 HeLlO!") == "123 hello!");
+	}
+
+	TEST_CASE("NibbleToBinary")
+	{
+		CHECK(strcmp(NibbleToBinary(0b0000), "0000") == 0);
+		CHECK(strcmp(NibbleToBinary(0b0001), "0001") == 0);
+		CHECK(strcmp(NibbleToBinary(0b0010), "0010") == 0);
+		CHECK(strcmp(NibbleToBinary(0b0011), "0011") == 0);
+		CHECK(strcmp(NibbleToBinary(0b0100), "0100") == 0);
+		CHECK(strcmp(NibbleToBinary(0b0101), "0101") == 0);
+		CHECK(strcmp(NibbleToBinary(0b0110), "0110") == 0);
+		CHECK(strcmp(NibbleToBinary(0b0111), "0111") == 0);
+		CHECK(strcmp(NibbleToBinary(0b1000), "1000") == 0);
+		CHECK(strcmp(NibbleToBinary(0b1001), "1001") == 0);
+		CHECK(strcmp(NibbleToBinary(0b1010), "1010") == 0);
+		CHECK(strcmp(NibbleToBinary(0b1011), "1011") == 0);
+		CHECK(strcmp(NibbleToBinary(0b1100), "1100") == 0);
+		CHECK(strcmp(NibbleToBinary(0b1101), "1101") == 0);
+		CHECK(strcmp(NibbleToBinary(0b1110), "1110") == 0);
+		CHECK(strcmp(NibbleToBinary(0b1111), "1111") == 0);
+
+		CHECK(strcmp(NibbleToBinary(0xfffffff0), "0000") == 0);
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/UnorderedMapTest.cpp

@@ -0,0 +1,129 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+
+#include <Jolt/Core/UnorderedMap.h>
+
+TEST_SUITE("UnorderedMapTest")
+{
+	TEST_CASE("TestUnorderedMap")
+	{
+		UnorderedMap<int, int> map;
+		map.reserve(10);
+
+		// Insert some entries
+		CHECK(map.insert({ 1, 2 }).first->first == 1);
+		CHECK(map.insert({ 3, 4 }).second);
+		CHECK(!map.insert({ 3, 5 }).second);
+		CHECK(map.size() == 2);
+		CHECK(map.find(1)->second == 2);
+		CHECK(map.find(3)->second == 4);
+		CHECK(map.find(5) == map.end());
+
+		// Use operator []
+		map[5] = 6;
+		CHECK(map.size() == 3);
+		CHECK(map.find(5)->second == 6);
+		map[5] = 7;
+		CHECK(map.size() == 3);
+		CHECK(map.find(5)->second == 7);
+
+		// Validate all elements are visited by a visitor
+		int count = 0;
+		bool visited[10] = { false };
+		for (UnorderedMap<int, int>::const_iterator i = map.begin(); i != map.end(); ++i)
+		{
+			visited[i->first] = true;
+			++count;
+		}
+		CHECK(count == 3);
+		CHECK(visited[1]);
+		CHECK(visited[3]);
+		CHECK(visited[5]);
+		for (UnorderedMap<int, int>::iterator i = map.begin(); i != map.end(); ++i)
+		{
+			visited[i->first] = false;
+			--count;
+		}
+		CHECK(count == 0);
+		CHECK(!visited[1]);
+		CHECK(!visited[3]);
+		CHECK(!visited[5]);
+
+		// Copy the map
+		UnorderedMap<int, int> map2;
+		map2 = map;
+		CHECK(map2.find(1)->second == 2);
+		CHECK(map2.find(3)->second == 4);
+		CHECK(map2.find(5)->second == 7);
+		CHECK(map2.find(7) == map2.end());
+
+		// Try emplace
+		map.try_emplace(7, 8);
+		CHECK(map.size() == 4);
+		CHECK(map.find(7)->second == 8);
+
+		// Swap
+		UnorderedMap<int, int> map3;
+		map3.swap(map);
+		CHECK(map3.find(1)->second == 2);
+		CHECK(map3.find(3)->second == 4);
+		CHECK(map3.find(5)->second == 7);
+		CHECK(map3.find(7)->second == 8);
+		CHECK(map3.find(9) == map3.end());
+		CHECK(map.empty());
+
+		// Move construct
+		UnorderedMap<int, int> map4(std::move(map3));
+		CHECK(map4.find(1)->second == 2);
+		CHECK(map4.find(3)->second == 4);
+		CHECK(map4.find(5)->second == 7);
+		CHECK(map4.find(7)->second == 8);
+		CHECK(map4.find(9) == map4.end());
+		CHECK(map3.empty());
+	}
+
+	TEST_CASE("TestUnorderedMapGrow")
+	{
+		UnorderedMap<int, int> map;
+		for (int i = 0; i < 10000; ++i)
+			CHECK(map.try_emplace(i, ~i).second);
+
+		CHECK(map.size() == 10000);
+
+		for (int i = 0; i < 10000; ++i)
+			CHECK(map.find(i)->second == ~i);
+
+		CHECK(map.find(10001) == map.end());
+
+		for (int i = 0; i < 5000; ++i)
+			CHECK(map.erase(i) == 1);
+
+		CHECK(map.size() == 5000);
+
+		for (int i = 0; i < 5000; ++i)
+			CHECK(map.find(i) == map.end());
+
+		for (int i = 5000; i < 10000; ++i)
+			CHECK(map.find(i)->second == ~i);
+
+		CHECK(map.find(10001) == map.end());
+
+		for (int i = 0; i < 5000; ++i)
+			CHECK(map.try_emplace(i, i + 1).second);
+
+		CHECK(!map.try_emplace(0, 0).second);
+
+		CHECK(map.size() == 10000);
+
+		for (int i = 0; i < 5000; ++i)
+			CHECK(map.find(i)->second == i + 1);
+
+		for (int i = 5000; i < 10000; ++i)
+			CHECK(map.find(i)->second == ~i);
+
+		CHECK(map.find(10001) == map.end());
+	}
+}

lib/All/JoltPhysics/UnitTests/Core/UnorderedSetTest.cpp

@@ -0,0 +1,333 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+
+#include <Jolt/Core/UnorderedSet.h>
+
+TEST_SUITE("UnorderedSetTest")
+{
+	TEST_CASE("TestUnorderedSet")
+	{
+		UnorderedSet<int> set;
+		CHECK(set.bucket_count() == 0);
+		set.reserve(10);
+		CHECK(set.bucket_count() == 16);
+
+		// Check system limits
+		CHECK(set.max_bucket_count() == 0x80000000);
+		CHECK(set.max_size() == uint64(0x80000000) * 7 / 8);
+
+		// Insert some entries
+		CHECK(*set.insert(1).first == 1);
+		CHECK(set.insert(3).second);
+		CHECK(!set.insert(3).second);
+		CHECK(set.size() == 2);
+		CHECK(*set.find(1) == 1);
+		CHECK(*set.find(3) == 3);
+		CHECK(set.find(5) == set.cend());
+
+		// Validate all elements are visited by a visitor
+		int count = 0;
+		bool visited[10] = { false };
+		for (UnorderedSet<int>::const_iterator i = set.begin(); i != set.end(); ++i)
+		{
+			visited[*i] = true;
+			++count;
+		}
+		CHECK(count == 2);
+		CHECK(visited[1]);
+		CHECK(visited[3]);
+		for (UnorderedSet<int>::iterator i = set.begin(); i != set.end(); ++i)
+		{
+			visited[*i] = false;
+			--count;
+		}
+		CHECK(count == 0);
+		CHECK(!visited[1]);
+		CHECK(!visited[3]);
+
+		// Copy the set
+		UnorderedSet<int> set2;
+		set2 = set;
+		CHECK(*set2.find(1) == 1);
+		CHECK(*set2.find(3) == 3);
+		CHECK(set2.find(5) == set2.cend());
+
+		// Swap
+		UnorderedSet<int> set3;
+		set3.swap(set);
+		CHECK(*set3.find(1) == 1);
+		CHECK(*set3.find(3) == 3);
+		CHECK(set3.find(5) == set3.end());
+		CHECK(set.empty());
+
+		// Move construct
+		UnorderedSet<int> set4(std::move(set3));
+		CHECK(*set4.find(1) == 1);
+		CHECK(*set4.find(3) == 3);
+		CHECK(set4.find(5) == set4.end());
+		CHECK(set3.empty());
+
+		// Move assign
+		UnorderedSet<int> set5;
+		set5.insert(999);
+		CHECK(*set5.find(999) == 999);
+		set5 = std::move(set4);
+		CHECK(set5.find(999) == set5.end());
+		CHECK(*set5.find(1) == 1);
+		CHECK(*set5.find(3) == 3);
+		CHECK(set4.empty());
+	}
+
+	TEST_CASE("TestUnorderedSetGrow")
+	{
+		UnorderedSet<int> set;
+		for (int i = 0; i < 10000; ++i)
+			CHECK(set.insert(i).second);
+
+		CHECK(set.size() == 10000);
+
+		for (int i = 0; i < 10000; ++i)
+			CHECK(*set.find(i) == i);
+
+		CHECK(set.find(10001) == set.cend());
+
+		for (int i = 0; i < 5000; ++i)
+			CHECK(set.erase(i) == 1);
+
+		CHECK(set.size() == 5000);
+
+		for (int i = 0; i < 5000; ++i)
+			CHECK(set.find(i) == set.end());
+
+		for (int i = 5000; i < 10000; ++i)
+			CHECK(*set.find(i) == i);
+
+		CHECK(set.find(10001) == set.cend());
+
+		for (int i = 0; i < 5000; ++i)
+			CHECK(set.insert(i).second);
+
+		CHECK(!set.insert(0).second);
+
+		CHECK(set.size() == 10000);
+
+		for (int i = 0; i < 10000; ++i)
+			CHECK(*set.find(i) == i);
+
+		CHECK(set.find(10001) == set.cend());
+	}
+
+	TEST_CASE("TestUnorderedSetHashCollision")
+	{
+		// A hash function that's guaranteed to collide
+		class MyBadHash
+		{
+		public:
+			size_t operator () (int inValue) const
+			{
+				return 0;
+			}
+		};
+
+		UnorderedSet<int, MyBadHash> set;
+		for (int i = 0; i < 10; ++i)
+			CHECK(set.insert(i).second);
+
+		CHECK(set.size() == 10);
+
+		for (int i = 0; i < 10; ++i)
+			CHECK(*set.find(i) == i);
+
+		CHECK(set.find(11) == set.cend());
+
+		for (int i = 0; i < 5; ++i)
+			CHECK(set.erase(i) == 1);
+
+		CHECK(set.size() == 5);
+
+		for (int i = 0; i < 5; ++i)
+			CHECK(set.find(i) == set.end());
+
+		for (int i = 5; i < 10; ++i)
+			CHECK(*set.find(i) == i);
+
+		CHECK(set.find(11) == set.cend());
+
+		for (int i = 0; i < 5; ++i)
+			CHECK(set.insert(i).second);
+
+		CHECK(!set.insert(0).second);
+
+		CHECK(set.size() == 10);
+
+		for (int i = 0; i < 10; ++i)
+			CHECK(*set.find(i) == i);
+
+		CHECK(set.find(11) == set.cend());
+	}
+
+	TEST_CASE("TestUnorderedSetAddRemoveCyles")
+	{
+		UnorderedSet<int> set;
+		constexpr int cBucketCount = 64;
+		set.reserve(int(set.max_load_factor() * cBucketCount));
+		CHECK(set.bucket_count() == cBucketCount);
+
+		// Repeatedly add and remove elements to see if the set cleans up tombstones
+		constexpr int cNumElements = 64 * 6 / 8; // We make sure that the map is max 6/8 full to ensure that we never grow the map but rehash it instead
+		int add_counter = 0;
+		int remove_counter = 0;
+		for (int i = 0; i < 100; ++i)
+		{
+			for (int j = 0; j < cNumElements; ++j)
+			{
+				CHECK(set.find(add_counter) == set.end());
+				CHECK(set.insert(add_counter).second);
+				CHECK(set.find(add_counter) != set.end());
+				++add_counter;
+			}
+
+			CHECK(set.size() == cNumElements);
+
+			for (int j = 0; j < cNumElements; ++j)
+			{
+				CHECK(set.find(remove_counter) != set.end());
+				CHECK(set.erase(remove_counter) == 1);
+				CHECK(set.erase(remove_counter) == 0);
+				CHECK(set.find(remove_counter) == set.end());
+				++remove_counter;
+			}
+
+			CHECK(set.size() == 0);
+			CHECK(set.empty());
+		}
+
+		// Test that adding and removing didn't resize the set
+		CHECK(set.bucket_count() == cBucketCount);
+	}
+
+	TEST_CASE("TestUnorderedSetManyTombStones")
+	{
+		// A hash function that makes sure that consecutive ints end up in consecutive buckets starting at bucket 63
+		class MyBadHash
+		{
+		public:
+			size_t operator () (int inValue) const
+			{
+				return (inValue + 63) << 7;
+			}
+		};
+
+		UnorderedSet<int, MyBadHash> set;
+		constexpr int cBucketCount = 64;
+		set.reserve(int(set.max_load_factor() * cBucketCount));
+		CHECK(set.bucket_count() == cBucketCount);
+
+		// Fill 32 buckets
+		int add_counter = 0;
+		for (int i = 0; i < 32; ++i)
+			CHECK(set.insert(add_counter++).second);
+
+		// Since we control the hash, we know in which order we'll visit the elements
+		// The first element was inserted in bucket 63, so we start at 1
+		int expected = 1;
+		for (int i : set)
+		{
+			CHECK(i == expected);
+			expected = (expected + 1) & 31;
+		}
+		expected = 1;
+		for (int i : set)
+		{
+			CHECK(i == expected);
+			expected = (expected + 1) & 31;
+		}
+
+		// Remove a bucket in the middle with so that the number of occupied slots
+		// surrounding the bucket exceed 16 to force creating a tombstone,
+		// then add one at the end
+		int remove_counter = 16;
+		for (int i = 0; i < 100; ++i)
+		{
+			CHECK(set.find(remove_counter) != set.end());
+			CHECK(set.erase(remove_counter) == 1);
+			CHECK(set.find(remove_counter) == set.end());
+
+			CHECK(set.find(add_counter) == set.end());
+			CHECK(set.insert(add_counter).second);
+			CHECK(set.find(add_counter) != set.end());
+
+			++add_counter;
+			++remove_counter;
+		}
+
+		// Check that the elements we inserted are still there
+		CHECK(set.size() == 32);
+		for (int i = 0; i < 16; ++i)
+			CHECK(*set.find(i) == i);
+		for (int i = 0; i < 16; ++i)
+			CHECK(*set.find(add_counter - 1 - i) == add_counter - 1 - i);
+
+		// Test that adding and removing didn't resize the set
+		CHECK(set.bucket_count() == cBucketCount);
+	}
+
+	static bool sReversedHash = false;
+
+	TEST_CASE("TestUnorderedSetRehash")
+	{
+		// A hash function for which we can switch the hashing algorithm
+		class MyBadHash
+		{
+		public:
+			size_t operator () (int inValue) const
+			{
+				return (sReversedHash? 127 - inValue : inValue) << 7;
+			}
+		};
+
+		using Set = UnorderedSet<int, MyBadHash>;
+		Set set;
+		constexpr int cBucketCount = 128;
+		set.reserve(int(set.max_load_factor() * cBucketCount));
+		CHECK(set.bucket_count() == cBucketCount);
+
+		// Fill buckets
+		sReversedHash = false;
+		constexpr int cNumElements = 96;
+		for (int i = 0; i < cNumElements; ++i)
+			CHECK(set.insert(i).second);
+
+		// Check that we get the elements in the expected order
+		int expected = 0;
+		for (int i : set)
+			CHECK(i == expected++);
+
+		// Change the hashing algorithm so that a rehash is forced to move elements.
+		// The test is designed in such a way that it will both need to move elements to empty slots
+		// and to move elements to slots that currently already have another element.
+		sReversedHash = true;
+		set.rehash(0);
+
+		// Check that all elements are still there
+		for (int i = 0; i < cNumElements; ++i)
+			CHECK(*set.find(i) == i);
+
+		// The hash went from filling buckets 0 .. 95 with values 0 .. 95 to bucket 127 .. 31 with values 0 .. 95
+		// However, we don't move elements if they still fall within the same batch, this means that the first 8
+		// elements didn't move
+		Set::const_iterator it = set.begin();
+		for (int i = 0; i < 8; ++i, ++it)
+			CHECK(*it == i);
+
+		// The rest will have been reversed
+		for (int i = 95; i > 7; --i, ++it)
+			CHECK(*it == i);
+
+		// Test that adding and removing didn't resize the set
+		CHECK(set.bucket_count() == cBucketCount);
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/ClosestPointTests.cpp

@@ -0,0 +1,112 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/ClosestPoint.h>
+
+TEST_SUITE("ClosestPointTests")
+{
+	// Test closest point from inPoint to triangle (inA, inB, inC)
+	inline static void TestClosestPointToTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inPoint, Vec3Arg inExpectedClosestPoint, uint32 inExpectedSet)
+	{
+		// Make triangle relative to inPoint so we can get the closest point to the origin
+		Vec3 a = inA - inPoint;
+		Vec3 b = inB - inPoint;
+		Vec3 c = inC - inPoint;
+
+		// Extract bits for A, B and C
+		uint32 expected_a = inExpectedSet & 1;
+		uint32 expected_b = (inExpectedSet & 2) >> 1;
+		uint32 expected_c = (inExpectedSet & 4) >> 2;
+
+		// Test all permutations of ABC
+		uint32 set = 0;
+		Vec3 closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(a, b, c, set);
+		CHECK(set == inExpectedSet);
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(a, c, b, set);
+		CHECK(set == ((expected_b << 2) | (expected_c << 1) | expected_a));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(b, a, c, set);
+		CHECK(set == ((expected_c << 2) | (expected_a << 1) | expected_b));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(b, c, a, set);
+		CHECK(set == ((expected_a << 2) | (expected_c << 1) | expected_b));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(c, a, b, set);
+		CHECK(set == ((expected_b << 2) | (expected_a << 1) | expected_c));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(c, b, a, set);
+		CHECK(set == ((expected_a << 2) | (expected_b << 1) | expected_c));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+	}
+
+	TEST_CASE("TestLongTriangle")
+	{
+		Vec3 a(100, 1, 0);
+		Vec3 b(100, 1, 1);
+		Vec3 c(-100, 1, 0);
+
+		// Test interior
+		TestClosestPointToTriangle(a, b, c, Vec3(0, 0, 0.1f), Vec3(0, 1, 0.1f), 0b0111);
+
+		// Edge AB
+		TestClosestPointToTriangle(a, b, c, Vec3(101, 0, 0.5f), Vec3(100, 1, 0.5f), 0b0011);
+
+		// Edge AC
+		TestClosestPointToTriangle(a, b, c, Vec3(0, 0, -0.1f), Vec3(0, 1, 0), 0b0101);
+
+		// Edge BC
+		Vec3 point_bc(0, 0, 1);
+		Vec3 bc = c - b;
+		Vec3 closest_bc = b + ((point_bc - b).Dot(bc) / bc.LengthSq()) * bc;
+		TestClosestPointToTriangle(a, b, c, point_bc, closest_bc, 0b0110);
+
+		// Vertex A
+		TestClosestPointToTriangle(a, b, c, Vec3(101, 0, -1), a, 0b0001);
+
+		// Vertex B
+		TestClosestPointToTriangle(a, b, c, Vec3(101, 0, 2), b, 0b0010);
+
+		// Vertex C
+		TestClosestPointToTriangle(a, b, c, Vec3(-101, 0, 0), c, 0b0100);
+	}
+
+	TEST_CASE("TestNearColinearTriangle")
+	{
+		// A very long triangle that is nearly colinear
+		Vec3 a(99.9999847f, 0.946687222f, 99.9999847f);
+		Vec3 b(-100.010002f, 0.977360725f, -100.010002f);
+		Vec3 c(-100.000137f, 0.977310658f, -100.000137f);
+
+		// Closest point is on edge AC
+		Vec3 ac = c - a;
+		Vec3 expected_closest = a + (-a.Dot(ac) / ac.LengthSq()) * ac;
+
+		TestClosestPointToTriangle(a, b, c, Vec3::sZero(), expected_closest, 0b0101);
+	}
+
+	TEST_CASE("TestSmallTriangleWithPlaneGoingThroughOrigin")
+	{
+		// A small but non-degenerate triangle whose plane almost goes through the origin
+		Vec3 a(-0.132395342f, -0.294095188f, -0.164812326f);
+		Vec3 b(-0.126054004f, -0.283950001f, -0.159065604f);
+		Vec3 c(-0.154956535f, -0.284792334f, -0.160523415f);
+
+		float u, v, w;
+		ClosestPoint::GetBaryCentricCoordinates(a, b, c, u, v, w);
+
+		// Closest point should be close to origin
+		Vec3 p = a * u + b * v + c * w;
+		CHECK_APPROX_EQUAL(p, Vec3::sZero());
+
+		// Closest point should be outside triangle
+		CHECK((u < 0.0f || v > 0.0f || w < 0.0f));
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/ConvexHullBuilderTest.cpp

@@ -0,0 +1,199 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/ConvexHullBuilder.h>
+
+TEST_SUITE("ConvexHullBuilderTest")
+{
+	constexpr float cTolerance = 1.0e-3f;
+	using Face = ConvexHullBuilder::Face;
+	using Positions = ConvexHullBuilder::Positions;
+
+	TEST_CASE("TestDegenerate")
+	{
+		const char *error = nullptr;
+
+		{
+			// Too few points / coinciding points should be degenerate
+			Positions positions { Vec3(1, 2, 3) };
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::TooFewPoints);
+			positions.push_back(Vec3(1 + 0.5f * cTolerance, 2, 3));
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::TooFewPoints);
+			positions.push_back(Vec3(1, 2 + 0.5f * cTolerance, 3));
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Degenerate);
+			positions.push_back(Vec3(1, 2, 3 + 0.5f * cTolerance));
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Degenerate);
+		}
+
+		{
+			// A line should be degenerate
+			Positions positions;
+			for (float v = 0.0f; v < 1.01f; v += 0.1f)
+				positions.push_back(Vec3(v, 0, 0));
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Degenerate);
+		}
+	}
+
+	TEST_CASE("Test2DHull")
+	{
+		const char *error = nullptr;
+
+		{
+			// A triangle
+			Positions positions { Vec3(-1, 0, -1), Vec3(1, 0, -1), Vec3(-1, 0, 1) };
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 3);
+			CHECK(builder.GetFaces().size() == 2);
+			CHECK(builder.ContainsFace({ 0, 1, 2 }));
+			CHECK(builder.ContainsFace({ 2, 1, 0 }));
+		}
+
+		{
+			// A quad with many interior points
+			Positions positions;
+			for (int x = 0; x < 10; ++x)
+				for (int z = 0; z < 10; ++z)
+					positions.push_back(Vec3(0.1f * x, 0, 1.0f * 0.2f * z));
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 4);
+			CHECK(builder.GetFaces().size() == 2);
+			CHECK(builder.ContainsFace({ 0, 9, 99, 90 }));
+			CHECK(builder.ContainsFace({ 90, 99, 9, 0 }));
+		}
+
+		{
+			// Add disc with many interior points
+			Positions positions;
+			for (int r = 0; r < 10; ++r)
+				for (int phi = 0; phi < 10; ++phi)
+				{
+					float f_r = 2.0f * r;
+					float f_phi = 2.0f * JPH_PI * phi / 10;
+					positions.push_back(Vec3(f_r * Cos(f_phi), f_r * Sin(f_phi), 0));
+				}
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 10);
+			CHECK(builder.GetFaces().size() == 2);
+			CHECK(builder.ContainsFace({ 90, 91, 92, 93, 94, 95, 96, 97, 98, 99 }));
+			CHECK(builder.ContainsFace({ 99, 98, 97, 96, 95, 94, 93, 92, 91, 90 }));
+		}
+	}
+
+	TEST_CASE("Test3DHull")
+	{
+		const char *error = nullptr;
+
+		{
+			// A cube with lots of interior points
+			Positions positions;
+			for (int x = 0; x < 10; ++x)
+				for (int y = 0; y < 10; ++y)
+					for (int z = 0; z < 10; ++z)
+					positions.push_back(Vec3(0.1f * x, 1.0f + 0.2f * y, 2.0f * 0.3f * z));
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 8);
+			CHECK(builder.GetFaces().size() == 6);
+			CHECK(builder.ContainsFace({ 0, 9, 99, 90 }));
+			CHECK(builder.ContainsFace({ 0, 90, 990, 900 }));
+			CHECK(builder.ContainsFace({ 900, 990, 999, 909 }));
+			CHECK(builder.ContainsFace({ 9, 909, 999, 99 }));
+			CHECK(builder.ContainsFace({ 90, 99, 999, 990 }));
+			CHECK(builder.ContainsFace({ 0, 900, 909, 9 }));
+		}
+
+		{
+			// Add sphere with many interior points
+			Positions positions;
+			for (int r = 0; r < 10; ++r)
+				for (int phi = 0; phi < 10; ++phi)
+					for (int theta = 0; theta < 10; ++theta)
+					{
+						float f_r = 2.0f * r;
+						float f_phi = 2.0f * JPH_PI * phi / 10; // [0, 2 PI)
+						float f_theta = JPH_PI * theta / 9; // [0, PI] (inclusive!)
+						positions.push_back(f_r * Vec3::sUnitSpherical(f_theta, f_phi));
+					}
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 82); // The two ends of the sphere have 10 points that have the same position
+
+			// Too many faces, calculate the error instead
+			Face *error_face;
+			float max_error, coplanar_distance;
+			int error_position_idx;
+			builder.DetermineMaxError(error_face, max_error, error_position_idx, coplanar_distance);
+			CHECK(max_error < max(coplanar_distance, cTolerance));
+		}
+	}
+
+	TEST_CASE("TestRandomHull")
+	{
+		const char *error = nullptr;
+
+		UnitTestRandom random(0x1ee7c0de);
+
+		uniform_real_distribution<float> zero_one(0.0f, 1.0f);
+		uniform_real_distribution<float> zero_two(0.0f, 2.0f);
+		uniform_real_distribution<float> scale_start(0.1f, 0.5f);
+		uniform_real_distribution<float> scale_range(0.1f, 2.0f);
+
+		for (int iteration = 0; iteration < 100; ++iteration)
+		{
+			// Define vertex scale
+			float start = scale_start(random);
+			uniform_real_distribution<float> vertex_scale(start, start + scale_range(random));
+
+			// Define shape scale to make shape less sphere like
+			uniform_real_distribution<float> shape_scale(0.1f, 1.0f);
+			Vec3 scale(shape_scale(random), shape_scale(random), shape_scale(random));
+
+			// Add some random points
+			Positions positions;
+			for (int i = 0; i < 100; ++i)
+			{
+				// Add random point
+				Vec3 p1 = vertex_scale(random) * Vec3::sRandom(random) * scale;
+				positions.push_back(p1);
+
+				// Point close to p1
+				Vec3 p2 = p1 + cTolerance * zero_two(random) * Vec3::sRandom(random);
+				positions.push_back(p2);
+
+				// Point on a line to another point
+				float fraction = zero_one(random);
+				Vec3 p3 = fraction * p1 + (1.0f - fraction) * positions[random() % positions.size()];
+				positions.push_back(p3);
+
+				// Point close to p3
+				Vec3 p4 = p3 + cTolerance * zero_two(random) * Vec3::sRandom(random);
+				positions.push_back(p4);
+			}
+
+			// Build hull
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+
+			// Calculate error
+			Face *error_face;
+			float max_error, coplanar_distance;
+			int error_position_idx;
+			builder.DetermineMaxError(error_face, max_error, error_position_idx, coplanar_distance);
+			CHECK(max_error < max(coplanar_distance, 1.2f * cTolerance));
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/EPATests.cpp

@@ -0,0 +1,215 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/Sphere.h>
+#include <random>
+
+// Enable to trace accuracy of EPA algorithm
+#define EPA_TESTS_TRACE(...)
+//#define EPA_TESTS_TRACE(...) printf(__VA_ARGS__)
+
+TEST_SUITE("EPATests")
+{
+	/// Helper function to return the angle between two vectors in degrees
+	static float AngleBetweenVectors(Vec3Arg inV1, Vec3Arg inV2)
+	{
+		float dot = inV1.Dot(inV2);
+		float len = inV1.Length() * inV2.Length();
+		return RadiansToDegrees(ACos(dot / len));
+	}
+
+	/// Test box versus sphere and compare analytical solution with that of the EPA algorithm
+	/// @return If a collision was detected
+	static bool CollideBoxSphere(Mat44Arg inMatrix, const AABox &inBox, const Sphere &inSphere)
+	{
+		TransformedConvexObject transformed_box(inMatrix, inBox);
+		TransformedConvexObject transformed_sphere(inMatrix, inSphere);
+
+		// Use EPA algorithm. Don't use convex radius to avoid EPA being skipped because the inner hulls are not touching.
+		EPAPenetrationDepth epa;
+		Vec3 v1 = Vec3::sAxisX(), pa1, pb1;
+		bool intersect1 = epa.GetPenetrationDepth(transformed_box, transformed_box, 0.0f, transformed_sphere, transformed_sphere, 0.0f, 1.0e-2f, FLT_EPSILON, v1, pa1, pb1);
+
+		// Analytical solution
+		Vec3 pa2 = inBox.GetClosestPoint(inSphere.GetCenter());
+		Vec3 v2 = inSphere.GetCenter() - pa2;
+		bool intersect2 = v2.LengthSq() <= Square(inSphere.GetRadius());
+
+		CHECK(intersect1 == intersect2);
+		if (intersect1 && intersect2)
+		{
+			// Analytical solution of contact on B
+			Vec3 pb2 = inSphere.GetCenter() - inSphere.GetRadius() * v2.NormalizedOr(Vec3::sZero());
+
+			// Transform analytical solution
+			v2 = inMatrix.Multiply3x3(v2);
+			pa2 = inMatrix * pa2;
+			pb2 = inMatrix * pb2;
+
+			// Check angle between v1 and v2
+			float angle = AngleBetweenVectors(v1, v2);
+			CHECK(angle < 0.1f);
+			EPA_TESTS_TRACE("Angle = %.9g\n", (double)angle);
+
+			// Check delta between contact on A
+			Vec3 dpa = pa2 - pa1;
+			CHECK(dpa.Length() < 8.0e-4f);
+			EPA_TESTS_TRACE("Delta A = %.9g\n", (double)dpa.Length());
+
+			// Check delta between contact on B
+			Vec3 dpb = pb2 - pb1;
+			CHECK(dpb.Length() < 8.0e-4f);
+			EPA_TESTS_TRACE("Delta B = %.9g\n", (double)dpb.Length());
+		}
+
+		return intersect1;
+	}
+
+	/// Test multiple boxes against spheres and transform both with inMatrix
+	static void CollideBoxesWithSpheres(Mat44Arg inMatrix)
+	{
+		{
+			// Sphere just missing face of box
+			AABox box(Vec3(-2, -3, -4), Vec3(2, 3, 4));
+			Sphere sphere(Vec3(4, 0, 0), 1.99f);
+			CHECK(!CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just touching face of box
+			AABox box(Vec3(-2, -3, -4), Vec3(2, 3, 4));
+			Sphere sphere(Vec3(4, 0, 0), 2.01f);
+			CHECK(CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere deeply penetrating box on face
+			AABox box(Vec3(-2, -3, -4), Vec3(2, 3, 4));
+			Sphere sphere(Vec3(3, 0, 0), 2);
+			CHECK(CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just missing box on edge
+			AABox box(Vec3(1, 1, -2), Vec3(2, 2, 2));
+			Sphere sphere(Vec3(4, 4, 0), sqrt(8.0f) - 0.01f);
+			CHECK(!CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just penetrating box on edge
+			AABox box(Vec3(1, 1, -2), Vec3(2, 2, 2));
+			Sphere sphere(Vec3(4, 4, 0), sqrt(8.0f) + 0.01f);
+			CHECK(CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just missing box on vertex
+			AABox box(Vec3(1, 1, 1), Vec3(2, 2, 2));
+			Sphere sphere(Vec3(4, 4, 4), sqrt(12.0f) - 0.01f);
+			CHECK(!CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just penetrating box on vertex
+			AABox box(Vec3(1, 1, 1), Vec3(2, 2, 2));
+			Sphere sphere(Vec3(4, 4, 4), sqrt(12.0f) + 0.01f);
+			CHECK(CollideBoxSphere(inMatrix, box, sphere));
+		}
+	}
+
+	TEST_CASE("TestEPASphereBox")
+	{
+		// Test identity transform
+		CollideBoxesWithSpheres(Mat44::sIdentity());
+
+		// Test some random rotations/translations
+		UnitTestRandom random;
+		for (int i = 0; i < 10; ++i)
+			CollideBoxesWithSpheres(Mat44::sRotationTranslation(Quat::sRandom(random), Vec3::sRandom(random)));
+	}
+
+	TEST_CASE("TestEPASphereSphereOverlapping")
+	{
+		// Worst case: Two spheres exactly overlapping
+		// In this case the Minkowski sum is a sphere which means the EPA algorithm will be building a convex hull of a full sphere and run out of triangles resulting in a pretty bad approximation
+		Sphere sphere(Vec3(1, 2, 3), 2.0f);
+		EPAPenetrationDepth epa;
+		Vec3 v = Vec3::sAxisX(), pa, pb;
+		CHECK(epa.GetPenetrationDepth(sphere, sphere, 0.0f, sphere, sphere, 0.0f, 1.0e-4f, FLT_EPSILON, v, pa, pb));
+		float delta_a = (pa - sphere.GetCenter()).Length() - sphere.GetRadius();
+		CHECK(abs(delta_a) < 0.07f);
+		float delta_b = (pb - sphere.GetCenter()).Length() - sphere.GetRadius();
+		CHECK(abs(delta_b) < 0.07f);
+		float delta_penetration = (pa - pb).Length() - 2.0f * sphere.GetRadius();
+		CHECK(abs(delta_penetration) < 0.14f);
+		float angle = AngleBetweenVectors(v, pa - pb);
+		CHECK(angle < 0.02f);
+	}
+
+	TEST_CASE("TestEPASphereSphereNearOverlapping")
+	{
+		// Near worst case: Two spheres almost exactly overlapping
+		// Still limited by amount of triangles in the hull but more precise
+		Sphere sphere1(Vec3(1, 2, 3), 2.0f);
+		Sphere sphere2(Vec3(1.1f, 2, 3), 1.8f);
+		EPAPenetrationDepth epa;
+		Vec3 v = Vec3::sAxisX(), pa, pb;
+		CHECK(epa.GetPenetrationDepth(sphere1, sphere1, 0.0f, sphere2, sphere2, 0.0f, 1.0e-4f, FLT_EPSILON, v, pa, pb));
+		float delta_a = (pa - sphere1.GetCenter()).Length() - sphere1.GetRadius();
+		CHECK(abs(delta_a) < 0.05f);
+		float delta_b = (pb - sphere2.GetCenter()).Length() - sphere2.GetRadius();
+		CHECK(abs(delta_b) < 0.05f);
+		float delta_penetration = (pa - pb).Length() - (sphere1.GetRadius() + sphere2.GetRadius() - (sphere1.GetCenter() - sphere2.GetCenter()).Length());
+		CHECK(abs(delta_penetration) < 0.06f);
+		float angle = AngleBetweenVectors(v, pa - pb);
+		CHECK(angle < 0.02f);
+	}
+
+	TEST_CASE("TestEPACastSphereSphereMiss")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		const Vec3 invalid(-999, -999, -999);
+		Vec3 pa = invalid, pb = invalid, normal = invalid;
+		CHECK(!epa.CastShape(Mat44::sTranslation(Vec3(-10, 2.1f, 0)), Vec3(20, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK(lambda == 1.0f + FLT_EPSILON); // Check input values didn't change
+		CHECK(pa == invalid);
+		CHECK(pb == invalid);
+		CHECK(normal == invalid);
+	}
+
+	TEST_CASE("TestEPACastSphereSphereInitialOverlap")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		const Vec3 invalid(-999, -999, -999);
+		Vec3 pa = invalid, pb = invalid, normal = invalid;
+		CHECK(epa.CastShape(Mat44::sTranslation(Vec3(-1, 0, 0)), Vec3(10, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK(lambda == 0.0f);
+		CHECK_APPROX_EQUAL(pa, Vec3::sZero(), 5.0e-3f);
+		CHECK_APPROX_EQUAL(pb, Vec3(-1, 0, 0), 5.0e-3f);
+		CHECK_APPROX_EQUAL(normal.NormalizedOr(Vec3::sZero()), Vec3(1, 0, 0), 1.0e-2f);
+	}
+
+	TEST_CASE("TestEPACastSphereSphereHit")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		const Vec3 invalid(-999, -999, -999);
+		Vec3 pa = invalid, pb = invalid, normal = invalid;
+		CHECK(epa.CastShape(Mat44::sTranslation(Vec3(-10, 0, 0)), Vec3(20, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK_APPROX_EQUAL(lambda, 8.0f / 20.0f);
+		CHECK_APPROX_EQUAL(pa, Vec3(-1, 0, 0));
+		CHECK_APPROX_EQUAL(pb, Vec3(-1, 0, 0));
+		CHECK_APPROX_EQUAL(normal.NormalizedOr(Vec3::sZero()), Vec3(1, 0, 0));
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/EllipseTest.cpp

@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/Ellipse.h>
+
+TEST_SUITE("EllipseTests")
+{
+	TEST_CASE("TestEllipseIsInside")
+	{
+		Ellipse e(1.0f, 2.0f);
+
+		CHECK(e.IsInside(Float2(0.1f, 0.1f)));
+
+		CHECK_FALSE(e.IsInside(Float2(2.0f, 0.0f)));
+	}
+
+	TEST_CASE("TestEllipseClosestPoint")
+	{
+		Ellipse e(1.0f, 2.0f);
+
+		Float2 c = e.GetClosestPoint(Float2(2.0f, 0.0f));
+		CHECK(c == Float2(1.0f, 0.0f));
+
+		c = e.GetClosestPoint(Float2(-2.0f, 0.0f));
+		CHECK(c == Float2(-1.0f, 0.0f));
+
+		c = e.GetClosestPoint(Float2(0.0f, 4.0f));
+		CHECK(c == Float2(0.0f, 2.0f));
+
+		c = e.GetClosestPoint(Float2(0.0f, -4.0f));
+		CHECK(c == Float2(0.0f, -2.0f));
+
+		Ellipse e2(2.0f, 2.0f);
+
+		c = e2.GetClosestPoint(Float2(4.0f, 4.0f));
+		CHECK_APPROX_EQUAL(c, Float2(sqrt(2.0f), sqrt(2.0f)));
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/GJKTests.cpp

@@ -0,0 +1,248 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/GJKClosestPoint.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/Sphere.h>
+#include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Geometry/RaySphere.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/RayCapsule.h>
+#include <Jolt/Geometry/RayCylinder.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <random>
+
+TEST_SUITE("GJKTests")
+{
+	TEST_CASE("TestGJKIntersectSphere")
+	{
+		GJKClosestPoint gjk;
+
+		// Sphere 1 is centered around the origin
+		Sphere s1(Vec3::sZero(), 1.0f);
+
+		// Shere 2 is far away from s1
+		Vec3 c2(10.0f, 10.0f, 10.0f);
+		Sphere s2(c2, 1.0f);
+
+		// Sphere 3 is exactly 2 away from s1
+		float l = 2.0f / sqrt(3.0f);
+		Vec3 c3(l, l, l);
+		Sphere s3(c3, 1.0f);
+
+		{
+			// Test sphere s1 and s2, they should not collide
+			Vec3 v = Vec3::sZero();
+			CHECK_FALSE(gjk.Intersects(s1, s2, 1.0e-4f, v));
+		}
+
+		{
+			// Test sphere s1 and s3, they should touch exactly
+			Vec3 v = Vec3::sZero();
+			CHECK(gjk.Intersects(s1, s3, 1.0e-4f, v));
+		}
+
+		{
+			// Test sphere s1 and s2, they should not collide, verify their closest points
+			Vec3 pa, pb, v = Vec3::sZero();
+			float d = sqrt(gjk.GetClosestPoints(s1, s2, 1.0e-4f, cLargeFloat, v, pa, pb));
+			CHECK_APPROX_EQUAL(c2.Length() - 2.0f, d, 1.0e-4f);
+			CHECK_APPROX_EQUAL(c2.Normalized(), pa, 1.0e-4f);
+			CHECK_APPROX_EQUAL(c2 - c2.Normalized(), pb, 1.0e-4f);
+		}
+
+		{
+			// Test sphere s1 and s3, they should touch exactly, verify their closest points
+			Vec3 pa, pb, v = Vec3::sZero();
+			float d = sqrt(gjk.GetClosestPoints(s1, s3, 1.0e-4f, cLargeFloat, v, pa, pb));
+			CHECK_APPROX_EQUAL(0.0f, d, 1.0e-4f);
+			CHECK_APPROX_EQUAL(c2.Normalized(), pa, 1.0e-4f);
+			CHECK_APPROX_EQUAL(c2.Normalized(), pb, 1.0e-4f);
+		}
+	}
+
+	template <typename A, typename B>
+	static void TestIntersect(
+		A (*inCreateFuncA)(UnitTestRandom &),
+		B (*inCreateFuncB)(UnitTestRandom &),
+		bool (*inCompareFunc)(const A &inA, const B &inB, bool inIsIntersecting, float inTolerance))
+	{
+		UnitTestRandom random(12345);
+
+		const int count = 10000;
+
+		int hits = 0;
+
+		GJKClosestPoint gjk;
+
+		for (int i = 0; i < count; ++i)
+		{
+			A shape1 = inCreateFuncA(random);
+			B shape2 = inCreateFuncB(random);
+
+			// Use GJK to test for intersection
+			Vec3 v = Vec3::sZero();
+			const float cTolerance = 1.0e-4f;
+			bool result_gjk = gjk.Intersects(shape1, shape2, cTolerance, v);
+
+			// Compare with reference function and increase tolerance a bit to account for floating point imprecision
+			CHECK(inCompareFunc(shape1, shape2, result_gjk, 2.0f * cTolerance));
+
+			if (result_gjk)
+				++hits;
+		}
+
+		// Check that there were enough hits so that the test is representative
+		float hit_rate = 100.0f * hits / count;
+		CHECK(hit_rate > 30.0f);
+		CHECK(hit_rate < 70.0f);
+	}
+
+	TEST_CASE("TestGJKSphereVsSphereIntersect")
+	{
+		auto sphere_creator = [](UnitTestRandom &inRandom) {
+				uniform_real_distribution<float> pos(-2.0f, 2.0f);
+				uniform_real_distribution<float> rad(0.5f, 2.0f);
+				return Sphere(Vec3(pos(inRandom), pos(inRandom), pos(inRandom)), rad(inRandom));
+			};
+
+		TestIntersect<Sphere, Sphere>(
+			sphere_creator,
+			sphere_creator,
+			[](const Sphere &inSphereA, const Sphere &inSphereB, bool inIsIntersecting, float inTolerance) {
+
+				// Test without and with tolerance if the results are equal
+				return inSphereA.Overlaps(inSphereB) == inIsIntersecting
+					|| Sphere(inSphereA.GetCenter(), inSphereA.GetRadius() + inTolerance).Overlaps(inSphereB) == inIsIntersecting;
+			});
+	}
+
+	TEST_CASE("TestGJKSphereVsBoxIntersect")
+	{
+		auto sphere_creator = [](UnitTestRandom &inRandom) {
+				uniform_real_distribution<float> pos(-2.0f, 2.0f);
+				uniform_real_distribution<float> rad(0.5f, 2.0f);
+				return Sphere(Vec3(pos(inRandom), pos(inRandom), pos(inRandom)), rad(inRandom));
+			};
+
+		auto box_creator = [](UnitTestRandom &inRandom) {
+				uniform_real_distribution<float> pos(-2.0f, 2.0f);
+				Vec3 p1 = Vec3(pos(inRandom), pos(inRandom), pos(inRandom));
+				Vec3 p2 = Vec3(pos(inRandom), pos(inRandom), pos(inRandom));
+				return AABox::sFromTwoPoints(p1, p2);
+			};
+
+		TestIntersect<Sphere, AABox>(
+			sphere_creator,
+			box_creator,
+			[](const Sphere &inSphereA, const AABox &inBoxB, bool inIsIntersecting, float inTolerance) {
+
+				// Test without and with tolerance if the results are equal
+				return inSphereA.Overlaps(inBoxB) == inIsIntersecting
+					|| Sphere(inSphereA.GetCenter(), inSphereA.GetRadius() + inTolerance).Overlaps(inBoxB) == inIsIntersecting;
+			});
+	}
+
+	template <typename A, typename Context>
+	static void TestRay(const A &inA, const Context &inContext, float (*inCompareFunc)(const Context &inContext, Vec3Arg inRayOrigin, Vec3Arg inRayDirection))
+	{
+		UnitTestRandom random(12345);
+		uniform_real_distribution<float> random_scale(-2.0f, 2.0f);
+
+		const int count = 1000;
+
+		for (int i = 0; i < count; ++i)
+		{
+			Vec3 from(random_scale(random), random_scale(random), random_scale(random));
+			Vec3 to(random_scale(random), random_scale(random), random_scale(random));
+			Vec3 direction = to - from;
+
+			// Use GJK to cast a ray
+			float fraction1 = 1.0f + FLT_EPSILON;
+			GJKClosestPoint gjk;
+			if (!gjk.CastRay(from, direction, 1.0e-4f, inA, fraction1))
+				fraction1 = FLT_MAX;
+
+			// Use the comparison function
+			float fraction2 = inCompareFunc(inContext, from, direction);
+
+			// The comparison functions work with infinite rays, so a fraction > 1 means a miss
+			if (fraction2 > 1.0f)
+				fraction2 = FLT_MAX;
+
+			CHECK_APPROX_EQUAL(fraction1, fraction2, 0.01f);
+		}
+	}
+
+	TEST_CASE("TestGJKRaySphere")
+	{
+		Sphere sphere(Vec3(0.1f, 0.2f, 0.3f), 1.1f);
+		TestRay<Sphere, Sphere>(sphere, sphere, [](const Sphere &inSphere, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RaySphere(inRayOrigin, inRayDirection, inSphere.GetCenter(), inSphere.GetRadius());
+		});
+	}
+
+	TEST_CASE("TestGJKRaySphereShape")
+	{
+		SphereShape sphere_shape(1.1f);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = sphere_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, SphereShape>(*support, sphere_shape, [](const SphereShape &inSphere, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RaySphere(inRayOrigin, inRayDirection, Vec3::sZero(), inSphere.GetRadius());
+		});
+	}
+
+	TEST_CASE("TestGJKRayBox")
+	{
+		AABox box(Vec3(-0.9f, -1.0f, -1.1f), Vec3(0.8f, 0.9f, 1.0f));
+		TestRay<AABox, AABox>(box, box, [](const AABox &inBox, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			float fraction = RayAABox(inRayOrigin, RayInvDirection(inRayDirection), inBox.mMin, inBox.mMax);
+			return max(fraction, 0.0f);
+		});
+	}
+
+	TEST_CASE("TestGJKRayBoxShape")
+	{
+		BoxShape box_shape(Vec3(0.9f, 1.0f, 1.1f), 0.0f);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = box_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, BoxShape>(*support, box_shape, [](const BoxShape &inBox, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			float fraction = RayAABox(inRayOrigin, RayInvDirection(inRayDirection), -inBox.GetHalfExtent(), inBox.GetHalfExtent());
+			return max(fraction, 0.0f);
+		});
+	}
+
+	TEST_CASE("TestGJKRayCapsuleShape")
+	{
+		CapsuleShape capsule_shape(1.1f, 0.6f);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = capsule_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, CapsuleShape>(*support, capsule_shape, [](const CapsuleShape &inCapsule, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayCapsule(inRayOrigin, inRayDirection, inCapsule.GetHalfHeightOfCylinder(), inCapsule.GetRadius());
+		});
+	}
+
+	TEST_CASE("TestGJKRayCylinderShape")
+	{
+		CylinderShape cylinder_shape(1.5f, 0.6f, 0.0f);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = cylinder_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, CylinderShape>(*support, cylinder_shape, [](const CylinderShape &inCylinder, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayCylinder(inRayOrigin, inRayDirection, inCylinder.GetHalfHeight(), inCylinder.GetRadius());
+		});
+	}
+
+	TEST_CASE("TestGJKRayTriangle")
+	{
+		TriangleConvexSupport triangle(Vec3(0.1f, 0.9f, 0.3f), Vec3(-0.9f, -0.5f, 0.2f), Vec3(0.7f, -0.3f, -0.1f));
+		TestRay<TriangleConvexSupport, TriangleConvexSupport>(triangle, triangle, [](const TriangleConvexSupport &inTriangle, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayTriangle(inRayOrigin, inRayDirection, inTriangle.mV1, inTriangle.mV2, inTriangle.mV3);
+		});
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/PlaneTests.cpp

@@ -0,0 +1,51 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/Plane.h>
+
+TEST_SUITE("PlaneTests")
+{
+	TEST_CASE("TestPlaneSignedDistance")
+	{
+		Plane p = Plane::sFromPointAndNormal(Vec3(0, 2, 0), Vec3(0, 1, 0));
+		CHECK(p.SignedDistance(Vec3(5, 7, 0)) == 5.0f);
+		CHECK(p.SignedDistance(Vec3(5, -3, 0)) == -5.0f);
+	}
+
+	TEST_CASE("TestPlaneGetTransformed")
+	{
+		Mat44 transform = Mat44::sRotationTranslation(Quat::sRotation(Vec3(1.0f, 2.0f, 3.0f).Normalized(), 0.1f * JPH_PI), Vec3(5.0f, -7.0f, 9.0f));
+
+		Vec3 point = Vec3(11.0f, 13.0f, 15.0f);
+		Vec3 normal = Vec3(-3.0f, 5.0f, -7.0f).Normalized();
+
+		Plane p1 = Plane::sFromPointAndNormal(point, normal).GetTransformed(transform);
+		Plane p2 = Plane::sFromPointAndNormal(transform * point, transform.Multiply3x3(normal));
+
+		CHECK_APPROX_EQUAL(p1.GetNormal(), p2.GetNormal());
+		CHECK_APPROX_EQUAL(p1.GetConstant(), p2.GetConstant());
+	}
+
+	TEST_CASE("TestPlaneIntersectPlanes")
+	{
+		Plane p1 = Plane::sFromPointAndNormal(Vec3(0, 2, 0), Vec3(0, 1, 0));
+		Plane p2 = Plane::sFromPointAndNormal(Vec3(3, 0, 0), Vec3(1, 0, 0));
+		Plane p3 = Plane::sFromPointAndNormal(Vec3(0, 0, 4), Vec3(0, 0, 1));
+
+		{
+			Vec3 point;
+			bool found = Plane::sIntersectPlanes(p1, p2, p3, point);
+			CHECK(found);
+			CHECK(point == Vec3(3, 2, 4));
+		}
+
+		{
+			Plane p4 = Plane::sFromPointAndNormal(Vec3(0, 3, 0), Vec3(0, 1, 0));
+			Vec3 point;
+			bool found = Plane::sIntersectPlanes(p1, p2, p4, point);
+			CHECK_FALSE(found);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/RayAABoxTests.cpp

@@ -0,0 +1,86 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/RayAABox.h>
+
+TEST_SUITE("RayAABoxTests")
+{
+	TEST_CASE("TestRayAABox")
+	{
+		AABox box(Vec3::sReplicate(-1.0f), Vec3::sOne());
+
+		for (int axis = 0; axis < 3; ++axis)
+		{
+			{
+				// Ray starting in the center of the box, pointing high
+				Vec3 origin = Vec3::sZero();
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, 1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK_APPROX_EQUAL(-1.0f, fraction, 1.0e-6f);
+			}
+
+			{
+				// Ray starting in the center of the box, pointing low
+				Vec3 origin = Vec3::sZero();
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, -1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK_APPROX_EQUAL(-1.0f, fraction, 1.0e-6f);
+			}
+
+			{
+				// Ray starting high, pointing to low
+				Vec3 origin = Vec3::sZero();
+				origin.SetComponent(axis, 1.1f);
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, -1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK_APPROX_EQUAL(0.1f, fraction, 1.0e-6f);
+			}
+
+			{
+				// Ray starting high, pointing to high
+				Vec3 origin = Vec3::sZero();
+				origin.SetComponent(axis, 1.1f);
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, 1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK(fraction == FLT_MAX);
+			}
+
+			{
+				// Ray starting low, pointing to high
+				Vec3 origin = Vec3::sZero();
+				origin.SetComponent(axis, -1.1f);
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, 1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK_APPROX_EQUAL(0.1f, fraction, 1.0e-6f);
+			}
+
+			{
+				// Ray starting low, pointing to low
+				Vec3 origin = Vec3::sZero();
+				origin.SetComponent(axis, -1.1f);
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, -1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK(fraction == FLT_MAX);
+			}
+		}
+
+		{
+			// Test ray that hits top plane under an angle
+			Vec3 expected_hit = Vec3(0, 1, 0);
+			float expected_fraction = 0.123f;
+			Vec3 direction = Vec3(4, -4, 0);
+			Vec3 origin = expected_hit - expected_fraction * direction;
+			float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+			CHECK_APPROX_EQUAL(expected_fraction, fraction, 1.0e-6f);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Layers.h

@@ -0,0 +1,153 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+/// Layer that objects can be in, determines which other objects it can collide with
+namespace Layers
+{
+	static constexpr ObjectLayer UNUSED1 = 0; // 5 unused values so that broadphase layers values don't match with object layer values (for testing purposes)
+	static constexpr ObjectLayer UNUSED2 = 1;
+	static constexpr ObjectLayer UNUSED3 = 2;
+	static constexpr ObjectLayer UNUSED4 = 3;
+	static constexpr ObjectLayer UNUSED5 = 4;
+	static constexpr ObjectLayer NON_MOVING = 5;
+	static constexpr ObjectLayer MOVING = 6;
+	static constexpr ObjectLayer MOVING2 = 7; // Another moving layer that acts as MOVING but doesn't collide with MOVING
+	static constexpr ObjectLayer HQ_DEBRIS = 8; // High quality debris collides with MOVING and NON_MOVING but not with any debris
+	static constexpr ObjectLayer LQ_DEBRIS = 9; // Low quality debris only collides with NON_MOVING
+	static constexpr ObjectLayer SENSOR = 10; // Sensors only collide with MOVING objects
+	static constexpr ObjectLayer NUM_LAYERS = 11;
+};
+
+/// Class that determines if two object layers can collide
+class ObjectLayerPairFilterImpl : public ObjectLayerPairFilter
+{
+public:
+	virtual bool					ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override
+	{
+		switch (inObject1)
+		{
+		case Layers::UNUSED1:
+		case Layers::UNUSED2:
+		case Layers::UNUSED3:
+		case Layers::UNUSED4:
+		case Layers::UNUSED5:
+			return false;
+		case Layers::NON_MOVING:
+			return inObject2 == Layers::MOVING || inObject2 == Layers::MOVING2 || inObject2 == Layers::HQ_DEBRIS || inObject2 == Layers::LQ_DEBRIS;
+		case Layers::MOVING:
+			return inObject2 == Layers::NON_MOVING || inObject2 == Layers::MOVING || inObject2 == Layers::HQ_DEBRIS || inObject2 == Layers::SENSOR;
+		case Layers::MOVING2:
+			return inObject2 == Layers::NON_MOVING || inObject2 == Layers::MOVING2 || inObject2 == Layers::HQ_DEBRIS || inObject2 == Layers::SENSOR;
+		case Layers::HQ_DEBRIS:
+			return inObject2 == Layers::NON_MOVING || inObject2 == Layers::MOVING || inObject2 == Layers::MOVING2;
+		case Layers::LQ_DEBRIS:
+			return inObject2 == Layers::NON_MOVING;
+		case Layers::SENSOR:
+			return inObject2 == Layers::MOVING || inObject2 == Layers::MOVING2;
+		default:
+			JPH_ASSERT(false);
+			return false;
+		}
+	}
+};
+
+/// Broadphase layers
+namespace BroadPhaseLayers
+{
+	static constexpr BroadPhaseLayer NON_MOVING(0);
+	static constexpr BroadPhaseLayer MOVING(1);
+	static constexpr BroadPhaseLayer MOVING2(2);
+	static constexpr BroadPhaseLayer LQ_DEBRIS(3);
+	static constexpr BroadPhaseLayer UNUSED(4);
+	static constexpr BroadPhaseLayer SENSOR(5);
+	static constexpr uint NUM_LAYERS(6);
+};
+
+/// BroadPhaseLayerInterface implementation
+class BPLayerInterfaceImpl final : public BroadPhaseLayerInterface
+{
+public:
+									BPLayerInterfaceImpl()
+	{
+		// Create a mapping table from object to broad phase layer
+		mObjectToBroadPhase[Layers::UNUSED1] = BroadPhaseLayers::UNUSED;
+		mObjectToBroadPhase[Layers::UNUSED2] = BroadPhaseLayers::UNUSED;
+		mObjectToBroadPhase[Layers::UNUSED3] = BroadPhaseLayers::UNUSED;
+		mObjectToBroadPhase[Layers::UNUSED4] = BroadPhaseLayers::UNUSED;
+		mObjectToBroadPhase[Layers::UNUSED5] = BroadPhaseLayers::UNUSED;
+		mObjectToBroadPhase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING;
+		mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING;
+		mObjectToBroadPhase[Layers::MOVING2] = BroadPhaseLayers::MOVING2;
+		mObjectToBroadPhase[Layers::HQ_DEBRIS] = BroadPhaseLayers::MOVING; // HQ_DEBRIS is also in the MOVING layer as an example on how to map multiple layers onto the same broadphase layer
+		mObjectToBroadPhase[Layers::LQ_DEBRIS] = BroadPhaseLayers::LQ_DEBRIS;
+		mObjectToBroadPhase[Layers::SENSOR] = BroadPhaseLayers::SENSOR;
+	}
+
+	virtual uint					GetNumBroadPhaseLayers() const override
+	{
+		return BroadPhaseLayers::NUM_LAYERS;
+	}
+
+	virtual BroadPhaseLayer			GetBroadPhaseLayer(ObjectLayer inLayer) const override
+	{
+		JPH_ASSERT(inLayer < Layers::NUM_LAYERS);
+		return mObjectToBroadPhase[inLayer];
+	}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	virtual const char *			GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override
+	{
+		switch ((BroadPhaseLayer::Type)inLayer)
+		{
+		case (BroadPhaseLayer::Type)BroadPhaseLayers::NON_MOVING:	return "NON_MOVING";
+		case (BroadPhaseLayer::Type)BroadPhaseLayers::MOVING:		return "MOVING";
+		case (BroadPhaseLayer::Type)BroadPhaseLayers::MOVING2:		return "MOVING2";
+		case (BroadPhaseLayer::Type)BroadPhaseLayers::LQ_DEBRIS:	return "LQ_DEBRIS";
+		case (BroadPhaseLayer::Type)BroadPhaseLayers::UNUSED:		return "UNUSED";
+		case (BroadPhaseLayer::Type)BroadPhaseLayers::SENSOR:		return "SENSOR";
+		default:													JPH_ASSERT(false); return "INVALID";
+		}
+	}
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+private:
+	BroadPhaseLayer					mObjectToBroadPhase[Layers::NUM_LAYERS];
+};
+
+/// Class that determines if an object layer can collide with a broadphase layer
+class ObjectVsBroadPhaseLayerFilterImpl : public ObjectVsBroadPhaseLayerFilter
+{
+public:
+	virtual bool					ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
+	{
+		switch (inLayer1)
+		{
+		case Layers::NON_MOVING:
+			return inLayer2 == BroadPhaseLayers::MOVING;
+		case Layers::MOVING:
+		case Layers::HQ_DEBRIS:
+			return inLayer2 == BroadPhaseLayers::NON_MOVING || inLayer2 == BroadPhaseLayers::MOVING || inLayer2 == BroadPhaseLayers::SENSOR;
+		case Layers::MOVING2:
+			return inLayer2 == BroadPhaseLayers::NON_MOVING || inLayer2 == BroadPhaseLayers::MOVING2 || inLayer2 == BroadPhaseLayers::SENSOR;
+		case Layers::LQ_DEBRIS:
+			return inLayer2 == BroadPhaseLayers::NON_MOVING;
+		case Layers::SENSOR:
+			return inLayer2 == BroadPhaseLayers::MOVING;
+		case Layers::UNUSED1:
+		case Layers::UNUSED2:
+		case Layers::UNUSED3:
+		case Layers::UNUSED4:
+		case Layers::UNUSED5:
+			return false;
+		default:
+			JPH_ASSERT(false);
+			return false;
+		}
+	}
+};

lib/All/JoltPhysics/UnitTests/LoggingBodyActivationListener.h

@@ -0,0 +1,63 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyActivationListener.h>
+
+/// Activation listener that just logs the activations/deactivations
+class LoggingBodyActivationListener : public BodyActivationListener
+{
+public:
+	// Activation callback type
+	enum class EType
+	{
+		Activated,
+		Deactivated,
+	};
+
+	// Entry written when an activation callback happens
+	struct LogEntry
+	{
+		EType				mType;
+		BodyID				mBodyID;
+	};
+
+	virtual void		OnBodyActivated(const BodyID &inBodyID, uint64 inBodyUserData) override
+	{
+		lock_guard lock(mLogMutex);
+		mLog.push_back({ EType::Activated, inBodyID });
+	}
+
+	virtual void		OnBodyDeactivated(const BodyID &inBodyID, uint64 inBodyUserData) override
+	{
+		lock_guard lock(mLogMutex);
+		mLog.push_back({ EType::Deactivated, inBodyID });
+	}
+
+	void				Clear()
+	{
+		mLog.clear();
+	}
+
+	size_t				GetEntryCount() const
+	{
+		return mLog.size();
+	}
+
+	// Check if we have logged an event with a particular type and involving a particular body
+	bool				Contains(EType inType, const BodyID &inBodyID) const
+	{
+		for (const LogEntry &e : mLog)
+			if (e.mType == inType && e.mBodyID == inBodyID)
+				return true;
+
+		return false;
+	}
+
+private:
+	Mutex				mLogMutex; // Callbacks are made from a thread, make sure we don't corrupt the log
+	Array<LogEntry>		mLog;
+};

lib/All/JoltPhysics/UnitTests/LoggingCharacterContactListener.h

@@ -0,0 +1,171 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Character/CharacterVirtual.h>
+
+// Character contact listener that just logs the calls made to it for later validation
+class LoggingCharacterContactListener : public CharacterContactListener
+{
+public:
+	// Contact callback type
+	enum class EType
+	{
+		ValidateBody,
+		ValidateCharacter,
+		AddBody,
+		PersistBody,
+		RemoveBody,
+		AddCharacter,
+		PersistCharacter,
+		RemoveCharacter
+	};
+
+	// Entry written when a contact callback happens
+	struct LogEntry
+	{
+		EType						mType;
+		const CharacterVirtual *	mCharacter;
+		BodyID						mBody2;
+		CharacterID					mCharacterID2;
+		SubShapeID					mSubShapeID2;
+	};
+
+	virtual bool					OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) override
+	{
+		mLog.push_back({ EType::ValidateBody, inCharacter, inBodyID2, CharacterID(), inSubShapeID2 });
+		return true;
+	}
+
+	virtual bool					OnCharacterContactValidate(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2) override
+	{
+		mLog.push_back({ EType::ValidateCharacter, inCharacter, BodyID(), inOtherCharacter->GetID(), inSubShapeID2 });
+		return true;
+	}
+
+	virtual void					OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+	{
+		mLog.push_back({ EType::AddBody, inCharacter, inBodyID2, CharacterID(), inSubShapeID2 });
+	}
+
+	virtual void					OnContactPersisted(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+	{
+		mLog.push_back({ EType::PersistBody, inCharacter, inBodyID2, CharacterID(), inSubShapeID2 });
+	}
+
+	virtual void					OnContactRemoved(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) override
+	{
+		mLog.push_back({ EType::RemoveBody, inCharacter, inBodyID2, CharacterID(), inSubShapeID2 });
+	}
+
+	virtual void					OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+	{
+		mLog.push_back({ EType::AddCharacter, inCharacter, BodyID(), inOtherCharacter->GetID(), inSubShapeID2 });
+	}
+
+	virtual void					OnCharacterContactPersisted(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+	{
+		mLog.push_back({ EType::PersistCharacter, inCharacter, BodyID(), inOtherCharacter->GetID(), inSubShapeID2 });
+	}
+
+	virtual void					OnCharacterContactRemoved(const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2) override
+	{
+		mLog.push_back({ EType::RemoveCharacter, inCharacter, BodyID(), inOtherCharacterID, inSubShapeID2 });
+	}
+
+	void							Clear()
+	{
+		mLog.clear();
+	}
+
+	size_t							GetEntryCount() const
+	{
+		return mLog.size();
+	}
+
+	const LogEntry &				GetEntry(size_t inIdx) const
+	{
+		return mLog[inIdx];
+	}
+
+	// Find first event with a particular type and involving a particular character vs body
+	int								Find(EType inType, const CharacterVirtual *inCharacter, const BodyID &inBody2) const
+	{
+		for (size_t i = 0; i < mLog.size(); ++i)
+		{
+			const LogEntry &e = mLog[i];
+			if (e.mType == inType && e.mCharacter == inCharacter && e.mBody2 == inBody2 && e.mCharacterID2.IsInvalid())
+				return int(i);
+		}
+
+		return -1;
+	}
+
+	// Check if event with a particular type and involving a particular character vs body
+	bool							Contains(EType inType, const CharacterVirtual *inCharacter, const BodyID &inBody2) const
+	{
+		return Find(inType, inCharacter, inBody2) >= 0;
+	}
+
+	// Find first event with a particular type and involving a particular character vs character
+	int								Find(EType inType, const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID) const
+	{
+		for (size_t i = 0; i < mLog.size(); ++i)
+		{
+			const LogEntry &e = mLog[i];
+			if (e.mType == inType && e.mCharacter == inCharacter && e.mBody2.IsInvalid() && e.mCharacterID2 == inOtherCharacterID)
+				return int(i);
+		}
+
+		return -1;
+	}
+
+	// Check if event with a particular type and involving a particular character vs character
+	bool							Contains(EType inType, const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID) const
+	{
+		return Find(inType, inCharacter, inOtherCharacterID) >= 0;
+	}
+
+	// Find first event with a particular type and involving a particular character vs body and sub shape ID
+	int								Find(EType inType, const CharacterVirtual *inCharacter, const BodyID &inBody2, const SubShapeID &inSubShapeID2) const
+	{
+		for (size_t i = 0; i < mLog.size(); ++i)
+		{
+			const LogEntry &e = mLog[i];
+			if (e.mType == inType && e.mCharacter == inCharacter && e.mBody2 == inBody2 && e.mCharacterID2.IsInvalid() && e.mSubShapeID2 == inSubShapeID2)
+				return int(i);
+		}
+
+		return -1;
+	}
+
+	// Check if a particular type and involving a particular character vs body and sub shape ID exists
+	bool							Contains(EType inType, const CharacterVirtual *inCharacter, const BodyID &inBody2, const SubShapeID &inSubShapeID2) const
+	{
+		return Find(inType, inCharacter, inBody2, inSubShapeID2) >= 0;
+	}
+
+	// Find first event with a particular type and involving a particular character vs character and sub shape ID
+	int								Find(EType inType, const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2) const
+	{
+		for (size_t i = 0; i < mLog.size(); ++i)
+		{
+			const LogEntry &e = mLog[i];
+			if (e.mType == inType && e.mCharacter == inCharacter && e.mBody2.IsInvalid() && e.mCharacterID2 == inOtherCharacterID && e.mSubShapeID2 == inSubShapeID2)
+				return int(i);
+		}
+
+		return -1;
+	}
+
+	// Check if a particular type and involving a particular character vs character and sub shape ID exists
+	bool							Contains(EType inType, const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2) const
+	{
+		return Find(inType, inCharacter, inOtherCharacterID, inSubShapeID2) >= 0;
+	}
+
+private:
+	Array<LogEntry>					mLog;
+};

lib/All/JoltPhysics/UnitTests/LoggingContactListener.h

@@ -0,0 +1,140 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ContactListener.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Core/Mutex.h>
+#include <Jolt/Core/UnorderedSet.h>
+
+// Contact listener that just logs the calls made to it for later validation
+class LoggingContactListener : public ContactListener
+{
+public:
+	// Contact callback type
+	enum class EType
+	{
+		Validate,
+		Add,
+		Persist,
+		Remove
+	};
+
+	// Entry written when a contact callback happens
+	struct LogEntry
+	{
+		EType						mType;
+		BodyID						mBody1;
+		BodyID						mBody2;
+		ContactManifold				mManifold;
+	};
+
+	virtual ValidateResult			OnContactValidate(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) override
+	{
+		// Check ordering contract between body 1 and body 2
+		bool contract = inBody1.GetMotionType() >= inBody2.GetMotionType()
+			|| (inBody1.GetMotionType() == inBody2.GetMotionType() && inBody1.GetID() < inBody2.GetID());
+		CHECK(contract);
+
+		lock_guard lock(mLogMutex);
+		mLog.push_back({ EType::Validate, inBody1.GetID(), inBody2.GetID(), ContactManifold() });
+		return ValidateResult::AcceptContact;
+	}
+
+	virtual void					OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+	{
+		// Check contract that body 1 < body 2
+		CHECK(inBody1.GetID() < inBody2.GetID());
+
+		lock_guard lock(mLogMutex);
+		SubShapeIDPair key(inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2);
+		CHECK(mExistingContacts.insert(key).second); // Validate that contact does not exist yet
+		mLog.push_back({ EType::Add, inBody1.GetID(), inBody2.GetID(), inManifold });
+	}
+
+	virtual void					OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+	{
+		// Check contract that body 1 < body 2
+		CHECK(inBody1.GetID() < inBody2.GetID());
+
+		lock_guard lock(mLogMutex);
+		SubShapeIDPair key(inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2);
+		CHECK(mExistingContacts.find(key) != mExistingContacts.end()); // Validate that OnContactAdded was called
+		mLog.push_back({ EType::Persist, inBody1.GetID(), inBody2.GetID(), inManifold });
+	}
+
+	virtual void					OnContactRemoved(const SubShapeIDPair &inSubShapePair) override
+	{
+		// Check contract that body 1 < body 2
+		CHECK(inSubShapePair.GetBody1ID() < inSubShapePair.GetBody2ID());
+
+		lock_guard lock(mLogMutex);
+		CHECK(mExistingContacts.erase(inSubShapePair) == 1); // Validate that OnContactAdded was called
+		ContactManifold manifold;
+		manifold.mSubShapeID1 = inSubShapePair.GetSubShapeID1();
+		manifold.mSubShapeID2 = inSubShapePair.GetSubShapeID2();
+		mLog.push_back({ EType::Remove, inSubShapePair.GetBody1ID(), inSubShapePair.GetBody2ID(), manifold });
+	}
+
+	void							Clear()
+	{
+		mLog.clear();
+	}
+
+	size_t							GetEntryCount() const
+	{
+		return mLog.size();
+	}
+
+	const LogEntry &				GetEntry(size_t inIdx) const
+	{
+		return mLog[inIdx];
+	}
+
+	// Find first event with a particular type and involving two particular bodies
+	int								Find(EType inType, const BodyID &inBody1, const BodyID &inBody2) const
+	{
+		for (size_t i = 0; i < mLog.size(); ++i)
+		{
+			const LogEntry &e = mLog[i];
+			if (e.mType == inType && ((e.mBody1 == inBody1 && e.mBody2 == inBody2) || (e.mBody1 == inBody2 && e.mBody2 == inBody1)))
+				return int(i);
+		}
+
+		return -1;
+	}
+
+	// Check if event with a particular type and involving two particular bodies exists
+	bool							Contains(EType inType, const BodyID &inBody1, const BodyID &inBody2) const
+	{
+		return Find(inType, inBody1, inBody2) >= 0;
+	}
+
+	// Find first event with a particular type and involving two particular bodies and sub shape IDs
+	int								Find(EType inType, const BodyID &inBody1, const SubShapeID &inSubShapeID1, const BodyID &inBody2, const SubShapeID &inSubShapeID2) const
+	{
+		for (size_t i = 0; i < mLog.size(); ++i)
+		{
+			const LogEntry &e = mLog[i];
+			if (e.mType == inType
+				&& ((e.mBody1 == inBody1 && e.mManifold.mSubShapeID1 == inSubShapeID1 && e.mBody2 == inBody2 && e.mManifold.mSubShapeID2 == inSubShapeID2)
+					|| (e.mBody1 == inBody2 && e.mManifold.mSubShapeID1 == inSubShapeID2 && e.mBody2 == inBody1 && e.mManifold.mSubShapeID2 == inSubShapeID1)))
+				return int(i);
+		}
+
+		return -1;
+	}
+
+	// Check if event with a particular type and involving two particular bodies and sub shape IDs exists
+	bool							Contains(EType inType, const BodyID &inBody1, const SubShapeID &inSubShapeID1, const BodyID &inBody2, const SubShapeID &inSubShapeID2) const
+	{
+		return Find(inType, inBody1, inSubShapeID1, inBody2, inSubShapeID2) >= 0;
+	}
+
+private:
+	Mutex							mLogMutex; // Callbacks are made from a thread, make sure we don't corrupt the log
+	Array<LogEntry>					mLog;
+	UnorderedSet<SubShapeIDPair>	mExistingContacts; // For validation purposes: the contacts that are currently active
+};

lib/All/JoltPhysics/UnitTests/Math/BVec16Tests.cpp

@@ -0,0 +1,91 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+
+#include <Jolt/Math/BVec16.h>
+#include <Jolt/Core/StringTools.h>
+
+TEST_SUITE("BVec16Tests")
+{
+	TEST_CASE("TestBVec16Construct")
+	{
+		BVec16 v(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16);
+
+		CHECK(v[0] == 1);
+		CHECK(v[1] == 2);
+		CHECK(v[2] == 3);
+		CHECK(v[3] == 4);
+		CHECK(v[4] == 5);
+		CHECK(v[5] == 6);
+		CHECK(v[6] == 7);
+		CHECK(v[7] == 8);
+		CHECK(v[8] == 9);
+		CHECK(v[9] == 10);
+		CHECK(v[10] == 11);
+		CHECK(v[11] == 12);
+		CHECK(v[12] == 13);
+		CHECK(v[13] == 14);
+		CHECK(v[14] == 15);
+		CHECK(v[15] == 16);
+
+		// Test == and != operators
+		CHECK(v == BVec16(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16));
+		CHECK(v != BVec16(1, 2, 3, 4, 5, 6, 7, 8, 10, 9, 11, 12, 13, 14, 15, 16));
+
+		// Check element modification
+		CHECK(const_cast<const BVec16 &>(v)[15] == 16); // Check const operator
+		v[15] = 17;
+		CHECK(v[15] == 17);
+		CHECK(v == BVec16(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 17));
+	}
+
+	TEST_CASE("TestBVec16LoadByte16")
+	{
+		uint8 u16[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 };
+		CHECK(BVec16::sLoadByte16(u16) == BVec16(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16));
+	}
+
+	TEST_CASE("TestBVec16Zero")
+	{
+		BVec16 v = BVec16::sZero();
+
+		for (int i = 0; i < 16; ++i)
+			CHECK(v[i] == 0);
+	}
+
+	TEST_CASE("TestBVec16Replicate")
+	{
+		CHECK(BVec16::sReplicate(2) == BVec16(2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2));
+	}
+
+	TEST_CASE("TestBVec16Comparisons")
+	{
+		BVec16 eq = BVec16::sEquals(BVec16(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16), BVec16(6, 7, 3, 4, 5, 6, 7, 5, 9, 10, 11, 12, 13, 14, 15, 13));
+		CHECK(eq.GetTrues() == 0b0111111101111100);
+		CHECK(eq.TestAnyTrue());
+		CHECK(!eq.TestAllTrue());
+	}
+
+	TEST_CASE("TestBVec16BitOps")
+	{
+		// Test all bit permutations
+		BVec16 v1(0b011, 0b0110, 0b01100, 0b011000, 0b0110000, 0b01100000, 0b011, 0b0110, 0b01100, 0b011000, 0b0110000, 0b01100000, 0b011, 0b0110, 0b01100, 0b011000);
+		BVec16 v2(0b101, 0b1010, 0b10100, 0b101000, 0b1010000, 0b10100000, 0b101, 0b1010, 0b10100, 0b101000, 0b1010000, 0b10100000, 0b101, 0b1010, 0b10100, 0b101000);
+
+		CHECK(BVec16::sOr(v1, v2) == BVec16(0b111, 0b1110, 0b11100, 0b111000, 0b1110000, 0b11100000, 0b111, 0b1110, 0b11100, 0b111000, 0b1110000, 0b11100000, 0b111, 0b1110, 0b11100, 0b111000));
+		CHECK(BVec16::sXor(v1, v2) == BVec16(0b110, 0b1100, 0b11000, 0b110000, 0b1100000, 0b11000000, 0b110, 0b1100, 0b11000, 0b110000, 0b1100000, 0b11000000, 0b110, 0b1100, 0b11000, 0b110000));
+		CHECK(BVec16::sAnd(v1, v2) == BVec16(0b001, 0b0010, 0b00100, 0b001000, 0b0010000, 0b00100000, 0b001, 0b0010, 0b00100, 0b001000, 0b0010000, 0b00100000, 0b001, 0b0010, 0b00100, 0b001000));
+
+		CHECK(BVec16::sNot(v1) == BVec16(0b11111100, 0b11111001, 0b11110011, 0b11100111, 0b11001111, 0b10011111, 0b11111100, 0b11111001, 0b11110011, 0b11100111, 0b11001111, 0b10011111, 0b11111100, 0b11111001, 0b11110011, 0b11100111));
+		CHECK(BVec16::sNot(v2) == BVec16(0b11111010, 0b11110101, 0b11101011, 0b11010111, 0b10101111, 0b01011111, 0b11111010, 0b11110101, 0b11101011, 0b11010111, 0b10101111, 0b01011111, 0b11111010, 0b11110101, 0b11101011, 0b11010111));
+	}
+
+	TEST_CASE("TestBVec16ToString")
+	{
+		BVec16 v(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16);
+
+		CHECK(ConvertToString(v) == "1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16");
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/DMat44Tests.cpp

@@ -0,0 +1,246 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/DMat44.h>
+#include <Jolt/Core/StringTools.h>
+
+TEST_SUITE("DMat44Tests")
+{
+	TEST_CASE("TestDMat44Zero")
+	{
+		DMat44 zero = DMat44::sZero();
+
+		CHECK(zero == DMat44(Vec4(0, 0, 0, 0), Vec4(0, 0, 0, 0), Vec4(0, 0, 0, 0), DVec3(0, 0, 0)));
+		CHECK(zero.GetAxisX() == Vec3::sZero());
+		CHECK(zero.GetAxisY() == Vec3::sZero());
+		CHECK(zero.GetAxisZ() == Vec3::sZero());
+		CHECK(zero.GetTranslation() == DVec3::sZero());
+	}
+
+	TEST_CASE("TestDMat44Identity")
+	{
+		DMat44 identity = DMat44::sIdentity();
+
+		CHECK(identity == DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3(0, 0, 0)));
+
+		// Check non-equality
+		CHECK(identity != DMat44(Vec4(0, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3(0, 0, 0)));
+		CHECK(identity != DMat44(Vec4(1, 0, 0, 0), Vec4(0, 0, 0, 0), Vec4(0, 0, 1, 0), DVec3(0, 0, 0)));
+		CHECK(identity != DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 0, 0), DVec3(0, 0, 0)));
+		CHECK(identity != DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3(1, 0, 0)));
+	}
+
+	TEST_CASE("TestDMat44Construct")
+	{
+		DMat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), DVec3(13, 14, 15));
+
+		CHECK(mat.GetColumn4(0) == Vec4(1, 2, 3, 4));
+		CHECK(mat.GetColumn4(1) == Vec4(5, 6, 7, 8));
+		CHECK(mat.GetColumn4(2) == Vec4(9, 10, 11, 12));
+		CHECK(mat.GetTranslation() == DVec3(13, 14, 15));
+
+		DMat44 mat2(mat);
+
+		CHECK(mat2.GetColumn4(0) == Vec4(1, 2, 3, 4));
+		CHECK(mat2.GetColumn4(1) == Vec4(5, 6, 7, 8));
+		CHECK(mat2.GetColumn4(2) == Vec4(9, 10, 11, 12));
+		CHECK(mat2.GetTranslation() == DVec3(13, 14, 15));
+	}
+
+	TEST_CASE("TestDMat44Scale")
+	{
+		CHECK(DMat44::sScale(Vec3(2, 3, 4)) == DMat44(Vec4(2, 0, 0, 0), Vec4(0, 3, 0, 0), Vec4(0, 0, 4, 0), DVec3(0, 0, 0)));
+	}
+
+	TEST_CASE("TestDMat44Rotation")
+	{
+		DMat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), DVec3(13, 14, 15));
+		CHECK(mat.GetRotation() == Mat44(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(0, 0, 0, 1)));
+	}
+
+	TEST_CASE("TestMat44SetRotation")
+	{
+		DMat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), DVec3(13, 14, 15));
+		Mat44 mat2(Vec4(17, 18, 19, 20), Vec4(21, 22, 23, 24), Vec4(25, 26, 27, 28), Vec4(29, 30, 31, 32));
+
+		mat.SetRotation(mat2);
+		CHECK(mat == DMat44(Vec4(17, 18, 19, 20), Vec4(21, 22, 23, 24), Vec4(25, 26, 27, 28), DVec3(13, 14, 15)));
+	}
+
+	TEST_CASE("TestDMat44Rotation")
+	{
+		Quat q = Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.2f * JPH_PI);
+		CHECK(DMat44::sRotation(q).ToMat44() == Mat44::sRotation(q));
+	}
+
+	TEST_CASE("TestDMat44Translation")
+	{
+		CHECK(DMat44::sTranslation(DVec3(1, 2, 3)) == DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3(1, 2, 3)));
+	}
+
+	TEST_CASE("TestDMat44RotationTranslation")
+	{
+		Quat q = Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.2f * JPH_PI);
+		CHECK(DMat44::sRotationTranslation(q, DVec3(1, 2, 3)).ToMat44() == Mat44::sRotationTranslation(q, Vec3(1, 2, 3)));
+	}
+
+	TEST_CASE("TestDMat44MultiplyMat44")
+	{
+		DMat44 mat(Vec4(1, 2, 3, 0), Vec4(5, 6, 7, 0), Vec4(9, 10, 11, 0), DVec3(13, 14, 15));
+		Mat44 mat2(Vec4(17, 18, 19, 0), Vec4(21, 22, 23, 0), Vec4(25, 26, 27, 0), Vec4(29, 30, 31, 1));
+
+		DMat44 result = mat * mat2;
+		CHECK(result == DMat44(Vec4(278, 332, 386, 0), Vec4(338, 404, 470, 0), Vec4(398, 476, 554, 0), DVec3(471, 562, 653)));
+	}
+
+	TEST_CASE("TestDMat44MultiplyDMat44")
+	{
+		DMat44 mat(Vec4(1, 2, 3, 0), Vec4(5, 6, 7, 0), Vec4(9, 10, 11, 0), DVec3(13, 14, 15));
+		DMat44 mat2(Vec4(17, 18, 19, 0), Vec4(21, 22, 23, 0), Vec4(25, 26, 27, 0), DVec3(29, 30, 31));
+
+		DMat44 result = mat * mat2;
+		CHECK(result == DMat44(Vec4(278, 332, 386, 0), Vec4(338, 404, 470, 0), Vec4(398, 476, 554, 0), DVec3(471, 562, 653)));
+	}
+
+	TEST_CASE("TestDMat44MultiplyVec3")
+	{
+		DMat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), DVec3(13, 14, 15));
+		Vec3 vec(17, 18, 19);
+
+		DVec3 result = mat * DVec3(vec);
+		CHECK(result == DVec3(291, 346, 401));
+
+		DVec3 result2 = mat * vec;
+		CHECK(result2 == DVec3(291, 346, 401));
+
+		Vec3 result3 = mat.Multiply3x3(vec);
+		CHECK(result3 == Vec3(278, 332, 386));
+
+		Vec3 result4 = mat.Multiply3x3Transposed(vec);
+		CHECK(result4 == Vec3(110, 326, 542));
+	}
+
+	TEST_CASE("TestDMat44Inversed")
+	{
+		DMat44 mat(Vec4(1, 16, 2, 0), Vec4(2, 8, 4, 0), Vec4(8, 4, 1, 0), DVec3(4, 2, 8));
+		DMat44 inverse = mat.Inversed();
+		DMat44 identity = mat * inverse;
+		CHECK_APPROX_EQUAL(identity, DMat44::sIdentity());
+	}
+
+	TEST_CASE("TestDMat44InverseRotateTranslate")
+	{
+		Quat rot = Quat::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);
+		DVec3 pos(2, 3, 4);
+
+		DMat44 m1 = DMat44::sRotationTranslation(rot, pos).Inversed();
+		DMat44 m2 = DMat44::sInverseRotationTranslation(rot, pos);
+
+		CHECK_APPROX_EQUAL(m1, m2);
+	}
+
+	TEST_CASE("TestDMat44InversedRotationTranslation")
+	{
+		Quat rot = Quat::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);
+		DVec3 pos(2, 3, 4);
+
+		DMat44 m1 = DMat44::sRotationTranslation(rot, pos).InversedRotationTranslation();
+		DMat44 m2 = DMat44::sInverseRotationTranslation(rot, pos);
+
+		CHECK_APPROX_EQUAL(m1, m2);
+	}
+
+	TEST_CASE("TestDMat44PrePostScaled")
+	{
+		DMat44 m(Vec4(2, 3, 4, 0), Vec4(5, 6, 7, 0), Vec4(8, 9, 10, 0), DVec3(11, 12, 13));
+		Vec3 v(14, 15, 16);
+
+		CHECK(m.PreScaled(v) == m * DMat44::sScale(v));
+		CHECK(m.PostScaled(v) == DMat44::sScale(v) * m);
+	}
+
+	TEST_CASE("TestDMat44PrePostTranslated")
+	{
+		DMat44 m(Vec4(2, 3, 4, 0), Vec4(5, 6, 7, 0), Vec4(8, 9, 10, 0), DVec3(11, 12, 13));
+		Vec3 v(14, 15, 16);
+
+		CHECK_APPROX_EQUAL(m.PreTranslated(v), m * DMat44::sTranslation(DVec3(v)));
+		CHECK_APPROX_EQUAL(m.PostTranslated(v), DMat44::sTranslation(DVec3(v)) * m);
+	}
+
+	TEST_CASE("TestDMat44Decompose")
+	{
+		// Create a rotation/translation matrix
+		Quat rot = Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.2f * JPH_PI);
+		DVec3 pos(2, 3, 4);
+		DMat44 rotation_translation = DMat44::sRotationTranslation(rot, pos);
+
+		// Scale the matrix
+		Vec3 scale(2, 1, 3);
+		DMat44 m1 = rotation_translation * DMat44::sScale(scale);
+
+		// Decompose scale
+		Vec3 scale_out;
+		DMat44 m2 = m1.Decompose(scale_out);
+
+		// Check individual components
+		CHECK_APPROX_EQUAL(rotation_translation, m2);
+		CHECK_APPROX_EQUAL(scale, scale_out);
+	}
+
+	TEST_CASE("TestDMat44ToMat44")
+	{
+		DMat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), DVec3(13, 14, 15));
+		CHECK(mat.ToMat44() == Mat44(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 1)));
+	}
+
+	TEST_CASE("TestDMat44Column")
+	{
+		DMat44 mat = DMat44::sZero();
+		mat.SetColumn4(0, Vec4(1, 2, 3, 4));
+		CHECK(mat.GetColumn4(0) == Vec4(1, 2, 3, 4));
+		mat.SetColumn3(0, Vec3(5, 6, 7));
+		CHECK(mat.GetColumn3(0) == Vec3(5, 6, 7));
+		CHECK(mat.GetColumn4(0) == Vec4(5, 6, 7, 0));
+
+		mat.SetAxisX(Vec3(8, 9, 10));
+		mat.SetAxisY(Vec3(11, 12, 13));
+		mat.SetAxisZ(Vec3(14, 15, 16));
+		mat.SetTranslation(DVec3(17, 18, 19));
+		CHECK(mat.GetAxisX() == Vec3(8, 9, 10));
+		CHECK(mat.GetAxisY() == Vec3(11, 12, 13));
+		CHECK(mat.GetAxisZ() == Vec3(14, 15, 16));
+		CHECK(mat.GetTranslation() == DVec3(17, 18, 19));
+	}
+
+	TEST_CASE("TestDMat44Transposed")
+	{
+		DMat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), DVec3(13, 14, 15));
+		Mat44 result = mat.Transposed3x3();
+		CHECK(result == Mat44(Vec4(1, 5, 9, 0), Vec4(2, 6, 10, 0), Vec4(3, 7, 11, 0), Vec4(0, 0, 0, 1)));
+	}
+
+	TEST_CASE("TestDMat44GetQuaternion")
+	{
+		Quat rot = Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.2f * JPH_PI);
+		DMat44 mat = DMat44::sRotation(rot);
+		CHECK_APPROX_EQUAL(mat.GetQuaternion(), rot);
+	}
+
+	TEST_CASE("TestDMat44PrePostTranslated")
+	{
+		DMat44 m(Vec4(2, 3, 4, 0), Vec4(5, 6, 7, 0), Vec4(8, 9, 10, 0), DVec3(11, 12, 13));
+		DVec3 v(14, 15, 16);
+
+		CHECK(m.PreTranslated(v) == m * DMat44::sTranslation(v));
+		CHECK(m.PostTranslated(v) == DMat44::sTranslation(v) * m);
+	}
+
+	TEST_CASE("TestDMat44ConvertToString")
+	{
+		DMat44 v(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), DVec3(13, 14, 15));
+		CHECK(ConvertToString(v) == "1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15");
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/DVec3Tests.cpp

@@ -0,0 +1,308 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/DVec3.h>
+#include <Jolt/Core/StringTools.h>
+
+TEST_SUITE("DVec3Tests")
+{
+	TEST_CASE("TestDVec3Zero")
+	{
+		DVec3 v = DVec3::sZero();
+
+		CHECK(v.GetX() == 0);
+		CHECK(v.GetY() == 0);
+		CHECK(v.GetZ() == 0);
+	}
+
+	TEST_CASE("TestDVec3Axis")
+	{
+		CHECK(DVec3::sAxisX() == DVec3(1, 0, 0));
+		CHECK(DVec3::sAxisY() == DVec3(0, 1, 0));
+		CHECK(DVec3::sAxisZ() == DVec3(0, 0, 1));
+	}
+
+	TEST_CASE("TestVec3NaN")
+	{
+		DVec3 v = DVec3::sNaN();
+
+		CHECK(isnan(v.GetX()));
+		CHECK(isnan(v.GetY()));
+		CHECK(isnan(v.GetZ()));
+		CHECK(v.IsNaN());
+
+		v.SetComponent(0, 0);
+		CHECK(v.IsNaN());
+		v.SetComponent(1, 0);
+		CHECK(v.IsNaN());
+		v.SetComponent(2, 0);
+		CHECK(!v.IsNaN());
+	}
+
+	TEST_CASE("TestDVec3ConstructComponents")
+	{
+		DVec3 v(1, 2, 3);
+
+		// Test component access
+		CHECK(v.GetX() == 1);
+		CHECK(v.GetY() == 2);
+		CHECK(v.GetZ() == 3);
+
+		// Test component access by [] operators
+		CHECK(v[0] == 1);
+		CHECK(v[1] == 2);
+		CHECK(v[2] == 3);
+
+		// Test == and != operators
+		CHECK(v == DVec3(1, 2, 3));
+		CHECK(v != DVec3(1, 2, 4));
+
+		// Set the components
+		v.SetComponent(0, 4);
+		v.SetComponent(1, 5);
+		v.SetComponent(2, 6);
+		CHECK(v == DVec3(4, 5, 6));
+
+		// Set the components again
+		v.SetX(7);
+		v.SetY(8);
+		v.SetZ(9);
+		CHECK(v == DVec3(7, 8, 9));
+
+		// Set all components
+		v.Set(10, 11, 12);
+		CHECK(v == DVec3(10, 11, 12));
+	}
+
+	TEST_CASE("TestVec4ToDVec3")
+	{
+		CHECK(DVec3(Vec4(1, 3, 5, 7)) == DVec3(1, 3, 5));
+	}
+
+	TEST_CASE("TestDVec3Replicate")
+	{
+		CHECK(DVec3::sReplicate(2) == DVec3(2, 2, 2));
+	}
+
+	TEST_CASE("TestDVec3ToVec3")
+	{
+		CHECK(Vec3(DVec3(1, 3, 5)) == Vec3(1, 3, 5));
+
+		// Check rounding up and down
+		CHECK(DVec3(2.0, 0x1.0000000000001p1, -0x1.0000000000001p1).ToVec3RoundUp() == Vec3(2.0, 0x1.000002p1f, -2.0));
+		CHECK(DVec3(2.0, 0x1.0000000000001p1, -0x1.0000000000001p1).ToVec3RoundDown() == Vec3(2.0, 2.0, -0x1.000002p1f));
+	}
+
+	TEST_CASE("TestVec3MinMax")
+	{
+		DVec3 v1(1, 5, 3);
+		DVec3 v2(4, 2, 6);
+
+		CHECK(DVec3::sMin(v1, v2) == DVec3(1, 2, 3));
+		CHECK(DVec3::sMax(v1, v2) == DVec3(4, 5, 6));
+	}
+
+	TEST_CASE("TestDVec3Clamp")
+	{
+		DVec3 v1(1, 2, 3);
+		DVec3 v2(4, 5, 6);
+		DVec3 v(-1, 3, 7);
+
+		CHECK(DVec3::sClamp(v, v1, v2) == DVec3(1, 3, 6));
+	}
+
+	TEST_CASE("TestDVec3Trues")
+	{
+		CHECK(DVec3(DVec3::cFalse, DVec3::cFalse, DVec3::cFalse).GetTrues() == 0b0000);
+		CHECK(DVec3(DVec3::cTrue, DVec3::cFalse, DVec3::cFalse).GetTrues() == 0b0001);
+		CHECK(DVec3(DVec3::cFalse, DVec3::cTrue, DVec3::cFalse).GetTrues() == 0b0010);
+		CHECK(DVec3(DVec3::cTrue, DVec3::cTrue, DVec3::cFalse).GetTrues() == 0b0011);
+		CHECK(DVec3(DVec3::cFalse, DVec3::cFalse, DVec3::cTrue).GetTrues() == 0b0100);
+		CHECK(DVec3(DVec3::cTrue, DVec3::cFalse, DVec3::cTrue).GetTrues() == 0b0101);
+		CHECK(DVec3(DVec3::cFalse, DVec3::cTrue, DVec3::cTrue).GetTrues() == 0b0110);
+		CHECK(DVec3(DVec3::cTrue, DVec3::cTrue, DVec3::cTrue).GetTrues() == 0b0111);
+
+		CHECK(!DVec3(DVec3::cFalse, DVec3::cFalse, DVec3::cFalse).TestAnyTrue());
+		CHECK(DVec3(DVec3::cTrue, DVec3::cFalse, DVec3::cFalse).TestAnyTrue());
+		CHECK(DVec3(DVec3::cFalse, DVec3::cTrue, DVec3::cFalse).TestAnyTrue());
+		CHECK(DVec3(DVec3::cTrue, DVec3::cTrue, DVec3::cFalse).TestAnyTrue());
+		CHECK(DVec3(DVec3::cFalse, DVec3::cFalse, DVec3::cTrue).TestAnyTrue());
+		CHECK(DVec3(DVec3::cTrue, DVec3::cFalse, DVec3::cTrue).TestAnyTrue());
+		CHECK(DVec3(DVec3::cFalse, DVec3::cTrue, DVec3::cTrue).TestAnyTrue());
+		CHECK(DVec3(DVec3::cTrue, DVec3::cTrue, DVec3::cTrue).TestAnyTrue());
+
+		CHECK(!DVec3(DVec3::cFalse, DVec3::cFalse, DVec3::cFalse).TestAllTrue());
+		CHECK(!DVec3(DVec3::cTrue, DVec3::cFalse, DVec3::cFalse).TestAllTrue());
+		CHECK(!DVec3(DVec3::cFalse, DVec3::cTrue, DVec3::cFalse).TestAllTrue());
+		CHECK(!DVec3(DVec3::cTrue, DVec3::cTrue, DVec3::cFalse).TestAllTrue());
+		CHECK(!DVec3(DVec3::cFalse, DVec3::cFalse, DVec3::cTrue).TestAllTrue());
+		CHECK(!DVec3(DVec3::cTrue, DVec3::cFalse, DVec3::cTrue).TestAllTrue());
+		CHECK(!DVec3(DVec3::cFalse, DVec3::cTrue, DVec3::cTrue).TestAllTrue());
+		CHECK(DVec3(DVec3::cTrue, DVec3::cTrue, DVec3::cTrue).TestAllTrue());
+	}
+
+	TEST_CASE("TestDVec3Comparisons")
+	{
+		CHECK(DVec3::sEquals(DVec3(1, 2, 3), DVec3(1, 4, 3)).GetTrues() == 0b101); // Can't directly check if equal to (true, false, true) because true = -NaN and -NaN != -NaN
+		CHECK(DVec3::sLess(DVec3(1, 2, 4), DVec3(1, 4, 3)).GetTrues() == 0b010);
+		CHECK(DVec3::sLessOrEqual(DVec3(1, 2, 4), DVec3(1, 4, 3)).GetTrues() == 0b011);
+		CHECK(DVec3::sGreater(DVec3(1, 2, 4), DVec3(1, 4, 3)).GetTrues() == 0b100);
+		CHECK(DVec3::sGreaterOrEqual(DVec3(1, 2, 4), DVec3(1, 4, 3)).GetTrues() == 0b101);
+	}
+
+	TEST_CASE("TestDVec3FMA")
+	{
+		CHECK(DVec3::sFusedMultiplyAdd(DVec3(1, 2, 3), DVec3(4, 5, 6), DVec3(7, 8, 9)) == DVec3(1 * 4 + 7, 2 * 5 + 8, 3 * 6 + 9));
+	}
+
+	TEST_CASE("TestDVec3Select")
+	{
+		const double cTrue2 = BitCast<double>(uint64(1) << 63);
+		const double cFalse2 = BitCast<double>(~uint64(0) >> 1);
+
+		CHECK(DVec3::sSelect(DVec3(1, 2, 3), DVec3(4, 5, 6), DVec3(DVec3::cTrue, DVec3::cFalse, DVec3::cTrue)) == DVec3(4, 2, 6));
+		CHECK(DVec3::sSelect(DVec3(1, 2, 3), DVec3(4, 5, 6), DVec3(DVec3::cFalse, DVec3::cTrue, DVec3::cFalse)) == DVec3(1, 5, 3));
+		CHECK(DVec3::sSelect(DVec3(1, 2, 3), DVec3(4, 5, 6), DVec3(cTrue2, cFalse2, cTrue2)) == DVec3(4, 2, 6));
+		CHECK(DVec3::sSelect(DVec3(1, 2, 3), DVec3(4, 5, 6), DVec3(cFalse2, cTrue2, cFalse2)) == DVec3(1, 5, 3));
+	}
+
+	TEST_CASE("TestDVec3BitOps")
+	{
+		// Test all bit permutations
+		DVec3 v1(BitCast<double, uint64>(0b0011), BitCast<double, uint64>(0b00110), BitCast<double, uint64>(0b001100));
+		DVec3 v2(BitCast<double, uint64>(0b0101), BitCast<double, uint64>(0b01010), BitCast<double, uint64>(0b010100));
+
+		CHECK(DVec3::sOr(v1, v2) == DVec3(BitCast<double, uint64>(0b0111), BitCast<double, uint64>(0b01110), BitCast<double, uint64>(0b011100)));
+		CHECK(DVec3::sXor(v1, v2) == DVec3(BitCast<double, uint64>(0b0110), BitCast<double, uint64>(0b01100), BitCast<double, uint64>(0b011000)));
+		CHECK(DVec3::sAnd(v1, v2) == DVec3(BitCast<double, uint64>(0b0001), BitCast<double, uint64>(0b00010), BitCast<double, uint64>(0b000100)));
+	}
+
+	TEST_CASE("TestDVec3Close")
+	{
+		CHECK(DVec3(1, 2, 3).IsClose(DVec3(1.001, 2.001, 3.001), 1.0e-4));
+		CHECK(!DVec3(1, 2, 3).IsClose(DVec3(1.001, 2.001, 3.001), 1.0e-6));
+
+		CHECK(DVec3(1.001, 0, 0).IsNormalized(1.0e-2));
+		CHECK(!DVec3(0, 1.001, 0).IsNormalized(1.0e-4));
+
+		CHECK(DVec3(-1.0e-7, 1.0e-7, 1.0e-8).IsNearZero(1.0e-12));
+		CHECK(!DVec3(-1.0e-7, 1.0e-7, -1.0e-5).IsNearZero(1.0e-12));
+	}
+
+	TEST_CASE("TestDVec3Operators")
+	{
+		CHECK(-DVec3(1, 2, 3) == DVec3(-1, -2, -3));
+
+		DVec3 neg_zero = -DVec3::sZero();
+		CHECK(neg_zero == DVec3::sZero());
+
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		// When cross platform deterministic, we want to make sure that -0 is represented as 0
+		CHECK(BitCast<uint64>(neg_zero.GetX()) == 0);
+		CHECK(BitCast<uint64>(neg_zero.GetY()) == 0);
+		CHECK(BitCast<uint64>(neg_zero.GetZ()) == 0);
+	#endif // JPH_CROSS_PLATFORM_DETERMINISTIC
+
+		CHECK(DVec3(1, 2, 3) + Vec3(4, 5, 6) == DVec3(5, 7, 9));
+		CHECK(DVec3(1, 2, 3) - Vec3(6, 5, 4) == DVec3(-5, -3, -1));
+
+		CHECK(DVec3(1, 2, 3) + DVec3(4, 5, 6) == DVec3(5, 7, 9));
+		CHECK(DVec3(1, 2, 3) - DVec3(6, 5, 4) == DVec3(-5, -3, -1));
+
+		CHECK(DVec3(1, 2, 3) * DVec3(4, 5, 6) == DVec3(4, 10, 18));
+		CHECK(DVec3(1, 2, 3) * 2 == DVec3(2, 4, 6));
+		CHECK(4 * DVec3(1, 2, 3) == DVec3(4, 8, 12));
+
+		CHECK(DVec3(1, 2, 3) / 2 == DVec3(0.5, 1.0, 1.5));
+		CHECK(DVec3(1, 2, 3) / DVec3(2, 8, 24) == DVec3(0.5, 0.25, 0.125));
+
+		DVec3 v = DVec3(1, 2, 3);
+		v *= DVec3(4, 5, 6);
+		CHECK(v == DVec3(4, 10, 18));
+		v *= 2;
+		CHECK(v == DVec3(8, 20, 36));
+		v /= 2;
+		CHECK(v == DVec3(4, 10, 18));
+		v += DVec3(1, 2, 3);
+		CHECK(v == DVec3(5, 12, 21));
+		v -= DVec3(1, 2, 3);
+		CHECK(v == DVec3(4, 10, 18));
+		v += Vec3(1, 2, 3);
+		CHECK(v == DVec3(5, 12, 21));
+		v -= Vec3(1, 2, 3);
+		CHECK(v == DVec3(4, 10, 18));
+
+		CHECK(DVec3(2, 4, 8).Reciprocal() == DVec3(0.5, 0.25, 0.125));
+	}
+
+	TEST_CASE("TestDVec3Abs")
+	{
+		CHECK(DVec3(1, -2, 3).Abs() == DVec3(1, 2, 3));
+		CHECK(DVec3(-1, 2, -3).Abs() == DVec3(1, 2, 3));
+	}
+
+	TEST_CASE("TestDVec3Dot")
+	{
+		CHECK(DVec3(2, 3, 4).Dot(DVec3(5, 6, 7)) == double(2 * 5 + 3 * 6 + 4 * 7));
+	}
+
+	TEST_CASE("TestDVec3Length")
+	{
+		CHECK(DVec3(2, 3, 4).LengthSq() == double(4 + 9 + 16));
+		CHECK(DVec3(2, 3, 4).Length() == sqrt(double(4 + 9 + 16)));
+	}
+
+	TEST_CASE("TestDVec3Sqrt")
+	{
+		CHECK(DVec3(13, 15, 17).Sqrt() == DVec3(sqrt(13.0), sqrt(15.0), sqrt(17.0)));
+	}
+
+	TEST_CASE("TestDVec3Equals")
+	{
+		CHECK_FALSE(DVec3(13, 15, 17) == DVec3(13, 15, 19));
+		CHECK(DVec3(13, 15, 17) == DVec3(13, 15, 17));
+		CHECK(DVec3(13, 15, 17) != DVec3(13, 15, 19));
+	}
+
+	TEST_CASE("TestDVec3LoadStoreDouble3Unsafe")
+	{
+		double d4[4] = { 1, 2, 3, 4 };
+		Double3 &d3 = *(Double3 *)d4;
+		DVec3 v = DVec3::sLoadDouble3Unsafe(d3);
+		DVec3 v2(1, 2, 3);
+		CHECK(v == v2);
+
+		Double3 d3_out;
+		DVec3(1, 2, 3).StoreDouble3(&d3_out);
+		CHECK(d3 == d3_out);
+	}
+
+	TEST_CASE("TestDVec3Cross")
+	{
+		CHECK(DVec3(1, 0, 0).Cross(DVec3(0, 1, 0)) == DVec3(0, 0, 1));
+		CHECK(DVec3(0, 1, 0).Cross(DVec3(1, 0, 0)) == DVec3(0, 0, -1));
+		CHECK(DVec3(0, 1, 0).Cross(DVec3(0, 0, 1)) == DVec3(1, 0, 0));
+		CHECK(DVec3(0, 0, 1).Cross(DVec3(0, 1, 0)) == DVec3(-1, 0, 0));
+		CHECK(DVec3(0, 0, 1).Cross(DVec3(1, 0, 0)) == DVec3(0, 1, 0));
+		CHECK(DVec3(1, 0, 0).Cross(DVec3(0, 0, 1)) == DVec3(0, -1, 0));
+	}
+
+	TEST_CASE("TestDVec3Normalize")
+	{
+		CHECK(DVec3(3, 2, 1).Normalized() == DVec3(3, 2, 1) / sqrt(9.0 + 4.0 + 1.0));
+	}
+
+	TEST_CASE("TestDVec3Sign")
+	{
+		CHECK(DVec3(1.2345, -6.7891, 0).GetSign() == DVec3(1, -1, 1));
+		CHECK(DVec3(0, 2.3456, -7.8912).GetSign() == DVec3(1, 1, -1));
+	}
+
+	TEST_CASE("TestDVec3ConvertToString")
+	{
+		DVec3 v(1, 2, 3);
+		CHECK(ConvertToString(v) == "1, 2, 3");
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/EigenValueSymmetricTests.cpp

@@ -0,0 +1,57 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/EigenValueSymmetric.h>
+#include <Jolt/Math/Matrix.h>
+
+TEST_SUITE("EigenValueSymmetricTests")
+{
+	TEST_CASE("TestEigenValueSymmetric")
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> angle_distribution(0, 2.0f * JPH_PI);
+		uniform_real_distribution<float> scale_distribution(0.1f, 10.0f);
+
+		for (int i = 0; i < 1000; ++i)
+		{
+			// Random scale vector
+			Vec3 scale(scale_distribution(random), scale_distribution(random), scale_distribution(random));
+
+			// Random rotation matrix
+			Mat44 rotation = Mat44::sRotation(Vec3::sRandom(random), angle_distribution(random));
+
+			// Construct a symmetric tensor from this rotation and scale
+			Mat44 tensor4 = rotation.Multiply3x3(Mat44::sScale(scale)).Multiply3x3RightTransposed(rotation);
+
+			// Get the eigenvalues and eigenvectors
+			Matrix<3, 3> tensor;
+			Matrix<3, 3> eigen_vec = Matrix<3, 3>::sIdentity();
+			Vector<3> eigen_val;
+			tensor.CopyPart(tensor4, 0, 0, 3, 3, 0, 0);
+			CHECK(EigenValueSymmetric(tensor, eigen_vec, eigen_val));
+
+			for (int c = 0; c < 3; ++c)
+			{
+				// Check that we found a valid eigenvalue
+				bool found = false;
+				for (int c2 = 0; c2 < 3; ++c2)
+					if (abs(scale[c2] - eigen_val[c]) < 1.0e-5f)
+					{
+						found = true;
+						break;
+					}
+				CHECK(found);
+
+				// Check if the eigenvector is normalized
+				CHECK(eigen_vec.GetColumn(c).IsNormalized());
+
+				// Check if matrix * eigen_vector = eigen_value * eigen_vector
+				Vector mat_eigvec = tensor * eigen_vec.GetColumn(c);
+				Vector eigval_eigvec = eigen_val[c] * eigen_vec.GetColumn(c);
+				CHECK(mat_eigvec.IsClose(eigval_eigvec, Square(1.0e-5f)));
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/HalfFloatTests.cpp

@@ -0,0 +1,98 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/HalfFloat.h>
+
+TEST_SUITE("HalfFloatTests")
+{
+#if defined(JPH_USE_F16C) || defined(JPH_USE_NEON)
+	TEST_CASE("TestHalfFloatToFloat")
+	{
+		// Check all half float values, 4 at a time, skip NaN's and INF
+		for (uint32 v = 0; v < 0x7c00; v += 2)
+		{
+			// Test value, next value and negative variants of both
+			UVec4 half_float(v | ((v + 1) << 16), (v | 0x8000) | (((v + 1) | 0x8000) << 16), 0, 0);
+
+			// Compare hardware intrinsic version with fallback version
+			Vec4 flt1 = HalfFloatConversion::ToFloat(half_float);
+			Vec4 flt2 = HalfFloatConversion::ToFloatFallback(half_float);
+
+			UVec4 flt1_as_int = flt1.ReinterpretAsInt();
+			UVec4 flt2_as_int = flt2.ReinterpretAsInt();
+			if (flt1_as_int != flt2_as_int)
+				CHECK(false); // Not using CHECK(flt1_as_int == flt2_as_int) macros as that makes the test very slow
+		}
+	}
+
+	// Helper function to compare the intrinsics version with the fallback version
+	static inline void CheckFloatToHalfFloat(uint32 inValue, uint32 inSign)
+	{
+		const float fvalue = BitCast<float>(inValue + inSign * 0x80000000U);
+
+		HalfFloat hf1 = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEAREST>(fvalue);
+		HalfFloat hf2 = HalfFloatConversion::FromFloatFallback<HalfFloatConversion::ROUND_TO_NEAREST>(fvalue);
+		bool result = (hf1 == hf2);
+		if (!result)
+			CHECK(false); // Not using CHECK(hf1 == hf2) macros as that makes the test very slow
+
+		hf1 = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(fvalue);
+		hf2 = HalfFloatConversion::FromFloatFallback<HalfFloatConversion::ROUND_TO_POS_INF>(fvalue);
+		result = (hf1 == hf2);
+		if (!result)
+			CHECK(false);
+
+		hf1 = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(fvalue);
+		hf2 = HalfFloatConversion::FromFloatFallback<HalfFloatConversion::ROUND_TO_NEG_INF>(fvalue);
+		result = (hf1 == hf2);
+		if (!result)
+			CHECK(false);
+	}
+
+	TEST_CASE("TestFloatToHalfFloat")
+	{
+		for (uint32 sign = 0; sign < 2; ++sign)
+		{
+			// Zero and smallest possible float
+			for (uint32 value = 0; value < 2; value++)
+				CheckFloatToHalfFloat(value, sign);
+
+			// Floats that are large enough to become a denormalized half float, incrementing by smallest increment that can make a difference
+			for (uint32 value = (HalfFloatConversion::FLOAT_EXPONENT_BIAS - HalfFloatConversion::HALF_FLT_EXPONENT_BIAS - HalfFloatConversion::HALF_FLT_MANTISSA_BITS) << HalfFloatConversion::FLOAT_EXPONENT_POS; value < HalfFloatConversion::FLOAT_EXPONENT_MASK << HalfFloatConversion::FLOAT_EXPONENT_POS; value += 1 << (HalfFloatConversion::FLOAT_MANTISSA_BITS - HalfFloatConversion::HALF_FLT_MANTISSA_BITS - 2))
+				CheckFloatToHalfFloat(value, sign);
+
+			// INF
+			CheckFloatToHalfFloat(0x7f800000U, sign);
+
+			// Nan
+			CheckFloatToHalfFloat(0x7fc00000U, sign);
+		}
+	}
+#endif
+
+	TEST_CASE("TestHalfFloatINF")
+	{
+		// Float -> half float
+		CHECK(HalfFloatConversion::FromFloatFallback<HalfFloatConversion::ROUND_TO_NEAREST>(BitCast<float>(0x7f800000U)) == HALF_FLT_INF);
+		CHECK(HalfFloatConversion::FromFloatFallback<HalfFloatConversion::ROUND_TO_NEAREST>(BitCast<float>(0xff800000U)) == HALF_FLT_INF_NEGATIVE);
+
+		// Half float -> float
+		UVec4 half_float(uint32(HALF_FLT_INF) | (uint32(HALF_FLT_INF_NEGATIVE) << 16), 0, 0, 0);
+		UVec4 flt = HalfFloatConversion::ToFloatFallback(half_float).ReinterpretAsInt();
+		CHECK(flt == UVec4(0x7f800000U, 0xff800000U, 0, 0));
+	}
+
+	TEST_CASE("TestHalfFloatNaN")
+	{
+		// Float -> half float
+		CHECK(HalfFloatConversion::FromFloatFallback<HalfFloatConversion::ROUND_TO_NEAREST>(BitCast<float>(0x7fc00000U)) == HALF_FLT_NANQ);
+		CHECK(HalfFloatConversion::FromFloatFallback<HalfFloatConversion::ROUND_TO_NEAREST>(BitCast<float>(0xffc00000U)) == HALF_FLT_NANQ_NEGATIVE);
+
+		// Half float -> float
+		UVec4 half_float(uint32(HALF_FLT_NANQ) | (uint32(HALF_FLT_NANQ_NEGATIVE) << 16), 0, 0, 0);
+		UVec4 flt = HalfFloatConversion::ToFloatFallback(half_float).ReinterpretAsInt();
+		CHECK(flt == UVec4(0x7fc00000U, 0xffc00000U, 0, 0));
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/Mat44Tests.cpp

@@ -0,0 +1,570 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/Mat44.h>
+#include <Jolt/Core/StringTools.h>
+
+TEST_SUITE("Mat44Tests")
+{
+	TEST_CASE("TestMat44Zero")
+	{
+		Mat44 zero = Mat44::sZero();
+
+		for (int row = 0; row < 4; ++row)
+			for (int col = 0; col < 4; ++col)
+				CHECK(zero(row, col) == 0.0f);
+	}
+
+	TEST_CASE("TestMat44Column")
+	{
+		Mat44 mat = Mat44::sZero();
+		mat.SetColumn4(0, Vec4(1, 2, 3, 4));
+		CHECK(mat.GetColumn4(0) == Vec4(1, 2, 3, 4));
+		mat.SetColumn3(0, Vec3(5, 6, 7));
+		CHECK(mat.GetColumn3(0) == Vec3(5, 6, 7));
+		CHECK(mat.GetColumn4(0) == Vec4(5, 6, 7, 0));
+
+		mat.SetAxisX(Vec3(8, 9, 10));
+		mat.SetAxisY(Vec3(11, 12, 13));
+		mat.SetAxisZ(Vec3(14, 15, 16));
+		mat.SetTranslation(Vec3(17, 18, 19));
+		CHECK(mat.GetAxisX() == Vec3(8, 9, 10));
+		CHECK(mat.GetAxisY() == Vec3(11, 12, 13));
+		CHECK(mat.GetAxisZ() == Vec3(14, 15, 16));
+		CHECK(mat.GetTranslation() == Vec3(17, 18, 19));
+
+		mat.SetDiagonal3(Vec3(20, 21, 22));
+		CHECK(mat.GetDiagonal3() == Vec3(20, 21, 22));
+		CHECK(mat.GetAxisX() == Vec3(20, 9, 10));
+		CHECK(mat.GetAxisY() == Vec3(11, 21, 13));
+		CHECK(mat.GetAxisZ() == Vec3(14, 15, 22));
+
+		mat.SetDiagonal4(Vec4(23, 24, 25, 26));
+		CHECK(mat.GetDiagonal4() == Vec4(23, 24, 25, 26));
+		CHECK(mat.GetAxisX() == Vec3(23, 9, 10));
+		CHECK(mat.GetAxisY() == Vec3(11, 24, 13));
+		CHECK(mat.GetAxisZ() == Vec3(14, 15, 25));
+		CHECK(mat.GetColumn4(3) == Vec4(17, 18, 19, 26));
+	}
+
+	TEST_CASE("TestMat44NaN")
+	{
+		Mat44 nan = Mat44::sNaN();
+
+		for (int row = 0; row < 4; ++row)
+			for (int col = 0; col < 4; ++col)
+				CHECK(isnan(nan(row, col)));
+	}
+
+	TEST_CASE("TestMat44Identity")
+	{
+		Mat44 identity = Mat44::sIdentity();
+
+		for (int row = 0; row < 4; ++row)
+			for (int col = 0; col < 4; ++col)
+				if (row != col)
+					CHECK(identity(row, col) == 0.0f);
+				else
+					CHECK(identity(row, col) == 1.0f);
+	}
+
+	TEST_CASE("TestMat44Construct")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+
+		CHECK(mat(0, 0) == 1.0f);
+		CHECK(mat(1, 0) == 2.0f);
+		CHECK(mat(2, 0) == 3.0f);
+		CHECK(mat(3, 0) == 4.0f);
+
+		CHECK(mat(0, 1) == 5.0f);
+		CHECK(mat(1, 1) == 6.0f);
+		CHECK(mat(2, 1) == 7.0f);
+		CHECK(mat(3, 1) == 8.0f);
+
+		CHECK(mat(0, 2) == 9.0f);
+		CHECK(mat(1, 2) == 10.0f);
+		CHECK(mat(2, 2) == 11.0f);
+		CHECK(mat(3, 2) == 12.0f);
+
+		CHECK(mat(0, 3) == 13.0f);
+		CHECK(mat(1, 3) == 14.0f);
+		CHECK(mat(2, 3) == 15.0f);
+		CHECK(mat(3, 3) == 16.0f);
+
+		Mat44 mat2(mat);
+
+		CHECK(mat2(0, 0) == 1.0f);
+		CHECK(mat2(1, 0) == 2.0f);
+		CHECK(mat2(2, 0) == 3.0f);
+		CHECK(mat2(3, 0) == 4.0f);
+
+		CHECK(mat2(0, 1) == 5.0f);
+		CHECK(mat2(1, 1) == 6.0f);
+		CHECK(mat2(2, 1) == 7.0f);
+		CHECK(mat2(3, 1) == 8.0f);
+
+		CHECK(mat2(0, 2) == 9.0f);
+		CHECK(mat2(1, 2) == 10.0f);
+		CHECK(mat2(2, 2) == 11.0f);
+		CHECK(mat2(3, 2) == 12.0f);
+
+		CHECK(mat2(0, 3) == 13.0f);
+		CHECK(mat2(1, 3) == 14.0f);
+		CHECK(mat2(2, 3) == 15.0f);
+		CHECK(mat2(3, 3) == 16.0f);
+
+		// Check equal
+		CHECK(mat == mat2);
+		CHECK(!(mat != mat2));
+
+		// Make unequal
+		mat(3, 3) = 1;
+
+		// Check non-equal
+		CHECK(!(mat == mat2));
+		CHECK(mat != mat2);
+	}
+
+	TEST_CASE("TestMat44IsClose")
+	{
+		Mat44 mat = Mat44::sIdentity();
+		Mat44 mat2(mat);
+
+		CHECK(mat.IsClose(mat2, Square(0.1f)));
+
+		mat2(0, 1) = 0.09f;
+		CHECK(mat.IsClose(mat2, Square(0.1f)));
+
+		mat2(0, 1) = 0.11f;
+		CHECK(!mat.IsClose(mat2, Square(0.1f)));
+	}
+
+	TEST_CASE("TestMat44Translation")
+	{
+		CHECK(Mat44::sTranslation(Vec3(2, 3, 4)) == Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(2, 3, 4, 1)));
+	}
+
+	TEST_CASE("TestMat44Scale")
+	{
+		CHECK(Mat44::sScale(2) == Mat44(Vec4(2, 0, 0, 0), Vec4(0, 2, 0, 0), Vec4(0, 0, 2, 0), Vec4(0, 0, 0, 1)));
+		CHECK(Mat44::sScale(Vec3(2, 3, 4)) == Mat44(Vec4(2, 0, 0, 0), Vec4(0, 3, 0, 0), Vec4(0, 0, 4, 0), Vec4(0, 0, 0, 1)));
+	}
+
+	TEST_CASE("TestMat44Rotation")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 0), Vec4(5, 6, 7, 0), Vec4(9, 10, 11, 0), Vec4(13, 14, 15, 16));
+		CHECK(mat.GetRotation() == Mat44(Vec4(1, 2, 3, 0), Vec4(5, 6, 7, 0), Vec4(9, 10, 11, 0), Vec4(0, 0, 0, 1)));
+	}
+
+	TEST_CASE("TestMat44SetRotation")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Mat44 mat2(Vec4(17, 18, 19, 20), Vec4(21, 22, 23, 24), Vec4(25, 26, 27, 28), Vec4(29, 30, 31, 32));
+
+		mat.SetRotation(mat2);
+		CHECK(mat == Mat44(Vec4(17, 18, 19, 20), Vec4(21, 22, 23, 24), Vec4(25, 26, 27, 28), Vec4(13, 14, 15, 16)));
+	}
+
+	TEST_CASE("TestMat44RotationSafe")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		CHECK(mat.GetRotationSafe() == Mat44(Vec4(1, 2, 3, 0), Vec4(5, 6, 7, 0), Vec4(9, 10, 11, 0), Vec4(0, 0, 0, 1)));
+	}
+
+	TEST_CASE("TestMat44LoadStore")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+
+		Float4 storage[4];
+		mat.StoreFloat4x4(storage);
+
+		CHECK(storage[0].x == 1.0f);
+		CHECK(storage[0].y == 2.0f);
+		CHECK(storage[0].z == 3.0f);
+		CHECK(storage[0].w == 4.0f);
+
+		CHECK(storage[1].x == 5.0f);
+		CHECK(storage[1].y == 6.0f);
+		CHECK(storage[1].z == 7.0f);
+		CHECK(storage[1].w == 8.0f);
+
+		CHECK(storage[2].x == 9.0f);
+		CHECK(storage[2].y == 10.0f);
+		CHECK(storage[2].z == 11.0f);
+		CHECK(storage[2].w == 12.0f);
+
+		CHECK(storage[3].x == 13.0f);
+		CHECK(storage[3].y == 14.0f);
+		CHECK(storage[3].z == 15.0f);
+		CHECK(storage[3].w == 16.0f);
+
+		Mat44 mat2 = Mat44::sLoadFloat4x4(storage);
+		CHECK(mat2 == mat);
+	}
+
+	TEST_CASE("TestMat44LoadAligned")
+	{
+		alignas(16) float values[16] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 };
+		Mat44 mat = Mat44::sLoadFloat4x4Aligned((const Float4 *)values);
+		CHECK(mat == Mat44(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16)));
+	}
+
+	TEST_CASE("TestMat44MultiplyMat44")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Mat44 mat2(Vec4(17, 18, 19, 20), Vec4(21, 22, 23, 24), Vec4(25, 26, 27, 28), Vec4(29, 30, 31, 32));
+
+		Mat44 result = mat * mat2;
+		CHECK(result == Mat44(Vec4(538, 612, 686, 760), Vec4(650, 740, 830, 920), Vec4(762, 868, 974, 1080), Vec4(874, 996, 1118, 1240)));
+	}
+
+	TEST_CASE("TestMat44Add")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Mat44 mat2(Vec4(17, 18, 19, 20), Vec4(21, 22, 23, 24), Vec4(25, 26, 27, 28), Vec4(29, 30, 31, 32));
+
+		Mat44 result = mat + mat2;
+		CHECK(result == Mat44(Vec4(18, 20, 22, 24), Vec4(26, 28, 30, 32), Vec4(34, 36, 38, 40), Vec4(42, 44, 46, 48)));
+
+		mat += mat2;
+		CHECK(mat == Mat44(Vec4(18, 20, 22, 24), Vec4(26, 28, 30, 32), Vec4(34, 36, 38, 40), Vec4(42, 44, 46, 48)));
+	}
+
+	TEST_CASE("TestMat44Sub")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Mat44 mat2(Vec4(32, 31, 30, 29), Vec4(28, 27, 26, 25), Vec4(24, 23, 22, 21), Vec4(20, 19, 18, 17));
+
+		Mat44 result = mat - mat2;
+		CHECK(result == Mat44(Vec4(-31, -29, -27, -25), Vec4(-23, -21, -19, -17), Vec4(-15, -13, -11, -9), Vec4(-7, -5, -3, -1)));
+	}
+
+	TEST_CASE("TestMat44Negate")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+
+		Mat44 result = -mat;
+		CHECK(result == Mat44(Vec4(-1, -2, -3, -4), Vec4(-5, -6, -7, -8), Vec4(-9, -10, -11, -12), Vec4(-13, -14, -15, -16)));
+	}
+
+	TEST_CASE("TestMat44MultiplyVec3")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Vec3 vec(17, 18, 19);
+
+		Vec3 result = mat * vec;
+		CHECK(result == Vec3(291, 346, 401));
+
+		result = mat.Multiply3x3(vec);
+		CHECK(result == Vec3(278, 332, 386));
+
+		result = mat.Multiply3x3Transposed(vec);
+		CHECK(result == Vec3(110, 326, 542));
+	}
+
+	TEST_CASE("TestMat44MultiplyVec4")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Vec4 vec(17, 18, 19, 20);
+
+		Vec4 result = mat * vec;
+		CHECK(result == Vec4(538, 612, 686, 760));
+	}
+
+	TEST_CASE("TestMat44Scale")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Mat44 result = mat * 2.0f;
+		CHECK(result == Mat44(Vec4(2, 4, 6, 8), Vec4(10, 12, 14, 16), Vec4(18, 20, 22, 24), Vec4(26, 28, 30, 32)));
+		CHECK(result != mat);
+		result *= 0.5f;
+		CHECK(result == mat);
+	}
+
+	TEST_CASE("TestMat44Transposed")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Mat44 result = mat.Transposed();
+		CHECK(result == Mat44(Vec4(1, 5, 9, 13), Vec4(2, 6, 10, 14), Vec4(3, 7, 11, 15), Vec4(4, 8, 12, 16)));
+	}
+
+	TEST_CASE("TestMat44Transposed3x3")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Mat44 result = mat.Transposed3x3();
+		CHECK(result == Mat44(Vec4(1, 5, 9, 0), Vec4(2, 6, 10, 0), Vec4(3, 7, 11, 0), Vec4(0, 0, 0, 1)));
+	}
+
+	TEST_CASE("TestMat44Multiply3x3")
+	{
+		Mat44 mat1(Vec4(1, 2, 3, 0), Vec4(4, 5, 6, 0), Vec4(7, 8, 9, 0), Vec4(10, 11, 12, 1));
+		Mat44 mat2(Vec4(13, 14, 15, 0), Vec4(16, 17, 18, 0), Vec4(19, 20, 21, 0), Vec4(22, 23, 24, 1));
+		Mat44 result = mat1.Multiply3x3(mat2);
+		CHECK(result.GetColumn4(3) == Vec4(0, 0, 0, 1));
+		Mat44 result2 = mat1.GetRotationSafe() * mat2.GetRotationSafe();
+		CHECK(result == result2);
+	}
+
+	TEST_CASE("TestMat44Multiply3x3LeftTransposed")
+	{
+		Mat44 mat1(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		Mat44 mat2(Vec4(17, 18, 19, 20), Vec4(21, 22, 23, 24), Vec4(25, 26, 27, 28), Vec4(29, 30, 31, 32));
+		Mat44 result = mat1.Multiply3x3LeftTransposed(mat2);
+		CHECK(result.GetColumn4(3) == Vec4(0, 0, 0, 1));
+		Mat44 result2 = mat1.GetRotationSafe().Transposed() * mat2.GetRotationSafe();
+		CHECK(result == result2);
+	}
+
+	TEST_CASE("TestMat44Multiply3x3RightTransposed")
+	{
+		Mat44 mat1(Vec4(1, 2, 3, 0), Vec4(4, 5, 6, 0), Vec4(7, 8, 9, 0), Vec4(10, 11, 12, 1));
+		Mat44 mat2(Vec4(13, 14, 15, 0), Vec4(16, 17, 18, 0), Vec4(19, 20, 21, 0), Vec4(22, 23, 24, 1));
+		Mat44 result = mat1.Multiply3x3RightTransposed(mat2);
+		CHECK(result.GetColumn4(3) == Vec4(0, 0, 0, 1));
+		Mat44 result2 = mat1.GetRotationSafe() * mat2.GetRotationSafe().Transposed();
+		CHECK(result == result2);
+	}
+
+	TEST_CASE("TestMat44Inversed")
+	{
+		Mat44 mat(Vec4(0, 2, 0, 8), Vec4(4, 0, 16, 0), Vec4(0, 16, 0, 4), Vec4(8, 0, 2, 0));
+		Mat44 inverse = mat.Inversed();
+		Mat44 identity = mat * inverse;
+		CHECK(identity == Mat44::sIdentity());
+	}
+
+	TEST_CASE("TestMat44Inversed3x3")
+	{
+		Mat44 mat(Vec4(1, 2, 0, 0), Vec4(4, 0, 8, 0), Vec4(0, 16, 0, 0), Vec4(1, 2, 3, 1));
+		Mat44 inverse = mat.Inversed3x3();
+		CHECK(inverse.GetColumn4(3) == Vec4(0, 0, 0, 1));
+		Mat44 identity = mat.Multiply3x3(inverse);
+		CHECK(identity == Mat44::sIdentity());
+	}
+
+	TEST_CASE("TestMat44SetInversed3x3")
+	{
+		Mat44 mat(Vec4(1, 2, 0, 0), Vec4(4, 0, 8, 0), Vec4(0, 16, 0, 0), Vec4(1, 2, 3, 1));
+
+		// First test succeeding inverse
+		Mat44 inverse;
+		CHECK(inverse.SetInversed3x3(mat));
+		CHECK(inverse.GetColumn4(3) == Vec4(0, 0, 0, 1));
+		Mat44 identity = mat.Multiply3x3(inverse);
+		CHECK(identity == Mat44::sIdentity());
+
+		// Now make singular
+		mat(0, 0) = 0.0f;
+		CHECK(!inverse.SetInversed3x3(mat));
+	}
+
+	TEST_CASE("TestMat44GetDeterminant3x3")
+	{
+		Mat44 mat(Vec4(1, 2, 0, 0), Vec4(4, 0, 8, 0), Vec4(0, 16, 0, 0), Vec4(1, 2, 3, 1));
+		CHECK(mat.GetDeterminant3x3() == -128);
+	}
+
+	TEST_CASE("TestMat44Adjointed3x3")
+	{
+		Mat44 mat(Vec4(1, 2, 3, 0), Vec4(5, 6, 7, 0), Vec4(9, 10, 11, 0), Vec4(13, 14, 15, 16));
+		Mat44 result = mat.Adjointed3x3();
+		CHECK(result == Mat44(Vec4(-4, 8, -4, 0), Vec4(8, -16, 8, 0), Vec4(-4, 8, -4, 0), Vec4(0, 0, 0, 1)));
+	}
+
+	TEST_CASE("TestMat44RotationXYZ")
+	{
+		Mat44 rot = Mat44::sRotationX(0.5f * JPH_PI);
+		Vec3 v = rot * Vec3(1, 0, 0);
+		CHECK(v == Vec3(1, 0, 0));
+		v = rot * Vec3(0, 1, 0);
+		CHECK_APPROX_EQUAL(v, Vec3(0, 0, 1));
+		v = rot * Vec3(0, 0, 1);
+		CHECK_APPROX_EQUAL(v, Vec3(0, -1, 0));
+
+		rot = Mat44::sRotationY(0.5f * JPH_PI);
+		v = rot * Vec3(1, 0, 0);
+		CHECK_APPROX_EQUAL(v, Vec3(0, 0, -1));
+		v = rot * Vec3(0, 1, 0);
+		CHECK(v == Vec3(0, 1, 0));
+		v = rot * Vec3(0, 0, 1);
+		CHECK_APPROX_EQUAL(v, Vec3(1, 0, 0));
+
+		rot = Mat44::sRotationZ(0.5f * JPH_PI);
+		v = rot * Vec3(1, 0, 0);
+		CHECK_APPROX_EQUAL(v, Vec3(0, 1, 0));
+		v = rot * Vec3(0, 1, 0);
+		CHECK_APPROX_EQUAL(v, Vec3(-1, 0, 0));
+		v = rot * Vec3(0, 0, 1);
+		CHECK(v == Vec3(0, 0, 1));
+	}
+
+	TEST_CASE("TestMat44RotationAxisAngle")
+	{
+		Mat44 r1 = Mat44::sRotationX(0.1f * JPH_PI);
+		Mat44 r2 = Mat44::sRotation(Vec3(1, 0, 0), 0.1f * JPH_PI);
+		CHECK_APPROX_EQUAL(r1, r2);
+
+		r1 = Mat44::sRotationY(0.2f * JPH_PI);
+		r2 = Mat44::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);
+		CHECK_APPROX_EQUAL(r1, r2);
+
+		r1 = Mat44::sRotationZ(0.3f * JPH_PI);
+		r2 = Mat44::sRotation(Vec3(0, 0, 1), 0.3f * JPH_PI);
+		CHECK_APPROX_EQUAL(r1, r2);
+	}
+
+	TEST_CASE("TestMat44CrossProduct")
+	{
+		Vec3 v1(1, 2, 3);
+		Vec3 v2(4, 5, 6);
+		Vec3 v3 = v1.Cross(v2);
+		Vec3 v4 = Mat44::sCrossProduct(v1) * v2;
+		CHECK(v3 == v4);
+	}
+
+	TEST_CASE("TestMat44OuterProduct")
+	{
+		Vec3 v1(1, 2, 3);
+		Vec3 v2(4, 5, 6);
+		CHECK(Mat44::sOuterProduct(v1, v2) == Mat44(Vec4(1 * 4, 2 * 4, 3 * 4, 0), Vec4(1 * 5, 2 * 5, 3 * 5, 0), Vec4(1 * 6, 2 * 6, 3 * 6, 0), Vec4(0, 0, 0, 1)));
+	}
+
+	TEST_CASE("TestMat44QuatLeftMultiply")
+	{
+		Quat p(2, 3, 4, 1);
+		Quat q(6, 7, 8, 5);
+
+		Quat r1 = p * q;
+		Quat r2 = Quat(Mat44::sQuatLeftMultiply(p) * q.GetXYZW());
+		CHECK(r1 == r2);
+	}
+
+	TEST_CASE("TestMat44QuatRightMultiply")
+	{
+		Quat p(2, 3, 4, 1);
+		Quat q(6, 7, 8, 5);
+
+		Quat r1 = q * p;
+		Quat r2 = Quat(Mat44::sQuatRightMultiply(p) * q.GetXYZW());
+		CHECK(r1 == r2);
+	}
+
+	TEST_CASE("TestMat44InverseRotateTranslate")
+	{
+		Quat rot = Quat::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);
+		Vec3 pos(2, 3, 4);
+
+		Mat44 m1 = Mat44::sRotationTranslation(rot, pos).Inversed();
+		Mat44 m2 = Mat44::sInverseRotationTranslation(rot, pos);
+
+		CHECK_APPROX_EQUAL(m1, m2);
+	}
+
+	TEST_CASE("TestMat44InversedRotationTranslation")
+	{
+		Quat rot = Quat::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);
+		Vec3 pos(2, 3, 4);
+
+		Mat44 m1 = Mat44::sRotationTranslation(rot, pos).InversedRotationTranslation();
+		Mat44 m2 = Mat44::sInverseRotationTranslation(rot, pos);
+
+		CHECK_APPROX_EQUAL(m1, m2);
+	}
+
+	TEST_CASE("TestMat44Decompose")
+	{
+		Mat44 rotation = Mat44::sRotationX(0.1f * JPH_PI) * Mat44::sRotationZ(0.2f * JPH_PI);
+		Vec3 scale = Vec3(-1, 2, 3);
+		Mat44 mat = rotation * Mat44::sScale(scale);
+		CHECK(mat.GetDeterminant3x3() < 0); // Left handed
+
+		Vec3 new_scale;
+		Mat44 new_rotation = mat.Decompose(new_scale);
+		CHECK(new_rotation.GetDeterminant3x3() > 0); // Right handed
+
+		Mat44 mat2 = new_rotation * Mat44::sScale(new_scale);
+		CHECK(mat.IsClose(mat2));
+	}
+
+	TEST_CASE("TestMat44PrePostScaled")
+	{
+		Mat44 m(Vec4(2, 3, 4, 0), Vec4(5, 6, 7, 0), Vec4(8, 9, 10, 0), Vec4(11, 12, 13, 1));
+		Vec3 v(14, 15, 16);
+
+		CHECK(m.PreScaled(v) == m * Mat44::sScale(v));
+		CHECK(m.PostScaled(v) == Mat44::sScale(v) * m);
+	}
+
+	TEST_CASE("TestMat44PrePostTranslated")
+	{
+		Mat44 m(Vec4(2, 3, 4, 0), Vec4(5, 6, 7, 0), Vec4(8, 9, 10, 0), Vec4(11, 12, 13, 1));
+		Vec3 v(14, 15, 16);
+
+		CHECK(m.PreTranslated(v) == m * Mat44::sTranslation(v));
+		CHECK(m.PostTranslated(v) == Mat44::sTranslation(v) * m);
+	}
+
+	TEST_CASE("TestMat44Decompose")
+	{
+		// Create a rotation/translation matrix
+		Quat rot = Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.2f * JPH_PI);
+		Vec3 pos(2, 3, 4);
+		Mat44 rotation_translation = Mat44::sRotationTranslation(rot, pos);
+
+		// Scale the matrix
+		Vec3 scale(2, 1, 3);
+		Mat44 m1 = rotation_translation * Mat44::sScale(scale);
+
+		// Decompose scale
+		Vec3 scale_out;
+		Mat44 m2 = m1.Decompose(scale_out);
+
+		// Check individual components
+		CHECK_APPROX_EQUAL(rotation_translation, m2);
+		CHECK_APPROX_EQUAL(scale, scale_out);
+	}
+
+	TEST_CASE("TestMat44DecomposeSkewed")
+	{
+		// Create a rotation/translation matrix
+		Quat rot = Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.2f * JPH_PI);
+		Vec3 pos(2, 3, 4);
+		Mat44 rotation_translation = Mat44::sRotationTranslation(rot, pos);
+
+		// Skew the matrix by applying a non-uniform scale
+		Mat44 skewed_rotation_translation = Mat44::sScale(Vec3(1.0f, 0.99f, 0.98f)) * rotation_translation;
+		float val = skewed_rotation_translation.GetAxisX().Cross(skewed_rotation_translation.GetAxisY()).Dot(skewed_rotation_translation.GetAxisZ());
+		CHECK(abs(val - 1.0f) > 0.01f); // Check not matrix is no longer perpendicular
+
+		// Scale the matrix
+		Vec3 scale(2, 1, 3);
+		Mat44 m1 = skewed_rotation_translation * Mat44::sScale(scale);
+
+		// Decompose scale
+		Vec3 scale_out;
+		Mat44 m2 = m1.Decompose(scale_out);
+
+		// Check individual components
+		CHECK_APPROX_EQUAL(m2.GetAxisX(), skewed_rotation_translation.GetAxisX().Normalized()); // Check X axis didn't change
+		CHECK_APPROX_EQUAL(m2.GetAxisY(), skewed_rotation_translation.GetAxisY().Normalized(), 0.003f); // Y axis may move a bit
+		CHECK_APPROX_EQUAL(m2.GetAxisZ(), skewed_rotation_translation.GetAxisZ().Normalized(), 0.02f); // Z axis may move a bit
+		CHECK_APPROX_EQUAL(m2.GetAxisX().Cross(m2.GetAxisY()).Dot(m2.GetAxisZ()), 1.0f); // Check perpendicular
+		CHECK_APPROX_EQUAL(scale, scale_out, 0.05f); // Scale may change a bit
+	}
+
+	TEST_CASE("TestDMat44GetQuaternion")
+	{
+		Quat rot = Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.2f * JPH_PI);
+		Mat44 mat = Mat44::sRotation(rot);
+		CHECK_APPROX_EQUAL(mat.GetQuaternion(), rot);
+	}
+
+	TEST_CASE("TestDMat44ConvertToString")
+	{
+		Mat44 v(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12), Vec4(13, 14, 15, 16));
+		CHECK(ConvertToString(v) == "1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16");
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/MathTests.cpp

@@ -0,0 +1,84 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/Mat44.h>
+
+TEST_SUITE("Mat44Tests")
+{
+	TEST_CASE("TestCountTrailingZeros")
+	{
+		CHECK(CountTrailingZeros(0) == 32);
+		for (int i = 0; i < 32; ++i)
+			CHECK(CountTrailingZeros(1U << i) == i);
+	}
+
+	TEST_CASE("TestCountLeadingZeros")
+	{
+		CHECK(CountLeadingZeros(0) == 32);
+		for (int i = 0; i < 32; ++i)
+			CHECK(CountLeadingZeros(1U << i) == 31 - i);
+	}
+
+	TEST_CASE("TestCountBits")
+	{
+		CHECK(CountBits(0) == 0);
+		CHECK(CountBits(0b10000000000000000000000000000000) == 1);
+		CHECK(CountBits(0b00000000000000000000000000000001) == 1);
+		CHECK(CountBits(0b10000000000000001000000000000000) == 2);
+		CHECK(CountBits(0b00000000000000010000000000000001) == 2);
+		CHECK(CountBits(0b10000000100000001000000010000000) == 4);
+		CHECK(CountBits(0b00000001000000010000000100000001) == 4);
+		CHECK(CountBits(0b10001000100010001000100010001000) == 8);
+		CHECK(CountBits(0b00010001000100010001000100010001) == 8);
+		CHECK(CountBits(0b10101010101010101010101010101010) == 16);
+		CHECK(CountBits(0b01010101010101010101010101010101) == 16);
+		CHECK(CountBits(0b11111111111111111111111111111111) == 32);
+	}
+
+	TEST_CASE("TestNextPowerOf2")
+	{
+		CHECK(GetNextPowerOf2(0) == 1);
+
+		for (int i = 0; i < 31; ++i)
+		{
+			uint32 pow = uint32(1) << i;
+			if (pow > 2)
+				CHECK(GetNextPowerOf2(pow - 1) == pow);
+			CHECK(GetNextPowerOf2(pow) == pow);
+			CHECK(GetNextPowerOf2(pow + 1) == pow << 1);
+		}
+
+		CHECK(GetNextPowerOf2(0x8000000U - 1) == 0x8000000U);
+		CHECK(GetNextPowerOf2(0x8000000U) == 0x8000000U);
+	}
+
+	TEST_CASE("TestCenterAngleAroundZero")
+	{
+		for (int i = 0; i < 10; i += 2)
+		{
+			CHECK_APPROX_EQUAL(CenterAngleAroundZero(i * JPH_PI), 0, 1.0e-5f);
+			CHECK_APPROX_EQUAL(CenterAngleAroundZero((0.5f + i) * JPH_PI), 0.5f * JPH_PI, 1.0e-5f);
+			CHECK_APPROX_EQUAL(CenterAngleAroundZero((1.5f + i) * JPH_PI), -0.5f * JPH_PI, 1.0e-5f);
+			CHECK_APPROX_EQUAL(CenterAngleAroundZero(-(0.5f + i) * JPH_PI), -0.5f * JPH_PI, 1.0e-5f);
+			CHECK_APPROX_EQUAL(CenterAngleAroundZero(-(1.5f + i) * JPH_PI), 0.5f * JPH_PI, 1.0e-5f);
+			CHECK_APPROX_EQUAL(CenterAngleAroundZero(-(0.99f + i) * JPH_PI), -0.99f * JPH_PI, 1.0e-5f);
+			CHECK_APPROX_EQUAL(CenterAngleAroundZero((0.99f + i) * JPH_PI), 0.99f * JPH_PI, 1.0e-5f);
+		}
+	}
+
+	TEST_CASE("TestIsPowerOf2")
+	{
+		for (int i = 0; i < 63; ++i)
+			CHECK(IsPowerOf2(uint64(1) << 1));
+		CHECK(!IsPowerOf2(-2));
+		CHECK(!IsPowerOf2(0));
+		CHECK(!IsPowerOf2(3));
+		CHECK(!IsPowerOf2(5));
+		CHECK(!IsPowerOf2(15));
+		CHECK(!IsPowerOf2(17));
+		CHECK(!IsPowerOf2(65535));
+		CHECK(!IsPowerOf2(65537));
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/MatrixTests.cpp

@@ -0,0 +1,118 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/Matrix.h>
+
+TEST_SUITE("MatrixTests")
+{
+	TEST_CASE("TestMatrixEquals")
+	{
+		Matrix<3, 5> m1 = Matrix<3, 5>::sZero();
+		Matrix<3, 5> m2 = Matrix<3, 5>::sZero();
+		Matrix<3, 5> m3 = Matrix<3, 5>::sIdentity();
+
+		CHECK(m1 == m2);
+		CHECK(!(m1 != m2));
+		CHECK(m1 != m3);
+		CHECK(!(m1 == m3));
+	}
+
+	TEST_CASE("TestMatrixStream")
+	{
+		Matrix<3, 5> m1 = Matrix<3, 5>::sIdentity();
+
+		std::stringstream ss;
+		ss << m1;
+		CHECK(ss.str() == "[1, 0, 0], [0, 1, 0], [0, 0, 1], [0, 0, 0], [0, 0, 0]");
+	}
+
+	TEST_CASE("TestMatrixZero")
+	{
+		Matrix<3, 5> m = Matrix<3, 5>::sZero();
+
+		for (uint r = 0; r < 3; ++r)
+			for (uint c = 0; c < 5; ++c)
+				CHECK(m(r, c) == 0.0f);
+	}
+
+	TEST_CASE("TestMatrixIdentity")
+	{
+		Matrix<3, 5> m = Matrix<3, 5>::sIdentity();
+
+		for (uint r = 0; r < 3; ++r)
+			for (uint c = 0; c < 5; ++c)
+				CHECK(m(r, c) == (r == c? 1.0f : 0.0f));
+	}
+
+	TEST_CASE("TestMatrixMultiply")
+	{
+		Matrix<3, 5> m1 = Matrix<3, 5>::sZero();
+		Matrix<5, 4> m2 = Matrix<5, 4>::sZero();
+
+		for (uint r = 0; r < 3; ++r)
+			for (uint c = 0; c < 5; ++c)
+				m1(r, c) = float(r * 5 + c + 1);
+
+		for (uint r = 0; r < 5; ++r)
+			for (uint c = 0; c < 4; ++c)
+				m2(r, c) = float(r * 4 + c + 1);
+
+		Matrix<3, 4> m3 = m1 * m2;
+
+		CHECK(m3(0, 0) == 175.0f);
+		CHECK(m3(1, 0) == 400.0f);
+		CHECK(m3(2, 0) == 625.0f);
+		CHECK(m3(0, 1) == 190.0f);
+		CHECK(m3(1, 1) == 440.0f);
+		CHECK(m3(2, 1) == 690.0f);
+		CHECK(m3(0, 2) == 205.0f);
+		CHECK(m3(1, 2) == 480.0f);
+		CHECK(m3(2, 2) == 755.0f);
+		CHECK(m3(0, 3) == 220.0f);
+		CHECK(m3(1, 3) == 520.0f);
+		CHECK(m3(2, 3) == 820.0f);
+	}
+
+	TEST_CASE("TestMatrixInversed")
+	{
+		Matrix<4, 4> mat = Matrix<4, 4>::sZero();
+		mat(1, 0) = 4;
+		mat(3, 0) = 8;
+		mat(0, 1) = 2;
+		mat(2, 1) = 16;
+		mat(1, 2) = 16;
+		mat(3, 2) = 4;
+		mat(0, 3) = 8;
+		mat(2, 3) = 2;
+		Matrix<4, 4> inverse;
+		CHECK(inverse.SetInversed(mat));
+		Matrix<4, 4> identity = mat * inverse;
+		CHECK(identity == Matrix<4, 4>::sIdentity());
+
+		// Make non-invertible
+		mat(1, 0) = 0;
+		mat(3, 0) = 0;
+		CHECK(!inverse.SetInversed(mat));
+	}
+
+	TEST_CASE("TestMatrix22Inversed")
+	{
+		// SetInverse is specialized for 2x2 matrices
+		Matrix<2, 2> mat;
+		mat(0, 0) = 1;
+		mat(0, 1) = 2;
+		mat(1, 0) = 3;
+		mat(1, 1) = 4;
+		Matrix<2, 2> inverse;
+		CHECK(inverse.SetInversed(mat));
+		Matrix<2, 2> identity = mat * inverse;
+		CHECK(identity == Matrix<2, 2>::sIdentity());
+
+		// Make non-invertible
+		mat(0, 0) = 0;
+		mat(1, 0) = 0;
+		CHECK(!inverse.SetInversed(mat));
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/QuatTests.cpp

@@ -0,0 +1,521 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/Mat44.h>
+#include <Jolt/Math/Quat.h>
+#include <Jolt/Core/StringTools.h>
+#include <random>
+
+TEST_SUITE("QuatTests")
+{
+	TEST_CASE("TestQuatSetXYZW")
+	{
+		Quat q(0, 0, 0, 0);
+		CHECK(q == Quat(0, 0, 0, 0));
+		q.SetX(1);
+		q.SetY(2);
+		q.SetZ(3);
+		q.SetW(4);
+		CHECK(q == Quat(1, 2, 3, 4));
+
+		q.Set(4, 3, 2, 1);
+		CHECK(q == Quat(4, 3, 2, 1));
+	}
+
+	TEST_CASE("TestQuatEqual")
+	{
+		CHECK(Quat(1, 2, 3, 4) == Quat(1, 2, 3, 4));
+		CHECK(Quat(1, 2, 3, 4) != Quat(0, 2, 3, 4));
+		CHECK(Quat(1, 2, 3, 4) != Quat(1, 0, 3, 4));
+		CHECK(Quat(1, 2, 3, 4) != Quat(1, 2, 0, 4));
+		CHECK(Quat(1, 2, 3, 4) != Quat(1, 2, 3, 0));
+	}
+
+	TEST_CASE("TestQuatZero")
+	{
+		Quat zero = Quat::sZero();
+		CHECK(zero == Quat(0, 0, 0, 0));
+	}
+
+	TEST_CASE("TestQuatIdentity")
+	{
+		Quat identity = Quat::sIdentity();
+
+		CHECK_APPROX_EQUAL(identity.GetX(), 0.0f);
+		CHECK_APPROX_EQUAL(identity.GetY(), 0.0f);
+		CHECK_APPROX_EQUAL(identity.GetZ(), 0.0f);
+		CHECK_APPROX_EQUAL(identity.GetW(), 1.0f);
+	}
+
+	TEST_CASE("TestQuatIsNaN")
+	{
+		CHECK(Quat(numeric_limits<float>::quiet_NaN(), 0, 0, 0).IsNaN());
+		CHECK(Quat(0, numeric_limits<float>::quiet_NaN(), 0, 0).IsNaN());
+		CHECK(Quat(0, 0, numeric_limits<float>::quiet_NaN(), 0).IsNaN());
+		CHECK(Quat(0, 0, 0, numeric_limits<float>::quiet_NaN()).IsNaN());
+	}
+
+	TEST_CASE("TestQuatOperators")
+	{
+		CHECK(-Quat(1, 2, 3, 4) == Quat(-1, -2, -3, -4));
+		CHECK(Quat(1, 2, 3, 4) + Quat(5, 6, 7, 8) == Quat(6, 8, 10, 12));
+		CHECK(Quat(5, 6, 7, 8) - Quat(4, 3, 2, 1) == Quat(1, 3, 5, 7));
+		CHECK(Quat(1, 2, 3, 4) * 5.0f == Quat(5, 10, 15, 20));
+		CHECK(5.0f * Quat(1, 2, 3, 4) == Quat(5, 10, 15, 20));
+		CHECK(Quat(2, 4, 6, 8) / 2.0f == Quat(1, 2, 3, 4));
+
+		Quat v(1, 2, 3, 4);
+		v += Quat(5, 6, 7, 8);
+		CHECK(v == Quat(6, 8, 10, 12));
+		v -= Quat(4, 3, 2, 1);
+		CHECK(v == Quat(2, 5, 8, 11));
+		v *= 2.0f;
+		CHECK(v == Quat(4, 10, 16, 22));
+		v /= 2.0f;
+		CHECK(v == Quat(2, 5, 8, 11));
+	}
+
+	TEST_CASE("TestQuatPerpendicular")
+	{
+		Quat q1(1, 2, 3, 4);
+		CHECK(q1.GetPerpendicular().Dot(q1) == 0.0f);
+
+		Quat q2(-5, 4, -3, 2);
+		CHECK(q2.GetPerpendicular().Dot(q2) == 0.0f);
+	}
+
+	TEST_CASE("TestQuatNormalized")
+	{
+		CHECK(Quat(1, 0, 0, 0).IsNormalized());
+		CHECK(Quat(-0.7071067f, 0.7071067f, 0, 0).IsNormalized());
+		CHECK(Quat(0.5773502f, -0.5773502f, 0.5773502f, 0).IsNormalized());
+		CHECK(Quat(0.5f, -0.5f, 0.5f, -0.5f).IsNormalized());
+		CHECK(!Quat(2, 0, 0, 0).IsNormalized());
+		CHECK(!Quat(0, 2, 0, 0).IsNormalized());
+		CHECK(!Quat(0, 0, 2, 0).IsNormalized());
+		CHECK(!Quat(0, 0, 0, 2).IsNormalized());
+	}
+
+	TEST_CASE("TestQuatConvertMatrix")
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 axis = Vec3::sRandom(random);
+			float angle = zero_to_two_pi(random);
+
+			Mat44 m1 = Mat44::sRotation(axis, angle);
+			Quat q1 = m1.GetQuaternion();
+			Quat q2 = Quat::sRotation(axis, angle);
+			CHECK_APPROX_EQUAL(q1, q2);
+			Mat44 m2 = Mat44::sRotation(q2);
+			CHECK_APPROX_EQUAL(m1, m2);
+		}
+	}
+
+	TEST_CASE("TestQuatMultiplyVec3")
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 axis = Vec3::sRandom(random);
+			float angle = zero_to_two_pi(random);
+			Mat44 m1 = Mat44::sRotation(axis, angle);
+			Quat q1 = Quat::sRotation(axis, angle);
+
+			Vec3 rv = 10.0f * Vec3::sRandom(random);
+			Vec3 r1 = m1 * rv;
+			Vec3 r2 = q1 * rv;
+			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);
+
+			Vec3 r3 = q1.InverseRotate(r2);
+			CHECK_APPROX_EQUAL(r3, rv, 1.0e-5f);
+		}
+	}
+
+	TEST_CASE("TestQuatRotateAxisXYZ")
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 axis = Vec3::sRandom(random);
+			float angle = zero_to_two_pi(random);
+			Quat q1 = Quat::sRotation(axis, angle);
+
+			Vec3 r1 = q1 * Vec3::sAxisX();
+			Vec3 r2 = q1.RotateAxisX();
+			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);
+
+			r1 = q1 * Vec3::sAxisY();
+			r2 = q1.RotateAxisY();
+			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);
+
+			r1 = q1 * Vec3::sAxisZ();
+			r2 = q1.RotateAxisZ();
+			CHECK_APPROX_EQUAL(r1, r2, 1.0e-5f);
+		}
+	}
+
+	TEST_CASE("TestQuatMultiplyQuat")
+	{
+		{
+			// We use a right handed system, so test that: i * j = k
+			Quat r1 = Quat(1, 0, 0, 0) * Quat(0, 1, 0, 0);
+			Quat r2 = Quat(0, 0, 1, 0);
+			CHECK(r1.IsClose(r2));
+		}
+
+		{
+			// Test: j * i = -k
+			Quat r1 = Quat(0, 1, 0, 0) * Quat(1, 0, 0, 0);
+			Quat r2 = Quat(0, 0, -1, 0);
+			CHECK(r1.IsClose(r2));
+		}
+
+		{
+			// Test predefined multiplication
+			Quat r1 = Quat(2, 3, 4, 1) * Quat(6, 7, 8, 5);
+			Quat r2 = Quat(12, 30, 24, -60);
+			CHECK(r1.IsClose(r2));
+		}
+
+		// Compare random matrix multiplications with quaternion multiplications
+		UnitTestRandom random;
+		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 axis1 = Vec3::sRandom(random);
+			float angle1 = zero_to_two_pi(random);
+			Quat q1 = Quat::sRotation(axis1, angle1);
+			Mat44 m1 = Mat44::sRotation(axis1, angle1);
+
+			Vec3 axis2 = Vec3::sRandom(random);
+			float angle2 = zero_to_two_pi(random);
+			Quat q2 = Quat::sRotation(axis2, angle2);
+			Mat44 m2 = Mat44::sRotation(axis2, angle2);
+
+			Quat r1 = q1 * q2;
+			Quat r2 = (m1 * m2).GetQuaternion();
+
+			CHECK_APPROX_EQUAL(r1, r2);
+		}
+	}
+
+	TEST_CASE("TestQuatRotationAxisAngle")
+	{
+		Mat44 r1 = Mat44::sRotation(Vec3(1, 0, 0), 0.1f * JPH_PI);
+		Mat44 r2 = Mat44::sRotation(Quat::sRotation(Vec3(1, 0, 0), 0.1f * JPH_PI));
+		CHECK_APPROX_EQUAL(r1, r2);
+
+		r1 = Mat44::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);
+		r2 = Mat44::sRotation(Quat::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI));
+		CHECK_APPROX_EQUAL(r1, r2);
+
+		r1 = Mat44::sRotation(Vec3(0, 0, 1), 0.3f * JPH_PI);
+		r2 = Mat44::sRotation(Quat::sRotation(Vec3(0, 0, 1), 0.3f * JPH_PI));
+		CHECK_APPROX_EQUAL(r1, r2);
+	}
+
+	TEST_CASE("TestQuatGetAxisAngle")
+	{
+		// Test identity rotation
+		{
+			Vec3 axis;
+			float angle;
+			Quat::sIdentity().GetAxisAngle(axis, angle);
+			CHECK_APPROX_EQUAL(Vec3::sZero(), axis);
+			CHECK_APPROX_EQUAL(0.0f, angle);
+		}
+
+		{
+			Vec3 axis;
+			float angle;
+			(-Quat::sIdentity()).GetAxisAngle(axis, angle);
+			CHECK_APPROX_EQUAL(Vec3::sZero(), axis);
+			CHECK_APPROX_EQUAL(0.0f, angle);
+		}
+
+		// Test positive rotation
+		Quat q1 = Quat::sRotation(Vec3(0, 1, 0), 0.2f * JPH_PI);
+
+		{
+			Vec3 axis;
+			float angle;
+			q1.GetAxisAngle(axis, angle);
+			CHECK_APPROX_EQUAL(Vec3(0, 1, 0), axis);
+			CHECK_APPROX_EQUAL(0.2f * JPH_PI, angle, 1.0e-5f);
+		}
+
+		{
+			Vec3 axis;
+			float angle;
+			(-q1).GetAxisAngle(axis, angle);
+			CHECK_APPROX_EQUAL(Vec3(0, 1, 0), axis);
+			CHECK_APPROX_EQUAL(0.2f * JPH_PI, angle, 1.0e-5f);
+		}
+
+		// Test negative rotation
+		Quat q2 = Quat::sRotation(Vec3(0, 1, 0), -0.2f * JPH_PI);
+
+		{
+			Vec3 axis;
+			float angle;
+			q2.GetAxisAngle(axis, angle);
+			CHECK_APPROX_EQUAL(Vec3(0, -1, 0), axis);
+			CHECK_APPROX_EQUAL(0.2f * JPH_PI, angle, 1.0e-5f);
+		}
+
+		{
+			Vec3 axis;
+			float angle;
+			(-q2).GetAxisAngle(axis, angle);
+			CHECK_APPROX_EQUAL(Vec3(0, -1, 0), axis);
+			CHECK_APPROX_EQUAL(0.2f * JPH_PI, angle, 1.0e-5f);
+		}
+
+		// Test keeping range between [0, PI]
+		Quat q3 = Quat::sRotation(Vec3(0, 1, 0), 1.1f * JPH_PI);
+
+		{
+			Vec3 axis;
+			float angle;
+			q3.GetAxisAngle(axis, angle);
+			CHECK_APPROX_EQUAL(Vec3(0, -1, 0), axis);
+			CHECK_APPROX_EQUAL(0.9f * JPH_PI, angle, 1.0e-5f);
+		}
+
+		{
+			Vec3 axis;
+			float angle;
+			(-q3).GetAxisAngle(axis, angle);
+			CHECK_APPROX_EQUAL(Vec3(0, -1, 0), axis);
+			CHECK_APPROX_EQUAL(0.9f * JPH_PI, angle, 1.0e-5f);
+		}
+	}
+
+	TEST_CASE("TestQuatInverse")
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 axis = Vec3::sRandom(random);
+			float angle = zero_to_two_pi(random);
+
+			Quat q1 = Quat::sRotation(axis, angle);
+			Quat q2 = q1.Inversed();
+
+			CHECK_APPROX_EQUAL(Quat::sIdentity(), q1 * q2);
+		}
+	}
+
+	TEST_CASE("TestQuatConjugate")
+	{
+		CHECK(Quat(1, 2, 3, 4).Conjugated() == Quat(-1, -2, -3, 4));
+		CHECK(Quat(-1, -2, -3, -4).Conjugated() == Quat(1, 2, 3, -4));
+	}
+
+	TEST_CASE("TestQuatEnsureWPositive")
+	{
+		CHECK(Quat(1, -2, 3, -4).EnsureWPositive() == Quat(-1, 2, -3, 4));
+		CHECK(Quat(-4, 5, -6, 7).EnsureWPositive() == Quat(-4, 5, -6, 7));
+		CHECK(Quat(1, 2, 3, 0).EnsureWPositive() == Quat(1, 2, 3, 0));
+	}
+
+	TEST_CASE("TestQuatStoreFloat3")
+	{
+		Float3 q1;
+		Quat(0.7071067f, 0, 0, -0.7071067f).StoreFloat3(&q1);
+		CHECK(q1 == Float3(-0.7071067f, 0, 0));
+
+		Float3 q2;
+		Quat(0, 0.7071067f, 0, 0.7071067f).StoreFloat3(&q2);
+		CHECK(q2 == Float3(0, 0.7071067f, 0));
+
+		Float3 q3;
+		Quat(0, 0, 1, 0).StoreFloat3(&q3);
+		CHECK(q3 == Float3(0, 0, 1));
+	}
+
+	TEST_CASE("TestQuatGetTwistAxis")
+	{
+		Quat q1 = Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(-10.0f));
+		Quat q2 = Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(20.0f));
+		Quat q = q1 * q2;
+
+		Quat twist1 = q.GetTwist(Vec3::sAxisX());
+		CHECK_APPROX_EQUAL(twist1, q1);
+		Quat swing1 = twist1.Inversed() * q;
+		CHECK_APPROX_EQUAL(swing1, q2);
+		Quat twist2 = swing1.GetTwist(Vec3::sAxisY());
+		CHECK_APPROX_EQUAL(twist2, q2);
+		Quat swing2 = twist2.Inversed() * swing1;
+		CHECK_APPROX_EQUAL(swing2, Quat::sIdentity());
+
+		CHECK(Quat::sZero().GetTwist(Vec3::sAxisX()) == Quat::sIdentity());
+	}
+
+	TEST_CASE("TestQuatGetRotationAngle")
+	{
+		Quat q1 = Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(-10.0f));
+		Quat q2 = Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(20.0f));
+		Quat q3 = Quat::sRotation(Vec3::sAxisZ(), DegreesToRadians(-95.0f));
+
+		float a = q1.GetRotationAngle(Vec3::sAxisX());
+		CHECK_APPROX_EQUAL(a, DegreesToRadians(-10.0f), 1.0e-5f);
+
+		a = q2.GetRotationAngle(Vec3::sAxisY());
+		CHECK_APPROX_EQUAL(a, DegreesToRadians(20.0f), 1.0e-5f);
+
+		a = q3.GetRotationAngle(Vec3::sAxisZ());
+		CHECK_APPROX_EQUAL(a, DegreesToRadians(-95.0f), 1.0e-5f);
+
+		a = (q1 * q2).GetRotationAngle(Vec3::sAxisX());
+		CHECK_APPROX_EQUAL(a, DegreesToRadians(-10.0f), 1.0e-5f);
+
+		a = (q3 * q1).GetRotationAngle(Vec3::sAxisX());
+		CHECK_APPROX_EQUAL(a, DegreesToRadians(-10.0f), 1.0e-5f);
+	}
+
+	TEST_CASE("TestQuatGetEulerAngles")
+	{
+		Vec3 input(DegreesToRadians(-10.0f), DegreesToRadians(20.0f), DegreesToRadians(-95.0f));
+
+		Quat qx = Quat::sRotation(Vec3::sAxisX(), input.GetX());
+		Quat qy = Quat::sRotation(Vec3::sAxisY(), input.GetY());
+		Quat qz = Quat::sRotation(Vec3::sAxisZ(), input.GetZ());
+		Quat q = qz * qy * qx;
+
+		Quat q2 = Quat::sEulerAngles(input);
+		CHECK_APPROX_EQUAL(q, q2);
+
+		Vec3 angles = q2.GetEulerAngles();
+		CHECK_APPROX_EQUAL(angles, input);
+	}
+
+	TEST_CASE("TestQuatRotationFromTo")
+	{
+		{
+			// Parallel vectors
+			Vec3 v1(10, 0, 0);
+			Vec3 v2(20, 0, 0);
+			Quat q = Quat::sFromTo(v1, v2);
+			CHECK_APPROX_EQUAL(q, Quat::sIdentity());
+		}
+
+		{
+			// Perpendicular vectors
+			Vec3 v1(10, 0, 0);
+			Vec3 v2(0, 20, 0);
+			Quat q = Quat::sFromTo(v1, v2);
+			CHECK_APPROX_EQUAL(v2.Normalized(), (q * v1).Normalized());
+		}
+
+		{
+			// Vectors with 180 degree angle
+			Vec3 v1(10, 0, 0);
+			Vec3 v2(-20, 0, 0);
+			Quat q = Quat::sFromTo(v1, v2);
+			CHECK_APPROX_EQUAL(v2.Normalized(), (q * v1).Normalized());
+		}
+
+		{
+			// Test v1 zero
+			Vec3 v1 = Vec3::sZero();
+			Vec3 v2(10, 0, 0);
+			Quat q = Quat::sFromTo(v1, v2);
+			CHECK(q == Quat::sIdentity());
+		}
+
+		{
+			// Test v2 zero
+			Vec3 v1(10, 0, 0);
+			Vec3 v2 = Vec3::sZero();
+			Quat q = Quat::sFromTo(v1, v2);
+			CHECK(q == Quat::sIdentity());
+		}
+
+		{
+			// Length of a vector is squared inside the function: try with sqrt(FLT_MIN) to see if that still returns a valid rotation
+			Vec3 v1(0, sqrt(FLT_MIN), 0);
+			Vec3 v2(1, 0, 0);
+			Quat q = Quat::sFromTo(v1, v2);
+			CHECK_APPROX_EQUAL(v2.Normalized(), (q * v1).Normalized());
+		}
+	}
+
+	TEST_CASE("TestQuatRotationFromToRandom")
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> one_to_ten(1.0f, 10.0f);
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 v1 = one_to_ten(random) * Vec3::sRandom(random);
+			Vec3 v2 = one_to_ten(random) * Vec3::sRandom(random);
+
+			Quat q = Quat::sFromTo(v1, v2);
+
+			Vec3 v1t = (q * v1).Normalized();
+			Vec3 v2t = v2.Normalized();
+			CHECK_APPROX_EQUAL(v2t, v1t, 1.0e-5f);
+		}
+	}
+
+	TEST_CASE("TestQuatConvertToString")
+	{
+		Quat v(1, 2, 3, 4);
+		CHECK(ConvertToString(v) == "1, 2, 3, 4");
+	}
+
+	TEST_CASE("TestQuatLERP")
+	{
+		Quat v1(1, 2, 3, 4);
+		Quat v2(5, 6, 7, 8);
+		CHECK(v1.LERP(v2, 0.25f) == Quat(2, 3, 4, 5));
+	}
+
+	TEST_CASE("TestQuatSLERP")
+	{
+		Quat v1 = Quat::sIdentity();
+		Quat v2 = Quat::sRotation(Vec3::sAxisX(), 0.99f * JPH_PI);
+		CHECK_APPROX_EQUAL(v1.SLERP(v2, 0.25f), Quat::sRotation(Vec3::sAxisX(), 0.25f * 0.99f * JPH_PI));
+
+		// Check that we ignore the sign
+		Quat v3 = Quat(1, 2, 3, 4).Normalized();
+		CHECK_APPROX_EQUAL(v3.SLERP(-v3, 0.5f), v3);
+	}
+
+	TEST_CASE("TestQuatMultiplyImaginary")
+	{
+		UnitTestRandom random;
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 imaginary = Vec3::sRandom(random);
+			Quat quat = Quat::sRandom(random);
+
+			Quat r1 = Quat::sMultiplyImaginary(imaginary, quat);
+			Quat r2 = Quat(Vec4(imaginary, 0)) * quat;
+			CHECK_APPROX_EQUAL(r1, r2);
+		}
+	}
+
+	TEST_CASE("TestQuatCompressUnitQuat")
+	{
+		UnitTestRandom random;
+		for (int i = 0; i < 1000; ++i)
+		{
+			Quat quat = Quat::sRandom(random);
+			uint32 compressed = quat.CompressUnitQuat();
+			Quat decompressed = Quat::sDecompressUnitQuat(compressed);
+			Vec3 axis;
+			float angle;
+			(quat * decompressed.Conjugated()).GetAxisAngle(axis, angle);
+			CHECK(abs(angle) < 0.009f);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/TrigonometryTests.cpp

@@ -0,0 +1,27 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/Trigonometry.h>
+
+TEST_SUITE("TrigonometryTests")
+{
+	TEST_CASE("TestACosApproximate")
+	{
+		// Check error over entire range [-1, 1]
+		for (int i = -1000; i <= 1000; i++)
+		{
+			float x = float(i) / 1000.0f;
+			float acos1 = acos(x);
+			float acos2 = ACosApproximate(x);
+			CHECK_APPROX_EQUAL(acos1, acos2, 4.3e-3f);
+		}
+
+		// Check edge cases for exact matches
+		CHECK(ACosApproximate(1.0f) == 0.0f);
+		CHECK(ACosApproximate(1.0e-12f) == JPH_PI / 2);
+		CHECK(ACosApproximate(-1.0e-12f) == JPH_PI / 2);
+		CHECK(ACosApproximate(-1.0f) == JPH_PI);
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/UVec4Tests.cpp

@@ -0,0 +1,574 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/StringTools.h>
+
+TEST_SUITE("UVec4Tests")
+{
+	TEST_CASE("TestUVec4Construct")
+	{
+		UVec4 v(1, 2, 3, 4);
+
+		CHECK(v.GetX() == 1);
+		CHECK(v.GetY() == 2);
+		CHECK(v.GetZ() == 3);
+		CHECK(v.GetW() == 4);
+
+		CHECK(v[0] == 1);
+		CHECK(v[1] == 2);
+		CHECK(v[2] == 3);
+		CHECK(v[3] == 4);
+
+		// Test == and != operators
+		CHECK(v == UVec4(1, 2, 3, 4));
+		CHECK(v != UVec4(1, 2, 4, 3));
+	}
+
+	TEST_CASE("TestUVec4Components")
+	{
+		UVec4 v(1, 2, 3, 4);
+		v.SetX(5);
+		v.SetY(6);
+		v.SetZ(7);
+		v.SetW(8);
+		CHECK(v == UVec4(5, 6, 7, 8));
+	}
+
+	TEST_CASE("TestUVec4LoadStoreInt4")
+	{
+		alignas(16) uint32 i4[] = { 1, 2, 3, 4 };
+		CHECK(UVec4::sLoadInt(i4) == UVec4(1, 0, 0, 0));
+		CHECK(UVec4::sLoadInt4(i4) == UVec4(1, 2, 3, 4));
+		CHECK(UVec4::sLoadInt4Aligned(i4) == UVec4(1, 2, 3, 4));
+
+		uint32 i4_out1[4];
+		UVec4(1, 2, 3, 4).StoreInt4(i4_out1);
+		CHECK(i4_out1[0] == 1);
+		CHECK(i4_out1[1] == 2);
+		CHECK(i4_out1[2] == 3);
+		CHECK(i4_out1[3] == 4);
+
+		alignas(16) uint32 i4_out2[4];
+		UVec4(1, 2, 3, 4).StoreInt4Aligned(i4_out2);
+		CHECK(i4_out2[0] == 1);
+		CHECK(i4_out2[1] == 2);
+		CHECK(i4_out2[2] == 3);
+		CHECK(i4_out2[3] == 4);
+
+		uint32 si[] = { 0, 0, 1, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 4, 0 };
+		CHECK(UVec4::sGatherInt4<2 * sizeof(uint32)>(si, UVec4(1, 3, 8, 9)) == UVec4(1, 2, 3, 4));
+	}
+
+	TEST_CASE("TestUVec4Zero")
+	{
+		UVec4 v = UVec4::sZero();
+
+		CHECK(v.GetX() == 0);
+		CHECK(v.GetY() == 0);
+		CHECK(v.GetZ() == 0);
+		CHECK(v.GetW() == 0);
+	}
+
+	TEST_CASE("TestUVec4Replicate")
+	{
+		CHECK(UVec4::sReplicate(2) == UVec4(2, 2, 2, 2));
+	}
+
+	TEST_CASE("TestUVec4MinMax")
+	{
+		UVec4 v1(1, 6, 3, 8);
+		UVec4 v2(5, 2, 7, 4);
+
+		CHECK(UVec4::sMin(v1, v2) == UVec4(1, 2, 3, 4));
+		CHECK(UVec4::sMax(v1, v2) == UVec4(5, 6, 7, 8));
+	}
+
+	TEST_CASE("TestUVec4Comparisons")
+	{
+		CHECK(UVec4::sEquals(UVec4(1, 2, 3, 4), UVec4(2, 1, 3, 4)) == UVec4(0, 0, 0xffffffffU, 0xffffffffU));
+
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0x00000000U).GetTrues() == 0b0000);
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0x00000000U).GetTrues() == 0b0001);
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0x00000000U).GetTrues() == 0b0010);
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0x00000000U).GetTrues() == 0b0011);
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0x00000000U).GetTrues() == 0b0100);
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0x00000000U).GetTrues() == 0b0101);
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0x00000000U).GetTrues() == 0b0110);
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0x00000000U).GetTrues() == 0b0111);
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0xffffffffU).GetTrues() == 0b1000);
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0xffffffffU).GetTrues() == 0b1001);
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0xffffffffU).GetTrues() == 0b1010);
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0xffffffffU).GetTrues() == 0b1011);
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0xffffffffU).GetTrues() == 0b1100);
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0xffffffffU).GetTrues() == 0b1101);
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0xffffffffU).GetTrues() == 0b1110);
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0xffffffffU).GetTrues() == 0b1111);
+
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0x00000000U).CountTrues() == 0);
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0x00000000U).CountTrues() == 1);
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0x00000000U).CountTrues() == 1);
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0x00000000U).CountTrues() == 2);
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0x00000000U).CountTrues() == 1);
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0x00000000U).CountTrues() == 2);
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0x00000000U).CountTrues() == 2);
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0x00000000U).CountTrues() == 3);
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0xffffffffU).CountTrues() == 1);
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0xffffffffU).CountTrues() == 2);
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0xffffffffU).CountTrues() == 2);
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0xffffffffU).CountTrues() == 3);
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0xffffffffU).CountTrues() == 2);
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0xffffffffU).CountTrues() == 3);
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0xffffffffU).CountTrues() == 3);
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0xffffffffU).CountTrues() == 4);
+
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0x00000000U).TestAllTrue());
+		CHECK(!UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0x00000000U).TestAllTrue());
+		CHECK(!UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0x00000000U).TestAllTrue());
+		CHECK(!UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0x00000000U).TestAllTrue());
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0x00000000U).TestAllTrue());
+		CHECK(!UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0x00000000U).TestAllTrue());
+		CHECK(!UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0x00000000U).TestAllTrue());
+		CHECK(!UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0x00000000U).TestAllTrue());
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0xffffffffU).TestAllTrue());
+		CHECK(!UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0xffffffffU).TestAllTrue());
+		CHECK(!UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0xffffffffU).TestAllTrue());
+		CHECK(!UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0xffffffffU).TestAllTrue());
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0xffffffffU).TestAllTrue());
+		CHECK(!UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0xffffffffU).TestAllTrue());
+		CHECK(!UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0xffffffffU).TestAllTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0xffffffffU).TestAllTrue());
+
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0x00000000U).TestAllXYZTrue());
+		CHECK(!UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0x00000000U).TestAllXYZTrue());
+		CHECK(!UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0x00000000U).TestAllXYZTrue());
+		CHECK(!UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0x00000000U).TestAllXYZTrue());
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0x00000000U).TestAllXYZTrue());
+		CHECK(!UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0x00000000U).TestAllXYZTrue());
+		CHECK(!UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0x00000000U).TestAllXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0x00000000U).TestAllXYZTrue());
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0xffffffffU).TestAllXYZTrue());
+		CHECK(!UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0xffffffffU).TestAllXYZTrue());
+		CHECK(!UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0xffffffffU).TestAllXYZTrue());
+		CHECK(!UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0xffffffffU).TestAllXYZTrue());
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0xffffffffU).TestAllXYZTrue());
+		CHECK(!UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0xffffffffU).TestAllXYZTrue());
+		CHECK(!UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0xffffffffU).TestAllXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0xffffffffU).TestAllXYZTrue());
+
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0x00000000U).TestAnyTrue());
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0x00000000U).TestAnyTrue());
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0x00000000U).TestAnyTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0x00000000U).TestAnyTrue());
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0x00000000U).TestAnyTrue());
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0x00000000U).TestAnyTrue());
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0x00000000U).TestAnyTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0x00000000U).TestAnyTrue());
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0xffffffffU).TestAnyTrue());
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0xffffffffU).TestAnyTrue());
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0xffffffffU).TestAnyTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0xffffffffU).TestAnyTrue());
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0xffffffffU).TestAnyTrue());
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0xffffffffU).TestAnyTrue());
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0xffffffffU).TestAnyTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0xffffffffU).TestAnyTrue());
+
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0x00000000U).TestAnyXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0x00000000U).TestAnyXYZTrue());
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0x00000000U).TestAnyXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0x00000000U).TestAnyXYZTrue());
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0x00000000U).TestAnyXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0x00000000U).TestAnyXYZTrue());
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0x00000000U).TestAnyXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0x00000000U).TestAnyXYZTrue());
+		CHECK(!UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0xffffffffU).TestAnyXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0xffffffffU).TestAnyXYZTrue());
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0xffffffffU).TestAnyXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0xffffffffU).TestAnyXYZTrue());
+		CHECK(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0xffffffffU).TestAnyXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0xffffffffU).TestAnyXYZTrue());
+		CHECK(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0xffffffffU).TestAnyXYZTrue());
+		CHECK(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0xffffffffU).TestAnyXYZTrue());
+	}
+
+	TEST_CASE("TestUVec4Select")
+	{
+		CHECK(UVec4::sSelect(UVec4(1, 2, 3, 4), UVec4(5, 6, 7, 8), UVec4(0x80000000U, 0, 0x80000000U, 0)) == UVec4(5, 2, 7, 4));
+		CHECK(UVec4::sSelect(UVec4(1, 2, 3, 4), UVec4(5, 6, 7, 8), UVec4(0, 0x80000000U, 0, 0x80000000U)) == UVec4(1, 6, 3, 8));
+		CHECK(UVec4::sSelect(UVec4(1, 2, 3, 4), UVec4(5, 6, 7, 8), UVec4(0xffffffffU, 0x7fffffffU, 0xffffffffU, 0x7fffffffU)) == UVec4(5, 2, 7, 4));
+		CHECK(UVec4::sSelect(UVec4(1, 2, 3, 4), UVec4(5, 6, 7, 8), UVec4(0x7fffffffU, 0xffffffffU, 0x7fffffffU, 0xffffffffU)) == UVec4(1, 6, 3, 8));
+	}
+
+	TEST_CASE("TestUVec4BitOps")
+	{
+		// Test all bit permutations
+		UVec4 v1(0b0011, 0b00110, 0b001100, 0b0011000);
+		UVec4 v2(0b0101, 0b01010, 0b010100, 0b0101000);
+
+		CHECK(UVec4::sOr(v1, v2) == UVec4(0b0111, 0b01110, 0b011100, 0b0111000));
+		CHECK(UVec4::sXor(v1, v2) == UVec4(0b0110, 0b01100, 0b011000, 0b0110000));
+		CHECK(UVec4::sAnd(v1, v2) == UVec4(0b0001, 0b00010, 0b000100, 0b0001000));
+
+		CHECK(UVec4::sNot(v1) == UVec4(0xfffffffcU, 0xfffffff9U, 0xfffffff3U, 0xffffffe7U));
+		CHECK(UVec4::sNot(v2) == UVec4(0xfffffffaU, 0xfffffff5U, 0xffffffebU, 0xffffffd7U));
+
+		CHECK(UVec4(0x80000000U, 0x40000000U, 0x20000000U, 0x10000000U).LogicalShiftRight<1>() == UVec4(0x40000000U, 0x20000000U, 0x10000000U, 0x08000000U));
+		CHECK(UVec4(0x80000000U, 0x40000000U, 0x20000000U, 0x10000000U).ArithmeticShiftRight<1>() == UVec4(0xC0000000U, 0x20000000U, 0x10000000U, 0x08000000U));
+		CHECK(UVec4(0x40000000U, 0x20000000U, 0x10000000U, 0x08000001U).LogicalShiftLeft<1>() == UVec4(0x80000000U, 0x40000000U, 0x20000000U, 0x10000002U));
+	}
+
+	TEST_CASE("TestUVec4Operators")
+	{
+		CHECK(UVec4(1, 2, 3, 4) + UVec4(5, 6, 7, 8) == UVec4(6, 8, 10, 12));
+
+		CHECK(UVec4(5, 6, 7, 8) - UVec4(4, 3, 2, 1) == UVec4(1, 3, 5, 7));
+
+		CHECK(UVec4(1, 2, 3, 4) * UVec4(5, 6, 7, 8) == UVec4(1 * 5, 2 * 6, 3 * 7, 4 * 8));
+
+		UVec4 v = UVec4(1, 2, 3, 4);
+		v += UVec4(5, 6, 7, 8);
+		CHECK(v == UVec4(6, 8, 10, 12));
+		v -= UVec4(4, 3, 2, 1);
+		CHECK(v == UVec4(2, 5, 8, 11));
+	}
+
+	TEST_CASE("TestUVec4Swizzle")
+	{
+		UVec4 v(1, 2, 3, 4);
+
+		CHECK(v.SplatX() == UVec4::sReplicate(1));
+		CHECK(v.SplatY() == UVec4::sReplicate(2));
+		CHECK(v.SplatZ() == UVec4::sReplicate(3));
+		CHECK(v.SplatW() == UVec4::sReplicate(4));
+
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == UVec4(1, 1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == UVec4(1, 1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == UVec4(1, 1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_W>() == UVec4(1, 1, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == UVec4(1, 1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(1, 1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(1, 1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W>() == UVec4(1, 1, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == UVec4(1, 1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(1, 1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(1, 1, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W>() == UVec4(1, 1, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_W, SWIZZLE_X>() == UVec4(1, 1, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y>() == UVec4(1, 1, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>() == UVec4(1, 1, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_W, SWIZZLE_W>() == UVec4(1, 1, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == UVec4(1, 2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == UVec4(1, 2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == UVec4(1, 2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() == UVec4(1, 2, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == UVec4(1, 2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(1, 2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(1, 2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W>() == UVec4(1, 2, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == UVec4(1, 2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(1, 2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(1, 2, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W>() == UVec4(1, 2, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X>() == UVec4(1, 2, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y>() == UVec4(1, 2, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>() == UVec4(1, 2, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>() == UVec4(1, 2, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == UVec4(1, 3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == UVec4(1, 3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == UVec4(1, 3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W>() == UVec4(1, 3, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == UVec4(1, 3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(1, 3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(1, 3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>() == UVec4(1, 3, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == UVec4(1, 3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(1, 3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(1, 3, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>() == UVec4(1, 3, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X>() == UVec4(1, 3, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y>() == UVec4(1, 3, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z>() == UVec4(1, 3, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>() == UVec4(1, 3, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_X, SWIZZLE_X>() == UVec4(1, 4, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>() == UVec4(1, 4, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z>() == UVec4(1, 4, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_X, SWIZZLE_W>() == UVec4(1, 4, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X>() == UVec4(1, 4, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(1, 4, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(1, 4, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W>() == UVec4(1, 4, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X>() == UVec4(1, 4, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(1, 4, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(1, 4, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>() == UVec4(1, 4, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_W, SWIZZLE_X>() == UVec4(1, 4, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y>() == UVec4(1, 4, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z>() == UVec4(1, 4, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_W, SWIZZLE_W>() == UVec4(1, 4, 4, 4));
+
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == UVec4(2, 1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == UVec4(2, 1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == UVec4(2, 1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_W>() == UVec4(2, 1, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == UVec4(2, 1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(2, 1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(2, 1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W>() == UVec4(2, 1, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == UVec4(2, 1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(2, 1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(2, 1, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W>() == UVec4(2, 1, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_X>() == UVec4(2, 1, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y>() == UVec4(2, 1, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>() == UVec4(2, 1, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_W>() == UVec4(2, 1, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == UVec4(2, 2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == UVec4(2, 2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == UVec4(2, 2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() == UVec4(2, 2, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == UVec4(2, 2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(2, 2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(2, 2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W>() == UVec4(2, 2, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == UVec4(2, 2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(2, 2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(2, 2, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W>() == UVec4(2, 2, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X>() == UVec4(2, 2, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y>() == UVec4(2, 2, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>() == UVec4(2, 2, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>() == UVec4(2, 2, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == UVec4(2, 3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == UVec4(2, 3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == UVec4(2, 3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W>() == UVec4(2, 3, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == UVec4(2, 3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(2, 3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(2, 3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>() == UVec4(2, 3, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == UVec4(2, 3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(2, 3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(2, 3, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>() == UVec4(2, 3, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X>() == UVec4(2, 3, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y>() == UVec4(2, 3, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z>() == UVec4(2, 3, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>() == UVec4(2, 3, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_X>() == UVec4(2, 4, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>() == UVec4(2, 4, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z>() == UVec4(2, 4, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_W>() == UVec4(2, 4, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X>() == UVec4(2, 4, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(2, 4, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(2, 4, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W>() == UVec4(2, 4, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X>() == UVec4(2, 4, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(2, 4, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(2, 4, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>() == UVec4(2, 4, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W, SWIZZLE_X>() == UVec4(2, 4, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y>() == UVec4(2, 4, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z>() == UVec4(2, 4, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W, SWIZZLE_W>() == UVec4(2, 4, 4, 4));
+
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == UVec4(3, 1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == UVec4(3, 1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == UVec4(3, 1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X, SWIZZLE_W>() == UVec4(3, 1, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == UVec4(3, 1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(3, 1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(3, 1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W>() == UVec4(3, 1, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == UVec4(3, 1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(3, 1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(3, 1, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W>() == UVec4(3, 1, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W, SWIZZLE_X>() == UVec4(3, 1, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y>() == UVec4(3, 1, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>() == UVec4(3, 1, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W, SWIZZLE_W>() == UVec4(3, 1, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == UVec4(3, 2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == UVec4(3, 2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == UVec4(3, 2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() == UVec4(3, 2, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == UVec4(3, 2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(3, 2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(3, 2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W>() == UVec4(3, 2, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == UVec4(3, 2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(3, 2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(3, 2, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W>() == UVec4(3, 2, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X>() == UVec4(3, 2, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y>() == UVec4(3, 2, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>() == UVec4(3, 2, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>() == UVec4(3, 2, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == UVec4(3, 3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == UVec4(3, 3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == UVec4(3, 3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W>() == UVec4(3, 3, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == UVec4(3, 3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(3, 3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(3, 3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>() == UVec4(3, 3, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == UVec4(3, 3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(3, 3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(3, 3, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>() == UVec4(3, 3, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X>() == UVec4(3, 3, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y>() == UVec4(3, 3, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z>() == UVec4(3, 3, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>() == UVec4(3, 3, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_X>() == UVec4(3, 4, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>() == UVec4(3, 4, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z>() == UVec4(3, 4, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_W>() == UVec4(3, 4, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X>() == UVec4(3, 4, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(3, 4, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(3, 4, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W>() == UVec4(3, 4, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X>() == UVec4(3, 4, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(3, 4, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(3, 4, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>() == UVec4(3, 4, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W, SWIZZLE_X>() == UVec4(3, 4, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y>() == UVec4(3, 4, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z>() == UVec4(3, 4, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W, SWIZZLE_W>() == UVec4(3, 4, 4, 4));
+
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == UVec4(4, 1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == UVec4(4, 1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == UVec4(4, 1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_X, SWIZZLE_W>() == UVec4(4, 1, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == UVec4(4, 1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(4, 1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(4, 1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W>() == UVec4(4, 1, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == UVec4(4, 1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(4, 1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(4, 1, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W>() == UVec4(4, 1, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_X>() == UVec4(4, 1, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y>() == UVec4(4, 1, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>() == UVec4(4, 1, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_W>() == UVec4(4, 1, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == UVec4(4, 2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == UVec4(4, 2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == UVec4(4, 2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() == UVec4(4, 2, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == UVec4(4, 2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(4, 2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(4, 2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W>() == UVec4(4, 2, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == UVec4(4, 2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(4, 2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(4, 2, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W>() == UVec4(4, 2, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X>() == UVec4(4, 2, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y>() == UVec4(4, 2, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>() == UVec4(4, 2, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>() == UVec4(4, 2, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == UVec4(4, 3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == UVec4(4, 3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == UVec4(4, 3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W>() == UVec4(4, 3, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == UVec4(4, 3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(4, 3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(4, 3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>() == UVec4(4, 3, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == UVec4(4, 3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(4, 3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(4, 3, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>() == UVec4(4, 3, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X>() == UVec4(4, 3, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y>() == UVec4(4, 3, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z>() == UVec4(4, 3, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>() == UVec4(4, 3, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_X, SWIZZLE_X>() == UVec4(4, 4, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>() == UVec4(4, 4, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z>() == UVec4(4, 4, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_X, SWIZZLE_W>() == UVec4(4, 4, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X>() == UVec4(4, 4, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y>() == UVec4(4, 4, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>() == UVec4(4, 4, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W>() == UVec4(4, 4, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X>() == UVec4(4, 4, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y>() == UVec4(4, 4, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z>() == UVec4(4, 4, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>() == UVec4(4, 4, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_W, SWIZZLE_X>() == UVec4(4, 4, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y>() == UVec4(4, 4, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z>() == UVec4(4, 4, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_W, SWIZZLE_W>() == UVec4(4, 4, 4, 4));
+	}
+
+	TEST_CASE("TestUVec4Dot")
+	{
+		CHECK(UVec4(1, 2, 3, 4).Dot(UVec4(5, 6, 7, 8)) == 1 * 5 + 2 * 6 + 3 * 7 + 4 * 8);
+		CHECK(UVec4(1, 2, 3, 4).DotV(UVec4(5, 6, 7, 8)) == UVec4::sReplicate(1 * 5 + 2 * 6 + 3 * 7 + 4 * 8));
+	}
+
+	TEST_CASE("TestUVec4Cast")
+	{
+		CHECK(UVec4(1, 2, 3, 4).ToFloat() == Vec4(1, 2, 3, 4));
+		CHECK(UVec4(0x3f800000U, 0x40000000U, 0x40400000U, 0x40800000U).ReinterpretAsFloat() == Vec4(1, 2, 3, 4));
+	}
+
+	TEST_CASE("TestUVec4ExtractUInt16")
+	{
+		uint32 data[] = { 0x0b020a01, 0x0d040c03, 0x0b060a05, 0x0d080c07 };
+		UVec4 vector = UVec4::sLoadInt4(data);
+
+		CHECK(vector.Expand4Uint16Lo() == UVec4(0x0a01, 0x0b02, 0x0c03, 0x0d04));
+		CHECK(vector.Expand4Uint16Hi() == UVec4(0x0a05, 0x0b06, 0x0c07, 0x0d08));
+	}
+
+	TEST_CASE("TestUVec4ExtractBytes")
+	{
+		uint32 data[] = { 0x14131211, 0x24232221, 0x34333231, 0x44434241 };
+		UVec4 vector = UVec4::sLoadInt4(data);
+
+		CHECK(vector.Expand4Byte0()  == UVec4(0x11, 0x12, 0x13, 0x14));
+		CHECK(vector.Expand4Byte4()  == UVec4(0x21, 0x22, 0x23, 0x24));
+		CHECK(vector.Expand4Byte8()  == UVec4(0x31, 0x32, 0x33, 0x34));
+		CHECK(vector.Expand4Byte12() == UVec4(0x41, 0x42, 0x43, 0x44));
+	}
+
+	TEST_CASE("TestUVec4ShiftComponents")
+	{
+		UVec4 v(1, 2, 3, 4);
+
+		CHECK(v.ShiftComponents4Minus(4) == UVec4(1, 2, 3, 4));
+		CHECK(v.ShiftComponents4Minus(3) == UVec4(2, 3, 4, 0));
+		CHECK(v.ShiftComponents4Minus(2) == UVec4(3, 4, 0, 0));
+		CHECK(v.ShiftComponents4Minus(1) == UVec4(4, 0, 0, 0));
+		CHECK(v.ShiftComponents4Minus(0) == UVec4(0, 0, 0, 0));
+	}
+
+	TEST_CASE("TestUVec4Sort4True")
+	{
+		CHECK(UVec4::sSort4True(UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0x00000000U), UVec4(1, 2, 3, 4)) == UVec4(4, 4, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0x00000000U), UVec4(1, 2, 3, 4)) == UVec4(1, 4, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0x00000000U), UVec4(1, 2, 3, 4)) == UVec4(2, 4, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0x00000000U), UVec4(1, 2, 3, 4)) == UVec4(1, 2, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0x00000000U), UVec4(1, 2, 3, 4)) == UVec4(3, 4, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0x00000000U), UVec4(1, 2, 3, 4)) == UVec4(1, 3, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0x00000000U), UVec4(1, 2, 3, 4)) == UVec4(2, 3, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0x00000000U), UVec4(1, 2, 3, 4)) == UVec4(1, 2, 3, 4));
+		CHECK(UVec4::sSort4True(UVec4(0x00000000U, 0x00000000U, 0x00000000U, 0xffffffffU), UVec4(1, 2, 3, 4)) == UVec4(4, 4, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0xffffffffU, 0x00000000U, 0x00000000U, 0xffffffffU), UVec4(1, 2, 3, 4)) == UVec4(1, 4, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0x00000000U, 0xffffffffU, 0x00000000U, 0xffffffffU), UVec4(1, 2, 3, 4)) == UVec4(2, 4, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0xffffffffU, 0xffffffffU, 0x00000000U, 0xffffffffU), UVec4(1, 2, 3, 4)) == UVec4(1, 2, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0x00000000U, 0x00000000U, 0xffffffffU, 0xffffffffU), UVec4(1, 2, 3, 4)) == UVec4(3, 4, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0xffffffffU, 0x00000000U, 0xffffffffU, 0xffffffffU), UVec4(1, 2, 3, 4)) == UVec4(1, 3, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0x00000000U, 0xffffffffU, 0xffffffffU, 0xffffffffU), UVec4(1, 2, 3, 4)) == UVec4(2, 3, 4, 4));
+		CHECK(UVec4::sSort4True(UVec4(0xffffffffU, 0xffffffffU, 0xffffffffU, 0xffffffffU), UVec4(1, 2, 3, 4)) == UVec4(1, 2, 3, 4));
+	}
+
+	TEST_CASE("TestUVec4ConvertToString")
+	{
+		UVec4 v(1, 2, 3, 4);
+		CHECK(ConvertToString(v) == "1, 2, 3, 4");
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/Vec3Tests.cpp

@@ -0,0 +1,408 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/StringTools.h>
+
+TEST_SUITE("Vec3Tests")
+{
+	TEST_CASE("TestVec3ConstructComponents")
+	{
+		Vec3 v(1, 2, 3);
+
+		// Test component access
+		CHECK(v.GetX() == 1);
+		CHECK(v.GetY() == 2);
+		CHECK(v.GetZ() == 3);
+
+		// Test component access by [] operators
+		CHECK(v[0] == 1);
+		CHECK(v[1] == 2);
+		CHECK(v[2] == 3);
+
+		// Test == and != operators
+		CHECK(v == Vec3(1, 2, 3));
+		CHECK(v != Vec3(1, 2, 4));
+
+		// Set the components
+		v.SetComponent(0, 4);
+		v.SetComponent(1, 5);
+		v.SetComponent(2, 6);
+		CHECK(v == Vec3(4, 5, 6));
+
+		// Set the components
+		v.SetX(7);
+		v.SetY(8);
+		v.SetZ(9);
+		CHECK(v == Vec3(7, 8, 9));
+
+		// Set all components
+		v.Set(10, 11, 12);
+		CHECK(v == Vec3(10, 11, 12));
+	}
+
+	TEST_CASE("TestVec3LoadStoreFloat3")
+	{
+		float f4[] = { 1, 2, 3, 4 }; // Extra element since we read one too many in sLoadFloat3Unsafe
+		Float3 &f3 = *(Float3 *)f4;
+		CHECK(Vec3(f3) == Vec3(1, 2, 3));
+		CHECK(Vec3::sLoadFloat3Unsafe(f3) == Vec3(1, 2, 3));
+
+		Float3 f3_out;
+		Vec3(1, 2, 3).StoreFloat3(&f3_out);
+		CHECK(f3 == f3_out);
+	}
+
+	TEST_CASE("TestVec3ConstructVec4")
+	{
+		Vec4 v4(1, 2, 3, 4);
+		CHECK(Vec3(v4) == Vec3(1, 2, 3));
+	}
+
+	TEST_CASE("TestVec3Zero")
+	{
+		Vec3 v = Vec3::sZero();
+
+		CHECK(v.GetX() == 0);
+		CHECK(v.GetY() == 0);
+		CHECK(v.GetZ() == 0);
+	}
+
+	TEST_CASE("TestVec3NaN")
+	{
+		Vec3 v = Vec3::sNaN();
+
+		CHECK(isnan(v.GetX()));
+		CHECK(isnan(v.GetY()));
+		CHECK(isnan(v.GetZ()));
+		CHECK(v.IsNaN());
+
+		v.SetComponent(0, 0);
+		CHECK(v.IsNaN());
+		v.SetComponent(1, 0);
+		CHECK(v.IsNaN());
+		v.SetComponent(2, 0);
+		CHECK(!v.IsNaN());
+	}
+
+	TEST_CASE("TestVec3Replicate")
+	{
+		CHECK(Vec3::sReplicate(2) == Vec3(2, 2, 2));
+	}
+
+	TEST_CASE("TestVec3MinMax")
+	{
+		Vec3 v1(1, 5, 3);
+		Vec3 v2(4, 2, 6);
+		Vec3 v3(6, 4, 2);
+
+		CHECK(Vec3::sMin(v1, v2) == Vec3(1, 2, 3));
+		CHECK(Vec3::sMax(v1, v2) == Vec3(4, 5, 6));
+
+		CHECK(v1.ReduceMin() == 1);
+		CHECK(v1.ReduceMax() == 5);
+		CHECK(v2.ReduceMin() == 2);
+		CHECK(v2.ReduceMax() == 6);
+
+		CHECK(v1.GetLowestComponentIndex() == 0);
+		CHECK(v1.GetHighestComponentIndex() == 1);
+		CHECK(v2.GetLowestComponentIndex() == 1);
+		CHECK(v2.GetHighestComponentIndex() == 2);
+		CHECK(v3.GetLowestComponentIndex() == 2);
+		CHECK(v3.GetHighestComponentIndex() == 0);
+	}
+
+	TEST_CASE("TestVec3Clamp")
+	{
+		Vec3 v1(1, 2, 3);
+		Vec3 v2(4, 5, 6);
+		Vec3 v(-1, 3, 7);
+
+		CHECK(Vec3::sClamp(v, v1, v2) == Vec3(1, 3, 6));
+	}
+
+	TEST_CASE("TestVec3Comparisons")
+	{
+		CHECK(Vec3::sEquals(Vec3(1, 2, 3), Vec3(1, 4, 3)) == UVec4(0xffffffffU, 0, 0xffffffffU, 0xffffffffU)); // W is always Z for comparisons
+		CHECK(Vec3::sLess(Vec3(1, 2, 4), Vec3(1, 4, 3)) == UVec4(0, 0xffffffffU, 0, 0));
+		CHECK(Vec3::sLessOrEqual(Vec3(1, 2, 4), Vec3(1, 4, 3)) == UVec4(0xffffffffU, 0xffffffffU, 0, 0));
+		CHECK(Vec3::sGreater(Vec3(1, 2, 4), Vec3(1, 4, 3)) == UVec4(0, 0, 0xffffffffU, 0xffffffffU));
+		CHECK(Vec3::sGreaterOrEqual(Vec3(1, 2, 4), Vec3(1, 4, 3)) == UVec4(0xffffffffU, 0, 0xffffffffU, 0xffffffffU));
+	}
+
+	TEST_CASE("TestVec3FMA")
+	{
+		CHECK(Vec3::sFusedMultiplyAdd(Vec3(1, 2, 3), Vec3(4, 5, 6), Vec3(7, 8, 9)) == Vec3(1 * 4 + 7, 2 * 5 + 8, 3 * 6 + 9));
+	}
+
+	TEST_CASE("TestVec3Select")
+	{
+		CHECK(Vec3::sSelect(Vec3(1, 2, 3), Vec3(4, 5, 6), UVec4(0x80000000U, 0, 0x80000000U, 0)) == Vec3(4, 2, 6));
+		CHECK(Vec3::sSelect(Vec3(1, 2, 3), Vec3(4, 5, 6), UVec4(0, 0x80000000U, 0, 0x80000000U)) == Vec3(1, 5, 3));
+		CHECK(Vec3::sSelect(Vec3(1, 2, 3), Vec3(4, 5, 6), UVec4(0xffffffffU, 0x7fffffffU, 0xffffffffU, 0x7fffffffU)) == Vec3(4, 2, 6));
+		CHECK(Vec3::sSelect(Vec3(1, 2, 3), Vec3(4, 5, 6), UVec4(0x7fffffffU, 0xffffffffU, 0x7fffffffU, 0xffffffffU)) == Vec3(1, 5, 3));
+	}
+
+	TEST_CASE("TestVec3BitOps")
+	{
+		// Test all bit permutations
+		Vec3 v1(UVec4(0b0011, 0b00110, 0b001100, 0).ReinterpretAsFloat());
+		Vec3 v2(UVec4(0b0101, 0b01010, 0b010100, 0).ReinterpretAsFloat());
+
+		CHECK(Vec3::sOr(v1, v2) == Vec3(UVec4(0b0111, 0b01110, 0b011100, 0).ReinterpretAsFloat()));
+		CHECK(Vec3::sXor(v1, v2) == Vec3(UVec4(0b0110, 0b01100, 0b011000, 0).ReinterpretAsFloat()));
+		CHECK(Vec3::sAnd(v1, v2) == Vec3(UVec4(0b0001, 0b00010, 0b000100, 0).ReinterpretAsFloat()));
+	}
+
+	TEST_CASE("TestVec3Close")
+	{
+		CHECK(Vec3(1, 2, 3).IsClose(Vec3(1.001f, 2.001f, 3.001f), 1.0e-4f));
+		CHECK(!Vec3(1, 2, 3).IsClose(Vec3(1.001f, 2.001f, 3.001f), 1.0e-6f));
+
+		CHECK(Vec3(1.001f, 0, 0).IsNormalized(1.0e-2f));
+		CHECK(!Vec3(0, 1.001f, 0).IsNormalized(1.0e-4f));
+
+		CHECK(Vec3(-1.0e-7f, 1.0e-7f, 1.0e-8f).IsNearZero());
+		CHECK(!Vec3(-1.0e-7f, 1.0e-7f, -1.0e-5f).IsNearZero());
+	}
+
+	TEST_CASE("TestVec3Operators")
+	{
+		CHECK(-Vec3(1, 2, 3) == Vec3(-1, -2, -3));
+
+		Vec3 neg_zero = -Vec3::sZero();
+		CHECK(neg_zero == Vec3::sZero());
+
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		// When cross platform deterministic, we want to make sure that -0 is represented as 0
+		UVec4 neg_zero_bin = neg_zero.ReinterpretAsInt();
+		CHECK(neg_zero_bin.GetX() == 0);
+		CHECK(neg_zero_bin.GetY() == 0);
+		CHECK(neg_zero_bin.GetZ() == 0);
+	#endif // JPH_CROSS_PLATFORM_DETERMINISTIC
+
+		CHECK(Vec3(1, 2, 3) + Vec3(4, 5, 6) == Vec3(5, 7, 9));
+		CHECK(Vec3(1, 2, 3) - Vec3(6, 5, 4) == Vec3(-5, -3, -1));
+
+		CHECK(Vec3(1, 2, 3) * Vec3(4, 5, 6) == Vec3(4, 10, 18));
+		CHECK(Vec3(1, 2, 3) * 2 == Vec3(2, 4, 6));
+		CHECK(4 * Vec3(1, 2, 3) == Vec3(4, 8, 12));
+
+		CHECK(Vec3(1, 2, 3) / 2 == Vec3(0.5f, 1.0f, 1.5f));
+		CHECK(Vec3(1, 2, 3) / Vec3(2, 8, 24) == Vec3(0.5f, 0.25f, 0.125f));
+
+		Vec3 v = Vec3(1, 2, 3);
+		v *= Vec3(4, 5, 6);
+		CHECK(v == Vec3(4, 10, 18));
+		v *= 2;
+		CHECK(v == Vec3(8, 20, 36));
+		v /= 2;
+		CHECK(v == Vec3(4, 10, 18));
+		v += Vec3(1, 2, 3);
+		CHECK(v == Vec3(5, 12, 21));
+		v -= Vec3(1, 2, 3);
+		CHECK(v == Vec3(4, 10, 18));
+
+		CHECK(Vec3(2, 4, 8).Reciprocal() == Vec3(0.5f, 0.25f, 0.125f));
+	}
+
+	TEST_CASE("TestVec3Swizzle")
+	{
+		Vec3 v(1, 2, 3);
+
+		CHECK(v.SplatX() == Vec4::sReplicate(1));
+		CHECK(v.SplatY() == Vec4::sReplicate(2));
+		CHECK(v.SplatZ() == Vec4::sReplicate(3));
+
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == Vec3(1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == Vec3(1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == Vec3(1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == Vec3(1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == Vec3(1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == Vec3(1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == Vec3(1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == Vec3(1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == Vec3(1, 3, 3));
+
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == Vec3(2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == Vec3(2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == Vec3(2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == Vec3(2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == Vec3(2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == Vec3(2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == Vec3(2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == Vec3(2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == Vec3(2, 3, 3));
+
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == Vec3(3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == Vec3(3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == Vec3(3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == Vec3(3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == Vec3(3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == Vec3(3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == Vec3(3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == Vec3(3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == Vec3(3, 3, 3));
+	}
+
+	TEST_CASE("TestVec3Abs")
+	{
+		CHECK(Vec3(1, -2, 3).Abs() == Vec3(1, 2, 3));
+		CHECK(Vec3(-1, 2, -3).Abs() == Vec3(1, 2, 3));
+	}
+
+	TEST_CASE("TestVec3Dot")
+	{
+		CHECK(Vec3(1, 2, 3).Dot(Vec3(4, 5, 6)) == float(1 * 4 + 2 * 5 + 3 * 6));
+		CHECK(Vec3(1, 2, 3).DotV(Vec3(4, 5, 6)) == Vec3::sReplicate(1 * 4 + 2 * 5 + 3 * 6));
+		CHECK(Vec3(1, 2, 3).DotV4(Vec3(4, 5, 6)) == Vec4::sReplicate(1 * 4 + 2 * 5 + 3 * 6));
+	}
+
+	TEST_CASE("TestVec3Length")
+	{
+		CHECK(Vec3(1, 2, 3).LengthSq() == float(1 + 4 + 9));
+		CHECK(Vec3(1, 2, 3).Length() == sqrt(float(1 + 4 + 9)));
+	}
+
+	TEST_CASE("TestVec3Sqrt")
+	{
+		CHECK_APPROX_EQUAL(Vec3(13, 15, 17).Sqrt(), Vec3(sqrt(13.0f), sqrt(15.0f), sqrt(17.0f)));
+	}
+
+	TEST_CASE("TestVec3Cross")
+	{
+		CHECK(Vec3(1, 0, 0).Cross(Vec3(0, 1, 0)) == Vec3(0, 0, 1));
+		CHECK(Vec3(0, 1, 0).Cross(Vec3(1, 0, 0)) == Vec3(0, 0, -1));
+		CHECK(Vec3(0, 1, 0).Cross(Vec3(0, 0, 1)) == Vec3(1, 0, 0));
+		CHECK(Vec3(0, 0, 1).Cross(Vec3(0, 1, 0)) == Vec3(-1, 0, 0));
+		CHECK(Vec3(0, 0, 1).Cross(Vec3(1, 0, 0)) == Vec3(0, 1, 0));
+		CHECK(Vec3(1, 0, 0).Cross(Vec3(0, 0, 1)) == Vec3(0, -1, 0));
+	}
+
+	TEST_CASE("TestVec3Normalize")
+	{
+		CHECK(Vec3(3, 2, 1).Normalized() == Vec3(3, 2, 1) / sqrt(9.0f + 4.0f + 1.0f));
+		CHECK(Vec3(3, 2, 1).NormalizedOr(Vec3(1, 2, 3)) == Vec3(3, 2, 1) / sqrt(9.0f + 4.0f + 1.0f));
+		CHECK(Vec3::sZero().NormalizedOr(Vec3(1, 2, 3)) == Vec3(1, 2, 3));
+		CHECK(Vec3(0.999f * sqrt(FLT_MIN), 0, 0).NormalizedOr(Vec3(1, 2, 3)) == Vec3(1, 2, 3)); // A vector that has a squared length that is denormal should also be treated as zero
+		CHECK_APPROX_EQUAL(Vec3(1.001f * sqrt(FLT_MIN), 0, 0).NormalizedOr(Vec3(1, 2, 3)), Vec3(1, 0, 0)); // A value that is just above being denormal should work normally
+	}
+
+	TEST_CASE("TestVec3Cast")
+	{
+		CHECK(UVec4::sEquals(Vec3(1, 2, 3).ToInt(), UVec4(1, 2, 3, 0)).TestAllXYZTrue());
+		CHECK(UVec4::sEquals(Vec3(1, 2, 3).ReinterpretAsInt(), UVec4(0x3f800000U, 0x40000000U, 0x40400000U, 0)).TestAllXYZTrue());
+	}
+
+	TEST_CASE("TestVec3NormalizedPerpendicular")
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> one_to_ten(1.0f, 10.0f);
+		for (int i = 0; i < 100; ++i)
+		{
+			Vec3 v = Vec3::sRandom(random);
+			CHECK(v.IsNormalized());
+			v *= one_to_ten(random);
+
+			Vec3 p = v.GetNormalizedPerpendicular();
+
+			CHECK(p.IsNormalized());
+			CHECK(abs(v.Dot(p)) < 1.0e-6f);
+		}
+	}
+
+	TEST_CASE("TestVec3Sign")
+	{
+		CHECK(Vec3(1.2345f, -6.7891f, 0).GetSign() == Vec3(1, -1, 1));
+		CHECK(Vec3(0, 2.3456f, -7.8912f).GetSign() == Vec3(1, 1, -1));
+	}
+
+	TEST_CASE("TestVec3FlipSign")
+	{
+		Vec3 v(1, 2, 3);
+		CHECK(v.FlipSign<-1, 1, 1>() == Vec3(-1, 2, 3));
+		CHECK(v.FlipSign<1, -1, 1>() == Vec3(1, -2, 3));
+		CHECK(v.FlipSign<1, 1, -1>() == Vec3(1, 2, -3));
+	}
+
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	TEST_CASE("TestVec3SyncW")
+	{
+		{
+			// Check that W equals Z
+			Vec3 v(1, 2, 3);
+			CHECK(Vec4(v) == Vec4(1, 2, 3, 3));
+		}
+
+		{
+			// Check that setting individual components syncs W and Z
+			Vec3 v;
+			v.SetComponent(2, 3);
+			v.SetComponent(1, 2);
+			v.SetComponent(0, 1);
+			CHECK(v == Vec3(1, 2, 3));
+			CHECK(Vec4(v) == Vec4(1, 2, 3, 3));
+		}
+
+		{
+			// Check that W and Z are still synced after a simple addition
+			CHECK(Vec4(Vec3(1, 2, 3) + Vec3(4, 5, 6)) == Vec4(5, 7, 9, 9));
+		}
+
+		{
+			// Test that casting a Vec4 to Vec3 syncs W and Z
+			CHECK(Vec4(Vec3(Vec4(1, 2, 3, 4))) == Vec4(1, 2, 3, 3));
+		}
+
+		{
+			// Test that loading from Float3 syncs W and Z
+			CHECK(Vec4(Vec3(Float3(1, 2, 3))) == Vec4(1, 2, 3, 3));
+		}
+
+		{
+			// Test that loading unsafe from Float3 syncs W and Z
+			Float4 v(1, 2, 3, 4);
+			CHECK(Vec4(Vec3::sLoadFloat3Unsafe(*(Float3 *)&v)) == Vec4(1, 2, 3, 3));
+		}
+
+		{
+			// Test swizzle syncs W and Z
+			CHECK(Vec4(Vec3(1, 2, 3).Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>()) == Vec4(3, 2, 1, 1));
+		}
+
+		{
+			// Test cross product syncs W and Z
+			CHECK(Vec4(Vec3(1, 0, 0).Cross(Vec3(0, 1, 0))) == Vec4(0, 0, 1, 1));
+			CHECK(Vec4(Vec3(0, 1, 0).Cross(Vec3(0, 0, 1))) == Vec4(1, 0, 0, 0));
+		}
+	}
+#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+
+	TEST_CASE("TestVec3ConvertToString")
+	{
+		Vec3 v(1, 2, 3);
+		CHECK(ConvertToString(v) == "1, 2, 3");
+	}
+
+	TEST_CASE("TestVec3CompressUnitVector")
+	{
+		// We want these to be preserved exactly
+		CHECK(Vec3::sDecompressUnitVector(Vec3::sAxisX().CompressUnitVector()) == Vec3::sAxisX());
+		CHECK(Vec3::sDecompressUnitVector(Vec3::sAxisY().CompressUnitVector()) == Vec3::sAxisY());
+		CHECK(Vec3::sDecompressUnitVector(Vec3::sAxisZ().CompressUnitVector()) == Vec3::sAxisZ());
+		CHECK(Vec3::sDecompressUnitVector((-Vec3::sAxisX()).CompressUnitVector()) == -Vec3::sAxisX());
+		CHECK(Vec3::sDecompressUnitVector((-Vec3::sAxisY()).CompressUnitVector()) == -Vec3::sAxisY());
+		CHECK(Vec3::sDecompressUnitVector((-Vec3::sAxisZ()).CompressUnitVector()) == -Vec3::sAxisZ());
+
+		UnitTestRandom random;
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 v = Vec3::sRandom(random);
+			uint32 compressed = v.CompressUnitVector();
+			Vec3 decompressed = Vec3::sDecompressUnitVector(compressed);
+			float diff = (decompressed - v).Length();
+			CHECK(diff < 1.0e-4f);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/Vec4Tests.cpp

@@ -0,0 +1,795 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/StringTools.h>
+
+TEST_SUITE("Vec4Tests")
+{
+	TEST_CASE("TestVec4Construct")
+	{
+		Vec4 v(1, 2, 3, 4);
+
+		// Test component access
+		CHECK(v.GetX() == 1);
+		CHECK(v.GetY() == 2);
+		CHECK(v.GetZ() == 3);
+		CHECK(v.GetW() == 4);
+
+		// Test component access by [] operators
+		CHECK(v[0] == 1);
+		CHECK(v[1] == 2);
+		CHECK(v[2] == 3);
+		CHECK(v[3] == 4);
+
+		// Test == and != operators
+		CHECK(v == Vec4(1, 2, 3, 4));
+		CHECK(v != Vec4(1, 2, 4, 3));
+	}
+
+	TEST_CASE("TestVec4LoadStoreFloat4")
+	{
+		alignas(16) Float4 f4 = { 1, 2, 3, 4 };
+		CHECK(Vec4::sLoadFloat4(&f4) == Vec4(1, 2, 3, 4));
+		CHECK(Vec4::sLoadFloat4Aligned(&f4) == Vec4(1, 2, 3, 4));
+
+		Float4 f4_out;
+		Vec4(1, 2, 3, 4).StoreFloat4(&f4_out);
+		CHECK(f4_out[0] == 1);
+		CHECK(f4_out[1] == 2);
+		CHECK(f4_out[2] == 3);
+		CHECK(f4_out[3] == 4);
+
+		float sf[] = { 0, 0, 1, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 4, 0 };
+		CHECK(Vec4::sGatherFloat4<2 * sizeof(float)>(sf, UVec4(1, 3, 8, 9)) == Vec4(1, 2, 3, 4));
+	}
+
+	TEST_CASE("TestVec4ConstructVec3")
+	{
+		Vec3 v3(1, 2, 3);
+		CHECK(Vec4(v3, 4) == Vec4(1, 2, 3, 4));
+	}
+
+	TEST_CASE("TestVec4Zero")
+	{
+		Vec4 v = Vec4::sZero();
+
+		CHECK(v.GetX() == 0);
+		CHECK(v.GetY() == 0);
+		CHECK(v.GetZ() == 0);
+		CHECK(v.GetW() == 0);
+	}
+
+	TEST_CASE("TestVec4NaN")
+	{
+		Vec4 v = Vec4::sNaN();
+
+		CHECK(isnan(v.GetX()));
+		CHECK(isnan(v.GetY()));
+		CHECK(isnan(v.GetZ()));
+		CHECK(isnan(v.GetW()));
+		CHECK(v.IsNaN());
+
+		v.SetX(0);
+		CHECK(v.IsNaN());
+		v.SetY(0);
+		CHECK(v.IsNaN());
+		v.SetZ(0);
+		CHECK(v.IsNaN());
+		v.SetW(0);
+		CHECK(!v.IsNaN());
+	}
+
+	TEST_CASE("TestVec4Replicate")
+	{
+		CHECK(Vec4::sReplicate(2) == Vec4(2, 2, 2, 2));
+	}
+
+	TEST_CASE("TestVec4MinMax")
+	{
+		Vec4 v1(1, 6, 3, 8);
+		Vec4 v2(5, 2, 7, 4);
+		Vec4 v3(5, 7, 2, 4);
+		Vec4 v4(7, 5, 4, 2);
+
+		CHECK(Vec4::sMin(v1, v2) == Vec4(1, 2, 3, 4));
+		CHECK(Vec4::sMax(v1, v2) == Vec4(5, 6, 7, 8));
+
+		CHECK(v1.ReduceMin() == 1);
+		CHECK(v1.ReduceMax() == 8);
+		CHECK(v2.ReduceMin() == 2);
+		CHECK(v2.ReduceMax() == 7);
+
+		CHECK(v1.GetLowestComponentIndex() == 0);
+		CHECK(v1.GetHighestComponentIndex() == 3);
+		CHECK(v2.GetLowestComponentIndex() == 1);
+		CHECK(v2.GetHighestComponentIndex() == 2);
+		CHECK(v3.GetLowestComponentIndex() == 2);
+		CHECK(v3.GetHighestComponentIndex() == 1);
+		CHECK(v4.GetLowestComponentIndex() == 3);
+		CHECK(v4.GetHighestComponentIndex() == 0);
+	}
+
+	TEST_CASE("TestVec4Clamp")
+	{
+		Vec4 v1(1, 2, 3, 4);
+		Vec4 v2(5, 6, 7, 8);
+		Vec4 v(-1, 3, 9, -11);
+
+		CHECK(Vec4::sClamp(v, v1, v2) == Vec4(1, 3, 7, 4));
+	}
+
+	TEST_CASE("TestVec4Comparisons")
+	{
+		CHECK(Vec4::sEquals(Vec4(1, 2, 3, 4), Vec4(2, 1, 3, 4)) == UVec4(0, 0, 0xffffffffU, 0xffffffffU));
+		CHECK(Vec4::sLess(Vec4(1, 2, 3, 4), Vec4(2, 1, 3, 4)) == UVec4(0xffffffffU, 0, 0, 0));
+		CHECK(Vec4::sLessOrEqual(Vec4(1, 2, 3, 4), Vec4(2, 1, 3, 4)) == UVec4(0xffffffffU, 0, 0xffffffffU, 0xffffffffU));
+		CHECK(Vec4::sGreater(Vec4(1, 2, 3, 4), Vec4(2, 1, 3, 4)) == UVec4(0, 0xffffffffU, 0, 0));
+		CHECK(Vec4::sGreaterOrEqual(Vec4(1, 2, 3, 4), Vec4(2, 1, 3, 4)) == UVec4(0, 0xffffffffU, 0xffffffffU, 0xffffffffU));
+	}
+
+	TEST_CASE("TestVec4FMA")
+	{
+		CHECK(Vec4::sFusedMultiplyAdd(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), Vec4(9, 10, 11, 12)) == Vec4(1 * 5 + 9, 2 * 6 + 10, 3 * 7 + 11, 4 * 8 + 12));
+	}
+
+	TEST_CASE("TestVec4Select")
+	{
+		CHECK(Vec4::sSelect(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), UVec4(0x80000000U, 0, 0x80000000U, 0)) == Vec4(5, 2, 7, 4));
+		CHECK(Vec4::sSelect(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), UVec4(0, 0x80000000U, 0, 0x80000000U)) == Vec4(1, 6, 3, 8));
+		CHECK(Vec4::sSelect(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), UVec4(0xffffffffU, 0x7fffffffU, 0xffffffffU, 0x7fffffffU)) == Vec4(5, 2, 7, 4));
+		CHECK(Vec4::sSelect(Vec4(1, 2, 3, 4), Vec4(5, 6, 7, 8), UVec4(0x7fffffffU, 0xffffffffU, 0x7fffffffU, 0xffffffffU)) == Vec4(1, 6, 3, 8));
+	}
+
+	TEST_CASE("TestVec4BitOps")
+	{
+		// Test all bit permutations
+		Vec4 v1(UVec4(0b0011, 0b00110, 0b001100, 0b0011000).ReinterpretAsFloat());
+		Vec4 v2(UVec4(0b0101, 0b01010, 0b010100, 0b0101000).ReinterpretAsFloat());
+
+		CHECK(Vec4::sOr(v1, v2) == Vec4(UVec4(0b0111, 0b01110, 0b011100, 0b0111000).ReinterpretAsFloat()));
+		CHECK(Vec4::sXor(v1, v2) == Vec4(UVec4(0b0110, 0b01100, 0b011000, 0b0110000).ReinterpretAsFloat()));
+		CHECK(Vec4::sAnd(v1, v2) == Vec4(UVec4(0b0001, 0b00010, 0b000100, 0b0001000).ReinterpretAsFloat()));
+	}
+
+	TEST_CASE("TestVec4Close")
+	{
+		CHECK(Vec4(1, 2, 3, 4).IsClose(Vec4(1.001f, 2.001f, 3.001f, 4.001f), 1.0e-4f));
+		CHECK(!Vec4(1, 2, 3, 4).IsClose(Vec4(1.001f, 2.001f, 3.001f, 4.001f), 1.0e-6f));
+
+		CHECK(Vec4(1.001f, 0, 0, 0).IsNormalized(1.0e-2f));
+		CHECK(!Vec4(0, 1.001f, 0, 0).IsNormalized(1.0e-4f));
+
+		CHECK(Vec4(-1.0e-7f, 1.0e-7f, 1.0e-8f, -1.0e-8f).IsNearZero());
+		CHECK(!Vec4(-1.0e-7f, 1.0e-7f, -1.0e-5f, 1.0e-5f).IsNearZero());
+	}
+
+	TEST_CASE("TestVec4Operators")
+	{
+		CHECK(-Vec4(1, 2, 3, 4) == Vec4(-1, -2, -3, -4));
+
+		Vec4 neg_zero = -Vec4::sZero();
+		CHECK(neg_zero == Vec4::sZero());
+
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		// When cross platform deterministic, we want to make sure that -0 is represented as 0
+		UVec4 neg_zero_bin = neg_zero.ReinterpretAsInt();
+		CHECK(neg_zero_bin.GetX() == 0);
+		CHECK(neg_zero_bin.GetY() == 0);
+		CHECK(neg_zero_bin.GetZ() == 0);
+		CHECK(neg_zero_bin.GetW() == 0);
+	#endif // JPH_CROSS_PLATFORM_DETERMINISTIC
+
+		CHECK(Vec4(1, 2, 3, 4) + Vec4(5, 6, 7, 8) == Vec4(6, 8, 10, 12));
+		CHECK(Vec4(1, 2, 3, 4) - Vec4(8, 7, 6, 5) == Vec4(-7, -5, -3, -1));
+
+		CHECK(Vec4(1, 2, 3, 4) * Vec4(5, 6, 7, 8) == Vec4(5, 12, 21, 32));
+		CHECK(Vec4(1, 2, 3, 4) * 2 == Vec4(2, 4, 6, 8));
+		CHECK(4 * Vec4(1, 2, 3, 4) == Vec4(4, 8, 12, 16));
+
+		CHECK(Vec4(1, 2, 3, 4) / 2 == Vec4(0.5f, 1.0f, 1.5f, 2.0f));
+		CHECK(Vec4(1, 2, 3, 4) / Vec4(2, 8, 24, 64) == Vec4(0.5f, 0.25f, 0.125f, 0.0625f));
+
+		Vec4 v = Vec4(1, 2, 3, 4);
+		v *= Vec4(5, 6, 7, 8);
+		CHECK(v == Vec4(5, 12, 21, 32));
+		v *= 2;
+		CHECK(v == Vec4(10, 24, 42, 64));
+		v /= 2;
+		CHECK(v == Vec4(5, 12, 21, 32));
+		v += Vec4(1, 2, 3, 4);
+		CHECK(v == Vec4(6, 14, 24, 36));
+
+		CHECK(Vec4(2, 4, 8, 16).Reciprocal() == Vec4(0.5f, 0.25f, 0.125f, 0.0625f));
+	}
+
+	TEST_CASE("TestVec4Swizzle")
+	{
+		Vec4 v(1, 2, 3, 4);
+
+		CHECK(v.SplatX() == Vec4::sReplicate(1));
+		CHECK(v.SplatY() == Vec4::sReplicate(2));
+		CHECK(v.SplatZ() == Vec4::sReplicate(3));
+		CHECK(v.SplatW() == Vec4::sReplicate(4));
+
+		CHECK(v.SplatX3() == Vec3::sReplicate(1));
+		CHECK(v.SplatY3() == Vec3::sReplicate(2));
+		CHECK(v.SplatZ3() == Vec3::sReplicate(3));
+		CHECK(v.SplatW3() == Vec3::sReplicate(4));
+
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == Vec4(1, 1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == Vec4(1, 1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == Vec4(1, 1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_X, SWIZZLE_W>() == Vec4(1, 1, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == Vec4(1, 1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(1, 1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(1, 1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W>() == Vec4(1, 1, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == Vec4(1, 1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(1, 1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(1, 1, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W>() == Vec4(1, 1, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_W, SWIZZLE_X>() == Vec4(1, 1, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y>() == Vec4(1, 1, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>() == Vec4(1, 1, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_X, SWIZZLE_W, SWIZZLE_W>() == Vec4(1, 1, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == Vec4(1, 2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == Vec4(1, 2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == Vec4(1, 2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() == Vec4(1, 2, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == Vec4(1, 2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(1, 2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(1, 2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W>() == Vec4(1, 2, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == Vec4(1, 2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(1, 2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(1, 2, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W>() == Vec4(1, 2, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X>() == Vec4(1, 2, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y>() == Vec4(1, 2, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>() == Vec4(1, 2, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>() == Vec4(1, 2, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == Vec4(1, 3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == Vec4(1, 3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == Vec4(1, 3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W>() == Vec4(1, 3, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == Vec4(1, 3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(1, 3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(1, 3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>() == Vec4(1, 3, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == Vec4(1, 3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(1, 3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(1, 3, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>() == Vec4(1, 3, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X>() == Vec4(1, 3, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y>() == Vec4(1, 3, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z>() == Vec4(1, 3, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>() == Vec4(1, 3, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_X, SWIZZLE_X>() == Vec4(1, 4, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>() == Vec4(1, 4, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z>() == Vec4(1, 4, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_X, SWIZZLE_W>() == Vec4(1, 4, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X>() == Vec4(1, 4, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(1, 4, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(1, 4, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W>() == Vec4(1, 4, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X>() == Vec4(1, 4, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(1, 4, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(1, 4, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>() == Vec4(1, 4, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_W, SWIZZLE_X>() == Vec4(1, 4, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y>() == Vec4(1, 4, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z>() == Vec4(1, 4, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_W, SWIZZLE_W>() == Vec4(1, 4, 4, 4));
+
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == Vec4(2, 1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == Vec4(2, 1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == Vec4(2, 1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_W>() == Vec4(2, 1, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == Vec4(2, 1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(2, 1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(2, 1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W>() == Vec4(2, 1, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == Vec4(2, 1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(2, 1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(2, 1, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W>() == Vec4(2, 1, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_X>() == Vec4(2, 1, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y>() == Vec4(2, 1, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>() == Vec4(2, 1, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_W>() == Vec4(2, 1, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == Vec4(2, 2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == Vec4(2, 2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == Vec4(2, 2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() == Vec4(2, 2, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == Vec4(2, 2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(2, 2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(2, 2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W>() == Vec4(2, 2, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == Vec4(2, 2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(2, 2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(2, 2, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W>() == Vec4(2, 2, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X>() == Vec4(2, 2, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y>() == Vec4(2, 2, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>() == Vec4(2, 2, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>() == Vec4(2, 2, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == Vec4(2, 3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == Vec4(2, 3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == Vec4(2, 3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W>() == Vec4(2, 3, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == Vec4(2, 3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(2, 3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(2, 3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>() == Vec4(2, 3, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == Vec4(2, 3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(2, 3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(2, 3, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>() == Vec4(2, 3, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X>() == Vec4(2, 3, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y>() == Vec4(2, 3, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z>() == Vec4(2, 3, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>() == Vec4(2, 3, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_X>() == Vec4(2, 4, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>() == Vec4(2, 4, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z>() == Vec4(2, 4, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_W>() == Vec4(2, 4, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X>() == Vec4(2, 4, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(2, 4, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(2, 4, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W>() == Vec4(2, 4, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X>() == Vec4(2, 4, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(2, 4, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(2, 4, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>() == Vec4(2, 4, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W, SWIZZLE_X>() == Vec4(2, 4, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y>() == Vec4(2, 4, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z>() == Vec4(2, 4, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W, SWIZZLE_W>() == Vec4(2, 4, 4, 4));
+
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == Vec4(3, 1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == Vec4(3, 1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == Vec4(3, 1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X, SWIZZLE_W>() == Vec4(3, 1, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == Vec4(3, 1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(3, 1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(3, 1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W>() == Vec4(3, 1, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == Vec4(3, 1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(3, 1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(3, 1, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W>() == Vec4(3, 1, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W, SWIZZLE_X>() == Vec4(3, 1, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y>() == Vec4(3, 1, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>() == Vec4(3, 1, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W, SWIZZLE_W>() == Vec4(3, 1, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == Vec4(3, 2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == Vec4(3, 2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == Vec4(3, 2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() == Vec4(3, 2, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == Vec4(3, 2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(3, 2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(3, 2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W>() == Vec4(3, 2, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == Vec4(3, 2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(3, 2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(3, 2, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W>() == Vec4(3, 2, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X>() == Vec4(3, 2, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y>() == Vec4(3, 2, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>() == Vec4(3, 2, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>() == Vec4(3, 2, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == Vec4(3, 3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == Vec4(3, 3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == Vec4(3, 3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W>() == Vec4(3, 3, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == Vec4(3, 3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(3, 3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(3, 3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>() == Vec4(3, 3, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == Vec4(3, 3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(3, 3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(3, 3, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>() == Vec4(3, 3, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X>() == Vec4(3, 3, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y>() == Vec4(3, 3, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z>() == Vec4(3, 3, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>() == Vec4(3, 3, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_X>() == Vec4(3, 4, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>() == Vec4(3, 4, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z>() == Vec4(3, 4, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_W>() == Vec4(3, 4, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X>() == Vec4(3, 4, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(3, 4, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(3, 4, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W>() == Vec4(3, 4, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X>() == Vec4(3, 4, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(3, 4, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(3, 4, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>() == Vec4(3, 4, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W, SWIZZLE_X>() == Vec4(3, 4, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y>() == Vec4(3, 4, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z>() == Vec4(3, 4, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W, SWIZZLE_W>() == Vec4(3, 4, 4, 4));
+
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_X, SWIZZLE_X>() == Vec4(4, 1, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Y>() == Vec4(4, 1, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_X, SWIZZLE_Z>() == Vec4(4, 1, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_X, SWIZZLE_W>() == Vec4(4, 1, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>() == Vec4(4, 1, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(4, 1, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(4, 1, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W>() == Vec4(4, 1, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_X>() == Vec4(4, 1, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(4, 1, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(4, 1, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W>() == Vec4(4, 1, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_X>() == Vec4(4, 1, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y>() == Vec4(4, 1, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>() == Vec4(4, 1, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_W>() == Vec4(4, 1, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() == Vec4(4, 2, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Y>() == Vec4(4, 2, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_Z>() == Vec4(4, 2, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() == Vec4(4, 2, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_X>() == Vec4(4, 2, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(4, 2, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(4, 2, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W>() == Vec4(4, 2, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() == Vec4(4, 2, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(4, 2, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(4, 2, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W>() == Vec4(4, 2, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X>() == Vec4(4, 2, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Y>() == Vec4(4, 2, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>() == Vec4(4, 2, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>() == Vec4(4, 2, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_X>() == Vec4(4, 3, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>() == Vec4(4, 3, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Z>() == Vec4(4, 3, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_W>() == Vec4(4, 3, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>() == Vec4(4, 3, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(4, 3, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(4, 3, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>() == Vec4(4, 3, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_X>() == Vec4(4, 3, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(4, 3, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(4, 3, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>() == Vec4(4, 3, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X>() == Vec4(4, 3, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y>() == Vec4(4, 3, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z>() == Vec4(4, 3, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>() == Vec4(4, 3, 4, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_X, SWIZZLE_X>() == Vec4(4, 4, 1, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>() == Vec4(4, 4, 1, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Z>() == Vec4(4, 4, 1, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_X, SWIZZLE_W>() == Vec4(4, 4, 1, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_X>() == Vec4(4, 4, 2, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Y>() == Vec4(4, 4, 2, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>() == Vec4(4, 4, 2, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_W>() == Vec4(4, 4, 2, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_X>() == Vec4(4, 4, 3, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y>() == Vec4(4, 4, 3, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Z>() == Vec4(4, 4, 3, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>() == Vec4(4, 4, 3, 4));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_W, SWIZZLE_X>() == Vec4(4, 4, 4, 1));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Y>() == Vec4(4, 4, 4, 2));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_W, SWIZZLE_Z>() == Vec4(4, 4, 4, 3));
+		CHECK(v.Swizzle<SWIZZLE_W, SWIZZLE_W, SWIZZLE_W, SWIZZLE_W>() == Vec4(4, 4, 4, 4));
+	}
+
+	TEST_CASE("TestVec4Abs")
+	{
+		CHECK(Vec4(1, -2, 3, -4).Abs() == Vec4(1, 2, 3, 4));
+		CHECK(Vec4(-1, 2, -3, 4).Abs() == Vec4(1, 2, 3, 4));
+	}
+
+
+	TEST_CASE("TestVec4Dot")
+	{
+		CHECK(Vec4(1, 2, 3, 4).Dot(Vec4(5, 6, 7, 8)) == float(1 * 5 + 2 * 6 + 3 * 7 + 4 * 8));
+		CHECK(Vec4(1, 2, 3, 4).DotV(Vec4(5, 6, 7, 8)) == Vec4::sReplicate(1 * 5 + 2 * 6 + 3 * 7 + 4 * 8));
+	}
+
+	TEST_CASE("TestVec4Length")
+	{
+		CHECK(Vec4(1, 2, 3, 4).LengthSq() == float(1 + 4 + 9 + 16));
+		CHECK(Vec4(1, 2, 3, 4).Length() == sqrt(float(1 + 4 + 9 + 16)));
+	}
+
+	TEST_CASE("TestVec4Sqrt")
+	{
+		CHECK_APPROX_EQUAL(Vec4(13, 15, 17, 19).Sqrt(), Vec4(sqrt(13.0f), sqrt(15.0f), sqrt(17.0f), sqrt(19.0f)));
+	}
+
+	TEST_CASE("TestVec4Normalize")
+	{
+		CHECK(Vec4(1, 2, 3, 4).Normalized() == Vec4(1, 2, 3, 4) / sqrt(30.0f));
+	}
+
+	TEST_CASE("TestVec4Cast")
+	{
+		CHECK(Vec4(1, 2, 3, 4).ToInt() == UVec4(1, 2, 3, 4));
+		CHECK(Vec4(1, 2, 3, 4).ReinterpretAsInt() == UVec4(0x3f800000U, 0x40000000U, 0x40400000U, 0x40800000U));
+	}
+
+	TEST_CASE("TestVec4Sign")
+	{
+		CHECK(Vec4(1.2345f, -6.7891f, 0, 1).GetSign() == Vec4(1, -1, 1, 1));
+		CHECK(Vec4(0, 2.3456f, -7.8912f, -1).GetSign() == Vec4(1, 1, -1, -1));
+	}
+
+	TEST_CASE("TestVec4FlipSign")
+	{
+		Vec4 v(1, 2, 3, 4);
+		CHECK(v.FlipSign<-1, 1, 1, 1>() == Vec4(-1, 2, 3, 4));
+		CHECK(v.FlipSign<1, -1, 1, 1>() == Vec4(1, -2, 3, 4));
+		CHECK(v.FlipSign<1, 1, -1, 1>() == Vec4(1, 2, -3, 4));
+		CHECK(v.FlipSign<1, 1, 1, -1>() == Vec4(1, 2, 3, -4));
+	}
+
+	TEST_CASE("TestVec4SignBit")
+	{
+		CHECK(Vec4(2, -3, 4, -5).GetSignBits() == 0b1010);
+		CHECK(Vec4(-2, 3, -4, 5).GetSignBits() == 0b0101);
+	}
+
+	TEST_CASE("TestVec4Sort")
+	{
+		for (int i = 0; i < 4; ++i)
+			for (int j = 0; j < 4; ++j)
+				if (i != j)
+					for (int k = 0; k < 4; ++k)
+						if (i != k && j != k)
+							for (int l = 0; l < 4; ++l)
+								if (i != l && j != l && k != l)
+								{
+									Vec4 v1((float)i, (float)j, (float)k, (float)l);
+									Vec4 v2 = v1;
+									UVec4 idx1 = UVec4(i + 4, j + 4, k + 4, l + 4);
+									UVec4 idx2 = idx1;
+									Vec4::sSort4(v1, idx1);
+									Vec4::sSort4Reverse(v2, idx2);
+									for (int m = 0; m < 4; ++m)
+									{
+										CHECK(v1[m] == float(m));
+										CHECK(v2[m] == float(3 - m));
+										CHECK(idx1[m] == uint32(m + 4));
+										CHECK(idx2[m] == uint32(3 - m + 4));
+									}
+								}
+	}
+
+	TEST_CASE("TestVec4SinCos")
+	{
+		// Check edge cases
+		Vec4 vs, vc;
+		Vec4(0, 0.5f * JPH_PI, JPH_PI, -0.5f * JPH_PI).SinCos(vs, vc);
+		CHECK(vs.IsClose(Vec4(0, 1, 0, -1), 1.0e-7f));
+		CHECK(vc.IsClose(Vec4(1, 0, -1, 0), 1.0e-7f));
+
+		double ms = 0.0, mc = 0.0;
+
+		for (float x = -100.0f * JPH_PI; x < 100.0f * JPH_PI; x += 1.0e-3f)
+		{
+			// Create a vector with intermediate values
+			Vec4 xv = Vec4::sReplicate(x) + Vec4(0.0e-4f, 2.5e-4f, 5.0e-4f, 7.5e-4f);
+
+			// Calculate sin and cos
+			xv.SinCos(vs, vc);
+
+			for (int i = 0; i < 4; ++i)
+			{
+				// Check accuracy of sin
+				double s1 = sin((double)xv[i]), s2 = (double)vs[i];
+				double ds = abs(s2 - s1);
+				ms = max(ms, ds);
+
+				// Check accuracy of cos
+				double c1 = cos((double)xv[i]), c2 = (double)vc[i];
+				double dc = abs(c2 - c1);
+				mc = max(mc, dc);
+			}
+		}
+
+		CHECK(ms < 1.0e-7);
+		CHECK(mc < 1.0e-7);
+	}
+
+	TEST_CASE("TestVec4Tan")
+	{
+		// Check edge cases
+		CHECK(Vec4::sReplicate(0.0f).Tan() == Vec4::sZero());
+		CHECK(Vec4::sReplicate(0.5f * JPH_PI - 1.0e-6f).Tan().GetX() > 1.0e6f);
+		CHECK(Vec4::sReplicate(0.5f * JPH_PI + 1.0e-6f).Tan().GetX() < -1.0e6f);
+
+		double mt = 0.0;
+
+		for (float x = -100.0f * JPH_PI; x < 100.0f * JPH_PI; x += 1.0e-3f)
+		{
+			// Create a vector with intermediate values
+			Vec4 xv = Vec4::sReplicate(x) + Vec4(0.0e-4f, 2.5e-4f, 5.0e-4f, 7.5e-4f);
+
+			// Calculate tan
+			Vec4 vt = xv.Tan();
+
+			for (int i = 0; i < 4; ++i)
+			{
+				// Check accuracy of tan
+				double t1 = tan((double)xv[i]), t2 = (double)vt[i];
+				double dt = abs(t2 - t1);
+				mt = max(mt, dt) / max(1.0, abs(t1)); // Take relative error
+			}
+		}
+
+		CHECK(mt < 1.5e-7);
+	}
+
+	TEST_CASE("TestVec4ASin")
+	{
+		// Check edge cases
+		CHECK(Vec4::sReplicate(0.0f).ASin() == Vec4::sZero());
+		CHECK(Vec4::sOne().ASin() == Vec4::sReplicate(0.5f * JPH_PI));
+		CHECK(Vec4::sReplicate(-1.0f).ASin() == Vec4::sReplicate(-0.5f * JPH_PI));
+
+		double ma = 0.0;
+
+		for (float x = -1.0f; x <= 1.0f; x += 1.0e-3f)
+		{
+			// Create a vector with intermediate values
+			Vec4 xv = Vec4::sMin(Vec4::sReplicate(x) + Vec4(0.0e-4f, 2.5e-4f, 5.0e-4f, 7.5e-4f), Vec4::sOne());
+
+			// Calculate asin
+			Vec4 va = xv.ASin();
+
+			for (int i = 0; i < 4; ++i)
+			{
+				// Check accuracy of asin
+				double a1 = asin((double)xv[i]), a2 = (double)va[i];
+				double da = abs(a2 - a1);
+				ma = max(ma, da);
+			}
+		}
+
+		CHECK(ma < 2.0e-7);
+
+		// Check that inputs are clamped as promised
+		CHECK(Vec4::sReplicate(-1.1f).ASin() == Vec4::sReplicate(-0.5f * JPH_PI));
+		CHECK(Vec4::sReplicate(1.1f).ASin() == Vec4::sReplicate(0.5f * JPH_PI));
+	}
+
+	TEST_CASE("TestVec4ACos")
+	{
+		// Check edge cases
+		CHECK(Vec4::sReplicate(0.0f).ACos() == Vec4::sReplicate(0.5f * JPH_PI));
+		CHECK(Vec4::sOne().ACos() == Vec4::sZero());
+		CHECK(Vec4::sReplicate(-1.0f).ACos() == Vec4::sReplicate(JPH_PI));
+
+		double ma = 0.0;
+
+		for (float x = -1.0f; x <= 1.0f; x += 1.0e-3f)
+		{
+			// Create a vector with intermediate values
+			Vec4 xv = Vec4::sMin(Vec4::sReplicate(x) + Vec4(0.0e-4f, 2.5e-4f, 5.0e-4f, 7.5e-4f), Vec4::sOne());
+
+			// Calculate acos
+			Vec4 va = xv.ACos();
+
+			for (int i = 0; i < 4; ++i)
+			{
+				// Check accuracy of acos
+				double a1 = acos((double)xv[i]), a2 = (double)va[i];
+				double da = abs(a2 - a1);
+				ma = max(ma, da);
+			}
+		}
+
+		CHECK(ma < 3.5e-7);
+
+		// Check that inputs are clamped as promised
+		CHECK(Vec4::sReplicate(-1.1f).ACos() == Vec4::sReplicate(JPH_PI));
+		CHECK(Vec4::sReplicate(1.1f).ACos() == Vec4::sZero());
+	}
+
+	TEST_CASE("TestVec4ATan")
+	{
+		// Check edge cases
+		CHECK(Vec4::sReplicate(0.0f).ATan() == Vec4::sZero());
+		CHECK(Vec4::sReplicate(FLT_MAX).ATan() == Vec4::sReplicate(0.5f * JPH_PI));
+		CHECK(Vec4::sReplicate(-FLT_MAX).ATan() == Vec4::sReplicate(-0.5f * JPH_PI));
+
+		double ma = 0.0;
+
+		for (float x = -100.0f; x < 100.0f; x += 1.0e-3f)
+		{
+			// Create a vector with intermediate values
+			Vec4 xv = Vec4::sReplicate(x) + Vec4(0.0e-4f, 2.5e-4f, 5.0e-4f, 7.5e-4f);
+
+			// Calculate atan
+			Vec4 va = xv.ATan();
+
+			for (int i = 0; i < 4; ++i)
+			{
+				// Check accuracy of atan
+				double a1 = atan((double)xv[i]), a2 = (double)va[i];
+				double da = abs(a2 - a1);
+				ma = max(ma, da);
+			}
+		}
+
+		CHECK(ma < 1.5e-7);
+	}
+
+	TEST_CASE("TestVec4ATan2")
+	{
+		double ma = 0.0;
+
+		// Test the axis
+		CHECK(Vec4::sATan2(Vec4::sZero(), Vec4::sReplicate(10.0f)) == Vec4::sZero());
+		CHECK(Vec4::sATan2(Vec4::sZero(), Vec4::sReplicate(-10.0f)) == Vec4::sReplicate(JPH_PI));
+		CHECK(Vec4::sATan2(Vec4::sReplicate(10.0f), Vec4::sZero()) == Vec4::sReplicate(0.5f * JPH_PI));
+		CHECK(Vec4::sATan2(Vec4::sReplicate(-10.0f), Vec4::sZero()) == Vec4::sReplicate(-0.5f * JPH_PI));
+
+		// Test the 4 quadrants
+		CHECK(Vec4::sATan2(Vec4::sReplicate(10.0f), Vec4::sReplicate(10.0f)) == Vec4::sReplicate(0.25f * JPH_PI));
+		CHECK(Vec4::sATan2(Vec4::sReplicate(10.0f), Vec4::sReplicate(-10.0f)) == Vec4::sReplicate(0.75f * JPH_PI));
+		CHECK(Vec4::sATan2(Vec4::sReplicate(-10.0f), Vec4::sReplicate(-10.0f)) == Vec4::sReplicate(-0.75f * JPH_PI));
+		CHECK(Vec4::sATan2(Vec4::sReplicate(-10.0f), Vec4::sReplicate(10.0f)) == Vec4::sReplicate(-0.25f * JPH_PI));
+
+		for (float y = -5.0f; y < 5.0f; y += 1.0e-2f)
+		{
+			// Create a vector with intermediate values
+			Vec4 yv = Vec4::sReplicate(y) + Vec4(0.0e-3f, 2.5e-3f, 5.0e-3f, 7.5e-3f);
+
+			for (float x = -5.0f; x < 5.0f; x += 1.0e-2f)
+			{
+				// Create a vector with intermediate values
+				Vec4 xv = Vec4::sReplicate(x) + Vec4(0.0e-3f, 2.5e-3f, 5.0e-3f, 7.5e-3f);
+
+				// Calculate atan
+				Vec4 va = Vec4::sATan2(yv, xv);
+
+				for (int i = 0; i < 4; ++i)
+				{
+					// Check accuracy of atan
+					double a1 = atan2((double)yv[i], (double)xv[i]), a2 = (double)va[i];
+					double da = abs(a2 - a1);
+					ma = max(ma, da);
+				}
+			}
+		}
+
+		CHECK(ma < 3.0e-7);
+	}
+
+	TEST_CASE("TestVec4ConvertToString")
+	{
+		Vec4 v(1, 2, 3, 4);
+		CHECK(ConvertToString(v) == "1, 2, 3, 4");
+	}
+
+	TEST_CASE("TestVec4CompressUnitVector")
+	{
+		// We want these to be preserved exactly
+		CHECK(Vec4::sDecompressUnitVector(Vec4(1, 0, 0, 0).CompressUnitVector()) == Vec4(1, 0, 0, 0));
+		CHECK(Vec4::sDecompressUnitVector(Vec4(0, 1, 0, 0).CompressUnitVector()) == Vec4(0, 1, 0, 0));
+		CHECK(Vec4::sDecompressUnitVector(Vec4(0, 0, 1, 0).CompressUnitVector()) == Vec4(0, 0, 1, 0));
+		CHECK(Vec4::sDecompressUnitVector(Vec4(0, 0, 0, 1).CompressUnitVector()) == Vec4(0, 0, 0, 1));
+		CHECK(Vec4::sDecompressUnitVector(Vec4(-1, 0, 0, 0).CompressUnitVector()) == Vec4(-1, 0, 0, 0));
+		CHECK(Vec4::sDecompressUnitVector(Vec4(0, -1, 0, 0).CompressUnitVector()) == Vec4(0, -1, 0, 0));
+		CHECK(Vec4::sDecompressUnitVector(Vec4(0, 0, -1, 0).CompressUnitVector()) == Vec4(0, 0, -1, 0));
+		CHECK(Vec4::sDecompressUnitVector(Vec4(0, 0, 0, -1).CompressUnitVector()) == Vec4(0, 0, 0, -1));
+
+		UnitTestRandom random;
+		for (int i = 0; i < 1000; ++i)
+		{
+			std::uniform_real_distribution<float> scale(-1.0f, 1.0f);
+			Vec4 v = Vec4(scale(random), scale(random), scale(random), scale(random)).Normalized();
+			uint32 compressed = v.CompressUnitVector();
+			Vec4 decompressed = Vec4::sDecompressUnitVector(compressed);
+			float diff = (decompressed - v).Length();
+			CHECK(diff < 5.0e-3f);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Math/VectorTests.cpp

@@ -0,0 +1,102 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/Vector.h>
+
+TEST_SUITE("VectorTests")
+{
+	TEST_CASE("TestVectorEquals")
+	{
+		Vector<3> v1;
+		v1[0] = 1;
+		v1[1] = 2;
+		v1[2] = 3;
+
+		Vector<3> v2;
+		v2[0] = 1;
+		v2[1] = 2;
+		v2[2] = 3;
+
+		Vector<3> v3;
+		v3[0] = 1;
+		v3[1] = 5;
+		v3[2] = 3;
+
+		CHECK(v1 == v2);
+		CHECK(!(v1 != v2));
+		CHECK(v1 != v3);
+		CHECK(!(v1 == v3));
+	}
+
+	TEST_CASE("TestVectorStream")
+	{
+		Vector<3> v1;
+		v1[0] = 1;
+		v1[1] = 2;
+		v1[2] = 3;
+
+		std::stringstream ss;
+		ss << v1;
+		CHECK(ss.str() == "[1, 2, 3]");
+	}
+
+	TEST_CASE("TestVectorMultiplyFloat")
+	{
+		Vector<5> v;
+		v[0] = 1;
+		v[1] = 2;
+		v[2] = 3;
+		v[3] = 4;
+		v[4] = 5;
+		Vector<5> v2 = v * 2;
+		CHECK(v2[0] == 2.0f);
+		CHECK(v2[1] == 4.0f);
+		CHECK(v2[2] == 6.0f);
+		CHECK(v2[3] == 8.0f);
+		CHECK(v2[4] == 10.0f);
+	}
+
+	TEST_CASE("TestVectorAdd")
+	{
+		Vector<5> v1 = Vector<5>::sZero();
+		Vector<5> v2 = Vector<5>::sZero();
+		v1[0] = 1;
+		v2[0] = 2;
+		v1[4] = 5;
+		Vector<5> v3 = v1 + v2;
+		CHECK(v3[0] == 3.0f);
+		CHECK(v3[1] == 0.0f);
+		CHECK(v3[2] == 0.0f);
+		CHECK(v3[3] == 0.0f);
+		CHECK(v3[4] == 5.0f);
+	}
+
+	TEST_CASE("TestVectorNegate")
+	{
+		Vector<5> v;
+		v[0] = 1;
+		v[1] = 2;
+		v[2] = 3;
+		v[3] = 4;
+		v[4] = 5;
+		Vector<5> v2 = -v;
+		CHECK(v2[0] == -1.0f);
+		CHECK(v2[1] == -2.0f);
+		CHECK(v2[2] == -3.0f);
+		CHECK(v2[3] == -4.0f);
+		CHECK(v2[4] == -5.0f);
+	}
+
+	TEST_CASE("TestVectorLength")
+	{
+		Vector<5> v;
+		v[0] = 1;
+		v[1] = 2;
+		v[2] = 3;
+		v[3] = 4;
+		v[4] = 5;
+		CHECK(v.LengthSq() == float(1 + 4 + 9 + 16 + 25));
+	}
+}

lib/All/JoltPhysics/UnitTests/ObjectStream/ObjectStreamTest.cpp

@@ -0,0 +1,257 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Core/Factory.h>
+#include <Jolt/ObjectStream/ObjectStreamTextIn.h>
+#include <Jolt/ObjectStream/ObjectStreamTextOut.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+enum TestEnum
+{
+	A,
+	B,
+	C
+};
+
+class TestSerializableBase : public RefTarget<TestSerializableBase>
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE(JPH_NO_EXPORT, TestSerializableBase)
+
+public:
+	virtual						~TestSerializableBase() = default;
+
+	uint8						mUInt8 = 0;
+	uint16						mUInt16 = 0;
+	int							mInt = 0;
+	uint32						mUInt32 = 0;
+	uint64						mUInt64 = 0;
+	float						mFloat = 0;
+	double						mDouble = 0;
+	bool						mBool = false;
+	Float3						mFloat3 = { };
+	Float4						mFloat4 = { };
+	Double3						mDouble3 = { };
+	Quat						mQuat = Quat::sIdentity();
+	Vec3						mVec3 = Vec3::sZero();
+	DVec3						mDVec3 = DVec3::sZero();
+	Vec4						mVec4 = Vec4::sZero();
+	UVec4						mUVec4 = UVec4::sZero();
+	Mat44						mMat44 = Mat44::sIdentity();
+	DMat44						mDMat44 = DMat44::sIdentity();
+	JPH::String					mString;
+};
+
+class TestSerializableBase2
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE(JPH_NO_EXPORT, TestSerializableBase2)
+
+public:
+	virtual						~TestSerializableBase2() = default;
+
+	uint32						mBase2 = 0;
+};
+
+class TestSerializable : public TestSerializableBase, public TestSerializableBase2
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_NO_EXPORT, TestSerializable)
+
+public:
+	TestEnum					mEnum = A;
+	Array<int>					mIntVector;
+	StaticArray<bool, 10>		mBoolVector;
+	float						mFloatVector[3] = { 0, 0, 0 };
+	Array<float>				mArrayOfVector[3];
+	Array<Array<int>>			mVectorOfVector;
+	TestSerializable *			mPointer = nullptr;
+	Ref<TestSerializable>		mReference;
+	RefConst<TestSerializable>	mReferenceConst;
+};
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TestSerializableBase)
+{
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mUInt8)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mUInt16)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mInt)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mUInt32)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mUInt64)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mFloat)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mDouble)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mBool)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mFloat3)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mFloat4)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mDouble3)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mQuat)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mVec3)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mDVec3)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mVec4)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mUVec4)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mMat44)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mDMat44)
+	JPH_ADD_ATTRIBUTE(TestSerializableBase, mString)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TestSerializableBase2)
+{
+	JPH_ADD_ATTRIBUTE(TestSerializableBase2, mBase2)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TestSerializable)
+{
+	JPH_ADD_BASE_CLASS(TestSerializable, TestSerializableBase)
+	JPH_ADD_BASE_CLASS(TestSerializable, TestSerializableBase2)
+
+	JPH_ADD_ENUM_ATTRIBUTE(TestSerializable, mEnum)
+	JPH_ADD_ATTRIBUTE(TestSerializable, mIntVector)
+	JPH_ADD_ATTRIBUTE(TestSerializable, mBoolVector)
+	JPH_ADD_ATTRIBUTE(TestSerializable, mFloatVector)
+	JPH_ADD_ATTRIBUTE(TestSerializable, mArrayOfVector)
+	JPH_ADD_ATTRIBUTE(TestSerializable, mVectorOfVector)
+	JPH_ADD_ATTRIBUTE(TestSerializable, mPointer)
+	JPH_ADD_ATTRIBUTE(TestSerializable, mReference)
+	JPH_ADD_ATTRIBUTE(TestSerializable, mReferenceConst)
+}
+
+TEST_SUITE("ObjectStreamTest")
+{
+	static TestSerializable *CreateTestObject()
+	{
+		TestSerializable *test = new TestSerializable();
+		test->mUInt8 = 0xff;
+		test->mUInt16 = 0xffff;
+		test->mInt = -1;
+		test->mUInt32 = 0xf1f2f3f4;
+		test->mUInt64 = 0xf5f6f7f8f9fafbfc;
+		test->mFloat = 0.12345f;
+		test->mDouble = DBL_EPSILON;
+		test->mBool = true;
+		test->mFloat3 = Float3(9, 10, 11);
+		test->mFloat4 = Float4(11, 9, 10, 12);
+		test->mDouble3 = Double3(10, 11, 12);
+		test->mVec3 = Vec3(6, 7, 8);
+		test->mDVec3 = DVec3(7, 8, 9);
+		test->mVec4 = Vec4(9, 10, 11, 12);
+		test->mUVec4 = UVec4(12, 11, 9, 10);
+		test->mQuat = Quat::sRotation(Vec3::sAxisX(), 0.1234f);
+		test->mMat44 = Mat44::sRotationTranslation(Quat::sRotation(Vec3::sAxisY(), 0.4567f), Vec3(13, 14, 15));
+		test->mDMat44 = DMat44::sRotationTranslation(Quat::sRotation(Vec3::sAxisY(), 0.789f), DVec3(20, 21, 22));
+		test->mString = "\"test string\"";
+		test->mEnum = B;
+		test->mIntVector = { 1, 2, 3, 4, 5 };
+		test->mBoolVector.push_back(true);
+		test->mBoolVector.push_back(false);
+		test->mBoolVector.push_back(true);
+		test->mFloatVector[0] = 1.0f;
+		test->mFloatVector[1] = 2.0f;
+		test->mFloatVector[2] = 3.0f;
+		test->mArrayOfVector[0] = { 1, 2, 3 };
+		test->mArrayOfVector[1] = { 4, 5 };
+		test->mArrayOfVector[2] = { 6, 7, 8, 9 };
+		test->mVectorOfVector = { { 10, 11 }, { 12, 13, 14 }, { 15, 16, 17, 18 }};
+		test->mBase2 = 0x9876;
+
+		TestSerializable *test2 = new TestSerializable();
+		test2->mFloat = 4.5f;
+		test->mPointer = test2;
+		test->mReference = test2;
+		test->mReferenceConst = test2;
+
+		return test;
+	}
+
+	static void CompareObjects(TestSerializable *inInput, TestSerializable *inOutput)
+	{
+		CHECK(inInput->mUInt8 == inOutput->mUInt8);
+		CHECK(int(inInput->mUInt16) == int(inOutput->mUInt16));
+		CHECK(inInput->mInt == inOutput->mInt);
+		CHECK(inInput->mUInt32 == inOutput->mUInt32);
+		CHECK(inInput->mUInt64 == inOutput->mUInt64);
+		CHECK(inInput->mFloat == inOutput->mFloat);
+		CHECK(inInput->mDouble == inOutput->mDouble);
+		CHECK(inInput->mBool == inOutput->mBool);
+		CHECK(inInput->mFloat3 == inOutput->mFloat3);
+		CHECK(inInput->mFloat4 == inOutput->mFloat4);
+		CHECK(inInput->mDouble3 == inOutput->mDouble3);
+		CHECK(inInput->mQuat == inOutput->mQuat);
+		CHECK(inInput->mVec3 == inOutput->mVec3);
+		CHECK(inInput->mDVec3 == inOutput->mDVec3);
+		CHECK(inInput->mVec4 == inOutput->mVec4);
+		CHECK(inInput->mUVec4 == inOutput->mUVec4);
+		CHECK(inInput->mMat44 == inOutput->mMat44);
+		CHECK(inInput->mDMat44 == inOutput->mDMat44);
+		CHECK(inInput->mString == inOutput->mString);
+		CHECK(inInput->mEnum == inOutput->mEnum);
+		CHECK(inInput->mIntVector == inOutput->mIntVector);
+		CHECK(inInput->mBoolVector == inOutput->mBoolVector);
+
+		for (uint32 i = 0; i < size(inInput->mFloatVector); ++i)
+			CHECK(inInput->mFloatVector[i] == inOutput->mFloatVector[i]);
+
+		for (uint32 i = 0; i < size(inInput->mArrayOfVector); ++i)
+			CHECK(inInput->mArrayOfVector[i] == inOutput->mArrayOfVector[i]);
+
+		CHECK(inInput->mVectorOfVector == inOutput->mVectorOfVector);
+
+		CHECK(inOutput->mPointer == inOutput->mReference);
+		CHECK(inOutput->mPointer == inOutput->mReferenceConst);
+
+		if (inInput->mPointer == nullptr)
+			CHECK(inOutput->mPointer == nullptr);
+		else
+		{
+			CHECK(inInput->mPointer != inOutput->mPointer);
+			CompareObjects(inInput->mPointer, inOutput->mPointer);
+			CHECK(inOutput->mReference->GetRefCount() == uint32(2));
+			CHECK(inOutput->mReferenceConst->GetRefCount() == uint32(2));
+		}
+
+		CHECK(inInput->mBase2 == inOutput->mBase2);
+	}
+
+	TEST_CASE("TestObjectStreamLoadSaveText")
+	{
+		Factory::sInstance->Register(JPH_RTTI(TestSerializable));
+
+		TestSerializable *test = CreateTestObject();
+
+		stringstream stream;
+		CHECK(ObjectStreamOut::sWriteObject(stream, ObjectStreamOut::EStreamType::Text, *test));
+
+		TestSerializable *test_out = nullptr;
+		CHECK(ObjectStreamIn::sReadObject(stream, test_out));
+		if (test_out == nullptr)
+			return;
+
+		// Check that DynamicCast returns the right offsets
+		CHECK(DynamicCast<TestSerializable>(test_out) == test_out);
+		CHECK(DynamicCast<TestSerializableBase>(test_out) == static_cast<TestSerializableBase *>(test_out));
+		CHECK(DynamicCast<TestSerializableBase2>(test_out) == static_cast<TestSerializableBase2 *>(test_out));
+
+		CompareObjects(test, test_out);
+
+		delete test;
+		delete test_out;
+	}
+
+	TEST_CASE("TestObjectStreamLoadSaveBinary")
+	{
+		Factory::sInstance->Register(JPH_RTTI(TestSerializable));
+
+		TestSerializable *test = CreateTestObject();
+
+		stringstream stream;
+		CHECK(ObjectStreamOut::sWriteObject(stream, ObjectStreamOut::EStreamType::Binary, *test));
+
+		TestSerializable *test_out = nullptr;
+		CHECK(ObjectStreamIn::sReadObject(stream, test_out));
+		if (test_out == nullptr)
+			return;
+
+		CompareObjects(test, test_out);
+
+		delete test;
+		delete test_out;
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ActiveEdgesTests.cpp

@@ -0,0 +1,406 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+
+TEST_SUITE("ActiveEdgesTest")
+{
+	static const float cCapsuleProbeOffset = 0.1f; // How much to offset the probe from y = 0 in order to avoid hitting a back instead of a front face
+	static const float cCapsuleRadius = 0.1f;
+
+	// Create a capsule as our probe
+	static Ref<Shape> sCreateProbeCapsule()
+	{
+		// Ensure capsule is long enough so that when active edges mode is on, we will always get a horizontal penetration axis rather than a vertical one
+		CapsuleShapeSettings capsule(1.0f, cCapsuleRadius);
+		capsule.SetEmbedded();
+		return capsule.Create().Get();
+	}
+
+	// Create a flat mesh shape consisting of 7 x 7 quads, we know that only the outer edges of this shape are active
+	static Ref<ShapeSettings> sCreateMeshShape()
+	{
+		TriangleList triangles;
+		for (int z = 0; z < 7; ++z)
+			for (int x = 0; x < 7; ++x)
+			{
+				float fx = (float)x - 3.5f, fz = (float)z - 3.5f;
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx, 0, fz + 1), Vec3(fx + 1, 0, fz + 1)));
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx + 1, 0, fz + 1), Vec3(fx + 1, 0, fz)));
+			}
+
+		return new MeshShapeSettings(triangles);
+	}
+
+	// Create a flat height field shape that has the same properties as the mesh shape
+	static Ref<ShapeSettings> sCreateHeightFieldShape()
+	{
+		float samples[8*8];
+		memset(samples, 0, sizeof(samples));
+		return new HeightFieldShapeSettings(samples, Vec3(-3.5f, 0, -3.5f), Vec3::sOne(), 8);
+	}
+
+	// This struct indicates what we hope to find as hit
+	struct ExpectedHit
+	{
+		Vec3		mPosition;
+		Vec3		mPenetrationAxis;
+	};
+
+	// Compare expected hits with returned hits
+	template <class ResultType>
+	static void sCheckMatch(const Array<ResultType> &inResult, const Array<ExpectedHit> &inExpectedHits, float inAccuracySq)
+	{
+		CHECK(inResult.size() == inExpectedHits.size());
+
+		for (const ExpectedHit &hit : inExpectedHits)
+		{
+			bool found = false;
+			for (const ResultType &result : inResult)
+				if (result.mContactPointOn2.IsClose(hit.mPosition, inAccuracySq)
+					&& result.mPenetrationAxis.Normalized().IsClose(hit.mPenetrationAxis, inAccuracySq))
+				{
+					found = true;
+					break;
+				}
+			CHECK(found);
+		}
+	}
+
+	// Collide our probe against the test shape and validate the hit results
+	static void sTestCollideShape(Shape *inProbeShape, Shape *inTestShape, Vec3Arg inTestShapeScale, const CollideShapeSettings &inSettings, Vec3Arg inProbeShapePos, const Array<ExpectedHit> &inExpectedHits)
+	{
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(inProbeShape, inTestShape, Vec3::sOne(), inTestShapeScale, Mat44::sTranslation(inProbeShapePos), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), inSettings, collector);
+
+		sCheckMatch(collector.mHits, inExpectedHits, 1.0e-8f);
+	}
+
+	// Collide a probe shape against our test shape in various locations to verify active edge behavior
+	static void sTestCollideShape(const ShapeSettings *inTestShape, Vec3Arg inTestShapeScale, bool inActiveEdgesOnly)
+	{
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = inActiveEdgesOnly? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
+
+		Ref<Shape> test_shape = inTestShape->Create().Get();
+		Ref<Shape> capsule = sCreateProbeCapsule();
+
+		// Test hitting all active edges
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-3.5f, cCapsuleProbeOffset, 0), { { Vec3(-3.5f, 0, 0), Vec3(1, 0, 0) } });
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(3.5f, cCapsuleProbeOffset, 0), { { Vec3(3.5f, 0, 0), Vec3(-1, 0, 0) } });
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, -3.5f), { { Vec3(0, 0, -3.5f), Vec3(0, 0, 1) } });
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, 3.5f), { { Vec3(0, 0, 3.5f), Vec3(0, 0, -1) } });
+
+		// Test hitting internal edges, this should return two hits
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-2.5f, cCapsuleProbeOffset, 0), { { Vec3(-2.5f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(-1, 0, 0) }, { Vec3(-2.5f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(1, 0, 0) } });
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, -2.5f), { { Vec3(0, 0, -2.5f), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(0, 0, -1) }, { Vec3(0, 0, -2.5f), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(0, 0, -1) } });
+
+		// Test hitting an interior diagonal, this should return two hits
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-3.0f, cCapsuleProbeOffset, 0), { { Vec3(-3.0f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : (inTestShapeScale * Vec3(1, 0, -1)).Normalized() }, { Vec3(-3.0f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : (inTestShapeScale * Vec3(-1, 0, 1)).Normalized() } });
+	}
+
+	TEST_CASE("CollideShapeMesh")
+	{
+		Ref<ShapeSettings> shape = sCreateMeshShape();
+
+		sTestCollideShape(shape, Vec3::sOne(), false);
+
+		sTestCollideShape(shape, Vec3::sOne(), true);
+
+		sTestCollideShape(shape, Vec3(-1, 1, 1), false);
+
+		sTestCollideShape(shape, Vec3(-1, 1, 1), true);
+	}
+
+	TEST_CASE("CollideShapeHeightField")
+	{
+		Ref<ShapeSettings> shape = sCreateHeightFieldShape();
+
+		sTestCollideShape(shape, Vec3::sOne(), false);
+
+		sTestCollideShape(shape, Vec3::sOne(), true);
+
+		sTestCollideShape(shape, Vec3(-1, 1, 1), false);
+
+		sTestCollideShape(shape, Vec3(-1, 1, 1), true);
+	}
+
+	// Cast our probe against the test shape and validate the hit results
+	static void sTestCastShape(Shape *inProbeShape, Shape *inTestShape, Vec3Arg inTestShapeScale, const ShapeCastSettings &inSettings, Vec3Arg inProbeShapePos, Vec3Arg inProbeShapeDirection, const Array<ExpectedHit> &inExpectedHits)
+	{
+		AllHitCollisionCollector<CastShapeCollector> collector;
+		ShapeCast shape_cast(inProbeShape, Vec3::sOne(), Mat44::sTranslation(inProbeShapePos), inProbeShapeDirection);
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inSettings, inTestShape, inTestShapeScale, ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+
+		sCheckMatch(collector.mHits, inExpectedHits, 1.0e-6f);
+	}
+
+	// Cast a probe shape against our test shape in various locations to verify active edge behavior
+	static void sTestCastShape(const ShapeSettings *inTestShape, Vec3Arg inTestShapeScale, bool inActiveEdgesOnly)
+	{
+		ShapeCastSettings settings;
+		settings.mActiveEdgeMode = inActiveEdgesOnly? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
+		settings.mReturnDeepestPoint = true;
+
+		Ref<Shape> test_shape = inTestShape->Create().Get();
+		Ref<Shape> capsule = sCreateProbeCapsule();
+
+		// Test hitting all active edges
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-4, cCapsuleProbeOffset, 0), Vec3(0.5f, 0, 0), { { Vec3(-3.5f, 0, 0), Vec3(1, 0, 0) } });
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(4, cCapsuleProbeOffset, 0), Vec3(-0.5f, 0, 0), { { Vec3(3.5f, 0, 0), Vec3(-1, 0, 0) } });
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, -4), Vec3(0, 0, 0.5f), { { Vec3(0, 0, -3.5f), Vec3(0, 0, 1) } });
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, 4), Vec3(0, 0, -0.5f), { { Vec3(0, 0, 3.5f), Vec3(0, 0, -1) } });
+
+		// Test hitting internal edges, this should return two hits
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-2.5f - 1.1f * cCapsuleRadius, cCapsuleProbeOffset, 0), Vec3(0.2f * cCapsuleRadius, 0, 0), { { Vec3(-2.5f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(-1, 0, 0) }, { Vec3(-2.5f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(1, 0, 0) } });
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, -2.5f - 1.1f * cCapsuleRadius), Vec3(0, 0, 0.2f * cCapsuleRadius), { { Vec3(0, 0, -2.5f), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(0, 0, -1) }, { Vec3(0, 0, -2.5f), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(0, 0, -1) } });
+	}
+
+	TEST_CASE("CastShapeMesh")
+	{
+		Ref<ShapeSettings> shape = sCreateMeshShape();
+
+		sTestCastShape(shape, Vec3::sOne(), false);
+
+		sTestCastShape(shape, Vec3::sOne(), true);
+
+		sTestCastShape(shape, Vec3(-1, 1, 1), false);
+
+		sTestCastShape(shape, Vec3(-1, 1, 1), true);
+	}
+
+	TEST_CASE("CastShapeHeightField")
+	{
+		Ref<ShapeSettings> shape = sCreateHeightFieldShape();
+
+		sTestCastShape(shape, Vec3::sOne(), false);
+
+		sTestCastShape(shape, Vec3::sOne(), true);
+
+		sTestCastShape(shape, Vec3(-1, 1, 1), false);
+
+		sTestCastShape(shape, Vec3(-1, 1, 1), true);
+	}
+
+	// Tests a discrete cube sliding over a mesh / heightfield shape
+	static void sDiscreteCubeSlide(Ref<ShapeSettings> inShape, bool inCheckActiveEdges)
+	{
+		PhysicsTestContext c;
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Set simulation settings
+		PhysicsSettings settings;
+		settings.mCheckActiveEdges = inCheckActiveEdges;
+		c.GetSystem()->SetPhysicsSettings(settings);
+
+		// Create frictionless floor
+		Body &floor = c.CreateBody(inShape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+		floor.SetFriction(0.0f);
+
+		// Create box sliding over the floor
+		RVec3 initial_position(-3, 0.1f - cPenetrationSlop, 0);
+		Vec3 initial_velocity(3, 0, 0);
+		Body &box = c.CreateBox(initial_position, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.1f));
+		box.SetLinearVelocity(initial_velocity);
+		box.SetFriction(0.0f);
+		box.GetMotionProperties()->SetLinearDamping(0.0f);
+
+		const float cSimulationTime = 2.0f;
+		c.Simulate(cSimulationTime);
+
+		RVec3 expected_position = initial_position + cSimulationTime * initial_velocity;
+		if (inCheckActiveEdges)
+		{
+			// Box should have slided frictionless over the plane without encountering any collisions
+			CHECK_APPROX_EQUAL(box.GetPosition(), expected_position, 1.0e-3f);
+			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), initial_velocity, 2.0e-3f);
+		}
+		else
+		{
+			// Box should have bumped into an internal edge and not reached its target
+			CHECK(box.GetPosition().GetX() < expected_position.GetX() - 1.0f);
+		}
+	}
+
+	TEST_CASE("DiscreteCubeSlideMesh")
+	{
+		Ref<ShapeSettings> shape = sCreateMeshShape();
+
+		sDiscreteCubeSlide(shape, false);
+
+		sDiscreteCubeSlide(shape, true);
+
+		Ref<ShapeSettings> scaled_shape = new ScaledShapeSettings(shape, Vec3(-1, 1, 1));
+
+		sDiscreteCubeSlide(scaled_shape, false);
+
+		sDiscreteCubeSlide(scaled_shape, true);
+	}
+
+	TEST_CASE("DiscreteCubeSlideHeightField")
+	{
+		Ref<ShapeSettings> shape = sCreateHeightFieldShape();
+
+		sDiscreteCubeSlide(shape, false);
+
+		sDiscreteCubeSlide(shape, true);
+
+		Ref<ShapeSettings> scaled_shape = new ScaledShapeSettings(shape, Vec3(-1, 1, 1));
+
+		sDiscreteCubeSlide(scaled_shape, false);
+
+		sDiscreteCubeSlide(scaled_shape, true);
+	}
+
+	// Tests a linear cast cube sliding over a mesh / heightfield shape
+	static void sLinearCastCubeSlide(Ref<ShapeSettings> inShape, bool inCheckActiveEdges)
+	{
+		PhysicsTestContext c;
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Set simulation settings
+		PhysicsSettings settings;
+		settings.mCheckActiveEdges = inCheckActiveEdges;
+		c.GetSystem()->SetPhysicsSettings(settings);
+
+		// Create frictionless floor
+		Body &floor = c.CreateBody(inShape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+		floor.SetFriction(0.0f);
+
+		// Create box starting a little bit above the floor and ending 0.5 * cPenetrationSlop below the floor so that if no internal edges are hit the motion should not be stopped
+		// Note that we need the vertical velocity or else back face culling will ignore the face
+		RVec3 initial_position(-3, 0.1f + cPenetrationSlop, 0);
+		Vec3 initial_velocity(6 * 60, -1.5f * cPenetrationSlop * 60, 0);
+		Body &box = c.CreateBox(initial_position, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(0.1f));
+		box.SetLinearVelocity(initial_velocity);
+		box.SetFriction(0.0f);
+		box.GetMotionProperties()->SetLinearDamping(0.0f);
+
+		// To avoid extra vertical velocity being picked up in 1 step, zero gravity
+		c.ZeroGravity();
+
+		c.SimulateSingleStep();
+
+		RVec3 expected_position = initial_position + initial_velocity / 60.0f;
+		if (inCheckActiveEdges)
+		{
+			// Box should stepped in one frame over the plane without encountering any linear cast collisions
+			CHECK_APPROX_EQUAL(box.GetPosition(), expected_position, 1.0e-4f);
+			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), initial_velocity, 1.0e-4f);
+		}
+		else
+		{
+			// Box should have bumped into an internal edge and not reached its target
+			CHECK(box.GetPosition().GetX() < expected_position.GetX() - 1.0f);
+		}
+	}
+
+	TEST_CASE("LinearCastCubeSlideMesh")
+	{
+		Ref<ShapeSettings> shape = sCreateMeshShape();
+
+		sLinearCastCubeSlide(shape, false);
+
+		sLinearCastCubeSlide(shape, true);
+
+		Ref<ShapeSettings> scaled_shape = new ScaledShapeSettings(shape, Vec3(-1, 1, 1));
+
+		sLinearCastCubeSlide(scaled_shape, false);
+
+		sLinearCastCubeSlide(scaled_shape, true);
+	}
+
+	TEST_CASE("LinearCastCubeSlideHeightField")
+	{
+		Ref<ShapeSettings> shape = sCreateHeightFieldShape();
+
+		sLinearCastCubeSlide(shape, false);
+
+		sLinearCastCubeSlide(shape, true);
+
+		Ref<ShapeSettings> scaled_shape = new ScaledShapeSettings(shape, Vec3(-1, 1, 1));
+
+		sLinearCastCubeSlide(scaled_shape, false);
+
+		sLinearCastCubeSlide(scaled_shape, true);
+	}
+
+	TEST_CASE("TestNonManifoldMesh")
+	{
+		// Test 3 triangles in a plane that all share the same edge
+		// Normally the shared edge would not be active, but since the mesh is non-manifold we expect all of them to be active
+		TriangleList triangles;
+		triangles.push_back(Triangle(Float3(0, 0, -1), Float3(0, 0, 1), Float3(1, 0, 0), 0));
+		triangles.push_back(Triangle(Float3(0, 0, 1), Float3(0, 0, -1), Float3(-1, 0, 0), 0));
+		triangles.push_back(Triangle(Float3(0, 0, 1), Float3(0, 0, -1), Float3(-0.5f, 0, 0), 0));
+
+		ShapeRefC shape = MeshShapeSettings(triangles).Create().Get();
+
+		ShapeRefC sphere = new SphereShape(0.1f);
+
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
+
+		// Collide a sphere on both sides of the active edge so that a 45 degree normal will be found then the edge is active.
+		// An inactive edge will return a normal that is perpendicular to the plane.
+		{
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			CollisionDispatch::sCollideShapeVsShape(sphere, shape, Vec3::sOne(), Vec3::sOne(), Mat44::sTranslation(Vec3(0.05f, 0.05f, 0)), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+			CHECK(collector.mHits.size() == 3);
+
+			// We expect one interior hit because the sphere is above the triangle and 2 active edge hits that provide a normal pointing towards the sphere
+			int num_interior = 0, num_on_shared_edge = 0;
+			for (const CollideShapeResult &r : collector.mHits)
+				if (r.mContactPointOn2.IsClose(Vec3(0.05f, 0.0f, 0.0f)))
+				{
+					CHECK_APPROX_EQUAL(r.mPenetrationAxis.Normalized(), Vec3(0, -1, 0));
+					++num_interior;
+				}
+				else if (r.mContactPointOn2.IsNearZero())
+				{
+					CHECK_APPROX_EQUAL(r.mPenetrationAxis.Normalized(), Vec3(-1, -1, 0).Normalized(), 1.0e-5f);
+					++num_on_shared_edge;
+				}
+			CHECK(num_interior == 1);
+			CHECK(num_on_shared_edge == 2);
+		}
+
+		{
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			CollisionDispatch::sCollideShapeVsShape(sphere, shape, Vec3::sOne(), Vec3::sOne(), Mat44::sTranslation(Vec3(-0.05f, 0.05f, 0)), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+			CHECK(collector.mHits.size() == 3);
+
+			// We expect 2 interior hits because the sphere is above the triangle and 1 active edge hit that provide a normal pointing towards the sphere
+			int num_interior = 0, num_on_shared_edge = 0;
+			for (const CollideShapeResult &r : collector.mHits)
+				if (r.mContactPointOn2.IsClose(Vec3(-0.05f, 0.0f, 0.0f)))
+				{
+					CHECK_APPROX_EQUAL(r.mPenetrationAxis.Normalized(), Vec3(0, -1, 0));
+					++num_interior;
+				}
+				else if (r.mContactPointOn2.IsNearZero())
+				{
+					CHECK_APPROX_EQUAL(r.mPenetrationAxis.Normalized(), Vec3(1, -1, 0).Normalized(), 1.0e-5f);
+					++num_on_shared_edge;
+				}
+			CHECK(num_interior == 2);
+			CHECK(num_on_shared_edge == 1);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/BroadPhaseTests.cpp

@@ -0,0 +1,115 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include "Layers.h"
+
+TEST_SUITE("BroadPhaseTests")
+{
+	TEST_CASE("TestBroadPhaseOptimize")
+	{
+		BPLayerInterfaceImpl broad_phase_layer_interface;
+
+		// Create body manager
+		BodyManager body_manager;
+		body_manager.Init(1, 0, broad_phase_layer_interface);
+
+		// Create quad tree
+		BroadPhaseQuadTree broadphase;
+		broadphase.Init(&body_manager, broad_phase_layer_interface);
+
+		// Create a box
+		BodyCreationSettings settings(new BoxShape(Vec3::sOne()), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		Body &body = *body_manager.AllocateBody(settings);
+		body_manager.AddBody(&body);
+
+		// Add it to the broadphase
+		BodyID id = body.GetID();
+		BroadPhase::AddState add_state = broadphase.AddBodiesPrepare(&id, 1);
+		broadphase.AddBodiesFinalize(&id, 1, add_state);
+
+		// Test that we hit the box at its current location and not where we're going to move it to
+		AllHitCollisionCollector<RayCastBodyCollector> collector;
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+
+		// Move the body
+		body.SetPositionAndRotationInternal(RVec3(2, 0, 0), Quat::sIdentity());
+		broadphase.NotifyBodiesAABBChanged(&id, 1, true);
+
+		// Test that we hit the box at its previous and current location
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+
+		// Optimize the broadphase
+		broadphase.Optimize();
+
+		// Test that we hit the box only at the new location
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+
+		// Move the body again (so that for the next optimize we'll have to discard a tree)
+		body.SetPositionAndRotationInternal(RVec3(4, 0, 0), Quat::sIdentity());
+		broadphase.NotifyBodiesAABBChanged(&id, 1, true);
+
+		// Test that we hit the box at its previous and current location
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+
+		// Optimize the broadphase (this will internally have to discard a tree)
+		broadphase.Optimize();
+
+		// Test that we hit the box only at the new location
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CastShapeTests.cpp

@@ -0,0 +1,514 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+
+TEST_SUITE("CastShapeTests")
+{
+	/// Helper function that tests a sphere against a triangle
+	static void sTestCastSphereVertexOrEdge(const Shape *inSphere, Vec3Arg inPosition, Vec3Arg inDirection, const Shape *inTriangle)
+	{
+		ShapeCast shape_cast(inSphere, Vec3::sOne(), Mat44::sTranslation(inPosition - inDirection), inDirection);
+		ShapeCastSettings cast_settings;
+		cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+		cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+		AllHitCollisionCollector<CastShapeCollector> collector;
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+		CHECK(collector.mHits.size() == 1);
+		const ShapeCastResult &result = collector.mHits.back();
+		CHECK_APPROX_EQUAL(result.mFraction, 1.0f - 0.2f / inDirection.Length(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), inDirection.Normalized(), 1.0e-3f);
+		CHECK_APPROX_EQUAL(result.mPenetrationDepth, 0.0f, 1.0e-3f);
+		CHECK_APPROX_EQUAL(result.mContactPointOn1, inPosition, 1.0e-3f);
+		CHECK_APPROX_EQUAL(result.mContactPointOn2, inPosition, 1.0e-3f);
+	}
+
+	/// Helper function that tests a sphere against a triangle centered on the origin with normal Z
+	static void sTestCastSphereTriangle(const Shape *inTriangle)
+	{
+		// Create sphere
+		Ref<Shape> sphere = SphereShapeSettings(0.2f).Create().Get();
+
+		{
+			// Hit front face
+			ShapeCast shape_cast(sphere, Vec3::sOne(), Mat44::sTranslation(Vec3(0, 0, 15)), Vec3(0, 0, -30));
+			ShapeCastSettings cast_settings;
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mReturnDeepestPoint = false;
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.back();
+			CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, -1), 1.0e-3f);
+			CHECK(result.mPenetrationDepth == 0.0f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3::sZero(), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
+			CHECK(!result.mIsBackFaceHit);
+		}
+
+		{
+			// Hit back face -> ignored
+			ShapeCast shape_cast(sphere, Vec3::sOne(), Mat44::sTranslation(Vec3(0, 0, -15)), Vec3(0, 0, 30));
+			ShapeCastSettings cast_settings;
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mReturnDeepestPoint = false;
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.empty());
+
+			// Hit back face -> collision
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.back();
+			CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, 1), 1.0e-3f);
+			CHECK(result.mPenetrationDepth == 0.0f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3::sZero(), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
+			CHECK(result.mIsBackFaceHit);
+		}
+
+		{
+			// Hit back face while starting in collision -> ignored
+			ShapeCast shape_cast(sphere, Vec3::sOne(), Mat44::sTranslation(Vec3(0, 0, -0.1f)), Vec3(0, 0, 15));
+			ShapeCastSettings cast_settings;
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mReturnDeepestPoint = true;
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.empty());
+
+			// Hit back face while starting in collision -> collision
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.back();
+			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, 1), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 0.1f, 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 0, 0.1f), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
+			CHECK(result.mIsBackFaceHit);
+		}
+
+		// Hit vertex 1, 2 and 3
+		sTestCastSphereVertexOrEdge(sphere, Vec3(50, 25, 0), Vec3(-10, -10, 0), inTriangle);
+		sTestCastSphereVertexOrEdge(sphere, Vec3(-50, 25, 0), Vec3(10, -10, 0), inTriangle);
+		sTestCastSphereVertexOrEdge(sphere, Vec3(0, -25, 0), Vec3(0, 10, 0), inTriangle);
+
+		// Hit edge 1, 2 and 3
+		sTestCastSphereVertexOrEdge(sphere, Vec3(0, 25, 0), Vec3(0, -10, 0), inTriangle); // Edge: Vec3(50, 25, 0), Vec3(-50, 25, 0)
+		sTestCastSphereVertexOrEdge(sphere, Vec3(-25, 0, 0), Vec3(10, 10, 0), inTriangle); // Edge: Vec3(-50, 25, 0), Vec3(0,-25, 0)
+		sTestCastSphereVertexOrEdge(sphere, Vec3(25, 0, 0), Vec3(-10, 10, 0), inTriangle); // Edge: Float3(0,-25, 0), Float3(50, 25, 0)
+	}
+
+	TEST_CASE("TestCastSphereTriangle")
+	{
+		// Create triangle
+		Ref<Shape> triangle = TriangleShapeSettings(Vec3(50, 25, 0), Vec3(-50, 25, 0), Vec3(0,-25, 0)).Create().Get();
+		sTestCastSphereTriangle(triangle);
+
+		// Create a triangle mesh shape
+		Ref<Shape> triangle_mesh = MeshShapeSettings({ Triangle(Float3(50, 25, 0), Float3(-50, 25, 0), Float3(0,-25, 0)) }).Create().Get();
+		sTestCastSphereTriangle(triangle_mesh);
+	}
+
+	// Test CastShape for a (scaled) sphere vs box
+	TEST_CASE("TestCastShapeSphereVsBox")
+	{
+		PhysicsTestContext c;
+
+		// Create box to collide against (shape 2)
+		// The box is scaled up by a factor 10 in the X axis and then rotated so that the X axis is up
+		BoxShapeSettings box(Vec3::sOne());
+		box.SetEmbedded();
+		ScaledShapeSettings scaled_box(&box, Vec3(10, 1, 1));
+		scaled_box.SetEmbedded();
+		Body &body2 = c.CreateBody(&scaled_box, RVec3(0, 1, 0), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Set settings
+		ShapeCastSettings settings;
+		settings.mReturnDeepestPoint = true;
+		settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+		settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+
+		{
+			// Create shape cast
+			Ref<Shape> normal_sphere = new SphereShape(1.0f);
+			RShapeCast shape_cast { normal_sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(0, 11, 0)), Vec3(0, 1, 0) };
+
+			// Shape is intersecting at the start
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, -1, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-3f);
+			CHECK(!result.mIsBackFaceHit);
+		}
+
+		{
+			// This repeats the same test as above but uses scaling at all levels and validate that the penetration depth is still correct
+			Ref<Shape> scaled_sphere = new ScaledShape(new SphereShape(0.1f), Vec3::sReplicate(5.0f));
+			RShapeCast shape_cast { scaled_sphere, Vec3::sReplicate(2.0f), RMat44::sTranslation(RVec3(0, 11, 0)), Vec3(0, 1, 0) };
+
+			// Shape is intersecting at the start
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, -1, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-3f);
+			CHECK(!result.mIsBackFaceHit);
+		}
+	}
+
+	// Test CastShape ordering according to penetration depth
+	TEST_CASE("TestCastShapePenetrationDepthOrdering")
+	{
+		PhysicsTestContext c;
+
+		// Create box to collide against (shape 2)
+		BoxShapeSettings box(Vec3(0.1f, 2.0f, 2.0f));
+		box.SetEmbedded();
+
+		// Create 10 boxes that are 0.2 thick in the X axis and 4 in Y and Z, put them all next to each other on the X axis starting from X = 0 going to X = 2
+		Array<Body *> bodies;
+		for (int i = 0; i < 10; ++i)
+			bodies.push_back(&c.CreateBody(&box, RVec3(0.1f + 0.2f * i, 0, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate));
+
+		// Set settings
+		ShapeCastSettings settings;
+		settings.mReturnDeepestPoint = true;
+		settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+		settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+		settings.mCollisionTolerance = 1.0e-5f; // Increased precision
+		settings.mPenetrationTolerance = 1.0e-5f;
+
+		{
+			// Create shape cast in X from -5 to 5
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			RShapeCast shape_cast { sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(-5, 0, 0)), Vec3(10, 0, 0) };
+
+			// We should hit the first body
+			ClosestHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			CHECK(collector.HadHit());
+			CHECK(collector.mHit.mBodyID2 == bodies.front()->GetID());
+			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 4.0f / 10.0f);
+			CHECK(collector.mHit.mPenetrationAxis.Normalized().Dot(Vec3(1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
+			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(0, 0, 0), 2.0e-3f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(0, 0, 0), 2.0e-3f);
+			CHECK(!collector.mHit.mIsBackFaceHit);
+		}
+
+		{
+			// Create shape cast in X from 5 to -5
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			RShapeCast shape_cast { sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(5, 0, 0)), Vec3(-10, 0, 0) };
+
+			// We should hit the last body
+			ClosestHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			CHECK(collector.HadHit());
+			CHECK(collector.mHit.mBodyID2 == bodies.back()->GetID());
+			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 2.0f / 10.0f, 1.0e-4f);
+			CHECK(collector.mHit.mPenetrationAxis.Normalized().Dot(Vec3(-1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
+			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(2, 0, 0), 4.0e-4f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(2, 0, 0), 4.0e-4f);
+			CHECK(!collector.mHit.mIsBackFaceHit);
+		}
+
+		{
+			// Create shape cast in X from 1.05 to 11, this should intersect with all bodies and have deepest penetration in bodies[5]
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			RShapeCast shape_cast { sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(1.05_r, 0, 0)), Vec3(10, 0, 0) };
+
+			// We should hit bodies[5]
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			collector.Sort();
+			CHECK(collector.mHits.size() == 10);
+			const ShapeCastResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == bodies[5]->GetID());
+			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
+			CHECK(result.mPenetrationAxis.Normalized().Dot(Vec3(1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.05f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(2.05f, 0, 0), 2.0e-5f); // Box starts at 1.0, center of sphere adds 0.05, radius of sphere is 1
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(1.0f, 0, 0), 2.0e-5f); // Box starts at 1.0
+			CHECK(!result.mIsBackFaceHit);
+		}
+	}
+
+	// Test casting a capsule against a mesh that is intersecting at fraction 0 and test that it returns the deepest penetration
+	TEST_CASE("TestDeepestPenetrationAtFraction0")
+	{
+		// Create an n x n grid of triangles
+		const int n = 10;
+		const float s = 0.1f;
+		TriangleList triangles;
+		for (int z = 0; z < n; ++z)
+			for (int x = 0; x < n; ++x)
+			{
+				float fx = s * x - s * n / 2, fz = s * z - s * n / 2;
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx, 0, fz + s), Vec3(fx + s, 0, fz + s)));
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx + s, 0, fz + s), Vec3(fx + s, 0, fz)));
+			}
+		MeshShapeSettings mesh_settings(triangles);
+		mesh_settings.SetEmbedded();
+
+		// Create a compound shape with two copies of the mesh
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.AddShape(Vec3::sZero(), Quat::sIdentity(), &mesh_settings);
+		compound_settings.AddShape(Vec3(0, -0.01f, 0), Quat::sIdentity(), &mesh_settings); // This will not result in the deepest penetration
+		compound_settings.SetEmbedded();
+
+		// Add it to the scene
+		PhysicsTestContext c;
+		c.CreateBody(&compound_settings, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Add the same compound a little bit lower (this will not result in the deepest penetration)
+		c.CreateBody(&compound_settings, RVec3(0, -0.1_r, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// We want the deepest hit
+		ShapeCastSettings cast_settings;
+		cast_settings.mReturnDeepestPoint = true;
+
+		// Create capsule to test
+		const float capsule_half_height = 2.0f;
+		const float capsule_radius = 1.0f;
+		RefConst<Shape> cast_shape = new CapsuleShape(capsule_half_height, capsule_radius);
+
+		// Cast the shape starting inside the mesh with a long distance so that internally in the mesh shape the RayAABox4 test will return a low negative fraction.
+		// This used to be confused with the penetration depth and would cause an early out and return the wrong result.
+		const float capsule_offset = 0.1f;
+		RShapeCast shape_cast(cast_shape, Vec3::sOne(), RMat44::sTranslation(RVec3(0, capsule_half_height + capsule_offset, 0)), Vec3(0, -100, 0));
+
+		// Cast first using the closest hit collector
+		ClosestHitCollisionCollector<CastShapeCollector> collector;
+		c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), collector);
+
+		// Check that it indeed found a hit at fraction 0 with the deepest penetration of all triangles
+		CHECK(collector.HadHit());
+		CHECK(collector.mHit.mFraction == 0.0f);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, capsule_radius - capsule_offset, 1.0e-4f);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(0, -1, 0));
+		CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3::sZero());
+
+		// Cast again while triggering a force early out after the first hit
+		class MyCollector : public CastShapeCollector
+		{
+		public:
+			virtual void	AddHit(const ShapeCastResult &inResult) override
+			{
+				++mNumHits;
+				ForceEarlyOut();
+			}
+
+			int				mNumHits = 0;
+		};
+		MyCollector collector2;
+		c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), collector2);
+
+		// Ensure that we indeed stopped after the first hit
+		CHECK(collector2.mNumHits == 1);
+	}
+
+	// Test a problem case where a sphere cast would incorrectly hit a degenerate triangle (see: https://github.com/jrouwe/JoltPhysics/issues/886)
+	TEST_CASE("TestCastSphereVsDegenerateTriangle")
+	{
+		AllHitCollisionCollector<CastShapeCollector> collector;
+		SphereShape sphere(0.2f);
+		sphere.SetEmbedded();
+		ShapeCast cast(&sphere, Vec3::sOne(), Mat44::sTranslation(Vec3(14.8314590f, 8.19055080f, -4.30825043f)), Vec3(-0.0988006592f, 5.96046448e-08f, 0.000732421875f));
+		ShapeCastSettings settings;
+		CastSphereVsTriangles caster(cast, settings, Vec3::sOne(), Mat44::sIdentity(), { }, collector);
+		caster.Cast(Vec3(14.5536213f, 10.5973721f, -0.00600051880f), Vec3(14.5536213f, 10.5969315f, -3.18638134f), Vec3(14.5536213f, 10.5969315f, -5.18637228f), 0b111, SubShapeID());
+		CHECK(!collector.HadHit());
+	}
+
+	// Test ClosestHitPerBodyCollisionCollector
+	TEST_CASE("TestClosestHitPerBodyCollisionCollector")
+	{
+		PhysicsTestContext c;
+
+		// Create a 1 by 1 by 1 box consisting of 10 slabs
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.SetEmbedded();
+		for (int i = 0; i < 10; ++i)
+			compound_settings.AddShape(Vec3(0.1f * i - 0.45f, 0, 0), Quat::sIdentity(), new BoxShape(Vec3(0.05f, 0.5f, 0.5f)));
+
+		// Create 2 instances
+		Body &body1 = c.CreateBody(&compound_settings, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+		Body &body2 = c.CreateBody(&compound_settings, RVec3(1.0_r, 0, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		ShapeCastSettings cast_settings;
+
+		SphereShape sphere(0.1f);
+		sphere.SetEmbedded();
+
+		// Override ClosestHitPerBodyCollisionCollector so that we can count the number of calls to AddHit
+		class MyClosestHitPerBodyCollisionCollector : public ClosestHitPerBodyCollisionCollector<CastShapeCollector>
+		{
+		public:
+			virtual void		AddHit(const ResultType &inResult) override
+			{
+				ClosestHitPerBodyCollisionCollector<CastShapeCollector>::AddHit(inResult);
+
+				++mNumCalls;
+			}
+
+			int					mNumCalls = 0;
+		};
+
+		{
+			RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(-1, 0, 0)), Vec3(3, 0, 0));
+
+			// Check that the all hit collector finds 20 hits (2 x 10 slabs)
+			AllHitCollisionCollector<CastShapeCollector> all_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), all_collector);
+			all_collector.Sort();
+			CHECK(all_collector.mHits.size() == 20);
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[i].mBodyID2 == body1.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[i].mContactPointOn1, Vec3(-0.5f + 0.1f * i, 0, 0));
+			}
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[10 + i].mBodyID2 == body2.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[10 + i].mContactPointOn1, Vec3(0.5f + 0.1f * i, 0, 0));
+			}
+
+			// Check that the closest hit per body collector only finds 2
+			MyClosestHitPerBodyCollisionCollector closest_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), closest_collector);
+			CHECK(closest_collector.mNumCalls == 2); // Spatial ordering by the broad phase and compound shape and the early out value should have resulted in only 2 calls to AddHit
+			closest_collector.Sort();
+			CHECK(closest_collector.mHits.size() == 2);
+			CHECK(closest_collector.mHits[0].mBodyID2 == body1.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[0].mContactPointOn1, Vec3(-0.5f, 0, 0));
+			CHECK(closest_collector.mHits[1].mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[1].mContactPointOn1, Vec3(0.5f, 0, 0));
+		}
+
+		{
+			// Cast in reverse direction
+			RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(2, 0, 0)), Vec3(-3, 0, 0));
+
+			// Check that the all hit collector finds 20 hits (2 x 10 slabs)
+			AllHitCollisionCollector<CastShapeCollector> all_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), all_collector);
+			all_collector.Sort();
+			CHECK(all_collector.mHits.size() == 20);
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[i].mBodyID2 == body2.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[i].mContactPointOn1, Vec3(1.5f - 0.1f * i, 0, 0));
+			}
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[10 + i].mBodyID2 == body1.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[10 + i].mContactPointOn1, Vec3(0.5f - 0.1f * i, 0, 0));
+			}
+
+			// Check that the closest hit per body collector only finds 2
+			MyClosestHitPerBodyCollisionCollector closest_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), closest_collector);
+			CHECK(closest_collector.mNumCalls == 2); // Spatial ordering by the broad phase and compound shape and the early out value should have resulted in only 2 calls to AddHit
+			closest_collector.Sort();
+			CHECK(closest_collector.mHits.size() == 2);
+			CHECK(closest_collector.mHits[0].mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[0].mContactPointOn1, Vec3(1.5f, 0, 0));
+			CHECK(closest_collector.mHits[1].mBodyID2 == body1.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[1].mContactPointOn1, Vec3(0.5f, 0, 0));
+		}
+	}
+
+	// Test 2D shape cast against a box
+	TEST_CASE("TestCast2DBoxVsBox")
+	{
+		RefConst<Shape> box_shape;
+		{
+			float size = 5.0f;
+			float thickness = 1.0f;
+			Array<Vec3> points = {
+				Vec3(-size, -size, thickness),
+				Vec3(size, -size, thickness),
+				Vec3(size, size, thickness),
+				Vec3(-size, size, thickness),
+				Vec3(-size, -size, -thickness),
+				Vec3(size, -size, -thickness),
+				Vec3(size, size, -thickness),
+				Vec3(-size, size, -thickness),
+			};
+			ConvexHullShapeSettings box_shape_settings(points);
+			box_shape_settings.SetEmbedded();
+			box_shape_settings.mMaxConvexRadius = 0.0f;
+			box_shape = box_shape_settings.Create().Get();
+		}
+
+		RefConst<Shape> cast_shape;
+		{
+			float size = 1.0f;
+			Array<Vec3> points = {
+				Vec3(-size, -size, 0),
+				Vec3(size, -size, 0),
+				Vec3(size, size, 0),
+				Vec3(-size, size, 0),
+			};
+			ConvexHullShapeSettings cast_shape_settings(points);
+			cast_shape_settings.SetEmbedded();
+			cast_shape_settings.mMaxConvexRadius = 0.0f;
+			cast_shape = cast_shape_settings.Create().Get();
+		}
+
+		// The 2d box cast touches the surface of the box at the start and moves into it
+		ShapeCastSettings settings;
+		settings.mReturnDeepestPoint = true;
+		ShapeCast shape_cast(cast_shape, Vec3::sOne(), Mat44::sTranslation(Vec3(0, 0, 1)), Vec3(0, 0, -10));
+		ClosestHitCollisionCollector<CastShapeCollector> cast_shape_collector;
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, settings, box_shape, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), cast_shape_collector);
+
+		CHECK(cast_shape_collector.HadHit());
+		CHECK(cast_shape_collector.mHit.mFraction == 0.0f);
+		CHECK_APPROX_EQUAL(cast_shape_collector.mHit.mPenetrationAxis.Normalized(), Vec3(0, 0, -1));
+		CHECK_APPROX_EQUAL(cast_shape_collector.mHit.mPenetrationDepth, 0.0f);
+		CHECK_APPROX_EQUAL(cast_shape_collector.mHit.mContactPointOn1, Vec3(0, 0, 1), 1.0e-4f);
+		CHECK_APPROX_EQUAL(cast_shape_collector.mHit.mContactPointOn2, Vec3(0, 0, 1), 1.0e-4f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CharacterVirtualTests.cpp

@@ -0,0 +1,873 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "LoggingCharacterContactListener.h"
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Character/CharacterVirtual.h>
+#include "Layers.h"
+
+TEST_SUITE("CharacterVirtualTests")
+{
+	class Character : public CharacterContactListener
+	{
+	public:
+		// Construct
+								Character(PhysicsTestContext &ioContext) : mContext(ioContext) { }
+
+		// Create the character
+		void					Create()
+		{
+			// Create capsule
+			Ref<Shape> capsule = new CapsuleShape(0.5f * mHeightStanding, mRadiusStanding);
+			mCharacterSettings.mShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * mHeightStanding + mRadiusStanding, 0), Quat::sIdentity(), capsule).Create().Get();
+
+			// Configure supporting volume
+			mCharacterSettings.mSupportingVolume = Plane(Vec3::sAxisY(), -mHeightStanding); // Accept contacts that touch the lower sphere of the capsule
+
+			// Create character
+			mCharacter = new CharacterVirtual(&mCharacterSettings, mInitialPosition, Quat::sIdentity(), 0, mContext.GetSystem());
+			mCharacter->SetListener(this);
+			mCharacter->SetCharacterVsCharacterCollision(&mCharacterVsCharacter);
+		}
+
+		// Step the character and the world
+		void					Step()
+		{
+			// Step the world
+			mContext.SimulateSingleStep();
+
+			// Determine new basic velocity
+			Vec3 current_vertical_velocity = Vec3(0, mCharacter->GetLinearVelocity().GetY(), 0);
+			Vec3 ground_velocity = mCharacter->GetGroundVelocity();
+			Vec3 new_velocity;
+			if (mCharacter->GetGroundState() == CharacterVirtual::EGroundState::OnGround // If on ground
+				&& (current_vertical_velocity.GetY() - ground_velocity.GetY()) < 0.1f) // And not moving away from ground
+			{
+				// Assume velocity of ground when on ground
+				new_velocity = ground_velocity;
+
+				// Jump
+				new_velocity += Vec3(0, mJumpSpeed, 0);
+				mJumpSpeed = 0.0f;
+			}
+			else
+				new_velocity = current_vertical_velocity;
+
+			// Gravity
+			PhysicsSystem *system = mContext.GetSystem();
+			float delta_time = mContext.GetDeltaTime();
+			new_velocity += system->GetGravity() * delta_time;
+
+			// Player input
+			new_velocity += mHorizontalSpeed;
+
+			// Update character velocity
+			mCharacter->SetLinearVelocity(new_velocity);
+
+			RVec3 start_pos = GetPosition();
+
+			// Update the character position
+			TempAllocatorMalloc allocator;
+			mCharacter->ExtendedUpdate(delta_time,
+				system->GetGravity(),
+				mUpdateSettings,
+				system->GetDefaultBroadPhaseLayerFilter(Layers::MOVING),
+				system->GetDefaultLayerFilter(Layers::MOVING),
+				{ },
+				{ },
+				allocator);
+
+			// Calculate effective velocity in this step
+			mEffectiveVelocity = Vec3(GetPosition() - start_pos) / delta_time;
+		}
+
+		// Simulate a longer period of time
+		void					Simulate(float inTime)
+		{
+			int num_steps = (int)round(inTime / mContext.GetDeltaTime());
+			for (int step = 0; step < num_steps; ++step)
+				Step();
+		}
+
+		// Get the number of active contacts
+		size_t					GetNumContacts() const
+		{
+			return mCharacter->GetActiveContacts().size();
+		}
+
+		// Check if the character is in contact with another body
+		bool					HasCollidedWith(const BodyID &inBody) const
+		{
+			return mCharacter->HasCollidedWith(inBody);
+		}
+
+		// Check if the character is in contact with another character
+		bool					HasCollidedWith(const CharacterVirtual *inCharacter) const
+		{
+			return mCharacter->HasCollidedWith(inCharacter);
+		}
+
+		// Get position of character
+		RVec3					GetPosition() const
+		{
+			return mCharacter->GetPosition();
+		}
+
+		// Configuration
+		RVec3					mInitialPosition = RVec3::sZero();
+		float					mHeightStanding = 1.35f;
+		float					mRadiusStanding = 0.3f;
+		CharacterVirtualSettings mCharacterSettings;
+		CharacterVirtual::ExtendedUpdateSettings mUpdateSettings;
+
+		// Character movement settings (update to control the movement of the character)
+		Vec3					mHorizontalSpeed = Vec3::sZero();
+		float					mJumpSpeed = 0.0f; // Character will jump when not 0, will auto reset
+
+		// The character
+		Ref<CharacterVirtual>	mCharacter;
+
+		// Character vs character
+		CharacterVsCharacterCollisionSimple mCharacterVsCharacter;
+
+		// Calculated effective velocity after a step
+		Vec3					mEffectiveVelocity = Vec3::sZero();
+
+		// Log of contact events
+		LoggingCharacterContactListener mContactLog;
+
+	private:
+		// CharacterContactListener callback
+		virtual bool			OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) override
+		{
+			return mContactLog.OnContactValidate(inCharacter, inBodyID2, inSubShapeID2);
+		}
+
+		virtual bool			OnCharacterContactValidate(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2) override
+		{
+			return mContactLog.OnCharacterContactValidate(inCharacter, inOtherCharacter, inSubShapeID2);
+		}
+
+		virtual void			OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+		{
+			mContactLog.OnContactAdded(inCharacter, inBodyID2, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+		}
+
+		virtual void			OnContactPersisted(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+		{
+			mContactLog.OnContactPersisted(inCharacter, inBodyID2, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+		}
+
+		virtual void			OnContactRemoved(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) override
+		{
+			mContactLog.OnContactRemoved(inCharacter, inBodyID2, inSubShapeID2);
+		}
+
+		virtual void			OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+		{
+			mContactLog.OnCharacterContactAdded(inCharacter, inOtherCharacter, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+		}
+
+		virtual void			OnCharacterContactPersisted(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+		{
+			mContactLog.OnCharacterContactPersisted(inCharacter, inOtherCharacter, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+		}
+
+		virtual void			OnCharacterContactRemoved(const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2) override
+		{
+			mContactLog.OnCharacterContactRemoved(inCharacter, inOtherCharacterID, inSubShapeID2);
+		}
+
+		virtual void			OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) override
+		{
+			// Don't allow sliding if the character doesn't want to move
+			if (mHorizontalSpeed.IsNearZero() && inContactVelocity.IsNearZero() && !inCharacter->IsSlopeTooSteep(inContactNormal))
+				ioNewCharacterVelocity = Vec3::sZero();
+		}
+
+		PhysicsTestContext &	mContext;
+	};
+
+	TEST_CASE("TestFallingAndJumping")
+	{
+		// Create floor
+		PhysicsTestContext c;
+		c.CreateFloor();
+
+		// Create character
+		Character character(c);
+		character.mInitialPosition = RVec3(0, 2, 0);
+		character.Create();
+
+		// After 1 step we should still be in air
+		character.Step();
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::InAir);
+
+		// After some time we should be on the floor
+		character.Simulate(1.0f);
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+		CHECK_APPROX_EQUAL(character.GetPosition(), RVec3::sZero());
+		CHECK_APPROX_EQUAL(character.mEffectiveVelocity, Vec3::sZero());
+
+		// Jump
+		character.mJumpSpeed = 1.0f;
+		character.Step();
+		Vec3 velocity(0, 1.0f + c.GetDeltaTime() * c.GetSystem()->GetGravity().GetY(), 0);
+		CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(velocity * c.GetDeltaTime()));
+		CHECK_APPROX_EQUAL(character.mEffectiveVelocity, velocity);
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::InAir);
+
+		// After some time we should be on the floor again
+		character.Simulate(1.0f);
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+		CHECK_APPROX_EQUAL(character.GetPosition(), RVec3::sZero());
+		CHECK_APPROX_EQUAL(character.mEffectiveVelocity, Vec3::sZero());
+	}
+
+	TEST_CASE("TestMovingOnSlope")
+	{
+		constexpr float cFloorHalfHeight = 1.0f;
+		constexpr float cMovementTime = 1.5f;
+
+		// Iterate various slope angles
+		for (float slope_angle = DegreesToRadians(5.0f); slope_angle < DegreesToRadians(85.0f); slope_angle += DegreesToRadians(10.0f))
+		{
+			// Create sloped floor
+			PhysicsTestContext c;
+			Quat slope_rotation = Quat::sRotation(Vec3::sAxisZ(), slope_angle);
+			c.CreateBox(RVec3::sZero(), slope_rotation, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(100.0f, cFloorHalfHeight, 100.0f));
+
+			// Create character so that it is touching the slope
+			Character character(c);
+			float radius_and_padding = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+			character.mInitialPosition = RVec3(0, (radius_and_padding + cFloorHalfHeight) / Cos(slope_angle) - radius_and_padding, 0);
+			character.Create();
+
+			// Determine if the slope is too steep for the character
+			bool too_steep = slope_angle > character.mCharacterSettings.mMaxSlopeAngle;
+			CharacterBase::EGroundState expected_ground_state = (too_steep? CharacterBase::EGroundState::OnSteepGround : CharacterBase::EGroundState::OnGround);
+
+			Vec3 gravity = c.GetSystem()->GetGravity();
+			float time_step = c.GetDeltaTime();
+			Vec3 slope_normal = slope_rotation.RotateAxisY();
+
+			// Calculate expected position after 1 time step
+			RVec3 position_after_1_step = character.mInitialPosition;
+			if (too_steep)
+			{
+				// Apply 1 frame of gravity and cancel movement in the slope normal direction
+				Vec3 velocity = gravity * time_step;
+				velocity -= velocity.Dot(slope_normal) * slope_normal;
+				position_after_1_step += velocity * time_step;
+			}
+
+			// After 1 step we should be on the slope
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == expected_ground_state);
+			CHECK_APPROX_EQUAL(character.GetPosition(), position_after_1_step, 2.0e-6f);
+
+			// Cancel any velocity to make the calculation below easier (otherwise we have to take gravity for 1 time step into account)
+			character.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+			RVec3 start_pos = character.GetPosition();
+
+			// Start moving in X direction
+			character.mHorizontalSpeed = Vec3(2.0f, 0, 0);
+			character.Simulate(cMovementTime);
+			CHECK(character.mCharacter->GetGroundState() == expected_ground_state);
+
+			// Calculate resulting translation
+			Vec3 translation = Vec3(character.GetPosition() - start_pos);
+
+			// Calculate expected translation
+			Vec3 expected_translation;
+			if (too_steep)
+			{
+				// If too steep, we're just falling. Integrate using an Euler integrator.
+				Vec3 velocity = Vec3::sZero();
+				expected_translation = Vec3::sZero();
+				int num_steps = (int)round(cMovementTime / time_step);
+				for (int i = 0; i < num_steps; ++i)
+				{
+					velocity += gravity * time_step;
+					expected_translation += velocity * time_step;
+				}
+			}
+			else
+			{
+				// Every frame we apply 1 delta time * gravity which gets reset on the next update, add this to the horizontal speed
+				expected_translation = (character.mHorizontalSpeed + gravity * time_step) * cMovementTime;
+			}
+
+			// Cancel movement in slope direction
+			expected_translation -= expected_translation.Dot(slope_normal) * slope_normal;
+
+			// Check that we traveled the right amount
+			CHECK_APPROX_EQUAL(translation, expected_translation, 1.0e-4f);
+		}
+	}
+
+	TEST_CASE("TestStickToFloor")
+	{
+		constexpr float cFloorHalfHeight = 1.0f;
+		constexpr float cSlopeAngle = DegreesToRadians(45.0f);
+		constexpr float cMovementTime = 1.5f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// If this run is with 'stick to floor' enabled
+			bool stick_to_floor = mode == 0;
+
+			// Create sloped floor
+			PhysicsTestContext c;
+			Quat slope_rotation = Quat::sRotation(Vec3::sAxisZ(), cSlopeAngle);
+			Body &floor = c.CreateBox(RVec3::sZero(), slope_rotation, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(100.0f, cFloorHalfHeight, 100.0f));
+
+			// Create character so that it is touching the slope
+			Character character(c);
+			float radius_and_padding = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+			character.mInitialPosition = RVec3(0, (radius_and_padding + cFloorHalfHeight) / Cos(cSlopeAngle) - radius_and_padding, 0);
+			character.mUpdateSettings.mStickToFloorStepDown = stick_to_floor? Vec3(0, -0.5f, 0) : Vec3::sZero();
+			character.Create();
+
+			// After 1 step we should be on the slope
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			CHECK(character.mContactLog.GetEntryCount() == 2);
+			CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::ValidateBody, character.mCharacter, floor.GetID()));
+			CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::AddBody, character.mCharacter, floor.GetID()));
+			character.mContactLog.Clear();
+
+			// Cancel any velocity to make the calculation below easier (otherwise we have to take gravity for 1 time step into account)
+			character.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+			RVec3 start_pos = character.GetPosition();
+
+			float time_step = c.GetDeltaTime();
+			int num_steps = (int)round(cMovementTime / time_step);
+
+			for (int i = 0; i < num_steps; ++i)
+			{
+				// Start moving down the slope at a speed high enough so that gravity will not keep us on the floor
+				character.mHorizontalSpeed = Vec3(-10.0f, 0, 0);
+				character.Step();
+
+				if (stick_to_floor)
+				{
+					// Should stick to floor
+					CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+					// Should have received callbacks
+					CHECK(character.mContactLog.GetEntryCount() == 2);
+					CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::ValidateBody, character.mCharacter, floor.GetID()));
+					CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::PersistBody, character.mCharacter, floor.GetID()));
+					character.mContactLog.Clear();
+				}
+				else
+				{
+					// Should be off ground
+					CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::InAir);
+
+					// Remove callbacks
+					CHECK(character.mContactLog.GetEntryCount() == 1);
+					CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::RemoveBody, character.mCharacter, floor.GetID()));
+				}
+			}
+
+			// Calculate resulting translation
+			Vec3 translation = Vec3(character.GetPosition() - start_pos);
+
+			// Calculate expected translation
+			Vec3 expected_translation;
+			if (stick_to_floor)
+			{
+				// We should stick to the floor, so the vertical translation follows the slope perfectly
+				expected_translation = character.mHorizontalSpeed * cMovementTime;
+				expected_translation.SetY(expected_translation.GetX() * Tan(cSlopeAngle));
+			}
+			else
+			{
+				// If too steep, we're just falling. Integrate using an Euler integrator.
+				Vec3 velocity = character.mHorizontalSpeed;
+				expected_translation = Vec3::sZero();
+				Vec3 gravity = c.GetSystem()->GetGravity();
+				for (int i = 0; i < num_steps; ++i)
+				{
+					velocity += gravity * time_step;
+					expected_translation += velocity * time_step;
+				}
+			}
+
+			// Check that we traveled the right amount
+			CHECK_APPROX_EQUAL(translation, expected_translation, 1.0e-4f);
+		}
+	}
+
+	TEST_CASE("TestWalkStairs")
+	{
+		const float cStepHeight = 0.3f;
+		const int cNumSteps = 10;
+
+		// Create stairs from triangles
+		TriangleList triangles;
+		for (int i = 0; i < cNumSteps; ++i)
+		{
+			// Start of step
+			Vec3 base(0, cStepHeight * i, cStepHeight * i);
+
+			// Left side
+			Vec3 b1 = base + Vec3(2.0f, 0, 0);
+			Vec3 s1 = b1 + Vec3(0, cStepHeight, 0);
+			Vec3 p1 = s1 + Vec3(0, 0, cStepHeight);
+
+			// Right side
+			Vec3 width(-4.0f, 0, 0);
+			Vec3 b2 = b1 + width;
+			Vec3 s2 = s1 + width;
+			Vec3 p2 = p1 + width;
+
+			triangles.push_back(Triangle(s1, b1, s2));
+			triangles.push_back(Triangle(b1, b2, s2));
+			triangles.push_back(Triangle(s1, p2, p1));
+			triangles.push_back(Triangle(s1, s2, p2));
+		}
+
+		MeshShapeSettings mesh(triangles);
+		mesh.SetEmbedded();
+		BodyCreationSettings mesh_stairs(&mesh, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+
+		// Stair stepping is very delta time sensitive, so test various update frequencies
+		float frequencies[] = { 60.0f, 120.0f, 240.0f, 360.0f };
+		for (float frequency : frequencies)
+		{
+			float time_step = 1.0f / frequency;
+
+			PhysicsTestContext c(time_step);
+			c.CreateFloor();
+			c.GetBodyInterface().CreateAndAddBody(mesh_stairs, EActivation::DontActivate);
+
+			// Create character so that it is touching the slope
+			Character character(c);
+			character.mInitialPosition = RVec3(0, 0, -2.0f); // Start in front of the stairs
+			character.mUpdateSettings.mWalkStairsStepUp = Vec3::sZero(); // No stair walking
+			character.Create();
+
+			// Start moving towards the stairs
+			character.mHorizontalSpeed = Vec3(0, 0, 4.0f);
+			character.Simulate(1.0f);
+
+			// We should have gotten stuck at the start of the stairs (can't move up)
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			float radius_and_padding = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+			CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(0, 0, -radius_and_padding), 1.1e-2f);
+
+			// Enable stair walking
+			character.mUpdateSettings.mWalkStairsStepUp = Vec3(0, 0.4f, 0);
+
+			// Calculate time it should take to move up the stairs at constant speed
+			float movement_time = (cNumSteps * cStepHeight + radius_and_padding) / character.mHorizontalSpeed.GetZ();
+			int max_steps = int(1.5f * round(movement_time / time_step)); // In practice there is a bit of slowdown while stair stepping, so add a bit of slack
+
+			// Step until we reach the top of the stairs
+			RVec3 last_position = character.GetPosition();
+			bool reached_goal = false;
+			for (int i = 0; i < max_steps; ++i)
+			{
+				character.Step();
+
+				// We should always be on the floor during stair stepping
+				CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+				// Check position progression
+				RVec3 position = character.GetPosition();
+				CHECK_APPROX_EQUAL(position.GetX(), 0); // No movement in X
+				CHECK(position.GetZ() > last_position.GetZ()); // Always moving forward
+				CHECK(position.GetZ() < cNumSteps * cStepHeight); // No movement beyond stairs
+				if (position.GetY() > cNumSteps * cStepHeight - 1.0e-3f)
+				{
+					reached_goal = true;
+					break;
+				}
+
+				last_position = position;
+			}
+			CHECK(reached_goal);
+		}
+	}
+
+	TEST_CASE("TestRotatingPlatform")
+	{
+		constexpr float cFloorHalfHeight = 1.0f;
+		constexpr float cFloorHalfWidth = 10.0f;
+		constexpr float cCharacterPosition = 0.9f * cFloorHalfWidth;
+		constexpr float cAngularVelocity = 2.0f * JPH_PI;
+
+		PhysicsTestContext c;
+
+		// Create box
+		Body &box = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(cFloorHalfWidth, cFloorHalfHeight, cFloorHalfWidth));
+		box.SetAllowSleeping(false);
+
+		// Create character so that it is touching the box at the
+		Character character(c);
+		character.mInitialPosition = RVec3(cCharacterPosition, cFloorHalfHeight, 0);
+		character.Create();
+
+		// Step to ensure the character is on the box
+		character.Step();
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+		// Set the box to rotate a full circle per second
+		box.SetAngularVelocity(Vec3(0, cAngularVelocity, 0));
+
+		// Rotate and check that character stays on the box
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+			// Note that the character moves according to the ground velocity and the ground velocity is updated at the end of the step
+			// so the character is always 1 time step behind the platform. This is why we use t and not t + 1 to calculate the expected position.
+			RVec3 expected_position = RMat44::sRotation(Quat::sRotation(Vec3::sAxisY(), float(t) * c.GetDeltaTime() * cAngularVelocity)) * character.mInitialPosition;
+			CHECK_APPROX_EQUAL(character.GetPosition(), expected_position, 1.0e-4f);
+		}
+	}
+
+	TEST_CASE("TestMovingPlatformUp")
+	{
+		constexpr float cFloorHalfHeight = 1.0f;
+		constexpr float cFloorHalfWidth = 10.0f;
+		constexpr float cLinearVelocity = 0.5f;
+
+		PhysicsTestContext c;
+
+		// Create box
+		Body &box = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(cFloorHalfWidth, cFloorHalfHeight, cFloorHalfWidth));
+		box.SetAllowSleeping(false);
+
+		// Create character so that it is touching the box at the
+		Character character(c);
+		character.mInitialPosition = RVec3(0, cFloorHalfHeight, 0);
+		character.Create();
+
+		// Step to ensure the character is on the box
+		character.Step();
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+		// Set the box to move up
+		box.SetLinearVelocity(Vec3(0, cLinearVelocity, 0));
+
+		// Check that character stays on the box
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			RVec3 expected_position = box.GetPosition() + character.mInitialPosition;
+			CHECK_APPROX_EQUAL(character.GetPosition(), expected_position, 1.0e-2f);
+		}
+
+		// Stop box
+		box.SetLinearVelocity(Vec3::sZero());
+		character.Simulate(0.5f);
+
+		// Set the box to move down
+		box.SetLinearVelocity(Vec3(0, -cLinearVelocity, 0));
+
+		// Check that character stays on the box
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			RVec3 expected_position = box.GetPosition() + character.mInitialPosition;
+			CHECK_APPROX_EQUAL(character.GetPosition(), expected_position, 1.0e-2f);
+		}
+	}
+
+	TEST_CASE("TestContactPointLimit")
+	{
+		PhysicsTestContext ctx;
+		Body &floor = ctx.CreateFloor();
+
+		// Create character at the origin
+		Character character(ctx);
+		character.mInitialPosition = RVec3(0, 1, 0);
+		character.mUpdateSettings.mStickToFloorStepDown = Vec3::sZero();
+		character.mUpdateSettings.mWalkStairsStepUp = Vec3::sZero();
+		character.Create();
+
+		// Radius including padding
+		const float character_radius = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+
+		// Create a half cylinder with caps for testing contact point limit
+		VertexList vertices;
+		IndexedTriangleList triangles;
+
+		// The half cylinder
+		const int cPosSegments = 2;
+		const int cAngleSegments = 768;
+		const float cCylinderLength = 2.0f;
+		for (int pos = 0; pos < cPosSegments; ++pos)
+			for (int angle = 0; angle < cAngleSegments; ++angle)
+			{
+				uint32 start = (uint32)vertices.size();
+
+				float radius = character_radius + 0.01f;
+				float angle_rad = (-0.5f + float(angle) / cAngleSegments) * JPH_PI;
+				float s = Sin(angle_rad);
+				float c = Cos(angle_rad);
+				float x = cCylinderLength * (-0.5f + float(pos) / (cPosSegments - 1));
+				float y = angle == 0 || angle == cAngleSegments - 1? 0.5f : (1.0f - c) * radius;
+				float z = s * radius;
+				vertices.push_back(Float3(x, y, z));
+
+				if (pos > 0 && angle > 0)
+				{
+					triangles.push_back(IndexedTriangle(start, start - 1, start - cAngleSegments));
+					triangles.push_back(IndexedTriangle(start - 1, start - cAngleSegments - 1, start - cAngleSegments));
+				}
+			}
+
+		// Add end caps
+		uint32 end = cAngleSegments * (cPosSegments - 1);
+		for (int angle = 0; angle < cAngleSegments - 1; ++angle)
+		{
+			triangles.push_back(IndexedTriangle(0, angle + 1, angle));
+			triangles.push_back(IndexedTriangle(end, end + angle, end + angle + 1));
+		}
+
+		// Create test body
+		MeshShapeSettings mesh(vertices, triangles);
+		mesh.SetEmbedded();
+		BodyCreationSettings mesh_cylinder(&mesh, character.mInitialPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		BodyID cylinder_id = ctx.GetBodyInterface().CreateAndAddBody(mesh_cylinder, EActivation::DontActivate);
+
+		// End positions that can be reached by character
+		RVec3 pos_end(0.5_r * cCylinderLength - character_radius, 1, 0);
+		RVec3 neg_end(-0.5_r * cCylinderLength + character_radius, 1, 0);
+
+		// Move towards positive cap and test if we hit the end
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() <= character.mCharacter->GetMaxNumHits());
+			CHECK(character.mCharacter->GetGroundBodyID() == cylinder_id);
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.GetPosition(), pos_end, 1.0e-4f);
+
+		// Move towards negative cap and test if we hit the end
+		character.mHorizontalSpeed = Vec3(-cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() <= character.mCharacter->GetMaxNumHits());
+			CHECK(character.mCharacter->GetGroundBodyID() == cylinder_id);
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.GetPosition(), neg_end, 1.0e-4f);
+
+		// Turn off contact point reduction
+		character.mCharacter->SetHitReductionCosMaxAngle(-1.0f);
+
+		// Move towards positive cap and test that we did not reach the end
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() == character.mCharacter->GetMaxNumHits());
+		}
+		RVec3 cur_pos = character.GetPosition();
+		CHECK((pos_end - cur_pos).Length() > 0.01_r);
+
+		// Move towards negative cap and test that we got stuck
+		character.mHorizontalSpeed = Vec3(-cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() == character.mCharacter->GetMaxNumHits());
+		}
+		CHECK(cur_pos.IsClose(character.GetPosition(), 1.0e-6f));
+
+		// Now teleport the character next to the half cylinder
+		character.mCharacter->SetPosition(RVec3(0, 0, 1));
+
+		// Move in positive X and check that we did not exceed max hits and that we were able to move unimpeded
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(!character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() == 1); // We should only hit the floor
+			CHECK(character.mCharacter->GetGroundBodyID() == floor.GetID());
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(cCylinderLength, 0, 1), 1.0e-4f);
+	}
+
+	TEST_CASE("TestStairWalkAlongWall")
+	{
+		// Stair stepping is very delta time sensitive, so test various update frequencies
+		float frequencies[] = { 60.0f, 120.0f, 240.0f, 360.0f };
+		for (float frequency : frequencies)
+		{
+			float time_step = 1.0f / frequency;
+
+			PhysicsTestContext c(time_step);
+			c.CreateFloor();
+
+			// Create character
+			Character character(c);
+			character.Create();
+
+			// Create a wall
+			const float cWallHalfThickness = 0.05f;
+			c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(50.0f, 1.0f, cWallHalfThickness)), RVec3(0, 1.0_r, Real(-character.mRadiusStanding - character.mCharacter->GetCharacterPadding() - cWallHalfThickness)), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+			// Start moving along the wall, if the stair stepping algorithm is working correctly it should not trigger and not apply extra speed to the character
+			character.mHorizontalSpeed = Vec3(5.0f, 0, -1.0f);
+			character.Simulate(1.0f);
+
+			// We should have moved along the wall at the desired speed
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(5.0f, 0, 0), 1.0e-2f);
+		}
+	}
+
+	TEST_CASE("TestInitiallyIntersecting")
+	{
+		PhysicsTestContext c;
+		c.CreateFloor();
+
+		// Create box that is intersecting with the character
+		c.CreateBox(RVec3(-0.5f, 0.5f, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3::sReplicate(0.5f));
+
+		// Try various penetration recovery values
+		for (float penetration_recovery : { 0.0f, 0.5f, 0.75f, 1.0f })
+		{
+			// Create character
+			Character character(c);
+			character.mCharacterSettings.mPenetrationRecoverySpeed = penetration_recovery;
+			character.Create();
+			CHECK_APPROX_EQUAL(character.GetPosition(), RVec3::sZero());
+
+			// Total radius of character
+			float radius_and_padding = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+
+			float x = 0.0f;
+			for (int step = 0; step < 3; ++step)
+			{
+				// Calculate expected position
+				x += penetration_recovery * (radius_and_padding - x);
+
+				// Step character and check that it matches expected recovery
+				character.Step();
+				CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(x, 0, 0));
+			}
+		}
+	}
+
+	TEST_CASE("TestCharacterVsCharacter")
+	{
+		PhysicsTestContext c;
+		BodyID floor_id = c.CreateFloor().GetID();
+
+		// Create characters with different radii and padding
+		Character character1(c);
+		character1.mInitialPosition = RVec3::sZero();
+		character1.mRadiusStanding = 0.2f;
+		character1.mCharacterSettings.mCharacterPadding = 0.04f;
+		character1.Create();
+
+		Character character2(c);
+		character2.mInitialPosition = RVec3(1, 0, 0);
+		character2.mRadiusStanding = 0.3f;
+		character2.mCharacterSettings.mCharacterPadding = 0.03f;
+		character2.Create();
+
+		// Make both collide
+		character1.mCharacterVsCharacter.Add(character2.mCharacter);
+		character2.mCharacterVsCharacter.Add(character1.mCharacter);
+
+		// Add a box behind character 2, we should never hit this
+		Vec3 box_extent(0.1f, 1.0f, 1.0f);
+		c.CreateBox(RVec3(1.5f, 0, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, box_extent, EActivation::DontActivate);
+
+		// Move character 1 towards character 2 so that in 1 step it will hit both character 2 and the box
+		character1.mHorizontalSpeed = Vec3(600.0f, 0, 0);
+		character1.Step();
+
+		// Character 1 should have stopped at character 2
+		float character1_radius = character1.mRadiusStanding + character1.mCharacterSettings.mCharacterPadding;
+		float character2_radius = character2.mRadiusStanding + character2.mCharacterSettings.mCharacterPadding;
+		float separation = character1_radius + character2_radius;
+		RVec3 expected_colliding_with_character = character2.mInitialPosition - Vec3(separation, 0, 0);
+		CHECK_APPROX_EQUAL(character1.GetPosition(), expected_colliding_with_character, 1.0e-3f);
+		CHECK(character1.GetNumContacts() == 2);
+		CHECK(character1.HasCollidedWith(floor_id));
+		CHECK(character1.HasCollidedWith(character2.mCharacter));
+
+		// Move character 1 back to its initial position
+		character1.mCharacter->SetPosition(character1.mInitialPosition);
+		character1.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+		// Now move slowly so that we will detect the collision during the normal collide shape step
+		character1.mHorizontalSpeed = Vec3(1.0f, 0, 0);
+		character1.Step();
+		CHECK(character1.GetNumContacts() == 1);
+		CHECK(character1.HasCollidedWith(floor_id));
+		character1.Simulate(1.0f);
+
+		// Character 1 should have stopped at character 2
+		CHECK_APPROX_EQUAL(character1.GetPosition(), expected_colliding_with_character, 1.0e-3f);
+		CHECK(character1.GetNumContacts() == 2);
+		CHECK(character1.HasCollidedWith(floor_id));
+		CHECK(character1.HasCollidedWith(character2.mCharacter));
+
+		// Move character 1 back to its initial position
+		character1.mCharacter->SetPosition(character1.mInitialPosition);
+		character1.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+		// Add a box in between the characters
+		RVec3 box_position(0.5f, 0, 0);
+		BodyID box_id = c.CreateBox(box_position, Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, box_extent, EActivation::DontActivate).GetID();
+
+		// Move character 1 so that it will step through both the box and the character in 1 time step
+		character1.mHorizontalSpeed = Vec3(600.0f, 0, 0);
+		character1.Step();
+
+		// Expect that it ends up at the box
+		RVec3 expected_colliding_with_box = box_position - Vec3(character1_radius + box_extent.GetX(), 0, 0);
+		CHECK_APPROX_EQUAL(character1.GetPosition(), expected_colliding_with_box, 1.0e-3f);
+		CHECK(character1.GetNumContacts() == 2);
+		CHECK(character1.HasCollidedWith(floor_id));
+		CHECK(character1.HasCollidedWith(box_id));
+
+		// Move character 1 back to its initial position
+		character1.mCharacter->SetPosition(character1.mInitialPosition);
+		character1.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+		// Now move slowly so that we will detect the collision during the normal collide shape step
+		character1.mHorizontalSpeed = Vec3(1.0f, 0, 0);
+		character1.Step();
+		CHECK(character1.GetNumContacts() == 1);
+		CHECK(character1.HasCollidedWith(floor_id));
+		character1.Simulate(1.0f);
+
+		// Expect that it ends up at the box
+		CHECK_APPROX_EQUAL(character1.GetPosition(), expected_colliding_with_box, 1.0e-3f);
+		CHECK(character1.GetNumContacts() == 2);
+		CHECK(character1.HasCollidedWith(floor_id));
+		CHECK(character1.HasCollidedWith(box_id));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CollidePointTests.cpp

@@ -0,0 +1,433 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+
+TEST_SUITE("CollidePointTests")
+{
+	// Probe directions in the direction of the faces
+	static Vec3 cube_probes[] = {
+		Vec3(-1.0f, 0, 0),
+		Vec3(1.0f, 0, 0),
+		Vec3(0, -1.0f, 0),
+		Vec3(0, 1.0f, 0),
+		Vec3(0, 0, -1.0f),
+		Vec3(0, 0, 1.0f)
+	};
+
+	// Probe directions in the direction of the faces
+	static Vec3 cube_and_zero_probes[] = {
+		Vec3(0, 0, 0),
+		Vec3(-1.0f, 0, 0),
+		Vec3(1.0f, 0, 0),
+		Vec3(0, -1.0f, 0),
+		Vec3(0, 1.0f, 0),
+		Vec3(0, 0, -1.0f),
+		Vec3(0, 0, 1.0f)
+	};
+
+	// Probes in the xy-plane
+	static Vec3 xy_probes[] = {
+		Vec3(-1.0f, 0, 0),
+		Vec3(1.0f, 0, 0),
+		Vec3(0, 0, -1.0f),
+		Vec3(0, 0, 1.0f)
+	};
+
+	// Probes in the xy-plane and zero
+	static Vec3 xy_and_zero_probes[] = {
+		Vec3(0, 0, 0),
+		Vec3(-1.0f, 0, 0),
+		Vec3(1.0f, 0, 0),
+		Vec3(0, 0, -1.0f),
+		Vec3(0, 0, 1.0f)
+	};
+
+	// Vertices of a cube
+	static Vec3 cube_vertices[] = {
+		Vec3(-1.0f, -1.0f, -1.0f),
+		Vec3( 1.0f, -1.0f, -1.0f),
+		Vec3(-1.0f, -1.0f,  1.0f),
+		Vec3( 1.0f, -1.0f,  1.0f),
+		Vec3(-1.0f,  1.0f, -1.0f),
+		Vec3( 1.0f,  1.0f, -1.0f),
+		Vec3(-1.0f,  1.0f,  1.0f),
+		Vec3( 1.0f,  1.0f,  1.0f)
+	};
+
+	static void sTestHit(const Shape *inShape, Vec3Arg inPosition)
+	{
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		inShape->CollidePoint(inPosition - inShape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK(collector.mHits.size() == 1);
+	}
+
+	static void sTestHit(const NarrowPhaseQuery &inNarrowPhase, RVec3Arg inPosition, const BodyID &inBodyID)
+	{
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		inNarrowPhase.CollidePoint(inPosition, collector);
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == inBodyID);
+	}
+
+	static void sTestMiss(const Shape *inShape, Vec3Arg inPosition)
+	{
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		inShape->CollidePoint(inPosition - inShape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK(collector.mHits.empty());
+	}
+
+	static void sTestMiss(const NarrowPhaseQuery &inNarrowPhase, RVec3Arg inPosition)
+	{
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		inNarrowPhase.CollidePoint(inPosition, collector);
+		CHECK(collector.mHits.empty());
+	}
+
+	TEST_CASE("TestCollidePointVsBox")
+	{
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		ShapeRefC shape = new BoxShape(half_box_size);
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, 0.99f * half_box_size * probe);
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * half_box_size * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsSphere")
+	{
+		const float radius = 0.1f;
+		ShapeRefC shape = new SphereShape(radius);
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, 0.99f * Vec3::sReplicate(radius) * probe);
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * Vec3::sReplicate(radius) * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsCapsule")
+	{
+		const float half_height = 0.2f;
+		const float radius = 0.1f;
+		ShapeRefC shape = new CapsuleShape(half_height, radius);
+
+		// Top hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * radius * probe + Vec3(0, half_height, 0));
+
+		// Center hit
+		sTestHit(shape, Vec3::sZero());
+
+		// Bottom hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * radius * probe + Vec3(0, -half_height, 0));
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * Vec3(radius, half_height + radius, radius) * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsTaperedCapsule")
+	{
+		const float half_height = 0.4f;
+		const float top_radius = 0.1f;
+		const float bottom_radius = 0.2f;
+		TaperedCapsuleShapeSettings settings(half_height, top_radius, bottom_radius);
+		ShapeRefC shape = settings.Create().Get();
+
+		// Top hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * top_radius * probe + Vec3(0, half_height, 0));
+
+		// Center hit
+		sTestHit(shape, Vec3::sZero());
+
+		// Bottom hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * bottom_radius * probe + Vec3(0, -half_height, 0));
+
+		// Top misses
+		sTestMiss(shape, Vec3(0, half_height + top_radius + 0.01f, 0));
+		for (Vec3 probe : xy_probes)
+			sTestMiss(shape, 1.01f * top_radius * probe + Vec3(0, half_height, 0));
+
+		// Bottom misses
+		sTestMiss(shape, Vec3(0, -half_height - bottom_radius - 0.01f, 0));
+		for (Vec3 probe : xy_probes)
+			sTestMiss(shape, 1.01f * bottom_radius * probe + Vec3(0, -half_height, 0));
+	}
+
+	TEST_CASE("TestCollidePointVsCylinder")
+	{
+		const float half_height = 0.2f;
+		const float radius = 0.1f;
+		ShapeRefC shape = new CylinderShape(half_height, radius);
+
+		// Top hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * (radius * probe + Vec3(0, half_height, 0)));
+
+		// Center hit
+		sTestHit(shape, Vec3::sZero());
+
+		// Bottom hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * (radius * probe + Vec3(0, -half_height, 0)));
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * Vec3(radius, half_height, radius) * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsConvexHull")
+	{
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		Vec3 offset(10.0f, 11.0f, 12.0f);
+
+		ConvexHullShapeSettings settings;
+		for (uint i = 0; i < size(cube_vertices); ++i)
+			settings.mPoints.push_back(offset + cube_vertices[i] * half_box_size);
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, offset + 0.99f * half_box_size * probe);
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, offset + 1.01f * half_box_size * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsRotatedTranslated")
+	{
+		Vec3 translation(10.0f, 11.0f, 12.0f);
+		Quat rotation = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.3f * JPH_PI);
+		Mat44 transform = Mat44::sRotationTranslation(rotation, translation);
+
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		RotatedTranslatedShapeSettings settings(translation, rotation, new BoxShape(half_box_size));
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, transform * (0.99f * half_box_size * probe));
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, transform * (1.01f * half_box_size * probe));
+	}
+
+	TEST_CASE("TestCollidePointVsScaled")
+	{
+		Vec3 scale(2.0f, 3.0f, -4.0f);
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		ShapeRefC shape = new ScaledShape(new BoxShape(half_box_size), scale);
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, scale * (0.99f * half_box_size * probe));
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, scale * (1.01f * half_box_size * probe));
+	}
+
+	TEST_CASE("TestCollidePointVsOffsetCenterOfMass")
+	{
+		Vec3 offset(10.0f, 11.0f, 12.0f);
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		OffsetCenterOfMassShapeSettings settings(offset, new BoxShape(half_box_size));
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, 0.99f * half_box_size * probe);
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * half_box_size * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsStaticCompound")
+	{
+		Vec3 translation1(10.0f, 11.0f, 12.0f);
+		Quat rotation1 = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.3f * JPH_PI);
+		Mat44 transform1 = Mat44::sRotationTranslation(rotation1, translation1);
+
+		Vec3 translation2(-1.0f, -2.0f, -3.0f);
+		Quat rotation2 = Quat::sRotation(Vec3(4, 5, 6).Normalized(), 0.2f * JPH_PI);
+		Mat44 transform2 = Mat44::sRotationTranslation(rotation2, translation2);
+
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		ShapeRefC box = new BoxShape(half_box_size);
+
+		StaticCompoundShapeSettings settings;
+		settings.AddShape(translation1, rotation1, box);
+		settings.AddShape(translation2, rotation2, box);
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+		{
+			Vec3 point = 0.99f * half_box_size * probe;
+			sTestHit(shape, transform1 * point);
+			sTestHit(shape, transform2 * point);
+		}
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+		{
+			Vec3 point = 1.01f * half_box_size * probe;
+			sTestMiss(shape, transform1 * point);
+			sTestMiss(shape, transform2 * point);
+		}
+	}
+
+	TEST_CASE("TestCollidePointVsMutableCompound")
+	{
+		Vec3 translation1(10.0f, 11.0f, 12.0f);
+		Quat rotation1 = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.3f * JPH_PI);
+		Mat44 transform1 = Mat44::sRotationTranslation(rotation1, translation1);
+
+		Vec3 translation2(-1.0f, -2.0f, -3.0f);
+		Quat rotation2 = Quat::sRotation(Vec3(4, 5, 6).Normalized(), 0.2f * JPH_PI);
+		Mat44 transform2 = Mat44::sRotationTranslation(rotation2, translation2);
+
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		ShapeRefC box = new BoxShape(half_box_size);
+
+		MutableCompoundShapeSettings settings;
+		settings.AddShape(translation1, rotation1, box);
+		settings.AddShape(translation2, rotation2, box);
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+		{
+			Vec3 point = 0.99f * half_box_size * probe;
+			sTestHit(shape, transform1 * point);
+			sTestHit(shape, transform2 * point);
+		}
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+		{
+			Vec3 point = 1.01f * half_box_size * probe;
+			sTestMiss(shape, transform1 * point);
+			sTestMiss(shape, transform2 * point);
+		}
+	}
+
+	TEST_CASE("TestCollidePointVsMesh")
+	{
+		// Face indices of a cube
+		int indices[][3] = {
+			{ 0, 1, 3 },
+			{ 0, 3, 2 },
+			{ 4, 7, 5 },
+			{ 4, 6, 7 },
+			{ 2, 3, 6 },
+			{ 3, 7, 6 },
+			{ 1, 0, 4 },
+			{ 1, 4, 5 },
+			{ 1, 7, 3 },
+			{ 1, 5, 7 },
+			{ 0, 2, 6 },
+			{ 0, 6, 4 }
+		};
+
+		const int grid_size = 2;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> range(0.1f, 0.3f);
+
+		// Create a grid of closed shapes
+		MeshShapeSettings settings;
+		settings.SetEmbedded();
+		int num_cubes = Cubed(2 * grid_size + 1);
+		settings.mTriangleVertices.reserve(num_cubes * size(cube_vertices));
+		settings.mIndexedTriangles.reserve(num_cubes * size(indices));
+		for (int x = -grid_size; x <= grid_size; ++x)
+			for (int y = -grid_size; y <= grid_size; ++y)
+				for (int z = -grid_size; z <= grid_size; ++z)
+				{
+					Vec3 center((float)x, (float)y, (float)z);
+
+					// Create vertices with randomness
+					uint vtx = (uint)settings.mTriangleVertices.size();
+					settings.mTriangleVertices.resize(vtx + size(cube_vertices));
+					for (uint i = 0; i < size(cube_vertices); ++i)
+					{
+						Vec3 vertex(center + cube_vertices[i] * Vec3(range(random), range(random), range(random)));
+						vertex.StoreFloat3(&settings.mTriangleVertices[vtx + i]);
+					}
+
+					// Flip inside out? (inside out shapes should act the same as normal shapes for CollidePoint)
+					bool flip = (y & 1) == 0;
+
+					// Create face indices
+					uint idx = (uint)settings.mIndexedTriangles.size();
+					settings.mIndexedTriangles.resize(idx + size(indices));
+					for (uint i = 0; i < size(indices); ++i)
+						settings.mIndexedTriangles[idx + i] = IndexedTriangle(vtx + indices[i][0], vtx + indices[i][flip? 2 : 1], vtx + indices[i][flip? 1 : 2]);
+				}
+
+		// Create body with random orientation
+		PhysicsTestContext context;
+		Body &mesh_body = context.CreateBody(&settings, RVec3(Vec3::sRandom(random)), Quat::sRandom(random), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Get the shape
+		ShapeRefC mesh_shape = mesh_body.GetShape();
+
+		// Get narrow phase
+		const NarrowPhaseQuery &narrow_phase = context.GetSystem()->GetNarrowPhaseQuery();
+
+		// Get transform
+		RMat44 body_transform = mesh_body.GetWorldTransform();
+		CHECK(body_transform != RMat44::sIdentity());
+
+		// Test points
+		for (int x = -grid_size; x <= grid_size; ++x)
+			for (int y = -grid_size; y <= grid_size; ++y)
+				for (int z = -grid_size; z <= grid_size; ++z)
+				{
+					Vec3 center((float)x, (float)y, (float)z);
+
+					// The center point should hit
+					sTestHit(mesh_shape, center);
+					sTestHit(narrow_phase, body_transform * center, mesh_body.GetID());
+
+					// Points outside the hull should not hit
+					for (Vec3 probe : cube_probes)
+					{
+						Vec3 point = center + 0.4f * probe;
+						sTestMiss(mesh_shape, point);
+						sTestMiss(narrow_phase, body_transform * point);
+					}
+				}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CollideShapeTests.cpp

@@ -0,0 +1,560 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include "Layers.h"
+
+TEST_SUITE("CollideShapeTests")
+{
+	// Compares CollideShapeResult for two spheres with given positions and radii
+	static void sCompareCollideShapeResultSphere(Vec3Arg inPosition1, float inRadius1, Vec3Arg inPosition2, float inRadius2, const CollideShapeResult &inResult)
+	{
+		// Test if spheres overlap
+		Vec3 delta = inPosition2 - inPosition1;
+		float len = delta.Length();
+		CHECK(len > 0.0f);
+		CHECK(len <= inRadius1 + inRadius2);
+
+		// Calculate points on surface + vector that will push 2 out of collision
+		Vec3 expected_point1 = inPosition1 + delta * (inRadius1 / len);
+		Vec3 expected_point2 = inPosition2 - delta * (inRadius2 / len);
+		Vec3 expected_penetration_axis = delta / len;
+
+		// Get actual results
+		Vec3 penetration_axis = inResult.mPenetrationAxis.Normalized();
+
+		// Compare
+		CHECK_APPROX_EQUAL(expected_point1, inResult.mContactPointOn1);
+		CHECK_APPROX_EQUAL(expected_point2, inResult.mContactPointOn2);
+		CHECK_APPROX_EQUAL(expected_penetration_axis, penetration_axis);
+	}
+
+	// Test CollideShape function for spheres
+	TEST_CASE("TestCollideShapeSphere")
+	{
+		// Locations of test sphere
+		static const RVec3 cPosition1A(10.0f, 11.0f, 12.0f);
+		static const RVec3 cPosition1B(10.0f, 21.0f, 12.0f);
+		static const float cRadius1 = 2.0f;
+
+		// Locations of sphere in the physics system
+		static const RVec3 cPosition2A(13.0f, 11.0f, 12.0f);
+		static const RVec3 cPosition2B(13.0f, 22.0f, 12.0f);
+		static const float cRadius2 = 1.5f;
+
+		// Create sphere to test with (shape 1)
+		Ref<Shape> shape1 = new SphereShape(cRadius1);
+		Mat44 shape1_com = Mat44::sTranslation(shape1->GetCenterOfMass());
+		RMat44 shape1_transform = RMat44::sTranslation(cPosition1A) * Mat44::sRotationX(0.1f * JPH_PI) * shape1_com;
+
+		// Create sphere to collide against (shape 2)
+		PhysicsTestContext c;
+		Body &body2 = c.CreateSphere(cPosition2A, cRadius2, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING);
+
+		// Filters
+		SpecifiedBroadPhaseLayerFilter broadphase_moving_filter(BroadPhaseLayers::MOVING);
+		SpecifiedBroadPhaseLayerFilter broadphase_non_moving_filter(BroadPhaseLayers::NON_MOVING);
+		SpecifiedObjectLayerFilter object_moving_filter(Layers::MOVING);
+		SpecifiedObjectLayerFilter object_non_moving_filter(Layers::NON_MOVING);
+
+		// Collector that fails the test
+		class FailCollideShapeCollector : public CollideShapeCollector
+		{
+		public:
+			virtual void	AddHit(const CollideShapeResult &inResult) override
+			{
+				FAIL("Callback should not be called");
+			}
+		};
+		FailCollideShapeCollector fail_collector;
+
+		// Set settings
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+		settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+
+		// Test against wrong layer
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), fail_collector, broadphase_moving_filter, object_moving_filter);
+
+		// Collector that tests that collision happens at position A
+		class PositionACollideShapeCollector : public CollideShapeCollector
+		{
+		public:
+							PositionACollideShapeCollector(const Body &inBody2) :
+				mBody2(inBody2)
+			{
+			}
+
+			virtual void	AddHit(const CollideShapeResult &inResult) override
+			{
+				CHECK(mBody2.GetID() == GetContext()->mBodyID);
+				sCompareCollideShapeResultSphere(Vec3(cPosition1A), cRadius1, Vec3(cPosition2A), cRadius2, inResult);
+				mWasHit = true;
+			}
+
+			bool			mWasHit = false;
+
+		private:
+			const Body &	mBody2;
+		};
+		PositionACollideShapeCollector position_a_collector(body2);
+
+		// Test collision against correct layer
+		CHECK(!position_a_collector.mWasHit);
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), position_a_collector, broadphase_non_moving_filter, object_non_moving_filter);
+		CHECK(position_a_collector.mWasHit);
+
+		// Now move body to position B
+		c.GetSystem()->GetBodyInterface().SetPositionAndRotation(body2.GetID(), cPosition2B, Quat::sRotation(Vec3::sAxisY(), 0.2f * JPH_PI), EActivation::DontActivate);
+
+		// Test that original position doesn't collide anymore
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), fail_collector, broadphase_non_moving_filter, object_non_moving_filter);
+
+		// Move test shape to position B
+		shape1_transform = RMat44::sTranslation(cPosition1B) * Mat44::sRotationZ(0.3f * JPH_PI) * shape1_com;
+
+		// Test against wrong layer
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), fail_collector, broadphase_moving_filter, object_moving_filter);
+
+		// Callback that tests that collision happens at position B
+		class PositionBCollideShapeCollector : public CollideShapeCollector
+		{
+		public:
+							PositionBCollideShapeCollector(const Body &inBody2) :
+				mBody2(inBody2)
+			{
+			}
+
+			virtual void	Reset() override
+			{
+				CollideShapeCollector::Reset();
+
+				mWasHit = false;
+			}
+
+			virtual void	AddHit(const CollideShapeResult &inResult) override
+			{
+				CHECK(mBody2.GetID() == GetContext()->mBodyID);
+				sCompareCollideShapeResultSphere(Vec3(cPosition1B), cRadius1, Vec3(cPosition2B), cRadius2, inResult);
+				mWasHit = true;
+			}
+
+			bool			mWasHit = false;
+
+		private:
+			const Body &	mBody2;
+		};
+		PositionBCollideShapeCollector position_b_collector(body2);
+
+		// Test collision
+		CHECK(!position_b_collector.mWasHit);
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), position_b_collector, broadphase_non_moving_filter, object_non_moving_filter);
+		CHECK(position_b_collector.mWasHit);
+
+		// Update the physics system (optimizes the broadphase)
+		c.Simulate(c.GetDeltaTime());
+
+		// Test against wrong layer
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), fail_collector, broadphase_moving_filter, object_moving_filter);
+
+		// Test collision again
+		position_b_collector.Reset();
+		CHECK(!position_b_collector.mWasHit);
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), position_b_collector, broadphase_non_moving_filter, object_non_moving_filter);
+		CHECK(position_b_collector.mWasHit);
+	}
+
+	// Test CollideShape function for a (scaled) sphere vs box
+	TEST_CASE("TestCollideShapeSphereVsBox")
+	{
+		PhysicsTestContext c;
+
+		// Create box to collide against (shape 2)
+		// The box is scaled up by a factor 10 in the X axis and then rotated so that the X axis is up
+		BoxShapeSettings box(Vec3::sOne());
+		box.SetEmbedded();
+		ScaledShapeSettings scaled_box(&box, Vec3(10, 1, 1));
+		scaled_box.SetEmbedded();
+		Body &body2 = c.CreateBody(&scaled_box, RVec3(0, 1, 0), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Set settings
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+		settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+
+		{
+			// Create sphere
+			Ref<Shape> normal_sphere = new SphereShape(1.0f);
+
+			// Collect hit with normal sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CollideShape(normal_sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(0, 11, 0)), settings, RVec3::sZero(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-4f);
+			Vec3 pen_axis = result.mPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(pen_axis, Vec3(0, -1, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
+		}
+
+		{
+			// This repeats the same test as above but uses scaling at all levels
+			Ref<Shape> scaled_sphere = new ScaledShape(new SphereShape(0.1f), Vec3::sReplicate(5.0f));
+
+			// Collect hit with scaled sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CollideShape(scaled_sphere, Vec3::sReplicate(2.0f), RMat44::sTranslation(RVec3(0, 11, 0)), settings, RVec3::sZero(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-4f);
+			Vec3 pen_axis = result.mPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(pen_axis, Vec3(0, -1, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
+		}
+	}
+
+	// Test colliding a very long capsule vs a box that is intersecting with the line segment inside the capsule
+	// This particular config reported the wrong penetration due to accuracy problems before
+	TEST_CASE("TestCollideShapeLongCapsuleVsEmbeddedBox")
+	{
+		// Create box
+		Vec3 box_min(-1.0f, -2.0f, 0.5f);
+		Vec3 box_max(2.0f, -0.5f, 3.0f);
+		Ref<RotatedTranslatedShapeSettings> box_settings = new RotatedTranslatedShapeSettings(0.5f * (box_min + box_max), Quat::sIdentity(), new BoxShapeSettings(0.5f * (box_max - box_min)));
+		Ref<Shape> box_shape = box_settings->Create().Get();
+		Mat44 box_transform(Vec4(0.516170502f, -0.803887904f, -0.295520246f, 0.0f), Vec4(0.815010250f, 0.354940295f, 0.458012700f, 0.0f), Vec4(-0.263298869f, -0.477264702f, 0.838386655f, 0.0f), Vec4(-10.2214508f, -18.6808319f, 40.7468987f, 1.0f));
+
+		// Create capsule
+		float capsule_half_height = 75.0f;
+		float capsule_radius = 1.5f;
+		Ref<RotatedTranslatedShapeSettings> capsule_settings = new RotatedTranslatedShapeSettings(Vec3(0, 0, 75), Quat(0.499999970f, -0.499999970f, -0.499999970f, 0.499999970f), new CapsuleShapeSettings(capsule_half_height, capsule_radius));
+		Ref<Shape> capsule_shape = capsule_settings->Create().Get();
+		Mat44 capsule_transform = Mat44::sTranslation(Vec3(-9.68538570f, -18.0328083f, 41.3212280f));
+
+		// Collision settings
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+		settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+		settings.mCollectFacesMode = ECollectFacesMode::NoFaces;
+
+		// Collide the two shapes
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(capsule_shape, box_shape, Vec3::sOne(), Vec3::sOne(), capsule_transform, box_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		// Check that there was a hit
+		CHECK(collector.mHits.size() == 1);
+		const CollideShapeResult &result = collector.mHits.front();
+
+		// Now move the box 1% further than the returned penetration depth and check that it is no longer in collision
+		Vec3 distance_to_move_box = result.mPenetrationAxis.Normalized() * result.mPenetrationDepth;
+		collector.Reset();
+		CHECK(!collector.HadHit());
+		CollisionDispatch::sCollideShapeVsShape(capsule_shape, box_shape, Vec3::sOne(), Vec3::sOne(), capsule_transform, Mat44::sTranslation(1.01f * distance_to_move_box) * box_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+		CHECK(!collector.HadHit());
+
+		// Now check that moving 1% less than the penetration distance makes the shapes still overlap
+		CollisionDispatch::sCollideShapeVsShape(capsule_shape, box_shape, Vec3::sOne(), Vec3::sOne(), capsule_transform, Mat44::sTranslation(0.99f * distance_to_move_box) * box_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+		CHECK(collector.mHits.size() == 1);
+	}
+
+	// Another test case found in practice of a very large oriented box (convex hull) vs a small triangle outside the hull. This should not report a collision
+	TEST_CASE("TestCollideShapeSmallTriangleVsLargeBox")
+	{
+		// Triangle vertices
+		Vec3 v0(-81.5637589f, -126.987244f, -146.771729f);
+		Vec3 v1(-81.8749924f, -127.270691f, -146.544403f);
+		Vec3 v2(-81.6972275f, -127.383545f, -146.773254f);
+
+		// Oriented box vertices
+		Array<Vec3> obox_points = {
+			Vec3(125.932892f, -374.712250f, 364.192169f),
+			Vec3(319.492218f, -73.2614441f, 475.009613f),
+			Vec3(-122.277550f, -152.200287f, 192.441437f),
+			Vec3(71.2817841f, 149.250519f, 303.258881f),
+			Vec3(-77.8921967f, -359.410797f, 678.579712f),
+			Vec3(115.667137f, -57.9600067f, 789.397095f),
+			Vec3(-326.102631f, -136.898834f, 506.828949f),
+			Vec3(-132.543304f, 164.551971f, 617.646362f)
+		};
+		ConvexHullShapeSettings hull_settings(obox_points, 0.0f);
+		RefConst<ConvexShape> convex_hull = StaticCast<ConvexShape>(hull_settings.Create().Get());
+
+		// Create triangle support function
+		TriangleConvexSupport triangle(v0, v1, v2);
+
+		// Create the convex hull support function
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = convex_hull->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+
+		// Triangle is close enough to make GJK report indeterminate
+		Vec3 penetration_axis = Vec3::sAxisX(), point1, point2;
+		EPAPenetrationDepth pen_depth;
+		EPAPenetrationDepth::EStatus status = pen_depth.GetPenetrationDepthStepGJK(*support, support->GetConvexRadius(), triangle, 0.0f, cDefaultCollisionTolerance, penetration_axis, point1, point2);
+		CHECK(status == EPAPenetrationDepth::EStatus::Indeterminate);
+
+		// But there should not be an actual collision
+		CHECK(!pen_depth.GetPenetrationDepthStepEPA(*support, triangle, cDefaultPenetrationTolerance, penetration_axis, point1, point2));
+	}
+
+	// A test case of a triangle that's nearly parallel to a capsule and penetrating it. This one was causing numerical issues.
+	TEST_CASE("TestCollideParallelTriangleVsCapsule")
+	{
+		Vec3 v1(-0.479988575f, -1.36185002f, 0.269966960f);
+		Vec3 v2(-0.104996204f, 0.388152480f, 0.269967079f);
+		Vec3 v3(-0.104996204f, -1.36185002f, 0.269966960f);
+		TriangleShape triangle(v1, v2, v3);
+		triangle.SetEmbedded();
+
+		float capsule_radius = 0.37f;
+		float capsule_half_height = 0.5f;
+		CapsuleShape capsule(capsule_half_height, capsule_radius);
+		capsule.SetEmbedded();
+
+		CollideShapeSettings settings;
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(&triangle, &capsule, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		// The capsule's center is closest to the triangle's edge v2 v3
+		Vec3 capsule_center_to_triangle_v2_v3 = v3;
+		capsule_center_to_triangle_v2_v3.SetY(0); // The penetration axis will be in x, z only because the triangle is parallel to the capsule axis
+		float capsule_center_to_triangle_v2_v3_len = capsule_center_to_triangle_v2_v3.Length();
+		Vec3 expected_penetration_axis = -capsule_center_to_triangle_v2_v3 / capsule_center_to_triangle_v2_v3_len;
+		float expected_penetration_depth = capsule_radius - capsule_center_to_triangle_v2_v3_len;
+
+		CHECK(collector.mHits.size() == 1);
+		const CollideShapeResult &hit = collector.mHits[0];
+		Vec3 actual_penetration_axis = hit.mPenetrationAxis.Normalized();
+		float actual_penetration_depth = hit.mPenetrationDepth;
+
+		CHECK_APPROX_EQUAL(actual_penetration_axis, expected_penetration_axis);
+		CHECK_APPROX_EQUAL(actual_penetration_depth, expected_penetration_depth);
+	}
+
+	// A test case of a triangle that's nearly parallel to a capsule and penetrating it. This one was causing numerical issues.
+	TEST_CASE("TestCollideParallelTriangleVsCapsule2")
+	{
+		Vec3 v1(-0.0904417038f, -4.72410202f, 0.307858467f);
+		Vec3 v2(-0.0904417038f, 5.27589798f, 0.307857513f);
+		Vec3 v3(9.90955830f, 5.27589798f, 0.307864189f);
+		TriangleShape triangle(v1, v2, v3);
+		triangle.SetEmbedded();
+
+		float capsule_radius = 0.42f;
+		float capsule_half_height = 0.675f;
+		CapsuleShape capsule(capsule_half_height, capsule_radius);
+		capsule.SetEmbedded();
+
+		CollideShapeSettings settings;
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(&triangle, &capsule, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		// The capsule intersects with the triangle and the closest point is in the interior of the triangle
+		Vec3 expected_penetration_axis = Vec3(0, 0, -1); // Triangle is in the XY plane so the normal is Z
+		float expected_penetration_depth = capsule_radius - v1.GetZ();
+
+		CHECK(collector.mHits.size() == 1);
+		const CollideShapeResult &hit = collector.mHits[0];
+		Vec3 actual_penetration_axis = hit.mPenetrationAxis.Normalized();
+		float actual_penetration_depth = hit.mPenetrationDepth;
+
+		CHECK_APPROX_EQUAL(actual_penetration_axis, expected_penetration_axis);
+		CHECK_APPROX_EQUAL(actual_penetration_depth, expected_penetration_depth);
+	}
+
+	// A test case of a triangle that's nearly parallel to a capsule and almost penetrating it. This one was causing numerical issues.
+	TEST_CASE("TestCollideParallelTriangleVsCapsule3")
+	{
+		Vec3 v1(-0.474807739f, 17.2921791f, 0.212532043f);
+		Vec3 v2(-0.474807739f, -2.70782185f, 0.212535858f);
+		Vec3 v3(-0.857490540f, -2.70782185f, -0.711341858f);
+		TriangleShape triangle(v1, v2, v3);
+		triangle.SetEmbedded();
+
+		float capsule_radius = 0.5f;
+		float capsule_half_height = 0.649999976f;
+		CapsuleShape capsule(capsule_half_height, capsule_radius);
+		capsule.SetEmbedded();
+
+		CollideShapeSettings settings;
+		settings.mMaxSeparationDistance = 0.120000005f;
+		ClosestHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(&capsule, &triangle, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		CHECK(collector.HadHit());
+		Vec3 expected_normal = (v2 - v1).Cross(v3 - v1).Normalized();
+		Vec3 actual_normal = -collector.mHit.mPenetrationAxis.Normalized();
+		CHECK_APPROX_EQUAL(actual_normal, expected_normal, 1.0e-6f);
+		float expected_penetration_depth = capsule.GetRadius() + v1.Dot(expected_normal);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, expected_penetration_depth, 1.0e-6f);
+	}
+
+	// A test case of a triangle that's nearly parallel to a cylinder and is just penetrating it. This one was causing numerical issues. See issue #1008.
+	TEST_CASE("TestCollideParallelTriangleVsCylinder")
+	{
+		CylinderShape cylinder(0.85f, 0.25f, 0.02f);
+		cylinder.SetEmbedded();
+
+		Mat44 cylinder_transform = Mat44::sTranslation(Vec3(-42.8155518f, -4.32299995f, 12.1734285f));
+
+		CollideShapeSettings settings;
+		settings.mMaxSeparationDistance = 0.001f;
+		ClosestHitCollisionCollector<CollideShapeCollector> collector;
+		CollideConvexVsTriangles c(&cylinder, Vec3::sOne(), Vec3::sOne(), cylinder_transform, Mat44::sIdentity(), SubShapeID(), settings, collector);
+
+		Vec3 v0(-42.7954292f, -0.647318780f, 12.4227943f);
+		Vec3 v1(-29.9111290f, -0.647318780f, 12.4227943f);
+		Vec3 v2(-42.7954292f, -4.86970234f, 12.4227943f);
+		c.Collide(v0, v1, v2, 0, SubShapeID());
+
+		// Check there was a hit
+		CHECK(collector.HadHit());
+		CHECK(collector.mHit.mPenetrationDepth < 1.0e-4f);
+		CHECK(collector.mHit.mPenetrationAxis.Normalized().IsClose(Vec3::sAxisZ()));
+	}
+
+	// A test case of a box and a convex hull that are nearly touching and that should return a contact with correct normal because the collision settings specify a max separation distance. This was producing the wrong normal.
+	TEST_CASE("BoxVsConvexHullNoConvexRadius")
+	{
+		const float separation_distance = 0.001f;
+		const float box_separation_from_hull = 0.5f * separation_distance;
+		const float hull_height = 0.25f;
+
+		// Box with no convex radius
+		Ref<BoxShapeSettings> box_settings = new BoxShapeSettings(Vec3(0.25f, 0.75f, 0.375f), 0.0f);
+		Ref<Shape> box_shape = box_settings->Create().Get();
+
+		// Convex hull (also a box) with no convex radius
+		Vec3 hull_points[] =
+		{
+			Vec3(-2.5f, -hull_height, -1.5f),
+			Vec3(-2.5f, hull_height, -1.5f),
+			Vec3(2.5f, -hull_height, -1.5f),
+			Vec3(-2.5f, -hull_height, 1.5f),
+			Vec3(-2.5f, hull_height, 1.5f),
+			Vec3(2.5f, hull_height, -1.5f),
+			Vec3(2.5f, -hull_height, 1.5f),
+			Vec3(2.5f, hull_height, 1.5f)
+		};
+		Ref<ConvexHullShapeSettings> hull_settings = new ConvexHullShapeSettings(hull_points, 8, 0.0f);
+		Ref<Shape> hull_shape = hull_settings->Create().Get();
+
+		float angle = 0.0f;
+		for (int i = 0; i < 481; ++i)
+		{
+			// Slowly rotate both box and convex hull
+			angle += DegreesToRadians(45.0f) / 60.0f;
+			Mat44 hull_transform = Mat44::sRotationY(angle);
+			const Mat44 box_local_translation = Mat44::sTranslation(Vec3(0.1f, 1.0f + box_separation_from_hull, -0.5f));
+			const Mat44 box_local_rotation = Mat44::sRotationY(DegreesToRadians(-45.0f));
+			const Mat44 box_local_transform = box_local_translation * box_local_rotation;
+			const Mat44 box_transform = hull_transform * box_local_transform;
+
+			CollideShapeSettings settings;
+			settings.mMaxSeparationDistance = separation_distance;
+			ClosestHitCollisionCollector<CollideShapeCollector> collector;
+			CollisionDispatch::sCollideShapeVsShape(box_shape, hull_shape, Vec3::sOne(), Vec3::sOne(), box_transform, hull_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+			// Check that there was a hit and that the contact normal is correct
+			CHECK(collector.HadHit());
+			const CollideShapeResult &hit = collector.mHit;
+			CHECK_APPROX_EQUAL(hit.mContactPointOn1.GetY(), hull_height + box_separation_from_hull, 1.0e-3f);
+			CHECK_APPROX_EQUAL(hit.mContactPointOn2.GetY(), hull_height);
+			CHECK_APPROX_EQUAL(hit.mPenetrationAxis.NormalizedOr(Vec3::sZero()), -Vec3::sAxisY(), 1.0e-3f);
+		}
+
+		CHECK(angle >= 2.0f * JPH_PI);
+	}
+
+	// This test checks extreme values of the max separation distance and how it affects ConvexShape::sCollideConvexVsConvex
+	// See: https://github.com/jrouwe/JoltPhysics/discussions/1379
+	TEST_CASE("TestBoxVsSphereLargeSeparationDistance")
+	{
+		constexpr float cRadius = 1.0f;
+		constexpr float cHalfExtent = 10.0f;
+		RefConst<Shape> sphere_shape = new SphereShape(cRadius);
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(cHalfExtent));
+		float distances[] = { 0.0f, 0.5f, 1.0f, 5.0f, 10.0f, 50.0f, 100.0f, 500.0f, 1000.0f, 5000.0f, 10000.0f };
+		for (float x : distances)
+			for (float max_separation : distances)
+			{
+				CollideShapeSettings collide_settings;
+				collide_settings.mMaxSeparationDistance = max_separation;
+				ClosestHitCollisionCollector<CollideShapeCollector> collector;
+				CollisionDispatch::sCollideShapeVsShape(box_shape, sphere_shape, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sTranslation(Vec3(x, 0, 0)), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector);
+
+				float expected_penetration = cHalfExtent - (x - cRadius);
+				if (collector.HadHit())
+					CHECK_APPROX_EQUAL(expected_penetration, collector.mHit.mPenetrationDepth, 1.0e-3f);
+				else
+					CHECK(expected_penetration < -max_separation);
+			}
+	}
+
+	// This test case checks extreme values of the max separation distance and how it affects CollideConvexVsTriangles::Collide
+	// See: https://github.com/jrouwe/JoltPhysics/discussions/1379
+	TEST_CASE("TestTriangleVsBoxLargeSeparationDistance")
+	{
+		constexpr float cTriangleX = -0.1f;
+		constexpr float cHalfExtent = 10.0f;
+		RefConst<Shape> triangle_shape = new TriangleShape(Vec3(cTriangleX, -10, 10), Vec3(cTriangleX, -10, -10), Vec3(cTriangleX, 10, 0));
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(cHalfExtent));
+		float distances[] = { 0.0f, 0.5f, 1.0f, 5.0f, 10.0f, 50.0f, 100.0f, 500.0f, 1000.0f, 5000.0f, 10000.0f };
+		for (float x : distances)
+			for (float max_separation : distances)
+			{
+				CollideShapeSettings collide_settings;
+				collide_settings.mMaxSeparationDistance = max_separation;
+				ClosestHitCollisionCollector<CollideShapeCollector> collector;
+				CollisionDispatch::sCollideShapeVsShape(triangle_shape, box_shape, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sTranslation(Vec3(x, 0, 0)), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector);
+
+				float expected_penetration = cTriangleX - (x - cHalfExtent);
+				if (collector.HadHit())
+					CHECK_APPROX_EQUAL(expected_penetration, collector.mHit.mPenetrationDepth, 1.0e-3f);
+				else
+				{
+					CHECK(expected_penetration < -max_separation);
+					CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Vec3::sAxisX(), 1.0e-5f);
+				}
+			}
+	}
+
+	TEST_CASE("TestCollideTriangleVsTriangle")
+	{
+		constexpr float cPenetration = 0.01f;
+
+		// A triangle centered around the origin in the XZ plane
+		RefConst<Shape> t1 = new TriangleShape(Vec3(-1, 0, 1), Vec3(1, 0, 1), Vec3(0, 0, -1));
+
+		// A triangle in the XY plane with its tip just pointing in the origin
+		RefConst<Shape> t2 = new TriangleShape(Vec3(-1, 1, 0), Vec3(1, 1, 0), Vec3(0, -cPenetration, 0));
+
+		CollideShapeSettings collide_settings;
+		ClosestHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(t1, t2, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector);
+
+		CHECK(collector.HadHit());
+		CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3::sZero());
+		CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(0, -cPenetration, 0));
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, cPenetration);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(0, 1, 0));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CollisionGroupTests.cpp

@@ -0,0 +1,85 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include "Layers.h"
+
+TEST_SUITE("CollisionGroupTests")
+{
+	TEST_CASE("TestCollisionGroup1")
+	{
+		// Test group filter with no sub groups
+		Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+		// Check that doesn't collide with self
+		CollisionGroup g1(group_filter, 0, 0);
+		CHECK(!g1.CanCollide(g1));
+
+		// Check that collides with other group
+		CollisionGroup g2(group_filter, 1, 0);
+		CHECK(g1.CanCollide(g2));
+		CHECK(g2.CanCollide(g1));
+	}
+
+	TEST_CASE("TestCollisionGroup2")
+	{
+		// Test group filter with no sub groups
+		Ref<GroupFilterTable> group_filter1 = new GroupFilterTable(10);
+		Ref<GroupFilterTable> group_filter2 = new GroupFilterTable(10);
+
+		// Disable some pairs
+		using SubGroupPair = pair<CollisionGroup::SubGroupID, CollisionGroup::SubGroupID>;
+		Array<SubGroupPair> pairs = {
+			SubGroupPair(CollisionGroup::SubGroupID(1), CollisionGroup::SubGroupID(2)),
+			SubGroupPair(CollisionGroup::SubGroupID(9), CollisionGroup::SubGroupID(5)),
+			SubGroupPair(CollisionGroup::SubGroupID(3), CollisionGroup::SubGroupID(7)),
+			SubGroupPair(CollisionGroup::SubGroupID(6), CollisionGroup::SubGroupID(1)),
+			SubGroupPair(CollisionGroup::SubGroupID(8), CollisionGroup::SubGroupID(1))
+		};
+		for (const SubGroupPair &p : pairs)
+		{
+			group_filter1->DisableCollision(p.first, p.second);
+			group_filter2->DisableCollision(p.first, p.second);
+		}
+
+		for (CollisionGroup::SubGroupID i = 0; i < 10; ++i)
+			for (CollisionGroup::SubGroupID j = 0; j < 10; ++j)
+			{
+				// Check that doesn't collide with self
+				CollisionGroup g1(group_filter1, 0, i);
+				CHECK(!g1.CanCollide(g1));
+
+				// Same filter, same group, check if pairs collide
+				CollisionGroup g2(group_filter1, 0, j);
+				if (i == j
+					|| find(pairs.begin(), pairs.end(), SubGroupPair(i, j)) != pairs.end()
+					|| find(pairs.begin(), pairs.end(), SubGroupPair(j, i)) != pairs.end())
+				{
+					CHECK(!g1.CanCollide(g2));
+					CHECK(!g2.CanCollide(g1));
+				}
+				else
+				{
+					CHECK(g1.CanCollide(g2));
+					CHECK(g2.CanCollide(g1));
+				}
+
+				// Using different group always collides
+				CollisionGroup g3(group_filter1, 1, j);
+				CHECK(g1.CanCollide(g3));
+				CHECK(g3.CanCollide(g1));
+
+				// Using different filter with equal group should not collide
+				CollisionGroup g4(group_filter2, 0, j);
+				CHECK(!g1.CanCollide(g4));
+				CHECK(!g4.CanCollide(g1));
+
+				// Using different filter with non-equal group should collide
+				CollisionGroup g5(group_filter2, 1, j);
+				CHECK(g1.CanCollide(g5));
+				CHECK(g5.CanCollide(g1));
+			}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ContactListenerTests.cpp

@@ -0,0 +1,725 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+
+TEST_SUITE("ContactListenerTests")
+{
+	// Gravity vector
+	const Vec3 cGravity = Vec3(0.0f, -9.81f, 0.0f);
+
+	using LogEntry = LoggingContactListener::LogEntry;
+	using EType = LoggingContactListener::EType;
+
+	// Let a sphere bounce on the floor with restitution = 1
+	TEST_CASE("TestContactListenerElastic")
+	{
+		PhysicsTestContext c;
+
+		const float cSimulationTime = 1.0f;
+		const RVec3 cDistanceTraveled = c.PredictPosition(RVec3::sZero(), Vec3::sZero(), cGravity, cSimulationTime);
+		const float cFloorHitEpsilon = 1.0e-4f; // Apply epsilon so that we're sure that the collision algorithm will find a collision
+		const RVec3 cFloorHitPos(0.0f, 1.0f - cFloorHitEpsilon, 0.0f); // Sphere with radius 1 will hit floor when 1 above the floor
+		const RVec3 cInitialPos = cFloorHitPos - cDistanceTraveled;
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Create sphere
+		Body &floor = c.CreateFloor();
+		Body &body = c.CreateSphere(cInitialPos, 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+		body.SetRestitution(1.0f);
+		CHECK(floor.GetID() < body.GetID());
+
+		// Simulate until at floor
+		c.Simulate(cSimulationTime);
+
+		// Assert collision not yet processed
+		CHECK(listener.GetEntryCount() == 0);
+
+		// Simulate one more step to process the collision
+		c.Simulate(c.GetDeltaTime());
+
+		// We expect a validate and a contact point added message
+		CHECK(listener.GetEntryCount() == 2);
+		if (listener.GetEntryCount() == 2)
+		{
+			// Check validate callback
+			const LogEntry &validate = listener.GetEntry(0);
+			CHECK(validate.mType == EType::Validate);
+			CHECK(validate.mBody1 == body.GetID()); // Dynamic body should always be the 1st
+			CHECK(validate.mBody2 == floor.GetID());
+
+			// Check add contact callback
+			const LogEntry &add_contact = listener.GetEntry(1);
+			CHECK(add_contact.mType == EType::Add);
+			CHECK(add_contact.mBody1 == floor.GetID()); // Lowest ID should be first
+			CHECK(add_contact.mManifold.mSubShapeID1.GetValue() == SubShapeID().GetValue()); // Floor doesn't have any sub shapes
+			CHECK(add_contact.mBody2 == body.GetID()); // Highest ID should be second
+			CHECK(add_contact.mManifold.mSubShapeID2.GetValue() == SubShapeID().GetValue()); // Sphere doesn't have any sub shapes
+			CHECK_APPROX_EQUAL(add_contact.mManifold.mWorldSpaceNormal, Vec3::sAxisY()); // Normal should move body 2 out of collision
+			CHECK(add_contact.mManifold.mRelativeContactPointsOn1.size() == 1);
+			CHECK(add_contact.mManifold.mRelativeContactPointsOn2.size() == 1);
+			CHECK(add_contact.mManifold.GetWorldSpaceContactPointOn1(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+			CHECK(add_contact.mManifold.GetWorldSpaceContactPointOn2(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+		}
+		listener.Clear();
+
+		// Simulate same time, with a fully elastic body we should reach the initial position again
+		c.Simulate(cSimulationTime);
+
+		// We should only have a remove contact point
+		CHECK(listener.GetEntryCount() == 1);
+		if (listener.GetEntryCount() == 1)
+		{
+			// Check remove contact callback
+			const LogEntry &remove = listener.GetEntry(0);
+			CHECK(remove.mType == EType::Remove);
+			CHECK(remove.mBody1 == floor.GetID()); // Lowest ID should be first
+			CHECK(remove.mBody2 == body.GetID()); // Highest ID should be second
+		}
+	}
+
+	// Let a sphere fall on the floor with restitution = 0, then give it horizontal velocity, then take it away from the floor
+	TEST_CASE("TestContactListenerInelastic")
+	{
+		PhysicsTestContext c;
+
+		const float cSimulationTime = 1.0f;
+		const RVec3 cDistanceTraveled = c.PredictPosition(RVec3::sZero(), Vec3::sZero(), cGravity, cSimulationTime);
+		const float cFloorHitEpsilon = 1.0e-4f; // Apply epsilon so that we're sure that the collision algorithm will find a collision
+		const RVec3 cFloorHitPos(0.0f, 1.0f - cFloorHitEpsilon, 0.0f); // Sphere with radius 1 will hit floor when 1 above the floor
+		const RVec3 cInitialPos = cFloorHitPos - cDistanceTraveled;
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Create sphere
+		Body &floor = c.CreateFloor();
+		Body &body = c.CreateSphere(cInitialPos, 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+		body.SetRestitution(0.0f);
+		body.SetAllowSleeping(false);
+		CHECK(floor.GetID() < body.GetID());
+
+		// Simulate until at floor
+		c.Simulate(cSimulationTime);
+
+		// Assert collision not yet processed
+		CHECK(listener.GetEntryCount() == 0);
+
+		// Simulate one more step to process the collision
+		c.Simulate(c.GetDeltaTime());
+		CHECK_APPROX_EQUAL(body.GetPosition(), cFloorHitPos, cPenetrationSlop);
+
+		// We expect a validate and a contact point added message
+		CHECK(listener.GetEntryCount() == 2);
+		if (listener.GetEntryCount() == 2)
+		{
+			// Check validate callback
+			const LogEntry &validate = listener.GetEntry(0);
+			CHECK(validate.mType == EType::Validate);
+			CHECK(validate.mBody1 == body.GetID()); // Dynamic body should always be the 1st
+			CHECK(validate.mBody2 == floor.GetID());
+
+			// Check add contact callback
+			const LogEntry &add_contact = listener.GetEntry(1);
+			CHECK(add_contact.mType == EType::Add);
+			CHECK(add_contact.mBody1 == floor.GetID()); // Lowest ID first
+			CHECK(add_contact.mManifold.mSubShapeID1.GetValue() == SubShapeID().GetValue()); // Floor doesn't have any sub shapes
+			CHECK(add_contact.mBody2 == body.GetID()); // Highest ID second
+			CHECK(add_contact.mManifold.mSubShapeID2.GetValue() == SubShapeID().GetValue()); // Sphere doesn't have any sub shapes
+			CHECK_APPROX_EQUAL(add_contact.mManifold.mWorldSpaceNormal, Vec3::sAxisY()); // Normal should move body 2 out of collision
+			CHECK(add_contact.mManifold.mRelativeContactPointsOn1.size() == 1);
+			CHECK(add_contact.mManifold.mRelativeContactPointsOn2.size() == 1);
+			CHECK(add_contact.mManifold.GetWorldSpaceContactPointOn1(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+			CHECK(add_contact.mManifold.GetWorldSpaceContactPointOn2(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+		}
+		listener.Clear();
+
+		// Simulate 10 steps
+		c.Simulate(10 * c.GetDeltaTime());
+		CHECK_APPROX_EQUAL(body.GetPosition(), cFloorHitPos, cPenetrationSlop);
+
+		// We're not moving, we should have persisted contacts only
+		CHECK(listener.GetEntryCount() == 10);
+		for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+		{
+			// Check persist callback
+			const LogEntry &persist_contact = listener.GetEntry(i);
+			CHECK(persist_contact.mType == EType::Persist);
+			CHECK(persist_contact.mBody1 == floor.GetID()); // Lowest ID first
+			CHECK(persist_contact.mManifold.mSubShapeID1.GetValue() == SubShapeID().GetValue()); // Floor doesn't have any sub shapes
+			CHECK(persist_contact.mBody2 == body.GetID()); // Highest ID second
+			CHECK(persist_contact.mManifold.mSubShapeID2.GetValue() == SubShapeID().GetValue()); // Sphere doesn't have any sub shapes
+			CHECK_APPROX_EQUAL(persist_contact.mManifold.mWorldSpaceNormal, Vec3::sAxisY()); // Normal should move body 2 out of collision
+			CHECK(persist_contact.mManifold.mRelativeContactPointsOn1.size() == 1);
+			CHECK(persist_contact.mManifold.mRelativeContactPointsOn2.size() == 1);
+			CHECK(persist_contact.mManifold.GetWorldSpaceContactPointOn1(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+			CHECK(persist_contact.mManifold.GetWorldSpaceContactPointOn2(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+		}
+		listener.Clear();
+
+		// Make the body able to go to sleep
+		body.SetAllowSleeping(true);
+
+		// Let the body go to sleep
+		c.Simulate(1.0f);
+		CHECK_APPROX_EQUAL(body.GetPosition(), cFloorHitPos, cPenetrationSlop);
+
+		// Check it went to sleep and that we received a contact removal callback
+		CHECK(!body.IsActive());
+		CHECK(listener.GetEntryCount() > 0);
+		for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+		{
+			// Check persist / removed callbacks
+			const LogEntry &entry = listener.GetEntry(i);
+			CHECK(entry.mBody1 == floor.GetID());
+			CHECK(entry.mBody2 == body.GetID());
+			CHECK(entry.mType == ((i == listener.GetEntryCount() - 1)? EType::Remove : EType::Persist)); // The last entry should remove the contact as the body went to sleep
+		}
+		listener.Clear();
+
+		// Wake the body up again
+		c.GetBodyInterface().ActivateBody(body.GetID());
+		CHECK(body.IsActive());
+
+		// Simulate 1 time step to detect the collision with the floor again
+		c.SimulateSingleStep();
+
+		// Check that the contact got readded
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), body.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), body.GetID()));
+		listener.Clear();
+
+		// Prevent body from going to sleep again
+		body.SetAllowSleeping(false);
+
+		// Make the sphere move horizontal
+		body.SetLinearVelocity(Vec3::sAxisX());
+
+		// Simulate 10 steps
+		c.Simulate(10 * c.GetDeltaTime());
+
+		// We should have 10 persisted contacts events
+		int validate = 0;
+		int persisted = 0;
+		for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+		{
+			const LogEntry &entry = listener.GetEntry(i);
+			switch (entry.mType)
+			{
+			case EType::Validate:
+				++validate;
+				break;
+
+			case EType::Persist:
+				// Check persist callback
+				CHECK(entry.mBody1 == floor.GetID()); // Lowest ID first
+				CHECK(entry.mManifold.mSubShapeID1.GetValue() == SubShapeID().GetValue()); // Floor doesn't have any sub shapes
+				CHECK(entry.mBody2 == body.GetID()); // Highest ID second
+				CHECK(entry.mManifold.mSubShapeID2.GetValue() == SubShapeID().GetValue()); // Sphere doesn't have any sub shapes
+				CHECK_APPROX_EQUAL(entry.mManifold.mWorldSpaceNormal, Vec3::sAxisY()); // Normal should move body 2 out of collision
+				CHECK(entry.mManifold.mRelativeContactPointsOn1.size() == 1);
+				CHECK(entry.mManifold.mRelativeContactPointsOn2.size() == 1);
+				CHECK(abs(entry.mManifold.GetWorldSpaceContactPointOn1(0).GetY()) < cPenetrationSlop);
+				CHECK(abs(entry.mManifold.GetWorldSpaceContactPointOn2(0).GetY()) < cPenetrationSlop);
+				++persisted;
+				break;
+
+			case EType::Add:
+			case EType::Remove:
+			default:
+				CHECK(false); // Unexpected event
+			}
+		}
+		CHECK(validate <= 10); // We may receive extra validate callbacks when the object is moving
+		CHECK(persisted == 10);
+		listener.Clear();
+
+		// Move the sphere away from the floor
+		c.GetBodyInterface().SetPosition(body.GetID(), cInitialPos, EActivation::Activate);
+
+		// Simulate 10 steps
+		c.Simulate(10 * c.GetDeltaTime());
+
+		// We should only have a remove contact point
+		CHECK(listener.GetEntryCount() == 1);
+		if (listener.GetEntryCount() == 1)
+		{
+			// Check remove contact callback
+			const LogEntry &remove = listener.GetEntry(0);
+			CHECK(remove.mType == EType::Remove);
+			CHECK(remove.mBody1 == floor.GetID()); // Lowest ID first
+			CHECK(remove.mBody2 == body.GetID()); // Highest ID second
+		}
+	}
+
+	TEST_CASE("TestWereBodiesInContact")
+	{
+		for (int sign = -1; sign <= 1; sign += 2)
+		{
+			PhysicsTestContext c;
+
+			PhysicsSystem *s = c.GetSystem();
+			BodyInterface &bi = c.GetBodyInterface();
+
+			Body &floor = c.CreateFloor();
+
+			// Two spheres at a distance so that when one sphere leaves the floor the body can still be touching the floor with the other sphere
+			Ref<StaticCompoundShapeSettings> compound_shape = new StaticCompoundShapeSettings;
+			compound_shape->AddShape(Vec3(-2, 0, 0), Quat::sIdentity(), new SphereShape(1));
+			compound_shape->AddShape(Vec3(2, 0, 0), Quat::sIdentity(), new SphereShape(1));
+			Body &body = *bi.CreateBody(BodyCreationSettings(compound_shape, RVec3(0, 0.999f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+			bi.AddBody(body.GetID(), EActivation::Activate);
+
+			class ContactListenerImpl : public ContactListener
+			{
+			public:
+								ContactListenerImpl(PhysicsSystem *inSystem) : mSystem(inSystem) { }
+
+				virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					++mAdded;
+				}
+
+				virtual void	OnContactRemoved(const SubShapeIDPair &inSubShapePair) override
+				{
+					++mRemoved;
+					mWasInContact = mSystem->WereBodiesInContact(inSubShapePair.GetBody1ID(), inSubShapePair.GetBody2ID());
+					CHECK(mWasInContact == mSystem->WereBodiesInContact(inSubShapePair.GetBody2ID(), inSubShapePair.GetBody1ID())); // Returned value should be the same regardless of order
+				}
+
+				int				GetAddCount() const
+				{
+					return mAdded - mRemoved;
+				}
+
+				void			Reset()
+				{
+					mAdded = 0;
+					mRemoved = 0;
+					mWasInContact = false;
+				}
+
+				PhysicsSystem *	mSystem;
+
+				int				mAdded = 0;
+
+				int				mRemoved = 0;
+				bool			mWasInContact = false;
+			};
+
+			// Set listener
+			ContactListenerImpl listener(s);
+			s->SetContactListener(&listener);
+
+			// If the simulation hasn't run yet, we can't be in contact
+			CHECK(!s->WereBodiesInContact(floor.GetID(), body.GetID()));
+
+			// Step the simulation to allow detecting the contact
+			c.SimulateSingleStep();
+
+			// Should be in contact now
+			CHECK(s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == 1);
+			listener.Reset();
+
+			// Impulse on one side
+			bi.AddImpulse(body.GetID(), Vec3(0, 10000, 0), RVec3(Real(-sign * 2), 0, 0));
+			c.SimulateSingleStep(); // One step to detach from the ground (but starts penetrating so will not send a remove callback)
+			CHECK(listener.GetAddCount() == 0);
+			c.SimulateSingleStep(); // One step to get contact remove callback
+
+			// Should still be in contact
+			// Note that we may get a remove and an add callback because manifold reduction has combined the collision with both spheres into 1 contact manifold.
+			// At that point it has to select one of the sub shapes for the contact and if that sub shape no longer collides we get a remove for this sub shape and then an add callback for the other sub shape.
+			CHECK(s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == 0);
+			CHECK((listener.mRemoved == 0 || listener.mWasInContact));
+			listener.Reset();
+
+			// Impulse on the other side
+			bi.AddImpulse(body.GetID(), Vec3(0, 10000, 0), RVec3(Real(sign * 2), 0, 0));
+			c.SimulateSingleStep(); // One step to detach from the ground (but starts penetrating so will not send a remove callback)
+			CHECK(listener.GetAddCount() == 0);
+			c.SimulateSingleStep(); // One step to get contact remove callback
+
+			// Should no longer be in contact
+			CHECK(!s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(!s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == -1);
+			CHECK((listener.mRemoved == 1 && !listener.mWasInContact));
+		}
+	}
+
+	TEST_CASE("TestSurfaceVelocity")
+	{
+		PhysicsTestContext c;
+
+		Body &floor = c.CreateBox(RVec3(0, -1, 0), Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(10.0f)), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(100.0f, 1.0f, 100.0f));
+		floor.SetFriction(1.0f);
+
+		for (int iteration = 0; iteration < 2; ++iteration)
+		{
+			Body &box = c.CreateBox(RVec3(0, 0.999f, 0), Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(30.0f)), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sOne());
+			box.SetFriction(1.0f);
+
+			// Contact listener sets a constant surface velocity
+			class ContactListenerImpl : public ContactListener
+			{
+			public:
+								ContactListenerImpl(Body &inFloor, Body &inBox) : mFloor(inFloor), mBox(inBox) { }
+
+				virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					// Ensure that the body order is as expected
+					JPH_ASSERT(inBody1.GetID() == mFloor.GetID() || inBody2.GetID() == mBox.GetID());
+
+					// Calculate the relative surface velocity
+					ioSettings.mRelativeLinearSurfaceVelocity = -(inBody1.GetRotation() * mLocalSpaceLinearVelocity);
+					ioSettings.mRelativeAngularSurfaceVelocity = -(inBody1.GetRotation() * mLocalSpaceAngularVelocity);
+				}
+
+				virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+				}
+
+				Body &			mFloor;
+				Body &			mBox;
+				Vec3			mLocalSpaceLinearVelocity;
+				Vec3			mLocalSpaceAngularVelocity;
+			};
+
+			// Set listener
+			ContactListenerImpl listener(floor, box);
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Set linear velocity or angular velocity depending on the iteration
+			listener.mLocalSpaceLinearVelocity = iteration == 0? Vec3(0, 0, -2.0f) : Vec3::sZero();
+			listener.mLocalSpaceAngularVelocity = iteration == 0? Vec3::sZero() : Vec3(0, DegreesToRadians(30.0f), 0);
+
+			// Simulate
+			c.Simulate(5.0f);
+
+			// Check that the box is moving with the correct linear/angular velocity
+			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), floor.GetRotation() * listener.mLocalSpaceLinearVelocity, 0.005f);
+			CHECK_APPROX_EQUAL(box.GetAngularVelocity(), floor.GetRotation() * listener.mLocalSpaceAngularVelocity, 1.0e-4f);
+		}
+	}
+
+	static float sGetInvMassScale(const Body &inBody)
+	{
+		uint64 ud = inBody.GetUserData();
+		int index = ((ud & 1) != 0? (ud >> 1) : (ud >> 3)) & 0b11;
+		float mass_overrides[] = { 1.0f, 0.0f, 0.5f, 2.0f };
+		return mass_overrides[index];
+	}
+
+	TEST_CASE("TestMassOverride")
+	{
+		for (EMotionType m1 = EMotionType::Static; m1 <= EMotionType::Dynamic; m1 = EMotionType((int)m1 + 1))
+			for (EMotionType m2 = EMotionType::Static; m2 <= EMotionType::Dynamic; m2 = EMotionType((int)m2 + 1))
+				if (m1 != EMotionType::Static || m2 != EMotionType::Static)
+					for (int i = 0; i < 16; ++i)
+					{
+						PhysicsTestContext c;
+						c.ZeroGravity();
+
+						const float cInitialVelocity1 = m1 != EMotionType::Static? 3.0f : 0.0f;
+						const float cInitialVelocity2 = m2 != EMotionType::Static? -4.0f : 0.0f;
+
+						// Create two spheres on a collision course
+						BodyCreationSettings bcs(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), m1, m1 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING);
+						bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+						bcs.mMassPropertiesOverride.mMass = 1.0f;
+						bcs.mRestitution = 1.0f;
+						bcs.mLinearDamping = 0.0f;
+						bcs.mPosition = RVec3(-2, 0, 0);
+						bcs.mLinearVelocity = Vec3(cInitialVelocity1, 0, 0);
+						bcs.mUserData = i << 1;
+						Body &body1 = *c.GetBodyInterface().CreateBody(bcs);
+						c.GetBodyInterface().AddBody(body1.GetID(), EActivation::Activate);
+
+						bcs.mMotionType = m2;
+						bcs.mObjectLayer = m2 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING;
+						bcs.mMassPropertiesOverride.mMass = 2.0f;
+						bcs.mPosition = RVec3(2, 0, 0);
+						bcs.mLinearVelocity = Vec3(cInitialVelocity2, 0, 0);
+						bcs.mUserData++;
+						Body &body2 = *c.GetBodyInterface().CreateBody(bcs);
+						c.GetBodyInterface().AddBody(body2.GetID(), EActivation::Activate);
+
+						// Contact listener that modifies mass
+						class ContactListenerImpl : public ContactListener
+						{
+						public:
+							virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+							{
+								// Override the mass of body 1
+								float scale1 = sGetInvMassScale(inBody1);
+								ioSettings.mInvMassScale1 = scale1;
+								ioSettings.mInvInertiaScale1 = scale1;
+
+								// Override the mass of body 2
+								float scale2 = sGetInvMassScale(inBody2);
+								ioSettings.mInvMassScale2 = scale2;
+								ioSettings.mInvInertiaScale2 = scale2;
+							}
+
+							virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+							{
+								OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+							}
+						};
+
+						// Set listener
+						ContactListenerImpl listener;
+						c.GetSystem()->SetContactListener(&listener);
+
+						// Simulate
+						c.Simulate(1.0f);
+
+						// Calculate resulting inverse mass
+						float inv_m1 = body1.GetMotionType() == EMotionType::Dynamic? sGetInvMassScale(body1) * body1.GetMotionProperties()->GetInverseMass() : 0.0f;
+						float inv_m2 = body2.GetMotionType() == EMotionType::Dynamic? sGetInvMassScale(body2) * body2.GetMotionProperties()->GetInverseMass() : 0.0f;
+
+						float v1, v2;
+						if (inv_m1 == 0.0f && inv_m2 == 0.0f)
+						{
+							// If both bodies became kinematic they will pass through each other
+							v1 = cInitialVelocity1;
+							v2 = cInitialVelocity2;
+						}
+						else
+						{
+							// Calculate resulting velocity using conservation of momentum and energy
+							// See: https://en.wikipedia.org/wiki/Elastic_collision where m1 = 1 / inv_m1 and m2 = 1 / inv_m2
+							v1 = (2.0f * inv_m1 * cInitialVelocity2 + (inv_m2 - inv_m1) * cInitialVelocity1) / (inv_m1 + inv_m2);
+							v2 = (2.0f * inv_m2 * cInitialVelocity1 + (inv_m1 - inv_m2) * cInitialVelocity2) / (inv_m1 + inv_m2);
+						}
+
+						// Check that the spheres move according to their overridden masses
+						CHECK_APPROX_EQUAL(body1.GetLinearVelocity(), Vec3(v1, 0, 0));
+						CHECK_APPROX_EQUAL(body2.GetLinearVelocity(), Vec3(v2, 0, 0));
+					}
+	}
+
+	TEST_CASE("TestInfiniteMassOverride")
+	{
+		for (bool do_swap : { false, true })
+			for (EMotionQuality quality : { EMotionQuality::Discrete, EMotionQuality::LinearCast })
+			{
+				// A contact listener that makes a body have infinite mass
+				class ContactListenerImpl : public ContactListener
+				{
+				public:
+									ContactListenerImpl(const BodyID &inBodyID) : mBodyID(inBodyID) { }
+
+					virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+					{
+						if (mBodyID == inBody1.GetID())
+						{
+							ioSettings.mInvInertiaScale1 = 0.0f;
+							ioSettings.mInvMassScale1 = 0.0f;
+						}
+						else if (mBodyID == inBody2.GetID())
+						{
+							ioSettings.mInvInertiaScale2 = 0.0f;
+							ioSettings.mInvMassScale2 = 0.0f;
+						}
+					}
+
+					virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+					{
+						OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+					}
+
+				private:
+					BodyID			mBodyID;
+				};
+
+				PhysicsTestContext c;
+				c.ZeroGravity();
+
+				// Create a box
+				const RVec3 cInitialBoxPos(0, 2, 0);
+				BodyCreationSettings box_settings(new BoxShape(Vec3::sReplicate(2)), cInitialBoxPos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+				box_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+				box_settings.mMassPropertiesOverride.mMass = 1.0f;
+
+				// Create a sphere
+				BodyCreationSettings sphere_settings(new SphereShape(2), RVec3(30, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+				sphere_settings.mLinearVelocity = Vec3(-100, 0, 0);
+				sphere_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+				sphere_settings.mMassPropertiesOverride.mMass = 10.0f;
+				sphere_settings.mRestitution = 0.1f;
+				sphere_settings.mLinearDamping = 0.0f;
+				sphere_settings.mMotionQuality = quality;
+
+				BodyID box_id, sphere_id;
+				if (do_swap)
+				{
+					// Swap the bodies so that the contact listener will receive the bodies in the opposite order
+					sphere_id = c.GetBodyInterface().CreateAndAddBody(sphere_settings, EActivation::Activate);
+					box_id = c.GetBodyInterface().CreateAndAddBody(box_settings, EActivation::Activate);
+				}
+				else
+				{
+					box_id = c.GetBodyInterface().CreateAndAddBody(box_settings, EActivation::Activate);
+					sphere_id = c.GetBodyInterface().CreateAndAddBody(sphere_settings, EActivation::Activate);
+				}
+
+				// Add listener that will make the box have infinite mass
+				ContactListenerImpl listener(box_id);
+				c.GetSystem()->SetContactListener(&listener);
+
+				// Simulate
+				const float cSimulationTime = 0.3f;
+				c.Simulate(cSimulationTime);
+
+				// Check that the box didn't move
+				BodyInterface &bi = c.GetBodyInterface();
+				CHECK(bi.GetPosition(box_id) == cInitialBoxPos);
+				CHECK(bi.GetLinearVelocity(box_id) == Vec3::sZero());
+				CHECK(bi.GetAngularVelocity(box_id) == Vec3::sZero());
+
+				// Check that the sphere bounced off the box
+				CHECK_APPROX_EQUAL(bi.GetLinearVelocity(sphere_id), -sphere_settings.mLinearVelocity * sphere_settings.mRestitution);
+			}
+		}
+
+	TEST_CASE("TestCollideKinematicVsNonDynamic")
+	{
+		for (EMotionType m1 = EMotionType::Static; m1 <= EMotionType::Dynamic; m1 = EMotionType((int)m1 + 1))
+			for (int allow1 = 0; allow1 < 2; ++allow1)
+				for (int active1 = 0; active1 < 2; ++active1)
+					for (EMotionType m2 = EMotionType::Static; m2 <= EMotionType::Dynamic; m2 = EMotionType((int)m2 + 1))
+						for (int allow2 = 0; allow2 < 2; ++allow2)
+							for (int active2 = 0; active2 < 2; ++active2)
+								if ((m1 != EMotionType::Static && active1) || (m2 != EMotionType::Static && active2))
+								{
+									PhysicsTestContext c;
+									c.ZeroGravity();
+
+									const Vec3 cInitialVelocity1(m1 != EMotionType::Static && active1 != 0? 1.0f : 0.0f, 0, 0);
+									const Vec3 cInitialVelocity2(m2 != EMotionType::Static && active2 != 0? -1.0f : 0.0f, 0, 0);
+
+									// Create two spheres that are colliding initially
+									BodyCreationSettings bcs(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), m1, m1 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING);
+									bcs.mPosition = RVec3(-0.5_r, 0, 0);
+									bcs.mLinearVelocity = cInitialVelocity1;
+									bcs.mCollideKinematicVsNonDynamic = allow1 != 0;
+									Body &body1 = *c.GetBodyInterface().CreateBody(bcs);
+									c.GetBodyInterface().AddBody(body1.GetID(), active1 != 0? EActivation::Activate : EActivation::DontActivate);
+
+									bcs.mMotionType = m2;
+									bcs.mObjectLayer = m2 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING;
+									bcs.mPosition = RVec3(0.5_r, 0, 0);
+									bcs.mLinearVelocity = cInitialVelocity2;
+									bcs.mCollideKinematicVsNonDynamic = allow2 != 0;
+									Body &body2 = *c.GetBodyInterface().CreateBody(bcs);
+									c.GetBodyInterface().AddBody(body2.GetID(), active2 != 0? EActivation::Activate : EActivation::DontActivate);
+
+									// Set listener
+									LoggingContactListener listener;
+									c.GetSystem()->SetContactListener(&listener);
+
+									// Step
+									c.SimulateSingleStep();
+
+									if ((allow1 || allow2) // In this case we always get a callback
+										|| (m1 == EMotionType::Dynamic || m2 == EMotionType::Dynamic)) // Otherwise we only get a callback when one of the bodies is dynamic
+									{
+										// Check that we received a callback
+										CHECK(listener.GetEntryCount() == 2);
+										CHECK(listener.Contains(EType::Validate, body1.GetID(), body2.GetID()));
+										CHECK(listener.Contains(EType::Add, body1.GetID(), body2.GetID()));
+									}
+									else
+									{
+										// No collision events should have been received
+										CHECK(listener.GetEntryCount() == 0);
+									}
+
+									// Velocities should only change if the body is dynamic
+									if (m1 == EMotionType::Dynamic)
+									{
+										CHECK(body1.GetLinearVelocity() != cInitialVelocity1);
+										CHECK(body1.IsActive());
+									}
+									else
+										CHECK(body1.GetLinearVelocity() == cInitialVelocity1);
+
+									if (m2 == EMotionType::Dynamic)
+									{
+										CHECK(body2.GetLinearVelocity() != cInitialVelocity2);
+										CHECK(body2.IsActive());
+									}
+									else
+										CHECK(body2.GetLinearVelocity() == cInitialVelocity2);
+								}
+	}
+
+	// Test that an update with zero delta time doesn't generate contact callbacks
+	TEST_CASE("TestZeroDeltaTime")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Create a sphere that intersects with the floor
+		Body &floor = c.CreateFloor();
+		Body &body1 = c.CreateSphere(RVec3::sZero(), 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+
+		// Step with zero delta time
+		c.GetSystem()->Update(0.0f, 1, c.GetTempAllocator(), c.GetJobSystem());
+
+		// No callbacks should trigger when delta time is zero
+		CHECK(listener.GetEntryCount() == 0);
+
+		// Simulate for 1 step
+		c.SimulateSingleStep();
+
+		// We expect a validate and a contact point added message
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), body1.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), body1.GetID()));
+		listener.Clear();
+
+		// Create a 2nd sphere that intersects with the floor
+		Body &body2 = c.CreateSphere(RVec3(4, 0, 0), 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+
+		// Step with zero delta time
+		c.GetSystem()->Update(0.0f, 1, c.GetTempAllocator(), c.GetJobSystem());
+
+		// No callbacks should trigger when delta time is zero
+		CHECK(listener.GetEntryCount() == 0);
+
+		// Simulate for 1 step
+		c.SimulateSingleStep();
+
+		// We expect callbacks for both bodies now
+		CHECK(listener.GetEntryCount() == 4);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), body1.GetID()));
+		CHECK(listener.Contains(EType::Persist, floor.GetID(), body1.GetID()));
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), body2.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), body2.GetID()));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ConvexVsTrianglesTest.cpp

@@ -0,0 +1,361 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include "Layers.h"
+
+TEST_SUITE("ConvexVsTrianglesTest")
+{
+	static constexpr float cEdgeLength = 4.0f;
+
+	template <class Collider>
+	static void sCheckCollisionNoHit(const CollideShapeSettings &inSettings, Vec3Arg inCenter, float inRadius, uint8 inActiveEdges)
+	{
+		// Our sphere
+		Ref<SphereShape> sphere = new SphereShape(inRadius);
+
+		// Our default triangle
+		Vec3 v1(0, 0, 0);
+		Vec3 v2(0, 0, cEdgeLength);
+		Vec3 v3(cEdgeLength, 0, 0);
+
+		{
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			Collider collider(sphere, Vec3::sOne(), Vec3::sOne(), Mat44::sTranslation(inCenter), Mat44::sIdentity(), SubShapeID(), inSettings, collector);
+			collider.Collide(v1, v2, v3, inActiveEdges, SubShapeID());
+			CHECK(!collector.HadHit());
+		}
+
+		// A triangle shape has all edges active, so only test if all edges are active
+		if (inActiveEdges == 0b111)
+		{
+			// Create the triangle shape
+			PhysicsTestContext context;
+			context.CreateBody(new TriangleShapeSettings(v1, v2, v3), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			context.GetSystem()->GetNarrowPhaseQuery().CollideShape(sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(inCenter)), inSettings, RVec3::sZero(), collector);
+			CHECK(!collector.HadHit());
+		}
+
+		// A mesh shape with a single triangle has all edges active, so only test if all edges are active
+		if (inActiveEdges == 0b111)
+		{
+			// Create a mesh with a single triangle
+			TriangleList triangles;
+			triangles.push_back(Triangle(v1, v2, v3));
+			PhysicsTestContext context;
+			context.CreateBody(new MeshShapeSettings(triangles), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			context.GetSystem()->GetNarrowPhaseQuery().CollideShape(sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(inCenter)), inSettings, RVec3::sZero(), collector);
+			CHECK(!collector.HadHit());
+		}
+	}
+
+	template <class Collider>
+	static void sCheckCollision(const CollideShapeSettings &inSettings, Vec3Arg inCenter, float inRadius, uint8 inActiveEdges, Vec3Arg inExpectedContactOn1, Vec3Arg inExpectedContactOn2, Vec3Arg inExpectedPenetrationAxis, float inExpectedPenetrationDepth)
+	{
+		// Our sphere
+		Ref<SphereShape> sphere = new SphereShape(inRadius);
+
+		// Our default triangle
+		Vec3 v1(0, 0, 0);
+		Vec3 v2(0, 0, cEdgeLength);
+		Vec3 v3(cEdgeLength, 0, 0);
+
+		// A semi random transform for the triangle
+		Vec3 translation = Vec3(1, 2, 3);
+		Quat rotation = Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI);
+		Mat44 transform = Mat44::sRotationTranslation(rotation, translation);
+		Mat44 inv_transform = transform.InversedRotationTranslation();
+
+		// The transform for the sphere
+		Mat44 sphere_transform = transform * Mat44::sTranslation(inCenter);
+
+		// Transform incoming settings
+		CollideShapeSettings settings = inSettings;
+		settings.mActiveEdgeMovementDirection = transform.Multiply3x3(inSettings.mActiveEdgeMovementDirection);
+
+		// Test the specified collider
+		{
+			SubShapeID sub_shape_id1, sub_shape_id2;
+			sub_shape_id1.SetValue(123);
+			sub_shape_id2.SetValue(456);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			Collider collider(sphere, Vec3::sOne(), Vec3::sOne(), sphere_transform, transform, sub_shape_id1, settings, collector);
+			collider.Collide(v1, v2, v3, inActiveEdges, sub_shape_id2);
+
+			// Test result
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &hit = collector.mHits[0];
+			CHECK(hit.mBodyID2 == BodyID());
+			CHECK(hit.mSubShapeID1.GetValue() == sub_shape_id1.GetValue());
+			CHECK(hit.mSubShapeID2.GetValue() == sub_shape_id2.GetValue());
+			Vec3 contact1 = inv_transform * hit.mContactPointOn1;
+			Vec3 contact2 = inv_transform * hit.mContactPointOn2;
+			Vec3 pen_axis = transform.Multiply3x3Transposed(hit.mPenetrationAxis).Normalized();
+			Vec3 expected_pen_axis = inExpectedPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(contact1, inExpectedContactOn1, 1.0e-4f);
+			CHECK_APPROX_EQUAL(contact2, inExpectedContactOn2, 1.0e-4f);
+			CHECK_APPROX_EQUAL(pen_axis, expected_pen_axis, 1.0e-4f);
+			CHECK_APPROX_EQUAL(hit.mPenetrationDepth, inExpectedPenetrationDepth, 1.0e-4f);
+		}
+
+		// A triangle shape has all edges active, so only test if all edges are active
+		if (inActiveEdges == 0b111)
+		{
+			// Create the triangle shape
+			PhysicsTestContext context;
+			Body &body = context.CreateBody(new TriangleShapeSettings(v1, v2, v3), RVec3(translation), rotation, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			context.GetSystem()->GetNarrowPhaseQuery().CollideShape(sphere, Vec3::sOne(), RMat44(sphere_transform), settings, RVec3::sZero(), collector);
+
+			// Test result
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &hit = collector.mHits[0];
+			CHECK(hit.mBodyID2 == body.GetID());
+			CHECK(hit.mSubShapeID1.GetValue() == SubShapeID().GetValue());
+			CHECK(hit.mSubShapeID2.GetValue() == SubShapeID().GetValue());
+			Vec3 contact1 = inv_transform * hit.mContactPointOn1;
+			Vec3 contact2 = inv_transform * hit.mContactPointOn2;
+			Vec3 pen_axis = transform.Multiply3x3Transposed(hit.mPenetrationAxis).Normalized();
+			Vec3 expected_pen_axis = inExpectedPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(contact1, inExpectedContactOn1, 1.0e-4f);
+			CHECK_APPROX_EQUAL(contact2, inExpectedContactOn2, 1.0e-4f);
+			CHECK_APPROX_EQUAL(pen_axis, expected_pen_axis, 1.0e-4f);
+			CHECK_APPROX_EQUAL(hit.mPenetrationDepth, inExpectedPenetrationDepth, 1.0e-4f);
+		}
+
+		// A mesh shape with a single triangle has all edges active, so only test if all edges are active
+		if (inActiveEdges == 0b111)
+		{
+			// Create a mesh with a single triangle
+			TriangleList triangles;
+			triangles.push_back(Triangle(v1, v2, v3));
+			PhysicsTestContext context;
+			Body &body = context.CreateBody(new MeshShapeSettings(triangles), RVec3(translation), rotation, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			context.GetSystem()->GetNarrowPhaseQuery().CollideShape(sphere, Vec3::sOne(), RMat44(sphere_transform), settings, RVec3::sZero(), collector);
+
+			// Test result
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &hit = collector.mHits[0];
+			CHECK(hit.mBodyID2 == body.GetID());
+			CHECK(hit.mSubShapeID1.GetValue() == SubShapeID().GetValue());
+			CHECK(hit.mSubShapeID2.GetValue() != SubShapeID().GetValue()); // We don't really know what SubShapeID a triangle in the mesh will get, but it should not be invalid
+			Vec3 contact1 = inv_transform * hit.mContactPointOn1;
+			Vec3 contact2 = inv_transform * hit.mContactPointOn2;
+			Vec3 pen_axis = transform.Multiply3x3Transposed(hit.mPenetrationAxis).Normalized();
+			Vec3 expected_pen_axis = inExpectedPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(contact1, inExpectedContactOn1, 1.0e-4f);
+			CHECK_APPROX_EQUAL(contact2, inExpectedContactOn2, 1.0e-4f);
+			CHECK_APPROX_EQUAL(pen_axis, expected_pen_axis, 1.0e-4f);
+			CHECK_APPROX_EQUAL(hit.mPenetrationDepth, inExpectedPenetrationDepth, 1.0e-4f);
+		}
+	}
+
+	// Compares CollideShapeResult for two spheres with given positions and radii
+	template <class Collider>
+	static void sTestConvexVsTriangles()
+	{
+		const float cRadius = 0.5f;
+		const float cRadiusRS2 = cRadius / sqrt(2.0f);
+		const float cDistanceToTriangle = 0.1f;
+		const float cDistanceToTriangleRS2 = cDistanceToTriangle / sqrt(2.0f);
+		const float cEpsilon = 1.0e-6f; // A small epsilon to ensure we hit the front side
+		const float cMaxSeparationDistance = 0.5f;
+		const float cSeparationDistance = 0.1f;
+
+		// Loop over all possible active edge combinations
+		for (uint8 active_edges = 0; active_edges <= 0b111; ++active_edges)
+		{
+			// Create settings
+			CollideShapeSettings settings;
+			settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+
+			// Settings with ignore back faces
+			CollideShapeSettings settings_no_bf;
+			settings_no_bf.mBackFaceMode = EBackFaceMode::IgnoreBackFaces;
+
+			// Settings with max separation distance
+			CollideShapeSettings settings_max_distance;
+			settings_max_distance.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+			settings_max_distance.mMaxSeparationDistance = cMaxSeparationDistance;
+
+			{
+				// There should be no hit in front of the triangle
+				Vec3 sphere_center(0.25f * cEdgeLength, cRadius + cSeparationDistance, 0.25f * cEdgeLength);
+				sCheckCollisionNoHit<Collider>(settings, sphere_center, cRadius, active_edges);
+
+				// But if there's a max separation distance there should be
+				Vec3 expected1 = sphere_center + Vec3(0, -cRadius, 0);
+				Vec3 expected2(0.25f * cEdgeLength, 0, 0.25f * cEdgeLength);
+				Vec3 pen_axis(0, -1, 0);
+				float pen_depth = -cSeparationDistance;
+				sCheckCollision<Collider>(settings_max_distance, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+			}
+
+			{
+				// But if we go beyond the separation distance we should again have no hit
+				Vec3 sphere_center(0.25f * cEdgeLength, cRadius + cMaxSeparationDistance + cSeparationDistance, 0.25f * cEdgeLength);
+				sCheckCollisionNoHit<Collider>(settings_max_distance, sphere_center, cRadius, active_edges);
+			}
+
+			{
+				// There should be no hit in behind the triangle
+				Vec3 sphere_center(0.25f * cEdgeLength, -cRadius - cSeparationDistance, 0.25f * cEdgeLength);
+				sCheckCollisionNoHit<Collider>(settings, sphere_center, cRadius, active_edges);
+
+				// But if there's a max separation distance there should be
+				Vec3 expected1 = sphere_center + Vec3(0, cRadius, 0);
+				Vec3 expected2(0.25f * cEdgeLength, 0, 0.25f * cEdgeLength);
+				Vec3 pen_axis(0, 1, 0);
+				float pen_depth = -cSeparationDistance;
+				sCheckCollision<Collider>(settings_max_distance, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+			}
+
+			{
+				// But if we go beyond the separation distance we should again have no hit
+				Vec3 sphere_center(0.25f * cEdgeLength, -cRadius - cMaxSeparationDistance - cSeparationDistance, 0.25f * cEdgeLength);
+				sCheckCollisionNoHit<Collider>(settings_max_distance, sphere_center, cRadius, active_edges);
+			}
+
+			{
+				// Hit interior from front side
+				Vec3 expected2(0.25f * cEdgeLength, 0, 0.25f * cEdgeLength);
+				Vec3 sphere_center = expected2 + Vec3(0, cDistanceToTriangle, 0);
+				Vec3 expected1 = sphere_center + Vec3(0, -cRadius, 0);
+				Vec3 pen_axis(0, -1, 0);
+				float pen_depth = cRadius - cDistanceToTriangle;
+				sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+
+				// Ignore back faces should not matter
+				sCheckCollision<Collider>(settings_no_bf, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+			}
+
+			{
+				// Hit interior from back side
+				Vec3 expected2(0.25f * cEdgeLength, 0, 0.25f * cEdgeLength);
+				Vec3 sphere_center = expected2 + Vec3(0, -cDistanceToTriangle, 0);
+				Vec3 expected1 = sphere_center + Vec3(0, cRadius, 0);
+				Vec3 pen_axis(0, 1, 0);
+				float pen_depth = cRadius - cDistanceToTriangle;
+				sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+
+				// Back face hit should be filtered
+				sCheckCollisionNoHit<Collider>(settings_no_bf, sphere_center, cRadius, active_edges);
+			}
+
+			// Loop over possible active edge movement direction permutations
+			for (int movement_direction = 0; movement_direction < 3; ++movement_direction)
+			{
+				switch (movement_direction)
+				{
+				case 0:
+					// Disable the system
+					settings.mActiveEdgeMovementDirection = Vec3::sZero();
+					break;
+
+				case 1:
+					// Move into the triangle, this should always give us the normal from the edge
+					settings.mActiveEdgeMovementDirection = Vec3(0, -1, 0);
+					break;
+
+				case 2:
+					// Move out of the triangle, we should always get the normal of the triangle
+					settings.mActiveEdgeMovementDirection = Vec3(0, 1, 0);
+					break;
+				}
+
+				{
+					// Hit edge 1
+					Vec3 expected2(0, 0, 0.5f * cEdgeLength);
+					Vec3 sphere_center = expected2 + Vec3(-cDistanceToTriangle, cEpsilon, 0);
+					Vec3 expected1 = sphere_center + Vec3(cRadius, 0, 0);
+					Vec3 pen_axis = (active_edges & 0b001) != 0 || movement_direction == 1? Vec3(1, 0, 0) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit edge 2
+					Vec3 expected2(0.5f * cEdgeLength, 0, 0.5f * cEdgeLength);
+					Vec3 sphere_center = expected2 + Vec3(cDistanceToTriangleRS2, cEpsilon, cDistanceToTriangleRS2);
+					Vec3 expected1 = sphere_center - Vec3(cRadiusRS2, 0, cRadiusRS2);
+					Vec3 pen_axis = (active_edges & 0b010) != 0 || movement_direction == 1? Vec3(-1, 0, -1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit edge 3
+					Vec3 expected2(0.5f * cEdgeLength, 0, 0);
+					Vec3 sphere_center = expected2 + Vec3(0, cEpsilon, -cDistanceToTriangle);
+					Vec3 expected1 = sphere_center + Vec3(0, 0, cRadius);
+					Vec3 pen_axis = (active_edges & 0b100) != 0 || movement_direction == 1? Vec3(0, 0, 1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit vertex 1
+					Vec3 expected2(0, 0, 0);
+					Vec3 sphere_center = expected2 + Vec3(-cDistanceToTriangleRS2, cEpsilon, -cDistanceToTriangleRS2);
+					Vec3 expected1 = sphere_center + Vec3(cRadiusRS2, 0, cRadiusRS2);
+					Vec3 pen_axis = (active_edges & 0b101) != 0 || movement_direction == 1? Vec3(1, 0, 1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit vertex 2
+					Vec3 expected2(0, 0, cEdgeLength);
+					Vec3 sphere_center = expected2 + Vec3(-cDistanceToTriangleRS2, cEpsilon, cDistanceToTriangleRS2);
+					Vec3 expected1 = sphere_center + Vec3(cRadiusRS2, 0, -cRadiusRS2);
+					Vec3 pen_axis = (active_edges & 0b011) != 0 || movement_direction == 1? Vec3(1, 0, -1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit vertex 3
+					Vec3 expected2(cEdgeLength, 0, 0);
+					Vec3 sphere_center = expected2 + Vec3(cDistanceToTriangleRS2, cEpsilon, -cDistanceToTriangleRS2);
+					Vec3 expected1 = sphere_center + Vec3(-cRadiusRS2, 0, cRadiusRS2);
+					Vec3 pen_axis = (active_edges & 0b110) != 0 || movement_direction == 1? Vec3(-1, 0, 1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+			}
+		}
+	}
+
+	TEST_CASE("TestConvexVsTriangles")
+	{
+		sTestConvexVsTriangles<CollideConvexVsTriangles>();
+	}
+
+	TEST_CASE("TestSphereVsTriangles")
+	{
+		sTestConvexVsTriangles<CollideSphereVsTriangles>();
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/DistanceConstraintTests.cpp

@@ -0,0 +1,74 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include "Layers.h"
+
+TEST_SUITE("DistanceConstraintTests")
+{
+	// Test if the distance constraint can be used to create a spring
+	TEST_CASE("TestDistanceSpring")
+	{
+		// Configuration of the spring
+		const RVec3 cInitialPosition(10, 0, 0);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			context.ZeroGravity();
+			Body &body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+			body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+			// Calculate stiffness and damping of spring
+			float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = m * Square(omega);
+			float c = 2.0f * m * cDamping * omega;
+
+			// Create spring
+			DistanceConstraintSettings constraint;
+			constraint.mPoint2 = cInitialPosition;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mMinDistance = constraint.mMaxDistance = 0.0f;
+			context.CreateConstraint<DistanceConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Simulate spring
+			Real x = cInitialPosition.GetX();
+			float v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				v = (v - dt * k / m * float(x)) / (1.0f + dt * c / m + Square(dt) * k / m);
+				x += v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(x, body.GetPosition().GetX(), 5.0e-6_r);
+				CHECK(body.GetPosition().GetY() == 0);
+				CHECK(body.GetPosition().GetZ() == 0);
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/EstimateCollisionResponseTest.cpp

@@ -0,0 +1,77 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/EstimateCollisionResponse.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+
+TEST_SUITE("EstimateCollisionResponseTests")
+{
+	// Test CastShape ordering according to penetration depth
+	TEST_CASE("TestEstimateCollisionResponse")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		const Vec3 cBox1HalfExtents(0.1f, 1, 2);
+		const Vec3 cBox2HalfExtents(0.2f, 3, 4);
+
+		// Test different motion types, restitution, positions and angular velocities
+		for (EMotionType mt : { EMotionType::Static, EMotionType::Kinematic, EMotionType::Dynamic })
+			for (float restitution : { 0.0f, 0.3f, 1.0f })
+				for (float friction : { 0.0f, 0.3f, 1.0f })
+					for (float y : { 0.0f, 0.5f, cBox2HalfExtents.GetY() })
+						for (float z : { 0.0f, 0.5f, cBox2HalfExtents.GetZ() })
+							for (float w : { 0.0f, -1.0f, 1.0f })
+							{
+								// Install a listener that predicts the collision response
+								class MyListener : public ContactListener
+								{
+								public:
+									virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+									{
+										EstimateCollisionResponse(inBody1, inBody2, inManifold, mResult, ioSettings.mCombinedFriction, ioSettings.mCombinedRestitution);
+									}
+
+									CollisionEstimationResult mResult;
+								};
+
+								MyListener listener;
+								c.GetSystem()->SetContactListener(&listener);
+
+								const RVec3 cBaseOffset(1, 2, 3);
+								const Real cEpsilon = 0.0001_r;
+
+								Body &box1 = c.CreateBox(cBaseOffset, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, cBox1HalfExtents);
+								box1.SetFriction(friction);
+								box1.SetRestitution(restitution);
+								box1.SetLinearVelocity(Vec3(1, 1, 0));
+								box1.SetAngularVelocity(Vec3(0, w, 0));
+
+								Body &box2 = c.CreateBox(cBaseOffset + RVec3(cBox1HalfExtents.GetX() + cBox2HalfExtents.GetX() - cEpsilon, y, z), Quat::sIdentity(), mt, EMotionQuality::Discrete, mt == EMotionType::Static? Layers::NON_MOVING : Layers::MOVING, cBox2HalfExtents);
+								box2.SetFriction(friction);
+								box2.SetRestitution(restitution);
+								if (mt != EMotionType::Static)
+									box2.SetLinearVelocity(Vec3(-1, 0, 0));
+
+								// Step the simulation
+								c.SimulateSingleStep();
+
+								// Check that the predicted velocities are correct
+								CHECK_APPROX_EQUAL(listener.mResult.mLinearVelocity1, box1.GetLinearVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mAngularVelocity1, box1.GetAngularVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mLinearVelocity2, box2.GetLinearVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mAngularVelocity2, box2.GetAngularVelocity());
+
+								// Remove the bodies in reverse order
+								BodyInterface &bi = c.GetBodyInterface();
+								bi.RemoveBody(box2.GetID());
+								bi.RemoveBody(box1.GetID());
+								bi.DestroyBody(box2.GetID());
+								bi.DestroyBody(box1.GetID());
+							}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/HeightFieldShapeTests.cpp

@@ -0,0 +1,464 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+
+TEST_SUITE("HeightFieldShapeTests")
+{
+	static void sRandomizeMaterials(HeightFieldShapeSettings &ioSettings, uint inMaxMaterials)
+	{
+		// Create materials
+		for (uint i = 0; i < inMaxMaterials; ++i)
+			ioSettings.mMaterials.push_back(new PhysicsMaterialSimple("Material " + ConvertToString(i), Color::sGetDistinctColor(i)));
+
+		if (inMaxMaterials > 1)
+		{
+			// Make random material indices
+			UnitTestRandom random;
+			uniform_int_distribution<uint> index_distribution(0, inMaxMaterials - 1);
+			ioSettings.mMaterialIndices.resize(Square(ioSettings.mSampleCount - 1));
+			for (uint y = 0; y < ioSettings.mSampleCount - 1; ++y)
+				for (uint x = 0; x < ioSettings.mSampleCount - 1; ++x)
+					ioSettings.mMaterialIndices[y * (ioSettings.mSampleCount - 1) + x] = uint8(index_distribution(random));
+		}
+	}
+
+	static Ref<HeightFieldShape> sValidateGetPosition(const HeightFieldShapeSettings &inSettings, float inMaxError)
+	{
+		// Create shape
+		Ref<HeightFieldShape> shape = StaticCast<HeightFieldShape>(inSettings.Create().Get());
+
+		// Validate it
+		float max_diff = -1.0f;
+		for (uint y = 0; y < inSettings.mSampleCount; ++y)
+			for (uint x = 0; x < inSettings.mSampleCount; ++x)
+			{
+				// Perform a raycast from above the height field on this location
+				RayCast ray { inSettings.mOffset + inSettings.mScale * Vec3((float)x, 100.0f, (float)y), inSettings.mScale.GetY() * Vec3(0, -200, 0) };
+				RayCastResult hit;
+				shape->CastRay(ray, SubShapeIDCreator(), hit);
+
+				// Get original (unscaled) height
+				float height = inSettings.mHeightSamples[y * inSettings.mSampleCount + x];
+				if (height != HeightFieldShapeConstants::cNoCollisionValue)
+				{
+					// Check there is collision
+					CHECK(!shape->IsNoCollision(x, y));
+
+					// Calculate position
+					Vec3 original_pos = inSettings.mOffset + inSettings.mScale * Vec3((float)x, height, (float)y);
+
+					// Calculate position from the shape
+					Vec3 shape_pos = shape->GetPosition(x, y);
+
+					// Calculate delta
+					float diff = (original_pos - shape_pos).Length();
+					max_diff = max(max_diff, diff);
+
+					// Materials are defined on the triangle, not on the sample points
+					if (x < inSettings.mSampleCount - 1 && y < inSettings.mSampleCount - 1)
+					{
+						const PhysicsMaterial *m1 = PhysicsMaterial::sDefault;
+						if (!inSettings.mMaterialIndices.empty())
+							m1 = inSettings.mMaterials[inSettings.mMaterialIndices[y * (inSettings.mSampleCount - 1) + x]];
+						else if (!inSettings.mMaterials.empty())
+							m1 = inSettings.mMaterials.front();
+
+						const PhysicsMaterial *m2 = shape->GetMaterial(x, y);
+						CHECK(m1 == m2);
+					}
+
+					// Don't test borders, the ray may or may not hit
+					if (x > 0 && y > 0 && x < inSettings.mSampleCount - 1 && y < inSettings.mSampleCount - 1)
+					{
+						// Check that the ray hit the height field
+						Vec3 hit_pos = ray.GetPointOnRay(hit.mFraction);
+						CHECK_APPROX_EQUAL(hit_pos, shape_pos, 1.0e-3f);
+					}
+				}
+				else
+				{
+					// Should be no collision here
+					CHECK(shape->IsNoCollision(x, y));
+
+					// Ray should not have given a hit
+					CHECK(hit.mFraction > 1.0f);
+				}
+			}
+
+		// Check error
+		CHECK(max_diff <= inMaxError);
+
+		return shape;
+	}
+
+	TEST_CASE("TestPlane")
+	{
+		// Create flat plane with offset and scale
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(3, 5, 7);
+		settings.mScale = Vec3(9, 13, 17);
+		settings.mSampleCount = 32;
+		settings.mBitsPerSample = 1;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(settings.mSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = 1.0f;
+
+		// Make some random holes
+		UnitTestRandom random;
+		uniform_int_distribution<uint> index_distribution(0, (uint)settings.mHeightSamples.size() - 1);
+		for (int i = 0; i < 10; ++i)
+			settings.mHeightSamples[index_distribution(random)] = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// We should be able to encode a flat plane in 1 bit
+		CHECK(settings.CalculateBitsPerSampleForError(0.0f) == 1);
+
+		sRandomizeMaterials(settings, 256);
+		sValidateGetPosition(settings, 0.0f);
+	}
+
+	TEST_CASE("TestPlaneCloseToOrigin")
+	{
+		// Create flat plane very close to origin, this tests that we don't introduce a quantization error on a flat plane
+		HeightFieldShapeSettings settings;
+		settings.mSampleCount = 32;
+		settings.mBitsPerSample = 1;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(settings.mSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = 1.0e-6f;
+
+		// We should be able to encode a flat plane in 1 bit
+		CHECK(settings.CalculateBitsPerSampleForError(0.0f) == 1);
+
+		sRandomizeMaterials(settings, 50);
+		sValidateGetPosition(settings, 0.0f);
+	}
+
+	TEST_CASE("TestRandomHeightField")
+	{
+		const float cMinHeight = -5.0f;
+		const float cMaxHeight = 10.0f;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> height_distribution(cMinHeight, cMaxHeight);
+
+		// Create height field with random samples
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(0.3f, 0.5f, 0.7f);
+		settings.mScale = Vec3(1.1f, 1.2f, 1.3f);
+		settings.mSampleCount = 32;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(settings.mSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = height_distribution(random);
+
+		// Check if bits per sample is ok
+		for (uint32 bits_per_sample = 1; bits_per_sample <= 8; ++bits_per_sample)
+		{
+			// Calculate maximum error you can get if you quantize using bits_per_sample.
+			// We ignore the fact that we have range blocks that give much better compression, although
+			// with random input data there shouldn't be much benefit of that.
+			float max_error = 0.5f * (cMaxHeight - cMinHeight) / ((1 << bits_per_sample) - 1);
+			uint32 calculated_bits_per_sample = settings.CalculateBitsPerSampleForError(max_error);
+			CHECK(calculated_bits_per_sample <= bits_per_sample);
+		}
+
+		sRandomizeMaterials(settings, 1);
+		sValidateGetPosition(settings, settings.mScale.GetY() * (cMaxHeight - cMinHeight) / ((1 << settings.mBitsPerSample) - 1));
+	}
+
+	TEST_CASE("TestEmptyHeightField")
+	{
+		// Create height field with no collision
+		HeightFieldShapeSettings settings;
+		settings.mSampleCount = 32;
+		settings.mHeightSamples.resize(Square(settings.mSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// This should use the minimum amount of bits
+		CHECK(settings.CalculateBitsPerSampleForError(0.0f) == 1);
+
+		sRandomizeMaterials(settings, 50);
+		Ref<HeightFieldShape> shape = sValidateGetPosition(settings, 0.0f);
+
+		// Check that we allocated the minimum amount of memory
+		Shape::Stats stats = shape->GetStats();
+		CHECK(stats.mNumTriangles == 0);
+		CHECK(stats.mSizeBytes == sizeof(HeightFieldShape));
+	}
+
+	TEST_CASE("TestGetHeights")
+	{
+		const float cMinHeight = -5.0f;
+		const float cMaxHeight = 10.0f;
+		const uint cSampleCount = 32;
+		const uint cNoCollisionIndex = 10;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> height_distribution(cMinHeight, cMaxHeight);
+
+		// Create height field with random samples
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(0.3f, 0.5f, 0.7f);
+		settings.mScale = Vec3(1.1f, 1.2f, 1.3f);
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = height_distribution(random);
+
+		// Add 1 sample that has no collision
+		settings.mHeightSamples[cNoCollisionIndex] = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// Create shape
+		ShapeRefC shape = settings.Create().Get();
+		const HeightFieldShape *height_field = StaticCast<HeightFieldShape>(shape);
+
+		{
+			// Check that the GetHeights function returns the same values as the original height samples
+			Array<float> sampled_heights;
+			sampled_heights.resize(Square(cSampleCount));
+			height_field->GetHeights(0, 0, cSampleCount, cSampleCount, sampled_heights.data(), cSampleCount);
+			for (uint i = 0; i < Square(cSampleCount); ++i)
+				if (i == cNoCollisionIndex)
+					CHECK(sampled_heights[i] == HeightFieldShapeConstants::cNoCollisionValue);
+				else
+					CHECK_APPROX_EQUAL(sampled_heights[i], settings.mOffset.GetY() + settings.mScale.GetY() * settings.mHeightSamples[i], 0.05f);
+		}
+
+		{
+			// With a random height field the max error is going to be limited by the amount of bits we have per sample as we will not get any benefit from a reduced range per block
+			float tolerance = (cMaxHeight - cMinHeight) / ((1 << settings.mBitsPerSample) - 2);
+
+			// Check a sub rect of the height field
+			uint sx = 4, sy = 8, cx = 16, cy = 8;
+			Array<float> sampled_heights;
+			sampled_heights.resize(cx * cy);
+			height_field->GetHeights(sx, sy, cx, cy, sampled_heights.data(), cx);
+			for (uint y = 0; y < cy; ++y)
+				for (uint x = 0; x < cx; ++x)
+					CHECK_APPROX_EQUAL(sampled_heights[y * cx + x], settings.mOffset.GetY() + settings.mScale.GetY() * settings.mHeightSamples[(sy + y) * cSampleCount + sx + x], tolerance);
+		}
+	}
+
+	TEST_CASE("TestSetHeights")
+	{
+		const float cMinHeight = -5.0f;
+		const float cMaxHeight = 10.0f;
+		const uint cSampleCount = 32;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> height_distribution(cMinHeight, cMaxHeight);
+
+		// Create height field with random samples
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(0.3f, 0.5f, 0.7f);
+		settings.mScale = Vec3(1.1f, 1.2f, 1.3f);
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		settings.mMinHeightValue = cMinHeight;
+		settings.mMaxHeightValue = cMaxHeight;
+		for (float &h : settings.mHeightSamples)
+			h = height_distribution(random);
+
+		// Create shape
+		Ref<Shape> shape = settings.Create().Get();
+		HeightFieldShape *height_field = StaticCast<HeightFieldShape>(shape);
+
+		// Get the original (quantized) heights
+		Array<float> original_heights;
+		original_heights.resize(Square(cSampleCount));
+		height_field->GetHeights(0, 0, cSampleCount, cSampleCount, original_heights.data(), cSampleCount);
+
+		// Create new data for height field
+		Array<float> patched_heights;
+		uint sx = 4, sy = 16, cx = 16, cy = 8;
+		patched_heights.resize(cx * cy);
+		for (uint y = 0; y < cy; ++y)
+			for (uint x = 0; x < cx; ++x)
+				patched_heights[y * cx + x] = height_distribution(random);
+
+		// Add 1 sample that has no collision
+		uint no_collision_idx = (sy + 1) * cSampleCount + sx + 2;
+		patched_heights[1 * cx + 2] = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// Update the height field
+		TempAllocatorMalloc temp_allocator;
+		height_field->SetHeights(sx, sy, cx, cy, patched_heights.data(), cx, temp_allocator);
+
+		// With a random height field the max error is going to be limited by the amount of bits we have per sample as we will not get any benefit from a reduced range per block
+		float tolerance = (cMaxHeight - cMinHeight) / ((1 << settings.mBitsPerSample) - 2);
+
+		// Check a sub rect of the height field
+		Array<float> verify_heights;
+		verify_heights.resize(cSampleCount * cSampleCount);
+		height_field->GetHeights(0, 0, cSampleCount, cSampleCount, verify_heights.data(), cSampleCount);
+		for (uint y = 0; y < cSampleCount; ++y)
+			for (uint x = 0; x < cSampleCount; ++x)
+			{
+				uint idx = y * cSampleCount + x;
+				if (idx == no_collision_idx)
+					CHECK(verify_heights[idx] == HeightFieldShapeConstants::cNoCollisionValue);
+				else if (x >= sx && x < sx + cx && y >= sy && y < sy + cy)
+					CHECK_APPROX_EQUAL(verify_heights[y * cSampleCount + x], patched_heights[(y - sy) * cx + x - sx], tolerance);
+				else if (x >= sx - settings.mBlockSize && x < sx + cx && y >= sy - settings.mBlockSize && y < sy + cy)
+					CHECK_APPROX_EQUAL(verify_heights[idx], original_heights[idx], tolerance); // We didn't modify this but it has been quantized again
+				else
+					CHECK(verify_heights[idx] == original_heights[idx]); // We didn't modify this and it is outside of the affected range
+			}
+	}
+
+	TEST_CASE("TestSetMaterials")
+	{
+		constexpr uint cSampleCount = 32;
+
+		PhysicsMaterialRefC material_0 = new PhysicsMaterialSimple("Material 0", Color::sGetDistinctColor(0));
+		PhysicsMaterialRefC material_1 = new PhysicsMaterialSimple("Material 1", Color::sGetDistinctColor(1));
+		PhysicsMaterialRefC material_2 = new PhysicsMaterialSimple("Material 2", Color::sGetDistinctColor(2));
+		PhysicsMaterialRefC material_3 = new PhysicsMaterialSimple("Material 3", Color::sGetDistinctColor(3));
+		PhysicsMaterialRefC material_4 = new PhysicsMaterialSimple("Material 4", Color::sGetDistinctColor(4));
+		PhysicsMaterialRefC material_5 = new PhysicsMaterialSimple("Material 5", Color::sGetDistinctColor(5));
+
+		// Create height field with a single material
+		HeightFieldShapeSettings settings;
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = 0.0f;
+		settings.mMaterials.push_back(material_0);
+		settings.mMaterialIndices.resize(Square(cSampleCount - 1));
+		for (uint8 &m : settings.mMaterialIndices)
+			m = 0;
+
+		// Store the current state
+		Array<const PhysicsMaterial *> current_state;
+		current_state.resize(Square(cSampleCount - 1));
+		for (const PhysicsMaterial *&m : current_state)
+			m = material_0;
+
+		// Create shape
+		Ref<Shape> shape = settings.Create().Get();
+		HeightFieldShape *height_field = StaticCast<HeightFieldShape>(shape);
+
+		// Check that the material is set
+		auto check_materials = [height_field, &current_state]() {
+			const PhysicsMaterialList &material_list = height_field->GetMaterialList();
+
+			uint sample_count_min_1 = height_field->GetSampleCount() - 1;
+
+			Array<uint8> material_indices;
+			material_indices.resize(Square(sample_count_min_1));
+			height_field->GetMaterials(0, 0, sample_count_min_1, sample_count_min_1, material_indices.data(), sample_count_min_1);
+
+			for (uint i = 0; i < (uint)current_state.size(); ++i)
+				CHECK(current_state[i] == material_list[material_indices[i]]);
+		};
+		check_materials();
+
+		// Function to randomize materials
+		auto update_materials = [height_field, &current_state](uint inStartX, uint inStartY, uint inSizeX, uint inSizeY, const PhysicsMaterialList *inMaterialList) {
+			TempAllocatorMalloc temp_allocator;
+
+			const PhysicsMaterialList &material_list = inMaterialList != nullptr? *inMaterialList : height_field->GetMaterialList();
+
+			UnitTestRandom random;
+			uniform_int_distribution<uint> index_distribution(0, uint(material_list.size()) - 1);
+
+			uint sample_count_min_1 = height_field->GetSampleCount() - 1;
+
+			Array<uint8> patched_materials;
+			patched_materials.resize(inSizeX * inSizeY);
+			for (uint y = 0; y < inSizeY; ++y)
+				for (uint x = 0; x < inSizeX; ++x)
+				{
+					// Initialize the patch
+					uint8 index = uint8(index_distribution(random));
+					patched_materials[y * inSizeX + x] = index;
+
+					// Update reference state
+					current_state[(inStartY + y) * sample_count_min_1 + inStartX + x] = material_list[index];
+				}
+			CHECK(height_field->SetMaterials(inStartX, inStartY, inSizeX, inSizeY, patched_materials.data(), inSizeX, inMaterialList, temp_allocator));
+		};
+
+		{
+			// Add material 1
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_0);
+			update_materials(4, 16, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 2
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_2);
+			update_materials(8, 16, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 3
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_3);
+			update_materials(8, 8, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 4
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_4);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_3);
+			update_materials(0, 0, 30, 30, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 5
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_4);
+			patched_materials_list.push_back(material_3);
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_5);
+			update_materials(1, 1, 30, 30, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Update materials without new material list
+			update_materials(2, 5, 10, 15, nullptr);
+			check_materials();
+		}
+
+		// Check materials using GetMaterial call
+		for (uint y = 0; y < cSampleCount - 1; ++y)
+			for (uint x = 0; x < cSampleCount - 1; ++x)
+				CHECK(height_field->GetMaterial(x, y) == current_state[y * (cSampleCount - 1) + x]);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/HingeConstraintTests.cpp

@@ -0,0 +1,84 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include "Layers.h"
+
+TEST_SUITE("HingeConstraintTests")
+{
+	// Test if the hinge constraint can be used to create a spring
+	TEST_CASE("TestHingeSpring")
+	{
+		// Configuration of the spring
+		const float cInitialAngle = DegreesToRadians(100.0f);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			Body &body = context.CreateBody(new SphereShapeSettings(0.5f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::Activate);
+			body.GetMotionProperties()->SetAngularDamping(0.0f);
+			body.SetAllowSleeping(false);
+
+			// Calculate stiffness and damping of spring
+			float inertia = body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity()).Inversed3x3().GetAxisY().Length();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = inertia * Square(omega);
+			float c = 2.0f * inertia * cDamping * omega;
+
+			// Create spring
+			HingeConstraintSettings constraint;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mLimitsMin = constraint.mLimitsMax = 0.0f;
+			context.CreateConstraint<HingeConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Rotate the body to the initial angle
+			context.GetBodyInterface().SetRotation(body.GetID(), Quat::sRotation(Vec3::sAxisY(), cInitialAngle), EActivation::Activate);
+
+			// Simulate angular spring
+			float angle = cInitialAngle;
+			float angular_v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				angular_v = (angular_v - dt * k / inertia * angle) / (1.0f + dt * c / inertia + Square(dt) * k / inertia);
+				angle += angular_v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Decompose body rotation
+				Vec3 actual_axis;
+				float actual_angle;
+				body.GetRotation().GetAxisAngle(actual_axis, actual_angle);
+				if (actual_axis.GetY() < 0.0f)
+					actual_angle = -actual_angle;
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(angle, actual_angle, DegreesToRadians(0.1f));
+				CHECK_APPROX_EQUAL(actual_axis.GetX(), 0);
+				CHECK_APPROX_EQUAL(actual_axis.GetZ(), 0);
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/MotionQualityLinearCastTests.cpp

@@ -0,0 +1,379 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include "LoggingBodyActivationListener.h"
+
+TEST_SUITE("MotionQualityLinearCastTests")
+{
+	static const float cBoxExtent = 0.5f;
+	static const float cFrequency = 60.0f;
+	static const Vec3 cVelocity(2.0f * cFrequency, 0, 0); // High enough velocity to step 2 meters in a single simulation step
+	static const RVec3 cPos1(-1, 0, 0);
+	static const RVec3 cPos2(1, 0, 0);
+
+	// Two boxes colliding in the center, each has enough velocity to tunnel though in 1 step
+	TEST_CASE("TestDiscreteBoxVsDiscreteBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		// Test that the inner radius of the box makes sense (used internally by linear cast)
+		CHECK_APPROX_EQUAL(box1.GetShape()->GetInnerRadius(), cBoxExtent);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(-cVelocity);
+
+		c.SimulateSingleStep();
+
+		// No collisions should be reported and the bodies should have moved according to their velocity (tunneling through each other)
+		CHECK(listener.GetEntryCount() == 0);
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos1 + cVelocity / cFrequency);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2 - cVelocity / cFrequency);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding in the center, each has enough velocity to step over the other in 1 step, restitution = 1
+	TEST_CASE("TestLinearCastBoxVsLinearCastBoxElastic")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+		box1.SetRestitution(1.0f);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(-cVelocity);
+		box2.SetRestitution(1.0f);
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and the velocities reversed
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+
+		listener.Clear();
+		c.SimulateSingleStep();
+
+		// In the second step the bodies should have moved away, but since they were initially overlapping we should have a contact persist callback
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Persist, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0) - cVelocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0) + cVelocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+
+		listener.Clear();
+		c.SimulateSingleStep();
+
+		// In the third step the bodies have separated and a contact remove callback should have been received
+		CHECK(listener.GetEntryCount() == 1);
+		CHECK(listener.Contains(LoggingContactListener::EType::Remove, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0) - 2.0f * cVelocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0) + 2.0f * cVelocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding in the center, each has enough velocity to step over the other in 1 step, restitution = 0
+	TEST_CASE("TestLinearCastBoxVsLinearCastBoxInelastic")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(-cVelocity);
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and both are stopped
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+
+		// The bodies should persist to contact as they are not moving
+		for (int i = 0; i < 10; ++i)
+		{
+			listener.Clear();
+			c.SimulateSingleStep();
+
+			if (i == 0)
+			{
+				// Only in the first step we will receive a validate callback since after this step the contact cache will be used
+				CHECK(listener.GetEntryCount() == 2);
+				CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+			}
+			else
+				CHECK(listener.GetEntryCount() == 1);
+			CHECK(listener.Contains(LoggingContactListener::EType::Persist, box1.GetID(), box2.GetID()));
+			CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0), cPenetrationSlop);
+			CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), Vec3::sZero());
+			CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+			CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0), cPenetrationSlop);
+			CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), Vec3::sZero());
+			CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+		}
+	}
+
+	// Two boxes colliding in the center, linear cast vs inactive linear cast
+	TEST_CASE("TestLinearCastBoxVsInactiveLinearCastBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+		LoggingBodyActivationListener activation;
+		c.GetSystem()->SetBodyActivationListener(&activation);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent), EActivation::DontActivate);
+		CHECK(!box2.IsActive());
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and body 2 should be activated, have velocity, but not moved in this step
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		Vec3 new_velocity = 0.5f * cVelocity;
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos2 - Vec3(2.0f * cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+		CHECK(box2.IsActive());
+		CHECK(activation.Contains(LoggingBodyActivationListener::EType::Activated, box2.GetID()));
+
+		listener.Clear();
+		c.SimulateSingleStep();
+
+		// In the next step body 2 should have started to move
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Persist, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos2 - Vec3(2.0f * cBoxExtent, 0, 0) + new_velocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2 + new_velocity / cFrequency);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding in the center, linear cast vs inactive discrete
+	TEST_CASE("TestLinearCastBoxVsInactiveDiscreteBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+		LoggingBodyActivationListener activation;
+		c.GetSystem()->SetBodyActivationListener(&activation);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent), EActivation::DontActivate);
+		CHECK(!box2.IsActive());
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and body 2 should be activated, have velocity, but not moved in this step
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		Vec3 new_velocity = 0.5f * cVelocity;
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos2 - Vec3(2.0f * cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+		CHECK(box2.IsActive());
+		CHECK(activation.Contains(LoggingBodyActivationListener::EType::Activated, box2.GetID()));
+
+		listener.Clear();
+		c.SimulateSingleStep();
+
+		// In the next step body 2 should have started to move
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Persist, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos2 - Vec3(2.0f * cBoxExtent, 0, 0) + new_velocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2 + new_velocity / cFrequency);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding under an angle, linear cast vs inactive discrete
+	TEST_CASE("TestLinearCastBoxVsInactiveDiscreteBoxAngled")
+	{
+		const Vec3 cAngledOffset1(1, 0, -2);
+		const Vec3 cAngledVelocity = -cFrequency * 2 * cAngledOffset1;
+
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+		LoggingBodyActivationListener activation;
+		c.GetSystem()->SetBodyActivationListener(&activation);
+
+		// Make sure box1 exactly hits the face of box2 in the center
+		RVec3 pos1 = RVec3(2.0f * cBoxExtent, 0, 0) + cAngledOffset1;
+		Body &box1 = c.CreateBox(pos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cAngledVelocity);
+		box1.SetRestitution(1.0f);
+		box1.SetFriction(0.0f);
+
+		Body &box2 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent), EActivation::DontActivate);
+		box2.SetRestitution(1.0f);
+		box2.SetFriction(0.0f);
+		CHECK(!box2.IsActive());
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and body 2 should be activated, have inherited the x velocity of body 1, but not moved in this step. Body 1 should have lost all of its velocity in x direction.
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		Vec3 new_velocity1 = Vec3(0, 0, cAngledVelocity.GetZ());
+		Vec3 new_velocity2 = Vec3(cAngledVelocity.GetX(), 0, 0);
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(2.0f * cBoxExtent, 0, 0), 2.3f * cPenetrationSlop); // We're moving 2x as fast in the z direction and the slop is allowed in x direction: sqrt(1^2 + 2^2) ~ 2.3
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity1, 1.0e-4f);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero(), 2.0e-4f);
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity2, 1.0e-4f);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero(), 2.0e-4f);
+		CHECK(box2.IsActive());
+		CHECK(activation.Contains(LoggingBodyActivationListener::EType::Activated, box2.GetID()));
+	}
+
+	// Two boxes colliding in the center, linear cast vs fast moving discrete, should tunnel through because all discrete bodies are moved before linear cast bodies are tested
+	TEST_CASE("TestLinearCastBoxVsFastDiscreteBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(-cVelocity);
+
+		c.SimulateSingleStep();
+
+		// No collisions should be reported and the bodies should have moved according to their velocity (tunneling through each other)
+		CHECK(listener.GetEntryCount() == 0);
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos1 + cVelocity / cFrequency);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2 - cVelocity / cFrequency);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding in the center, linear cast vs moving discrete, discrete is slow enough not to tunnel through linear cast body
+	TEST_CASE("TestLinearCastBoxVsSlowDiscreteBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		// In 1 step it should move -0.1 meter on the X axis
+		const Vec3 cBox2Velocity = Vec3(-0.1f * cFrequency, 0, 0);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(cBox2Velocity);
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and body 2 should have moved according to its discrete step
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		RVec3 new_pos2 = cPos2 + cBox2Velocity / cFrequency;
+		Vec3 new_velocity = 0.5f * (cVelocity + cBox2Velocity);
+		CHECK_APPROX_EQUAL(box1.GetPosition(), new_pos2 - Vec3(2.0f * cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), new_pos2);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/MutableCompoundShapeTests.cpp

@@ -0,0 +1,209 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+
+TEST_SUITE("MutableCompoundShapeTests")
+{
+	TEST_CASE("TestMutableCompoundShapeAddRemove")
+	{
+		MutableCompoundShapeSettings settings;
+		Ref<Shape> sphere1 = new SphereShape(1.0f);
+		settings.AddShape(Vec3::sZero(), Quat::sIdentity(), sphere1);
+		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
+
+		auto check_shape_hit = [shape] (Vec3Arg inPosition) {
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPosition - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			SubShapeID remainder;
+			CHECK(collector.mHits.size() <= 1);
+			return !collector.mHits.empty()? shape->GetSubShape(shape->GetSubShapeIndexFromID(collector.mHits[0].mSubShapeID2, remainder)).mShape : nullptr;
+		};
+
+		CHECK(shape->GetNumSubShapes() == 1);
+		CHECK(shape->GetSubShape(0).mShape == sphere1);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+
+		Ref<Shape> sphere2 = new SphereShape(2.0f);
+		shape->AddShape(Vec3(10, 0, 0), Quat::sIdentity(), sphere2, 0, 0); // Insert at the start
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere1);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -2, -2), Vec3(12, 2, 2)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+
+		Ref<Shape> sphere3 = new SphereShape(3.0f);
+		shape->AddShape(Vec3(20, 0, 0), Quat::sIdentity(), sphere3, 0, 2); // Insert at the end
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere1);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3(0, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		Ref<Shape> sphere4 = new SphereShape(4.0f);
+		shape->AddShape(Vec3(0, 0, 0), Quat::sIdentity(), sphere4, 0); // Insert at the end
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetSubShape(2).mShape == sphere4);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-4, -4, -4), Vec3(23, 4, 4)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere4);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		Ref<Shape> sphere5 = new SphereShape(1.0f);
+		shape->AddShape(Vec3(15, 0, 0), Quat::sIdentity(), sphere5, 0, 1); // Insert in the middle
+		CHECK(shape->GetNumSubShapes() == 4);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere5);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetSubShape(3).mShape == sphere4);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-4, -4, -4), Vec3(23, 4, 4)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere4);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == sphere5);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(3);
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere5);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == sphere5);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 1);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -2, -2), Vec3(12, 2, 2)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == nullptr);
+
+		shape->RemoveShape(0);
+		CHECK(shape->GetNumSubShapes() == 0);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3::sZero(), Vec3::sZero()));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == nullptr);
+	}
+
+	TEST_CASE("TestMutableCompoundShapeAdjustCenterOfMass")
+	{
+		// Start with a box at (-1 0 0)
+		MutableCompoundShapeSettings settings;
+		Ref<Shape> box_shape1 = new BoxShape(Vec3::sOne());
+		box_shape1->SetUserData(1);
+		settings.AddShape(Vec3(-1.0f, 0.0f, 0.0f), Quat::sIdentity(), box_shape1);
+		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
+		CHECK(shape->GetCenterOfMass() == Vec3(-1.0f, 0.0f, 0.0f));
+		CHECK(shape->GetLocalBounds() == AABox(Vec3::sReplicate(-1.0f), Vec3::sOne()));
+
+		// Check that we can hit the box
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		CHECK(collector.mHits.empty());
+
+		// Now add another box at (1 0 0)
+		Ref<Shape> box_shape2 = new BoxShape(Vec3::sOne());
+		box_shape2->SetUserData(2);
+		shape->AddShape(Vec3(1.0f, 0.0f, 0.0f), Quat::sIdentity(), box_shape2);
+		CHECK(shape->GetCenterOfMass() == Vec3(-1.0f, 0.0f, 0.0f));
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1.0f, -1.0f, -1.0f), Vec3(3.0f, 1.0f, 1.0f)));
+
+		// Check that we can hit both boxes
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		shape->CollidePoint(Vec3(0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 2));
+		collector.Reset();
+
+		// Adjust the center of mass
+		shape->AdjustCenterOfMass();
+		CHECK(shape->GetCenterOfMass() == Vec3::sZero());
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-2.0f, -1.0f, -1.0f), Vec3(2.0f, 1.0f, 1.0f)));
+
+		// Check that we can hit both boxes
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		shape->CollidePoint(Vec3(0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 2));
+		collector.Reset();
+	}
+
+	TEST_CASE("TestEmptyMutableCompoundShape")
+	{
+		// Create an empty compound shape
+		PhysicsTestContext c;
+		MutableCompoundShapeSettings settings;
+		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
+		BodyCreationSettings bcs(shape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		bcs.mLinearDamping = 0.0f;
+		bcs.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+		bcs.mMassPropertiesOverride.mMass = 1.0f;
+		bcs.mMassPropertiesOverride.mInertia = Mat44::sIdentity();
+		BodyID body_id = c.GetBodyInterface().CreateAndAddBody(bcs, EActivation::Activate);
+
+		// Simulate with empty shape
+		c.Simulate(1.0f);
+		RVec3 expected_pos = c.PredictPosition(RVec3::sZero(), Vec3::sZero(), c.GetSystem()->GetGravity(), 1.0f);
+		CHECK_APPROX_EQUAL(c.GetBodyInterface().GetPosition(body_id), expected_pos);
+
+		// Check that we can't hit the shape
+		Ref<Shape> box_shape = new BoxShape(Vec3::sReplicate(10000));
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(box_shape, Vec3::sOne(), RMat44::sIdentity(), CollideShapeSettings(), RVec3::sZero(), collector);
+		CHECK(collector.mHits.empty());
+
+		// Add a box to the compound shape
+		Vec3 com = shape->GetCenterOfMass();
+		shape->AddShape(Vec3::sZero(), Quat::sIdentity(), new BoxShape(Vec3::sOne()));
+		c.GetBodyInterface().NotifyShapeChanged(body_id, com, false, EActivation::DontActivate);
+
+		// Check that we can now hit the shape
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(box_shape, Vec3::sOne(), RMat44::sIdentity(), CollideShapeSettings(), RVec3::sZero(), collector);
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID2 == body_id);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ObjectLayerPairFilterMaskTests.cpp

@@ -0,0 +1,161 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h>
+#include <Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h>
+#include <Jolt/Physics/Collision/ObjectLayerPairFilterMask.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Core/JobSystemSingleThreaded.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+
+TEST_SUITE("ObjectLayerPairFilterMaskTests")
+{
+	TEST_CASE("ObjectLayerPairFilterMaskTest")
+	{
+		// Some example layers
+		constexpr uint32 FilterDefault = 1;
+		constexpr uint32 FilterStatic = 2;
+		constexpr uint32 FilterDebris = 4;
+		constexpr uint32 FilterSensor = 8;
+		constexpr uint32 FilterAll = FilterDefault | FilterStatic | FilterDebris | FilterSensor;
+
+		ObjectLayerPairFilterMask pair_filter;
+
+		ObjectLayer layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault, FilterAll);
+		ObjectLayer layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterAll);
+		CHECK(pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(pair_filter.ShouldCollide(layer2, layer1));
+
+		layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault, FilterStatic);
+		layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterDefault);
+		CHECK(pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(pair_filter.ShouldCollide(layer2, layer1));
+
+		layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault, FilterDefault);
+		layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterDefault);
+		CHECK(!pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(!pair_filter.ShouldCollide(layer2, layer1));
+
+		layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault, FilterStatic);
+		layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterStatic);
+		CHECK(!pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(!pair_filter.ShouldCollide(layer2, layer1));
+
+		layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault | FilterDebris, FilterAll);
+		layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterStatic);
+		CHECK(!pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(!pair_filter.ShouldCollide(layer2, layer1));
+
+		BroadPhaseLayerInterfaceMask bp_interface(4);
+		bp_interface.ConfigureLayer(BroadPhaseLayer(0), FilterDefault, 0); // Default goes to 0
+		bp_interface.ConfigureLayer(BroadPhaseLayer(1), FilterStatic, FilterSensor); // Static but not sensor goes to 1
+		bp_interface.ConfigureLayer(BroadPhaseLayer(2), FilterStatic, 0); // Everything else static goes to 2
+		// Last layer is for everything else
+
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault)) == BroadPhaseLayer(0));
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll)) == BroadPhaseLayer(0));
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic)) == BroadPhaseLayer(1));
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic | FilterSensor)) == BroadPhaseLayer(2));
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterDebris)) == BroadPhaseLayer(3));
+
+		ObjectVsBroadPhaseLayerFilterMask bp_filter(bp_interface);
+
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterDefault), BroadPhaseLayer(0)));
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterDefault), BroadPhaseLayer(1)));
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterDefault), BroadPhaseLayer(2)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterDefault), BroadPhaseLayer(3)));
+
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterStatic), BroadPhaseLayer(0)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterStatic), BroadPhaseLayer(1)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterStatic), BroadPhaseLayer(2)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterStatic), BroadPhaseLayer(3)));
+
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterSensor), BroadPhaseLayer(0)));
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterSensor), BroadPhaseLayer(1)));
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterSensor), BroadPhaseLayer(2)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterSensor), BroadPhaseLayer(3)));
+	}
+
+	TEST_CASE("ThreeFloorTest")
+	{
+		// Define the group bits
+		constexpr uint32 GROUP_STATIC = 1;
+		constexpr uint32 GROUP_FLOOR1 = 2;
+		constexpr uint32 GROUP_FLOOR2 = 4;
+		constexpr uint32 GROUP_FLOOR3 = 8;
+		constexpr uint32 GROUP_ALL = GROUP_STATIC | GROUP_FLOOR1 | GROUP_FLOOR2 | GROUP_FLOOR3;
+
+		ObjectLayerPairFilterMask pair_filter;
+
+		constexpr uint NUM_BROAD_PHASE_LAYERS = 2;
+		BroadPhaseLayer BP_LAYER_STATIC(0);
+		BroadPhaseLayer BP_LAYER_DYNAMIC(1);
+		BroadPhaseLayerInterfaceMask bp_interface(NUM_BROAD_PHASE_LAYERS);
+		bp_interface.ConfigureLayer(BP_LAYER_STATIC, GROUP_STATIC, 0); // Anything that has the static bit set goes into the static broadphase layer
+		bp_interface.ConfigureLayer(BP_LAYER_DYNAMIC, GROUP_FLOOR1 | GROUP_FLOOR2 | GROUP_FLOOR3, 0); // Anything that has one of the floor bits set goes into the dynamic broadphase layer
+
+		ObjectVsBroadPhaseLayerFilterMask bp_filter(bp_interface);
+
+		PhysicsSystem system;
+		system.Init(1024, 0, 1024, 1024, bp_interface, bp_filter, pair_filter);
+		BodyInterface &body_interface = system.GetBodyInterface();
+
+		// Create 3 floors, each colliding with a different group
+		RefConst<Shape> floor_shape = new BoxShape(Vec3(10, 0.1f, 10));
+		BodyID ground = body_interface.CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(20, 0.1f, 20)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_STATIC, GROUP_FLOOR1)), EActivation::DontActivate);
+		BodyID floor1 = body_interface.CreateAndAddBody(BodyCreationSettings(floor_shape, RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Static, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_STATIC, GROUP_FLOOR1)), EActivation::DontActivate);
+		BodyID floor2 = body_interface.CreateAndAddBody(BodyCreationSettings(floor_shape, RVec3(0, 4, 0), Quat::sIdentity(), EMotionType::Static, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_STATIC, GROUP_FLOOR2)), EActivation::DontActivate);
+		BodyID floor3 = body_interface.CreateAndAddBody(BodyCreationSettings(floor_shape, RVec3(0, 6, 0), Quat::sIdentity(), EMotionType::Static, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_STATIC, GROUP_FLOOR3)), EActivation::DontActivate);
+
+		// Create dynamic bodies, each colliding with a different floor
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(0.5f));
+		BodyID dynamic_floor1 = body_interface.CreateAndAddBody(BodyCreationSettings(box_shape, RVec3(0, 8, 0), Quat::sIdentity(), EMotionType::Dynamic, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_FLOOR1, GROUP_ALL)), EActivation::Activate);
+		BodyID dynamic_floor2 = body_interface.CreateAndAddBody(BodyCreationSettings(box_shape, RVec3(0, 9, 0), Quat::sIdentity(), EMotionType::Dynamic, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_FLOOR2, GROUP_ALL)), EActivation::Activate);
+		BodyID dynamic_floor3 = body_interface.CreateAndAddBody(BodyCreationSettings(box_shape, RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Dynamic, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_FLOOR3, GROUP_ALL)), EActivation::Activate);
+		BodyID dynamic_ground = body_interface.CreateAndAddBody(BodyCreationSettings(box_shape, RVec3(15, 8, 0), Quat::sIdentity(), EMotionType::Dynamic, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_FLOOR1, GROUP_ALL)), EActivation::Activate);
+
+		// Start listening to collision events
+		LoggingContactListener listener;
+		system.SetContactListener(&listener);
+
+		// Simulate long enough for all objects to fall on the ground
+		TempAllocatorImpl allocator(4 * 1024 * 1024);
+		JobSystemSingleThreaded job_system(cMaxPhysicsJobs);
+		for (int i = 0; i < 100; ++i)
+			system.Update(1.0f/ 60.0f, 1, &allocator, &job_system);
+
+		// Dynamic 1 should rest on floor 1
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, dynamic_floor1, floor1));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor1, floor2));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor1, floor3));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor1, ground));
+		float tolerance = 1.1f * system.GetPhysicsSettings().mPenetrationSlop;
+		CHECK_APPROX_EQUAL(body_interface.GetPosition(dynamic_floor1), RVec3(0, 2.6_r, 0), tolerance);
+
+		// Dynamic 2 should rest on floor 2
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor2, floor1));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, dynamic_floor2, floor2));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor2, floor3));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor2, ground));
+		CHECK_APPROX_EQUAL(body_interface.GetPosition(dynamic_floor2), RVec3(0, 4.6_r, 0), tolerance);
+
+		// Dynamic 3 should rest on floor 3
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor3, floor1));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor3, floor2));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, dynamic_floor3, floor3));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor3, ground));
+		CHECK_APPROX_EQUAL(body_interface.GetPosition(dynamic_floor3), RVec3(0, 6.6_r, 0), tolerance);
+
+		// Dynamic 4 should rest on the ground floor
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_ground, floor1));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_ground, floor2));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_ground, floor3));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, dynamic_ground, ground));
+		CHECK_APPROX_EQUAL(body_interface.GetPosition(dynamic_ground), RVec3(15, 0.6_r, 0), tolerance);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ObjectLayerPairFilterTableTests.cpp

@@ -0,0 +1,133 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h>
+#include <Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h>
+#include <Jolt/Physics/Collision/ObjectLayerPairFilterTable.h>
+
+TEST_SUITE("ObjectLayerPairFilterTableTests")
+{
+	TEST_CASE("ObjectLayerPairFilterTableTest")
+	{
+		// Init object layers
+		ObjectLayerPairFilterTable obj_vs_obj_filter(Layers::NUM_LAYERS);
+		obj_vs_obj_filter.EnableCollision(Layers::MOVING, Layers::NON_MOVING);
+		obj_vs_obj_filter.EnableCollision(Layers::MOVING, Layers::MOVING);
+		obj_vs_obj_filter.EnableCollision(Layers::MOVING, Layers::SENSOR);
+		obj_vs_obj_filter.EnableCollision(Layers::LQ_DEBRIS, Layers::NON_MOVING);
+		obj_vs_obj_filter.EnableCollision(Layers::HQ_DEBRIS, Layers::NON_MOVING);
+		obj_vs_obj_filter.EnableCollision(Layers::HQ_DEBRIS, Layers::MOVING);
+
+		// Check collision pairs
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::NON_MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::HQ_DEBRIS));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::LQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::SENSOR));
+
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::NON_MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::HQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::LQ_DEBRIS));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::SENSOR));
+
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::NON_MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::MOVING));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::HQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::LQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::SENSOR));
+
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::NON_MOVING));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::MOVING));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::HQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::LQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::SENSOR));
+
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::NON_MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::MOVING));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::HQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::LQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::SENSOR));
+
+		// Init broad phase layers
+		BroadPhaseLayerInterfaceTable bp_layer_interface(Layers::NUM_LAYERS, BroadPhaseLayers::NUM_LAYERS);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::NON_MOVING, BroadPhaseLayers::NON_MOVING);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::MOVING, BroadPhaseLayers::MOVING);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::HQ_DEBRIS, BroadPhaseLayers::MOVING);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::LQ_DEBRIS, BroadPhaseLayers::LQ_DEBRIS);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::SENSOR, BroadPhaseLayers::SENSOR);
+
+	#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		// Set layer names
+		bp_layer_interface.SetBroadPhaseLayerName(BroadPhaseLayers::NON_MOVING, "NON_MOVING");
+		bp_layer_interface.SetBroadPhaseLayerName(BroadPhaseLayers::MOVING, "MOVING");
+		bp_layer_interface.SetBroadPhaseLayerName(BroadPhaseLayers::LQ_DEBRIS, "LQ_DEBRIS");
+		bp_layer_interface.SetBroadPhaseLayerName(BroadPhaseLayers::SENSOR, "SENSOR");
+
+		// Check layer name interface
+		CHECK(strcmp(bp_layer_interface.GetBroadPhaseLayerName(BroadPhaseLayers::NON_MOVING), "NON_MOVING") == 0);
+		CHECK(strcmp(bp_layer_interface.GetBroadPhaseLayerName(BroadPhaseLayers::MOVING), "MOVING") == 0);
+		CHECK(strcmp(bp_layer_interface.GetBroadPhaseLayerName(BroadPhaseLayers::LQ_DEBRIS), "LQ_DEBRIS") == 0);
+		CHECK(strcmp(bp_layer_interface.GetBroadPhaseLayerName(BroadPhaseLayers::SENSOR), "SENSOR") == 0);
+	#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+		// Init object vs broad phase layer filter
+		ObjectVsBroadPhaseLayerFilterTable obj_vs_bp_filter(bp_layer_interface, BroadPhaseLayers::NUM_LAYERS, obj_vs_obj_filter, Layers::NUM_LAYERS);
+
+		// Check collision pairs
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::NON_MOVING, BroadPhaseLayers::NON_MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::NON_MOVING, BroadPhaseLayers::MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::NON_MOVING, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::NON_MOVING, BroadPhaseLayers::SENSOR));
+
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::MOVING, BroadPhaseLayers::NON_MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::MOVING, BroadPhaseLayers::MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::MOVING, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::MOVING, BroadPhaseLayers::SENSOR));
+
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::HQ_DEBRIS, BroadPhaseLayers::NON_MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::HQ_DEBRIS, BroadPhaseLayers::MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::HQ_DEBRIS, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::HQ_DEBRIS, BroadPhaseLayers::SENSOR));
+
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::LQ_DEBRIS, BroadPhaseLayers::NON_MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::LQ_DEBRIS, BroadPhaseLayers::MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::LQ_DEBRIS, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::LQ_DEBRIS, BroadPhaseLayers::SENSOR));
+
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::SENSOR, BroadPhaseLayers::NON_MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::SENSOR, BroadPhaseLayers::MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::SENSOR, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::SENSOR, BroadPhaseLayers::SENSOR));
+	}
+
+	TEST_CASE("ObjectLayerPairFilterTableTest2")
+	{
+		const int n = 10;
+
+		std::pair<ObjectLayer, ObjectLayer> pairs[] = {
+			{ ObjectLayer(0), ObjectLayer(0) },
+			{ ObjectLayer(9), ObjectLayer(9) },
+			{ ObjectLayer(1), ObjectLayer(3) },
+			{ ObjectLayer(3), ObjectLayer(1) },
+			{ ObjectLayer(5), ObjectLayer(7) },
+			{ ObjectLayer(7), ObjectLayer(5) }
+		};
+
+		for (auto &p : pairs)
+		{
+			ObjectLayerPairFilterTable obj_vs_obj_filter(n);
+			obj_vs_obj_filter.EnableCollision(p.first, p.second);
+
+			for (ObjectLayer i = 0; i < n; ++i)
+				for (ObjectLayer j = 0; j < n; ++j)
+				{
+					bool should_collide = (i == p.first && j == p.second) || (i == p.second && j == p.first);
+					CHECK(obj_vs_obj_filter.ShouldCollide(i, j) == should_collide);
+				}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/OffsetCenterOfMassShapeTests.cpp

@@ -0,0 +1,154 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+
+TEST_SUITE("OffsetCenterOfMassShapeTests")
+{
+	TEST_CASE("TestAddAngularImpulseCOMZero")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset 0
+		OffsetCenterOfMassShapeSettings com(Vec3::sZero(), &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add impulse
+		Vec3 cImpulse(0, 10000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddAngularImpulse(body.GetID(), cImpulse);
+		CHECK(body.IsActive());
+
+		// Check resulting velocity change
+		// dv = I^-1 * L
+		float delta_v = (1.0f / inertia_y) * cImpulse.GetY();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+
+	TEST_CASE("TestAddAngularImpulseCOMOffset")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset
+		const Vec3 cCOMOffset(5.0f, 0, 0);
+		OffsetCenterOfMassShapeSettings com(cCOMOffset, &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())) + mass * Square(cCOMOffset.GetX()); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia & https://en.wikipedia.org/wiki/Parallel_axis_theorem
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add impulse
+		Vec3 cImpulse(0, 10000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddAngularImpulse(body.GetID(), cImpulse);
+		CHECK(body.IsActive());
+
+		// Check resulting velocity change
+		// dv = I^-1 * L
+		float delta_v = (1.0f / inertia_y) * cImpulse.GetY();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+
+	TEST_CASE("TestAddTorqueCOMZero")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset 0
+		OffsetCenterOfMassShapeSettings com(Vec3::sZero(), &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add torque
+		Vec3 cTorque(0, 100000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddTorque(body.GetID(), cTorque);
+		CHECK(body.IsActive());
+		CHECK(body.GetAngularVelocity() == Vec3::sZero()); // Angular velocity change should come after the next time step
+		c.SimulateSingleStep();
+
+		// Check resulting velocity change
+		// dv = I^-1 * T * dt
+		float delta_v = (1.0f / inertia_y) * cTorque.GetY() * c.GetDeltaTime();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+
+	TEST_CASE("TestAddTorqueCOMOffset")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset
+		const Vec3 cCOMOffset(5.0f, 0, 0);
+		OffsetCenterOfMassShapeSettings com(cCOMOffset, &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())) + mass * Square(cCOMOffset.GetX()); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia & https://en.wikipedia.org/wiki/Parallel_axis_theorem
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add torque
+		Vec3 cTorque(0, 100000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddTorque(body.GetID(), cTorque);
+		CHECK(body.IsActive());
+		CHECK(body.GetAngularVelocity() == Vec3::sZero()); // Angular velocity change should come after the next time step
+		c.SimulateSingleStep();
+
+		// Check resulting velocity change
+		// dv = I^-1 * T * dt
+		float delta_v = (1.0f / inertia_y) * cTorque.GetY() * c.GetDeltaTime();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/PathConstraintTests.cpp

@@ -0,0 +1,50 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/PathConstraintPathHermite.h>
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+#include "Layers.h"
+
+TEST_SUITE("PathConstraintTests")
+{
+	// Test a straight line using a hermite spline.
+	TEST_CASE("TestPathConstraintPathHermite")
+	{
+		// A straight spline
+		// This has e.g. for t = 0.1 a local minimum at 0.7 which breaks the Newton Raphson root finding if not doing the bisection algorithm first.
+		Vec3 p1 = Vec3(1424.96313f, 468.565399f, 483.655975f);
+		Vec3 t1 = Vec3(61.4222832f, 42.8926392f, -1.70530257e-13f);
+		Vec3 n1 = Vec3(0, 0, 1);
+		Vec3 p2 = Vec3(1445.20105f, 482.364319f, 483.655975f);
+		Vec3 t2 = Vec3(20.2380009f, 13.7989082f, -5.68434189e-14f);
+		Vec3 n2 = Vec3(0, 0, 1);
+
+		// Construct path
+		Ref<PathConstraintPathHermite> path = new PathConstraintPathHermite;
+		path->AddPoint(p1, t1, n1);
+		path->AddPoint(p2, t2, n2);
+
+		// Test that positions before and after the line return 0 and 1
+		float before_start = path->GetClosestPoint(p1 - 0.01f * t1, 0.0f);
+		CHECK(before_start == 0.0f);
+		float after_end = path->GetClosestPoint(p2 + 0.01f * t2, 0.0f);
+		CHECK(after_end == 1.0f);
+
+		for (int i = 0; i <= 10; ++i)
+		{
+			// Get point on the curve
+			float fraction = 0.1f * i;
+			Vec3 pos, tgt, nrm, bin;
+			path->GetPointOnPath(fraction, pos, tgt, nrm, bin);
+
+			// Let the path determine the fraction of the closest point
+			float closest_fraction = path->GetClosestPoint(pos, 0.0f);
+
+			// Validate that it is equal to what we put in
+			CHECK_APPROX_EQUAL(fraction, closest_fraction, 1.0e-4f);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/PhysicsDeterminismTests.cpp

@@ -0,0 +1,195 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+
+TEST_SUITE("PhysicsDeterminismTests")
+{
+	struct BodyProperties
+	{
+		RVec3	mPositionCOM;
+		Quat	mRotation;
+		Vec3	mLinearVelocity;
+		Vec3	mAngularVelocity;
+		AABox	mBounds;
+		bool	mIsActive;
+	};
+
+	/// Extract all relevant properties of a body for the test
+	static void GetBodyProperties(PhysicsTestContext &ioContext, const BodyID &inBodyID, BodyProperties &outProperties)
+	{
+		BodyLockRead lock(ioContext.GetSystem()->GetBodyLockInterface(), inBodyID);
+		if (lock.SucceededAndIsInBroadPhase())
+		{
+			const Body &body = lock.GetBody();
+			outProperties.mIsActive = body.IsActive();
+			outProperties.mPositionCOM = body.GetCenterOfMassPosition();
+			outProperties.mRotation = body.GetRotation();
+			outProperties.mLinearVelocity = body.GetLinearVelocity();
+			outProperties.mAngularVelocity = body.GetAngularVelocity();
+			outProperties.mBounds = body.GetWorldSpaceBounds();
+		}
+		else
+		{
+			CHECK(false);
+		}
+	}
+
+	/// Step two physics simulations for inTotalTime and check after each step that the simulations are identical
+	static void CompareSimulations(PhysicsTestContext &ioContext1, PhysicsTestContext &ioContext2, float inTotalTime)
+	{
+		CHECK(ioContext1.GetDeltaTime() == ioContext2.GetDeltaTime());
+
+		// Step until we've stepped for inTotalTime
+		for (float t = 0; t <= inTotalTime; t += ioContext1.GetDeltaTime())
+		{
+			// Step the simulation
+			ioContext1.SimulateSingleStep();
+			ioContext2.SimulateSingleStep();
+
+			// Get all bodies
+			BodyIDVector bodies1, bodies2;
+			ioContext1.GetSystem()->GetBodies(bodies1);
+			ioContext2.GetSystem()->GetBodies(bodies2);
+			CHECK(bodies1.size() == bodies2.size());
+
+			// Loop over all bodies
+			for (size_t b = 0; b < min(bodies1.size(), bodies2.size()); ++b)
+			{
+				// Check that the body ID's match
+				BodyID b1_id = bodies1[b];
+				BodyID b2_id = bodies2[b];
+				CHECK(b1_id == b2_id);
+
+				// Get the properties of the body
+				BodyProperties properties1, properties2;
+				GetBodyProperties(ioContext1, b1_id, properties1);
+				GetBodyProperties(ioContext2, b2_id, properties2);
+				CHECK(properties1.mIsActive == properties2.mIsActive);
+				CHECK(properties1.mPositionCOM == properties2.mPositionCOM);
+				CHECK(properties1.mRotation == properties2.mRotation);
+				CHECK(properties1.mLinearVelocity == properties2.mLinearVelocity);
+				CHECK(properties1.mAngularVelocity == properties2.mAngularVelocity);
+				CHECK(properties1.mBounds.mMin == properties2.mBounds.mMin);
+				CHECK(properties1.mBounds.mMax == properties2.mBounds.mMax);
+			}
+		}
+	}
+
+	static void CreateGridOfBoxesDiscrete(PhysicsTestContext &ioContext)
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> restitution(0.0f, 1.0f);
+
+		ioContext.CreateFloor();
+
+		for (int x = 0; x < 5; ++x)
+			for (int z = 0; z < 5; ++z)
+			{
+				Body &body = ioContext.CreateBox(RVec3(float(x), 5.0f, float(z)), Quat::sRandom(random), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.1f));
+				body.SetRestitution(restitution(random));
+				body.SetLinearVelocity(Vec3::sRandom(random));
+			}
+	}
+
+	TEST_CASE("TestGridOfBoxesDiscrete")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1, 0);
+		CreateGridOfBoxesDiscrete(c1);
+
+		PhysicsTestContext c2(1.0f / 60.0f, 1, 15);
+		CreateGridOfBoxesDiscrete(c2);
+
+		CompareSimulations(c1, c2, 5.0f);
+	}
+
+	static void CreateGridOfBoxesLinearCast(PhysicsTestContext &ioContext)
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> restitution(0.0f, 1.0f);
+
+		ioContext.CreateFloor();
+
+		for (int x = 0; x < 5; ++x)
+			for (int z = 0; z < 5; ++z)
+			{
+				Body &body = ioContext.CreateBox(RVec3(float(x), 5.0f, float(z)), Quat::sRandom(random), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(0.1f));
+				body.SetRestitution(restitution(random));
+				body.SetLinearVelocity(Vec3::sRandom(random) - Vec3(0, -5.0f, 0));
+			}
+	}
+
+	TEST_CASE("TestGridOfBoxesLinearCast")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1, 0);
+		CreateGridOfBoxesLinearCast(c1);
+
+		PhysicsTestContext c2(1.0f / 60.0f, 1, 15);
+		CreateGridOfBoxesLinearCast(c2);
+
+		CompareSimulations(c1, c2, 5.0f);
+	}
+
+	static void CreateGridOfBoxesConstrained(PhysicsTestContext &ioContext)
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> restitution(0.0f, 1.0f);
+
+		ioContext.CreateFloor();
+
+		const int cNumPerAxis = 5;
+
+		// Build a collision group filter that disables collision between adjacent bodies
+		Ref<GroupFilterTable> group_filter = new GroupFilterTable(cNumPerAxis);
+		for (CollisionGroup::SubGroupID i = 0; i < cNumPerAxis - 1; ++i)
+			group_filter->DisableCollision(i, i + 1);
+
+		// Create a number of chains
+		for (int x = 0; x < cNumPerAxis; ++x)
+		{
+			// Create a chain of bodies connected with swing twist constraints
+			Body *prev_body = nullptr;
+			for (int z = 0; z < cNumPerAxis; ++z)
+			{
+				RVec3 body_pos(float(x), 5.0f, 0.2f * float(z));
+				Body &body = ioContext.CreateBox(body_pos, Quat::sRandom(random), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.1f));
+				body.SetRestitution(restitution(random));
+				body.SetLinearVelocity(Vec3::sRandom(random));
+				body.SetCollisionGroup(CollisionGroup(group_filter, CollisionGroup::GroupID(x), CollisionGroup::SubGroupID(z)));
+
+				// Constrain the body to the previous body
+				if (prev_body != nullptr)
+				{
+					SwingTwistConstraintSettings st;
+					st.mPosition1 = st.mPosition2 = body_pos - Vec3(0, 0, 0.1f);
+					st.mTwistAxis1 = st.mTwistAxis2 = Vec3::sAxisZ();
+					st.mPlaneAxis1 = st.mPlaneAxis2 = Vec3::sAxisX();
+					st.mNormalHalfConeAngle = DegreesToRadians(45.0f);
+					st.mPlaneHalfConeAngle = DegreesToRadians(30.0f);
+					st.mTwistMinAngle = DegreesToRadians(-15.0f);
+					st.mTwistMaxAngle = DegreesToRadians(15.0f);
+					Ref<Constraint> constraint = st.Create(*prev_body, body);
+					ioContext.GetSystem()->AddConstraint(constraint);
+				}
+
+				prev_body = &body;
+			}
+		}
+	}
+
+	TEST_CASE("TestGridOfBoxesConstrained")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1, 0);
+		CreateGridOfBoxesConstrained(c1);
+
+		PhysicsTestContext c2(1.0f / 60.0f, 1, 15);
+		CreateGridOfBoxesConstrained(c2);
+
+		CompareSimulations(c1, c2, 5.0f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/PhysicsStepListenerTests.cpp

@@ -0,0 +1,141 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/PhysicsStepListener.h>
+
+TEST_SUITE("StepListenerTest")
+{
+	// Custom step listener that keeps track how often it has been called
+	class TestStepListener : public PhysicsStepListener
+	{
+	public:
+		virtual void			OnStep(const PhysicsStepListenerContext &inContext) override
+		{
+			CHECK(inContext.mDeltaTime == mExpectedDeltaTime);
+			CHECK(inContext.mIsFirstStep == ((mCount % mExpectedSteps) == 0));
+			int new_count = mCount.fetch_add(1) + 1;
+			CHECK(inContext.mIsLastStep == ((new_count % mExpectedSteps) == 0));
+		}
+
+		atomic<int>				mCount = 0;
+		int						mExpectedSteps;
+		float					mExpectedDeltaTime = 0.0f;
+	};
+
+	// Perform the actual listener test with a variable amount of collision steps
+	static void DoTest(int inCollisionSteps)
+	{
+		PhysicsTestContext c(1.0f / 60.0f, inCollisionSteps);
+
+		// Initialize and add listeners
+		TestStepListener listeners[10];
+		for (TestStepListener &l : listeners)
+		{
+			l.mExpectedDeltaTime = 1.0f / 60.0f / inCollisionSteps;
+			l.mExpectedSteps = inCollisionSteps;
+		}
+		for (TestStepListener &l : listeners)
+			c.GetSystem()->AddStepListener(&l);
+
+		// Stepping without delta time should not trigger step listeners
+		c.SimulateNoDeltaTime();
+		for (TestStepListener &l : listeners)
+			CHECK(l.mCount == 0);
+
+		// Stepping with delta time should call the step listeners as they can activate bodies
+		c.SimulateSingleStep();
+		for (TestStepListener &l : listeners)
+			CHECK(l.mCount == inCollisionSteps);
+
+		// Adding an active body should have no effect, step listeners should still be called
+		c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sOne());
+
+		// Step the simulation
+		c.SimulateSingleStep();
+		for (TestStepListener &l : listeners)
+			CHECK(l.mCount == 2 * inCollisionSteps);
+
+		// Unregister all listeners
+		for (TestStepListener &l : listeners)
+			c.GetSystem()->RemoveStepListener(&l);
+
+		// Step the simulation
+		c.SimulateSingleStep();
+
+		// Check that no further callbacks were triggered
+		for (TestStepListener &l : listeners)
+			CHECK(l.mCount == 2 * inCollisionSteps);
+	}
+
+	// Test the step listeners with a single collision step
+	TEST_CASE("TestStepListener1")
+	{
+		DoTest(1);
+	}
+
+	// Test the step listeners with two collision steps
+	TEST_CASE("TestStepListener2")
+	{
+		DoTest(2);
+	}
+
+	// Test the step listeners with four collision steps
+	TEST_CASE("TestStepListener4")
+	{
+		DoTest(4);
+	}
+
+	// Activate a body in a step listener
+	TEST_CASE("TestActivateInStepListener")
+	{
+		PhysicsTestContext c(1.0f / 60.0f, 2);
+		c.ZeroGravity();
+
+		// Create a box
+		Body &body = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sOne(), EActivation::DontActivate);
+		body.GetMotionProperties()->SetLinearDamping(0.0f);
+		BodyID body_id = body.GetID();
+
+		static const Vec3 cVelocity(10.0f, 0, 0);
+
+		class MyStepListener : public PhysicsStepListener
+		{
+		public:
+								MyStepListener(const BodyID &inBodyID, BodyInterface &inBodyInterface) : mBodyInterface(inBodyInterface), mBodyID(inBodyID) { }
+
+			virtual void		OnStep(const PhysicsStepListenerContext &inContext) override
+			{
+				if (inContext.mIsFirstStep)
+				{
+					// We activate the body and set a velocity in the first step
+					CHECK(!mBodyInterface.IsActive(mBodyID));
+					mBodyInterface.SetLinearVelocity(mBodyID, cVelocity);
+					CHECK(mBodyInterface.IsActive(mBodyID));
+				}
+				else
+				{
+					// In the second step, the body should already have been activated
+					CHECK(mBodyInterface.IsActive(mBodyID));
+				}
+			}
+
+		private:
+			BodyInterface &		mBodyInterface;
+			BodyID				mBodyID;
+		};
+
+		MyStepListener listener(body_id, c.GetSystem()->GetBodyInterfaceNoLock());
+		c.GetSystem()->AddStepListener(&listener);
+
+		c.SimulateSingleStep();
+
+		BodyInterface &bi = c.GetBodyInterface();
+		CHECK(bi.IsActive(body_id));
+		CHECK(bi.GetLinearVelocity(body_id) == cVelocity);
+		CHECK(bi.GetPosition(body_id) == RVec3(c.GetDeltaTime() * cVelocity));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/RayShapeTests.cpp

@@ -0,0 +1,593 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Layers.h>
+
+TEST_SUITE("RayShapeTests")
+{
+	// Function that does the actual ray cast test, inExpectedFraction1/2 should be FLT_MAX if no hit expected
+	using TestFunction = function<void(const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)>;
+
+	// Test ray against inShape with lines going through inHitA and inHitB (which should be surface positions of the shape)
+	static void TestRayHelperInternal(Vec3Arg inHitA, Vec3Arg inHitB, TestFunction inTestFunction)
+	{
+		// Determine points before and after the surface on both sides
+		Vec3 delta = inHitB - inHitA;
+		Vec3 l1 = inHitA - 2.0f * delta;
+		Vec3 l2 = inHitA - 0.1f * delta;
+		Vec3 i1 = inHitA + 0.1f * delta;
+		Vec3 i2 = inHitB - 0.1f * delta;
+		Vec3 r1 = inHitB + 0.1f * delta;
+		Vec3 r2 = inHitB + 2.0f * delta;
+
+		// -O---->-|--------|--------
+		inTestFunction(RayCast { l1, l2 - l1 }, FLT_MAX, FLT_MAX);
+
+		// -----O>-|--------|--------
+		inTestFunction(RayCast { l2, Vec3::sZero() }, FLT_MAX, FLT_MAX);
+
+		// ------O-|->------|--------
+		inTestFunction(RayCast { l2, i1 - l2 }, 0.5f, FLT_MAX);
+
+		// ------O-|--------|->------
+		inTestFunction(RayCast { l2, r1 - l2 }, 0.1f / 1.2f, 1.1f / 1.2f);
+
+		// --------|-----O>-|--------
+		inTestFunction(RayCast { i2, Vec3::sZero() }, 0.0f, FLT_MAX);
+
+		// --------|------O-|->------
+		inTestFunction(RayCast { i2, r1 - i2 }, 0.0f, 0.5f);
+
+		// --------|--------|-O---->-
+		inTestFunction(RayCast { r1, r2 - l1 }, FLT_MAX, FLT_MAX);
+	}
+
+	static void TestRayHelper(const Shape *inShape, Vec3Arg inHitA, Vec3Arg inHitB)
+	{
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRay = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			RayCastResult hit;
+			SubShapeIDCreator id_creator;
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(inShape->CastRay(ray, id_creator, hit));
+				CHECK_APPROX_EQUAL(hit.mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				CHECK_FALSE(inShape->CastRay(ray, id_creator, hit));
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRay);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRay);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape allowing multiple hits but no back facing hits, treating convex objects as solids
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRayMultiHitIgnoreBackFace = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::IgnoreBackFaces);
+			settings.mTreatConvexAsSolid = true;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			SubShapeIDCreator id_creator;
+			inShape->CastRay(ray, settings, id_creator, collector);
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() == 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.empty());
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRayMultiHitIgnoreBackFace);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRayMultiHitIgnoreBackFace);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape allowing multiple hits and back facing hits, treating convex objects as solids
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRayMultiHitWithBackFace = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces);
+			settings.mTreatConvexAsSolid = true;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			SubShapeIDCreator id_creator;
+			inShape->CastRay(ray, settings, id_creator, collector);
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				JPH_ASSERT(inExpectedFraction2 == FLT_MAX);
+				CHECK(collector.mHits.empty());
+			}
+
+			if (inExpectedFraction2 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 2);
+				CHECK_APPROX_EQUAL(collector.mHits[1].mFraction, inExpectedFraction2, 1.0e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.size() < 2);
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRayMultiHitWithBackFace);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRayMultiHitWithBackFace);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape allowing multiple hits but no back facing hits, treating convex object as non-solids
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRayMultiHitIgnoreBackFaceNonSolid = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::IgnoreBackFaces);
+			settings.mTreatConvexAsSolid = false;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			SubShapeIDCreator id_creator;
+			inShape->CastRay(ray, settings, id_creator, collector);
+
+			// A fraction of 0 means that the ray starts in solid, we treat this as a non-hit
+			if (inExpectedFraction1 != 0.0f && inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() == 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.empty());
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRayMultiHitIgnoreBackFaceNonSolid);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRayMultiHitIgnoreBackFaceNonSolid);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape allowing multiple hits and back facing hits, treating convex object as non-solids
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRayMultiHitWithBackFaceNonSolid = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces);
+			settings.mTreatConvexAsSolid = false;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			SubShapeIDCreator id_creator;
+			inShape->CastRay(ray, settings, id_creator, collector);
+
+			// A fraction of 0 means that the ray starts in solid, we treat this as a non-hit
+			if (inExpectedFraction1 == 0.0f)
+			{
+				inExpectedFraction1 = inExpectedFraction2;
+				inExpectedFraction2 = FLT_MAX;
+			}
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				JPH_ASSERT(inExpectedFraction2 == FLT_MAX);
+				CHECK(collector.mHits.empty());
+			}
+
+			if (inExpectedFraction2 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 2);
+				CHECK_APPROX_EQUAL(collector.mHits[1].mFraction, inExpectedFraction2, 1.0e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.size() < 2);
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRayMultiHitWithBackFaceNonSolid);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRayMultiHitWithBackFaceNonSolid);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Insert the shape into the world
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+
+		// A non-zero test position for the shape
+		const Vec3 cShapePosition(2, 3, 4);
+		const Quat cShapeRotation = Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI);
+		const Mat44 cShapeMatrix = Mat44::sRotationTranslation(cShapeRotation, cShapePosition);
+
+		// Make the shape part of a body and insert it into the physics system
+		BPLayerInterfaceImpl broad_phase_layer_interface;
+		ObjectVsBroadPhaseLayerFilter object_vs_broadphase_layer_filter;
+		ObjectLayerPairFilter object_vs_object_layer_filter;
+		PhysicsSystem system;
+		system.Init(1, 0, 4, 4, broad_phase_layer_interface, object_vs_broadphase_layer_filter, object_vs_object_layer_filter);
+		system.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(inShape, RVec3(cShapePosition), cShapeRotation, EMotionType::Static, 0), EActivation::DontActivate);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test a ray against a shape through a physics system
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestSystemRay = [&system, cShapeMatrix](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// inRay is relative to shape, transform it into world space
+			RayCast ray = inRay.Transformed(cShapeMatrix);
+
+			RayCastResult hit;
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(system.GetNarrowPhaseQuery().CastRay(RRayCast(ray), hit));
+				CHECK_APPROX_EQUAL(hit.mFraction, inExpectedFraction1, 2.5e-5f);
+			}
+			else
+			{
+				CHECK_FALSE(system.GetNarrowPhaseQuery().CastRay(RRayCast(ray), hit));
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestSystemRay);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestSystemRay);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test a ray against a shape through a physics system allowing multiple hits but no back facing hits
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestSystemRayMultiHitIgnoreBackFace = [&system, cShapeMatrix](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// inRay is relative to shape, transform it into world space
+			RayCast ray = inRay.Transformed(cShapeMatrix);
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::IgnoreBackFaces);
+			settings.mTreatConvexAsSolid = true;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			system.GetNarrowPhaseQuery().CastRay(RRayCast(ray), settings, collector);
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() == 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 2.5e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.empty());
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestSystemRayMultiHitIgnoreBackFace);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestSystemRayMultiHitIgnoreBackFace);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test a ray against a shape through a physics system allowing multiple hits and back facing hits
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestSystemRayMultiHitWithBackFace = [&system, cShapeMatrix](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// inRay is relative to shape, transform it into world space
+			RayCast ray = inRay.Transformed(cShapeMatrix);
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces);
+			settings.mTreatConvexAsSolid = true;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			system.GetNarrowPhaseQuery().CastRay(RRayCast(ray), settings, collector);
+			collector.Sort();
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 2.5e-5f);
+			}
+			else
+			{
+				JPH_ASSERT(inExpectedFraction2 == FLT_MAX);
+				CHECK(collector.mHits.empty());
+			}
+
+			if (inExpectedFraction2 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 2);
+				CHECK_APPROX_EQUAL(collector.mHits[1].mFraction, inExpectedFraction2, 2.5e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.size() < 2);
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestSystemRayMultiHitWithBackFace);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestSystemRayMultiHitWithBackFace);
+	}
+
+	/// Helper function to check that a ray misses a shape
+	static void TestRayMiss(const Shape *inShape, Vec3Arg inOrigin, Vec3Arg inDirection)
+	{
+		RayCastResult hit;
+		CHECK(!inShape->CastRay({ inOrigin - inShape->GetCenterOfMass(), inDirection }, SubShapeIDCreator(), hit));
+	}
+
+	TEST_CASE("TestBoxShapeRay")
+	{
+		// Create box shape
+		BoxShape box(Vec3(2, 3, 4)); // Allocate on the stack to test embedded refcounted structs
+		box.SetEmbedded();
+		Ref<Shape> shape = &box; // Add a reference to see if we don't hit free() of a stack allocated struct
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -3, 0), Vec3(0, 3, 0));
+		TestRayHelper(shape, Vec3(0, 0, -4), Vec3(0, 0, 4));
+	}
+
+	TEST_CASE("TestSphereShapeRay")
+	{
+		// Create sphere shape
+		Ref<Shape> shape = new SphereShape(2);
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -2, 0), Vec3(0, 2, 0));
+		TestRayHelper(shape, Vec3(0, 0, -2), Vec3(0, 0, 2));
+	}
+
+	TEST_CASE("TestConvexHullShapeRay")
+	{
+		// Create convex hull shape of a box (off center so the center of mass is not zero)
+		Array<Vec3> box;
+		box.push_back(Vec3(-2, -4, -6));
+		box.push_back(Vec3(-2, -4, 7));
+		box.push_back(Vec3(-2, 5, -6));
+		box.push_back(Vec3(-2, 5, 7));
+		box.push_back(Vec3(3, -4, -6));
+		box.push_back(Vec3(3, -4, 7));
+		box.push_back(Vec3(3, 5, -6));
+		box.push_back(Vec3(3, 5, 7));
+		RefConst<Shape> shape = ConvexHullShapeSettings(box).Create().Get();
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(3, 0, 0));
+		TestRayHelper(shape, Vec3(0, -4, 0), Vec3(0, 5, 0));
+		TestRayHelper(shape, Vec3(0, 0, -6), Vec3(0, 0, 7));
+
+		TestRayMiss(shape, Vec3(-3, -5, 0), Vec3(0, 1, 0));
+		TestRayMiss(shape, Vec3(-3, 0, 0), Vec3(0, 1, 0));
+		TestRayMiss(shape, Vec3(-3, 6, 0), Vec3(0, 1, 0));
+	}
+
+	TEST_CASE("TestCapsuleShapeRay")
+	{
+		// Create capsule shape
+		Ref<Shape> shape = new CapsuleShape(4, 2);
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -6, 0), Vec3(0, 6, 0));
+		TestRayHelper(shape, Vec3(0, 0, -2), Vec3(0, 0, 2));
+	}
+
+	TEST_CASE("TestTaperedCapsuleShapeRay")
+	{
+		// Create tapered capsule shape
+		RefConst<Shape> shape = TaperedCapsuleShapeSettings(3, 4, 2).Create().Get();
+
+		TestRayHelper(shape, Vec3(0, 7, 0), Vec3(0, -5, 0)); // Top to bottom
+		TestRayHelper(shape, Vec3(-4, 3, 0), Vec3(4, 3, 0)); // Top sphere
+		TestRayHelper(shape, Vec3(0, 3, -4), Vec3(0, 3, 4)); // Top sphere
+	}
+
+	TEST_CASE("TestCylinderShapeRay")
+	{
+		// Create cylinder shape
+		Ref<Shape> shape = new CylinderShape(4, 2);
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -4, 0), Vec3(0, 4, 0));
+		TestRayHelper(shape, Vec3(0, 0, -2), Vec3(0, 0, 2));
+	}
+
+	TEST_CASE("TestTaperedCylinderShapeRay")
+	{
+		// Create tapered cylinder shape
+		Ref<Shape> shape = TaperedCylinderShapeSettings(4, 1, 3).Create().Get();
+
+		// Ray through origin
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -4, 0), Vec3(0, 4, 0));
+		TestRayHelper(shape, Vec3(0, 0, -2), Vec3(0, 0, 2));
+
+		// Ray halfway to the top
+		TestRayHelper(shape, Vec3(-1.5f, 2, 0), Vec3(1.5f, 2, 0));
+		TestRayHelper(shape, Vec3(0, 2, -1.5f), Vec3(0, 2, 1.5f));
+
+		// Ray halfway to the bottom
+		TestRayHelper(shape, Vec3(-2.5f, -2, 0), Vec3(2.5f, -2, 0));
+		TestRayHelper(shape, Vec3(0, -2, -2.5f), Vec3(0, -2, 2.5f));
+	}
+
+	TEST_CASE("TestScaledShapeRay")
+	{
+		// Create convex hull shape of a box (off center so the center of mass is not zero)
+		Array<Vec3> box;
+		box.push_back(Vec3(-2, -4, -6));
+		box.push_back(Vec3(-2, -4, 7));
+		box.push_back(Vec3(-2, 5, -6));
+		box.push_back(Vec3(-2, 5, 7));
+		box.push_back(Vec3(3, -4, -6));
+		box.push_back(Vec3(3, -4, 7));
+		box.push_back(Vec3(3, 5, -6));
+		box.push_back(Vec3(3, 5, 7));
+		RefConst<Shape> hull = ConvexHullShapeSettings(box).Create().Get();
+
+		// Scale the hull
+		Ref<Shape> shape1 = new ScaledShape(hull, Vec3(2, 3, 4));
+
+		TestRayHelper(shape1, Vec3(-4, 0, 0), Vec3(6, 0, 0));
+		TestRayHelper(shape1, Vec3(0, -12, 0), Vec3(0, 15, 0));
+		TestRayHelper(shape1, Vec3(0, 0, -24), Vec3(0, 0, 28));
+
+		// Scale the hull (and flip it inside out)
+		Ref<Shape> shape2 = new ScaledShape(hull, Vec3(-2, 3, 4));
+
+		TestRayHelper(shape2, Vec3(-6, 0, 0), Vec3(4, 0, 0));
+		TestRayHelper(shape2, Vec3(0, -12, 0), Vec3(0, 15, 0));
+		TestRayHelper(shape2, Vec3(0, 0, -24), Vec3(0, 0, 28));
+	}
+
+	TEST_CASE("TestStaticCompoundShapeRay")
+	{
+		// Create convex hull shape of a box (off center so the center of mass is not zero)
+		Array<Vec3> box;
+		box.push_back(Vec3(-2, -4, -6));
+		box.push_back(Vec3(-2, -4, 7));
+		box.push_back(Vec3(-2, 5, -6));
+		box.push_back(Vec3(-2, 5, 7));
+		box.push_back(Vec3(3, -4, -6));
+		box.push_back(Vec3(3, -4, 7));
+		box.push_back(Vec3(3, 5, -6));
+		box.push_back(Vec3(3, 5, 7));
+		RefConst<ShapeSettings> hull = new ConvexHullShapeSettings(box);
+
+		// Translate/rotate the shape through a compound (off center to force center of mass not zero)
+		const Vec3 cShape1Position(10, 20, 30);
+		const Quat cShape1Rotation = Quat::sRotation(Vec3::sAxisX(), 0.1f * JPH_PI) * Quat::sRotation(Vec3::sAxisY(), 0.2f * JPH_PI);
+		const Vec3 cShape2Position(40, 50, 60);
+		const Quat cShape2Rotation = Quat::sRotation(Vec3::sAxisZ(), 0.3f * JPH_PI);
+
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.AddShape(cShape1Position, cShape1Rotation, hull); // Shape 1
+		compound_settings.AddShape(cShape2Position, cShape2Rotation, hull); // Shape 2
+		RefConst<Shape> compound = compound_settings.Create().Get();
+
+		// Hitting shape 1
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(-2, 0, 0), cShape1Position + cShape1Rotation * Vec3(3, 0, 0));
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(0, -4, 0), cShape1Position + cShape1Rotation * Vec3(0, 5, 0));
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(0, 0, -6), cShape1Position + cShape1Rotation * Vec3(0, 0, 7));
+
+		// Hitting shape 2
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(-2, 0, 0), cShape2Position + cShape2Rotation * Vec3(3, 0, 0));
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(0, -4, 0), cShape2Position + cShape2Rotation * Vec3(0, 5, 0));
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(0, 0, -6), cShape2Position + cShape2Rotation * Vec3(0, 0, 7));
+	}
+
+	TEST_CASE("TestMutableCompoundShapeRay")
+	{
+		// Create convex hull shape of a box (off center so the center of mass is not zero)
+		Array<Vec3> box;
+		box.push_back(Vec3(-2, -4, -6));
+		box.push_back(Vec3(-2, -4, 7));
+		box.push_back(Vec3(-2, 5, -6));
+		box.push_back(Vec3(-2, 5, 7));
+		box.push_back(Vec3(3, -4, -6));
+		box.push_back(Vec3(3, -4, 7));
+		box.push_back(Vec3(3, 5, -6));
+		box.push_back(Vec3(3, 5, 7));
+		RefConst<ShapeSettings> hull = new ConvexHullShapeSettings(box);
+
+		// Translate/rotate the shape through a compound (off center to force center of mass not zero)
+		const Vec3 cShape1Position(10, 20, 30);
+		const Quat cShape1Rotation = Quat::sRotation(Vec3::sAxisX(), 0.1f * JPH_PI) * Quat::sRotation(Vec3::sAxisY(), 0.2f * JPH_PI);
+		const Vec3 cShape2Position(40, 50, 60);
+		const Quat cShape2Rotation = Quat::sRotation(Vec3::sAxisZ(), 0.3f * JPH_PI);
+
+		MutableCompoundShapeSettings compound_settings;
+		compound_settings.AddShape(cShape1Position, cShape1Rotation, hull); // Shape 1
+		compound_settings.AddShape(cShape2Position, cShape2Rotation, hull); // Shape 2
+		RefConst<Shape> compound = compound_settings.Create().Get();
+
+		// Hitting shape 1
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(-2, 0, 0), cShape1Position + cShape1Rotation * Vec3(3, 0, 0));
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(0, -4, 0), cShape1Position + cShape1Rotation * Vec3(0, 5, 0));
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(0, 0, -6), cShape1Position + cShape1Rotation * Vec3(0, 0, 7));
+
+		// Hitting shape 2
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(-2, 0, 0), cShape2Position + cShape2Rotation * Vec3(3, 0, 0));
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(0, -4, 0), cShape2Position + cShape2Rotation * Vec3(0, 5, 0));
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(0, 0, -6), cShape2Position + cShape2Rotation * Vec3(0, 0, 7));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SensorTests.cpp

@@ -0,0 +1,764 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+
+TEST_SUITE("SensorTests")
+{
+	using LogEntry = LoggingContactListener::LogEntry;
+	using EType = LoggingContactListener::EType;
+
+	TEST_CASE("TestDynamicVsSensor")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+		// Dynamic body moving downwards
+		Body &dynamic = c.CreateBox(RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// After a single step the dynamic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second and one step we should be touching the sensor
+		c.Simulate(0.5f + c.GetStepDeltaTime());
+		CHECK(listener.Contains(EType::Add, dynamic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, dynamic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic.GetID(), sensor_id));
+		listener.Clear();
+
+		// After 3 more seconds we should have left the sensor at the bottom side
+		c.Simulate(3.0f);
+		CHECK(listener.Contains(EType::Remove, dynamic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(dynamic.GetPosition(), RVec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestKinematicVsSensor")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+		// Kinematic body moving downwards
+		Body &kinematic = c.CreateBox(RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		kinematic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// After a single step the kinematic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second and one step we should be touching the sensor
+		c.Simulate(0.5f + c.GetStepDeltaTime());
+		CHECK(listener.Contains(EType::Add, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, kinematic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// After 3 more seconds we should have left the sensor at the bottom side
+		c.Simulate(3.0f);
+		CHECK(listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(kinematic.GetPosition(), RVec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestKinematicVsKinematicSensor")
+	{
+		// Same as TestKinematicVsSensor but with the sensor being an active kinematic body
+
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Kinematic sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+		// Kinematic body moving downwards
+		Body &kinematic = c.CreateBox(RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		kinematic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// After a single step the kinematic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second and one step we should be touching the sensor
+		c.Simulate(0.5f + c.GetStepDeltaTime());
+		CHECK(listener.Contains(EType::Add, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, kinematic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// After 3 more seconds we should have left the sensor at the bottom side
+		c.Simulate(3.0f);
+		CHECK(listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(kinematic.GetPosition(), RVec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestKinematicVsKinematicSensorReversed")
+	{
+		// Same as TestKinematicVsKinematicSensor but with bodies created in reverse order (this matters for Body::sFindCollidingPairsCanCollide because MotionProperties::mIndexInActiveBodies is swapped between the bodies)
+
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Kinematic body moving downwards
+		Body &kinematic = c.CreateBox(RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		kinematic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// Kinematic sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+		// After a single step the kinematic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second and one step we should be touching the sensor
+		c.Simulate(0.5f + c.GetStepDeltaTime());
+		CHECK(listener.Contains(EType::Add, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, kinematic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// After 3 more seconds we should have left the sensor at the bottom side
+		c.Simulate(3.0f);
+		CHECK(listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(kinematic.GetPosition(), RVec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestDynamicSleepingVsStaticSensor")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
+		c.GetBodyInterface().AddBody(sensor.GetID(), EActivation::DontActivate);
+
+		// Floor
+		Body &floor = c.CreateFloor();
+
+		// Dynamic body on floor (make them penetrate)
+		Body &dynamic = c.CreateBox(RVec3(0, 0.5f - c.GetSystem()->GetPhysicsSettings().mMaxPenetrationDistance, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f), EActivation::DontActivate);
+
+		// After a single step (because the object is sleeping) there should not be a contact
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// The dynamic object should not be part of an island
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+
+		// Activate the body
+		c.GetBodyInterface().ActivateBody(dynamic.GetID());
+
+		// After a single step we should have detected the collision with the floor and the sensor
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 4);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Validate, sensor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Add, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The dynamic object should be part of an island now
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+
+		// After a second the body should have gone to sleep and the contacts should have been removed
+		c.Simulate(1.0f);
+		CHECK(!dynamic.IsActive());
+		CHECK(listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
+
+		// The dynamic object should not be part of an island
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+	}
+
+	TEST_CASE("TestDynamicSleepingVsKinematicSensor")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Kinematic sensor that is active (so will keep detecting contacts with sleeping bodies)
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
+		c.GetBodyInterface().AddBody(sensor.GetID(), EActivation::Activate);
+
+		// Floor
+		Body &floor = c.CreateFloor();
+
+		// Dynamic body on floor (make them penetrate)
+		Body &dynamic = c.CreateBox(RVec3(0, 0.5f - c.GetSystem()->GetPhysicsSettings().mMaxPenetrationDistance, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f), EActivation::DontActivate);
+
+		// After a single step, there should be a contact with the sensor only (the sensor is active)
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Validate, sensor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Add, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The sensor should be in its own island
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+
+		// The second step, the contact with the sensor should have persisted
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 1);
+		CHECK(listener.Contains(EType::Persist, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The sensor should still be in its own island
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+
+		// Activate the body
+		c.GetBodyInterface().ActivateBody(dynamic.GetID());
+
+		// After a single step we should have detected collision with the floor and the collision with the sensor should have persisted
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 3);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Persist, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The sensor should not be part of the same island as the dynamic body (they won't interact, so this is not needed)
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != dynamic.GetMotionProperties()->GetIslandIndexInternal());
+
+		// After another step we should have persisted the collision with the floor and sensor
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() >= 2); // Depending on if we used the contact cache or not there will be validate callbacks too
+		CHECK(listener.Contains(EType::Persist, floor.GetID(), dynamic.GetID()));
+		CHECK(!listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Persist, sensor.GetID(), dynamic.GetID()));
+		CHECK(!listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The same islands as the previous step should have been created
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != dynamic.GetMotionProperties()->GetIslandIndexInternal());
+
+		// After a second the body should have gone to sleep and the contacts with the floor should have been removed, but not with the sensor
+		c.Simulate(1.0f);
+		CHECK(!dynamic.IsActive());
+		CHECK(listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
+		CHECK(!listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
+	}
+
+	TEST_CASE("TestSensorVsSensor")
+	{
+		for (int test = 0; test < 2; ++test)
+		{
+			bool sensor_detects_sensor = test == 1;
+
+			PhysicsTestContext c;
+
+			// Register listener
+			LoggingContactListener listener;
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Depending on the iteration we either place the sensor in the moving layer which means it will collide with other sensors
+			// or we put it in the sensor layer which means it won't collide with other sensors
+			ObjectLayer layer = sensor_detects_sensor? Layers::MOVING : Layers::SENSOR;
+
+			// Sensor 1
+			BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, layer);
+			sensor_settings.mIsSensor = true;
+			BodyID sensor_id1 = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+			// Sensor 2 moving downwards
+			sensor_settings.mMotionType = EMotionType::Kinematic;
+			sensor_settings.mPosition = RVec3(0, 3, 0);
+			sensor_settings.mIsSensor = true;
+			sensor_settings.mLinearVelocity = Vec3(0, -2, 0);
+			BodyID sensor_id2 = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+			// After a single step the sensors should not touch yet
+			c.SimulateSingleStep();
+			CHECK(listener.GetEntryCount() == 0);
+
+			// After half a second and one step the sensors should be touching
+			c.Simulate(0.5f + c.GetDeltaTime());
+			if (sensor_detects_sensor)
+				CHECK(listener.Contains(EType::Add, sensor_id1, sensor_id2));
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			listener.Clear();
+
+			// The next step we require that the contact persists
+			c.SimulateSingleStep();
+			if (sensor_detects_sensor)
+			{
+				CHECK(listener.Contains(EType::Persist, sensor_id1, sensor_id2));
+				CHECK(!listener.Contains(EType::Remove, sensor_id1, sensor_id2));
+			}
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			listener.Clear();
+
+			// After 2 more seconds we should have left the sensor at the bottom side
+			c.Simulate(2.0f);
+			if (sensor_detects_sensor)
+				CHECK(listener.Contains(EType::Remove, sensor_id1, sensor_id2));
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			CHECK_APPROX_EQUAL(c.GetBodyInterface().GetPosition(sensor_id2), sensor_settings.mPosition + sensor_settings.mLinearVelocity * (2.5f + 3.0f * c.GetDeltaTime()), 1.0e-4f);
+		}
+	}
+
+	TEST_CASE("TestContactListenerMakesSensor")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		class SensorOverridingListener : public LoggingContactListener
+		{
+		public:
+			virtual void		OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatSeesSensorID || inBody2.GetID() == mBodyThatSeesSensorID)
+					ioSettings.mIsSensor = true;
+			}
+
+			virtual void		OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactPersisted(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatSeesSensorID || inBody2.GetID() == mBodyThatSeesSensorID)
+					ioSettings.mIsSensor = true;
+			}
+
+			BodyID				mBodyThatSeesSensorID;
+		};
+
+		// Register listener
+		SensorOverridingListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Body that will appear as a sensor to one object and as static to another
+		BodyID static_id = c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(5, 1, 5)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+		// Dynamic body moving down that will do a normal collision
+		Body &dynamic1 = c.CreateBox(RVec3(-2, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic1.SetAllowSleeping(false);
+		dynamic1.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// Dynamic body moving down that will only see the static object as a sensor
+		Body &dynamic2 = c.CreateBox(RVec3(2, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic2.SetAllowSleeping(false);
+		dynamic2.SetLinearVelocity(Vec3(0, -1, 0));
+		listener.mBodyThatSeesSensorID = dynamic2.GetID();
+
+		// After a single step the dynamic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second both bodies should be touching the sensor
+		c.Simulate(0.5f);
+		CHECK(listener.Contains(EType::Add, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Add, dynamic2.GetID(), static_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, dynamic1.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Persist, dynamic2.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic2.GetID(), static_id));
+		listener.Clear();
+
+		// After 3 more seconds one body should be resting on the static body, the other should have fallen through
+		c.Simulate(3.0f + c.GetDeltaTime());
+		CHECK(listener.Contains(EType::Persist, dynamic1.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Remove, dynamic2.GetID(), static_id));
+		CHECK_APPROX_EQUAL(dynamic1.GetPosition(), RVec3(-2, 1.5f, 0), 5.0e-3f);
+		CHECK_APPROX_EQUAL(dynamic2.GetPosition(), RVec3(2, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestContactListenerMakesSensorCCD")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		class SensorOverridingListener : public LoggingContactListener
+		{
+		public:
+			virtual void		OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatBecomesSensor || inBody2.GetID() == mBodyThatBecomesSensor)
+					ioSettings.mIsSensor = true;
+			}
+
+			virtual void		OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactPersisted(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatBecomesSensor || inBody2.GetID() == mBodyThatBecomesSensor)
+					ioSettings.mIsSensor = true;
+			}
+
+			BodyID				mBodyThatBecomesSensor;
+		};
+
+		// Register listener
+		SensorOverridingListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Body that blocks the path
+		BodyID static_id = c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(0.1f, 10, 10)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+		// Dynamic body moving to the static object that will do a normal CCD collision
+		RVec3 dynamic1_pos(-0.5f, 2, 0);
+		Vec3 initial_velocity(500, 0, 0);
+		Body &dynamic1 = c.CreateBox(dynamic1_pos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(0.1f));
+		dynamic1.SetAllowSleeping(false);
+		dynamic1.SetLinearVelocity(initial_velocity);
+		dynamic1.SetRestitution(1.0f);
+
+		// Dynamic body moving through the static object that will become a sensor an thus pass through
+		RVec3 dynamic2_pos(-0.5f, -2, 0);
+		Body &dynamic2 = c.CreateBox(dynamic2_pos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(0.1f));
+		dynamic2.SetAllowSleeping(false);
+		dynamic2.SetLinearVelocity(initial_velocity);
+		dynamic2.SetRestitution(1.0f);
+		listener.mBodyThatBecomesSensor = dynamic2.GetID();
+
+		// After a single step the we should have contact added callbacks for both bodies
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Add, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Add, dynamic2.GetID(), static_id));
+		listener.Clear();
+		CHECK_APPROX_EQUAL(dynamic1.GetPosition(), dynamic1_pos + RVec3(0.3f + cPenetrationSlop, 0, 0), 1.0e-4f); // Dynamic 1 should have moved to the surface of the static body
+		CHECK_APPROX_EQUAL(dynamic2.GetPosition(), dynamic2_pos + initial_velocity * c.GetDeltaTime(), 1.0e-4f); // Dynamic 2 should have passed through the static body because it became a sensor
+
+		// The next step the sensor should have its contact removed and the CCD body should have its contact persisted because it starts penetrating
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Remove, dynamic2.GetID(), static_id));
+		listener.Clear();
+
+		// The next step all contacts have been removed
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Remove, dynamic1.GetID(), static_id));
+		listener.Clear();
+	}
+
+	TEST_CASE("TestSensorVsSubShapes")
+	{
+		PhysicsTestContext c;
+		BodyInterface &bi = c.GetBodyInterface();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Create sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = bi.CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+		// We will be testing if we receive callbacks from the individual sub shapes
+		enum class EUserData
+		{
+			Bottom,
+			Middle,
+			Top,
+		};
+
+		// Create compound with 3 sub shapes
+		Ref<StaticCompoundShapeSettings> shape_settings = new StaticCompoundShapeSettings();
+		Ref<BoxShapeSettings> shape1 = new BoxShapeSettings(Vec3::sReplicate(0.4f));
+		shape1->mUserData = (uint64)EUserData::Bottom;
+		Ref<BoxShapeSettings> shape2 = new BoxShapeSettings(Vec3::sReplicate(0.4f));
+		shape2->mUserData = (uint64)EUserData::Middle;
+		Ref<BoxShapeSettings> shape3 = new BoxShapeSettings(Vec3::sReplicate(0.4f));
+		shape3->mUserData = (uint64)EUserData::Top;
+		shape_settings->AddShape(Vec3(0, -1.0f, 0), Quat::sIdentity(), shape1);
+		shape_settings->AddShape(Vec3(0, 0.0f, 0), Quat::sIdentity(), shape2);
+		shape_settings->AddShape(Vec3(0, 1.0f, 0), Quat::sIdentity(), shape3);
+		BodyCreationSettings compound_body_settings(shape_settings, RVec3(0, 20, 0), Quat::sIdentity(), JPH::EMotionType::Dynamic, Layers::MOVING);
+		compound_body_settings.mUseManifoldReduction = false; // Turn off manifold reduction for this body so that we can get proper callbacks for individual sub shapes
+		JPH::BodyID compound_body = bi.CreateAndAddBody(compound_body_settings, JPH::EActivation::Activate);
+
+		// Simulate until the body passes the origin
+		while (bi.GetPosition(compound_body).GetY() > 0.0f)
+			c.SimulateSingleStep();
+
+		// The expected events
+		struct Expected
+		{
+
+			EType		mType;
+			EUserData	mUserData;
+		};
+		const Expected expected[] = {
+			{ EType::Add, EUserData::Bottom },
+			{ EType::Add, EUserData::Middle },
+			{ EType::Add, EUserData::Top },
+			{ EType::Remove, EUserData::Bottom },
+			{ EType::Remove, EUserData::Middle },
+			{ EType::Remove, EUserData::Top }
+		};
+		const Expected *next = expected;
+		const Expected *end = expected + size(expected);
+
+		// Loop over events that we received
+		for (size_t e = 0; e < listener.GetEntryCount(); ++e)
+		{
+			const LoggingContactListener::LogEntry &entry = listener.GetEntry(e);
+
+			// Only interested in adds/removes
+			if (entry.mType != EType::Add && entry.mType != EType::Remove)
+				continue;
+
+			// Check if we have more expected events
+			if (next >= end)
+			{
+				CHECK(false);
+				break;
+			}
+
+			// Check if it is of expected type
+			CHECK(entry.mType == next->mType);
+			CHECK(entry.mBody1 == sensor_id);
+			CHECK(entry.mManifold.mSubShapeID1 == SubShapeID());
+			CHECK(entry.mBody2 == compound_body);
+			EUserData user_data = (EUserData)bi.GetShape(compound_body)->GetSubShapeUserData(entry.mManifold.mSubShapeID2);
+			CHECK(user_data == next->mUserData);
+
+			// Next expected event
+			++next;
+		}
+
+		// Check all expected events received
+		CHECK(next == end);
+	}
+
+	TEST_CASE("TestSensorVsStatic")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Static body 1
+		Body &static1 = c.CreateSphere(RVec3::sZero(), 1.0f, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING); // Put in layer that collides with static
+		sensor_settings.mIsSensor = true;
+		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
+		BodyID sensor_id = sensor.GetID();
+		c.GetBodyInterface().AddBody(sensor_id, EActivation::Activate);
+
+		// Static body 2 (created after sensor to force higher body ID)
+		Body &static2 = c.CreateSphere(RVec3::sZero(), 1.0f, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// After a step we should not detect the static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+		listener.Clear();
+
+		// Start detecting static
+		sensor.SetCollideKinematicVsNonDynamic(true);
+
+		// After a single step we should detect both static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 4); // Should also contain validates
+		CHECK(listener.Contains(EType::Add, static1.GetID(), sensor_id));
+		CHECK(listener.Contains(EType::Add, static2.GetID(), sensor_id));
+		listener.Clear();
+
+		// Stop detecting static
+		sensor.SetCollideKinematicVsNonDynamic(false);
+
+		// After a single step we should stop detecting both static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Remove, static1.GetID(), sensor_id));
+		CHECK(listener.Contains(EType::Remove, static2.GetID(), sensor_id));
+		listener.Clear();
+	}
+
+	static void sCollideBodyVsBodyAll(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Override the back face mode so we get hits with all triangles
+		ioCollideShapeSettings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+		PhysicsSystem::sDefaultSimCollideBodyVsBody(inBody1, inBody2, inCenterOfMassTransform1, inCenterOfMassTransform2, ioCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+
+	static void sCollideBodyVsBodyPerBody(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Max 1 hit per body pair
+		AnyHitCollisionCollector<CollideShapeCollector> collector;
+		SubShapeIDCreator part1, part2;
+		CollisionDispatch::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, collector);
+		if (collector.HadHit())
+			ioCollector.AddHit(collector.mHit);
+	}
+
+	static void sCollideBodyVsBodyPerLeaf(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Max 1 hit per leaf shape pair
+		SubShapeIDCreator part1, part2;
+		CollideShapeVsShapePerLeaf<AnyHitCollisionCollector<CollideShapeCollector>>(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+
+	TEST_CASE("TestSimCollideBodyVsBody")
+	{
+		// Create pyramid with flat top
+		MeshShapeSettings pyramid;
+		pyramid.mTriangleVertices = { Float3(1, 0, 1), Float3(1, 0, -1), Float3(-1, 0, -1), Float3(-1, 0, 1), Float3(0.1f, 1, 0.1f), Float3(0.1f, 1, -0.1f), Float3(-0.1f, 1, -0.1f), Float3(-0.1f, 1, 0.1f) };
+		pyramid.mIndexedTriangles = { IndexedTriangle(0, 1, 4), IndexedTriangle(4, 1, 5), IndexedTriangle(1, 2, 5), IndexedTriangle(2, 6, 5), IndexedTriangle(2, 3, 6), IndexedTriangle(3, 7, 6), IndexedTriangle(3, 0, 7), IndexedTriangle(0, 4, 7), IndexedTriangle(4, 5, 6), IndexedTriangle(4, 6, 7) };
+		pyramid.SetEmbedded();
+
+		// Create floor of many pyramids
+		StaticCompoundShapeSettings compound;
+		for (int x = -10; x <= 10; ++x)
+			for (int z = -10; z <= 10; ++z)
+				compound.AddShape(Vec3(x * 2.0f, 0, z * 2.0f), Quat::sIdentity(), &pyramid);
+		compound.SetEmbedded();
+
+		for (int type = 0; type < 3; ++type)
+		{
+			PhysicsTestContext c;
+
+			// Register listener
+			LoggingContactListener listener;
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Create floor
+			BodyID floor_id = c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(&compound, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+			// A kinematic sensor that also detects static bodies
+			// Note that the size has been picked so that it is slightly smaller than 11 x 11 pyramids and incorporates all triangles in those pyramids
+			BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(10.99f)), RVec3(0, 5, 0), Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING); // Put in a layer that collides with static
+			sensor_settings.mIsSensor = true;
+			sensor_settings.mCollideKinematicVsNonDynamic = true;
+			sensor_settings.mUseManifoldReduction = false;
+			BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+			// Select the body vs body collision function
+			switch (type)
+			{
+			case 0:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyAll);
+				break;
+
+			case 1:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyPerLeaf);
+				break;
+
+			case 2:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyPerBody);
+				break;
+			}
+
+			// Trigger collision callbacks
+			c.SimulateSingleStep();
+
+			// Count the number of hits that were detected
+			size_t count = 0;
+			for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+			{
+				const LoggingContactListener::LogEntry &entry = listener.GetEntry(i);
+				if (entry.mType == EType::Add && entry.mBody1 == floor_id && entry.mBody2 == sensor_id)
+					++count;
+			}
+
+			// Check that we received the number of hits that we expected
+			switch (type)
+			{
+			case 0:
+				// All hits
+				CHECK(count == 11 * 11 * pyramid.mIndexedTriangles.size());
+				break;
+
+			case 1:
+				// Only 1 per sub shape
+				CHECK(count == 11 * 11);
+				break;
+
+			case 2:
+				// Only 1 per body pair
+				CHECK(count == 1);
+				break;
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ShapeFilterTests.cpp

@@ -0,0 +1,93 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/SimShapeFilter.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+
+TEST_SUITE("ShapeFilterTests")
+{
+	// Tests two spheres in one simulated body, one collides with a static platform, the other doesn't
+	TEST_CASE("TestSimShapeFilter")
+	{
+		// Test once per motion quality type
+		for (int q = 0; q < 2; ++q)
+		{
+			PhysicsTestContext c;
+
+			// Log contacts
+			LoggingContactListener contact_listener;
+			c.GetSystem()->SetContactListener(&contact_listener);
+
+			// Install simulation shape filter
+			class Filter : public SimShapeFilter
+			{
+			public:
+				virtual bool	ShouldCollide(const Body &inBody1, const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Body &inBody2, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
+				{
+					// If the platform is colliding with the compound, filter out collisions where the shape has user data 1
+					if (inBody1.GetID() == mPlatformID && inBody2.GetID() == mCompoundID)
+						return inShape2->GetUserData() != 1;
+					else if (inBody1.GetID() == mCompoundID && inBody2.GetID() == mPlatformID)
+						return inShape1->GetUserData() != 1;
+					return true;
+				}
+
+				BodyID			mPlatformID;
+				BodyID			mCompoundID;
+			};
+			Filter shape_filter;
+			c.GetSystem()->SetSimShapeFilter(&shape_filter);
+
+			// Floor
+			BodyID floor_id = c.CreateFloor().GetID();
+
+			// Platform
+			BodyInterface &bi = c.GetBodyInterface();
+			shape_filter.mPlatformID = bi.CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(10, 0.5f, 10)), RVec3(0, 3.5f, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+			// Compound shape that starts above platform
+			Ref<Shape> sphere = new SphereShape(0.5f);
+			sphere->SetUserData(1); // Don't want sphere to collide with the platform
+			Ref<Shape> sphere2 = new SphereShape(0.5f);
+			Ref<StaticCompoundShapeSettings> compound_settings = new StaticCompoundShapeSettings;
+			compound_settings->AddShape(Vec3(0, -2, 0), Quat::sIdentity(), sphere);
+			compound_settings->AddShape(Vec3(0, 2, 0), Quat::sIdentity(), sphere2);
+			Ref<StaticCompoundShape> compound = StaticCast<StaticCompoundShape>(compound_settings->Create().Get());
+			BodyCreationSettings bcs(compound, RVec3(0, 7, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			if (q == 1)
+			{
+				// For the 2nd iteration activate CCD
+				bcs.mMotionQuality = EMotionQuality::LinearCast;
+				bcs.mLinearVelocity = Vec3(0, -50, 0);
+			}
+			shape_filter.mCompoundID = bi.CreateAndAddBody(bcs, EActivation::Activate);
+
+			// Get sub shape IDs
+			SubShapeID sphere_id = compound->GetSubShapeIDFromIndex(0, SubShapeIDCreator()).GetID();
+			SubShapeID sphere2_id = compound->GetSubShapeIDFromIndex(1, SubShapeIDCreator()).GetID();
+
+			// Simulate for 2 seconds
+			c.Simulate(2.0f);
+
+			// The compound should now be resting with sphere on the platform and the sphere2 on the floor
+			CHECK_APPROX_EQUAL(bi.GetPosition(shape_filter.mCompoundID), RVec3(0, 2.5f, 0), 1.01f * c.GetSystem()->GetPhysicsSettings().mPenetrationSlop);
+			CHECK_APPROX_EQUAL(bi.GetRotation(shape_filter.mCompoundID), Quat::sIdentity());
+
+			// Check that sphere2 collided with the platform but sphere did not
+			CHECK(contact_listener.Contains(LoggingContactListener::EType::Add, shape_filter.mPlatformID, SubShapeID(), shape_filter.mCompoundID, sphere2_id));
+			CHECK(!contact_listener.Contains(LoggingContactListener::EType::Add, shape_filter.mPlatformID, SubShapeID(), shape_filter.mCompoundID, sphere_id));
+
+			// Check that sphere2 didn't collide with the floor but that the sphere did
+			CHECK(contact_listener.Contains(LoggingContactListener::EType::Add, floor_id, SubShapeID(), shape_filter.mCompoundID, sphere_id));
+			CHECK(!contact_listener.Contains(LoggingContactListener::EType::Add, floor_id, SubShapeID(), shape_filter.mCompoundID, sphere2_id));
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SixDOFConstraintTests.cpp

@@ -0,0 +1,127 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include "Layers.h"
+
+TEST_SUITE("SixDOFConstraintTests")
+{
+	// Test if the 6DOF constraint can be used to create a spring
+	TEST_CASE("TestSixDOFSpring")
+	{
+		// Configuration of the spring
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		// Test all permutations of axis
+		for (uint spring_axis = 0b001; spring_axis <= 0b111; ++spring_axis)
+		{
+			// Test all spring modes
+			for (int mode = 0; mode < 2; ++mode)
+			{
+				const RVec3 cInitialPosition(10.0f * (spring_axis & 1), 8.0f * (spring_axis & 2), 6.0f * (spring_axis & 4));
+
+				// Create a sphere
+				PhysicsTestContext context;
+				context.ZeroGravity();
+				Body& body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+				body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+				// Calculate stiffness and damping of spring
+				float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+				float omega = 2.0f * JPH_PI * cFrequency;
+				float k = m * Square(omega);
+				float c = 2.0f * m * cDamping * omega;
+
+				// Create spring
+				SixDOFConstraintSettings constraint;
+				constraint.mPosition2 = cInitialPosition;
+				for (int axis = 0; axis < 3; ++axis)
+				{
+					// Check if this axis is supposed to be a spring
+					if (((1 << axis) & spring_axis) != 0)
+					{
+						if (mode == 0)
+						{
+							// First iteration use stiffness and damping
+							constraint.mLimitsSpringSettings[axis].mMode = ESpringMode::StiffnessAndDamping;
+							constraint.mLimitsSpringSettings[axis].mStiffness = k;
+							constraint.mLimitsSpringSettings[axis].mDamping = c;
+						}
+						else
+						{
+							// Second iteration use frequency and damping
+							constraint.mLimitsSpringSettings[axis].mMode = ESpringMode::FrequencyAndDamping;
+							constraint.mLimitsSpringSettings[axis].mFrequency = cFrequency;
+							constraint.mLimitsSpringSettings[axis].mDamping = cDamping;
+						}
+						constraint.mLimitMin[axis] = constraint.mLimitMax[axis] = 0.0f;
+					}
+				}
+				context.CreateConstraint<SixDOFConstraint>(Body::sFixedToWorld, body, constraint);
+
+				// Simulate spring
+				RVec3 x = cInitialPosition;
+				Vec3 v = Vec3::sZero();
+				float dt = context.GetDeltaTime();
+				for (int i = 0; i < 120; ++i)
+				{
+					// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+					for (int axis = 0; axis < 3; ++axis)
+						if (((1 << axis) & spring_axis) != 0) // Only update velocity for axis where there is a spring
+							v.SetComponent(axis, (v[axis] - dt * k / m * float(x[axis])) / (1.0f + dt * c / m + Square(dt) * k / m));
+					x += v * dt;
+
+					// Run physics simulation
+					context.SimulateSingleStep();
+
+					// Test if simulation matches prediction
+					CHECK_APPROX_EQUAL(x, body.GetPosition(), 1.0e-5_r);
+				}
+			}
+		}
+	}
+
+	// Test combination of locked rotation axis with a 6DOF constraint
+	TEST_CASE("TestSixDOFLockedRotation")
+	{
+		PhysicsTestContext context;
+		BodyInterface &bi = context.GetBodyInterface();
+		PhysicsSystem *system = context.GetSystem();
+
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(1.0f));
+
+		// Static 'anchor' body
+		BodyCreationSettings settings1(box_shape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		Body &body1 = *bi.CreateBody(settings1);
+		bi.AddBody(body1.GetID(), EActivation::Activate);
+
+		// Dynamic body that cannot rotate around X and Y
+		const RVec3 position2(3, 0, 0);
+		const Quat rotation2 = Quat::sIdentity();
+		BodyCreationSettings settings2(box_shape, position2, rotation2, EMotionType::Dynamic, Layers::MOVING);
+		settings2.mAllowedDOFs = EAllowedDOFs::RotationZ | EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ;
+		Body &body2 = *bi.CreateBody(settings2);
+		bi.AddBody(body2.GetID(), EActivation::Activate);
+
+		// Lock all 6 axis with a 6DOF constraint
+		SixDOFConstraintSettings six_dof;
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationX);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationY);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationZ);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationX);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationY);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationZ);
+		system->AddConstraint(six_dof.Create(body1, body2));
+
+		context.Simulate(1.0f);
+
+		// Check that body didn't rotate
+		CHECK_APPROX_EQUAL(body2.GetPosition(), position2, 5.0e-3f);
+		CHECK_APPROX_EQUAL(body2.GetRotation(), rotation2, 5.0e-3f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SliderConstraintTests.cpp

@@ -0,0 +1,507 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include "Layers.h"
+
+TEST_SUITE("SliderConstraintTests")
+{
+	// Test a box attached to a slider constraint, test that the body doesn't move beyond the min limit
+	TEST_CASE("TestSliderConstraintLimitMin")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const float cLimitMin = -7.0f;
+
+		// Create group filter
+		Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Give body 2 velocity towards min limit (and ensure that it arrives well before 1 second)
+		body2.SetLinearVelocity(-Vec3(10.0f, 0, 0));
+
+		// Bodies will go through each other, make sure they don't collide
+		body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		s.mLimitsMin = cLimitMin;
+		s.mLimitsMax = 0.0f;
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Simulate
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		CHECK_APPROX_EQUAL(Vec3::sZero(), body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position
+		CHECK_APPROX_EQUAL(cInitialPos + cLimitMin * s.mSliderAxis1, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test a box attached to a slider constraint, test that the body doesn't move beyond the max limit
+	TEST_CASE("TestSliderConstraintLimitMax")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const float cLimitMax = 7.0f;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Give body 2 velocity towards max limit (and ensure that it arrives well before 1 second)
+		body2.SetLinearVelocity(Vec3(10.0f, 0, 0));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		s.mLimitsMin = 0.0f;
+		s.mLimitsMax = cLimitMax;
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Simulate
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		CHECK_APPROX_EQUAL(Vec3::sZero(), body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position
+		CHECK_APPROX_EQUAL(cInitialPos + cLimitMax * s.mSliderAxis1, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test a box attached to a slider constraint, test that a motor can drive it to a specific velocity
+	TEST_CASE("TestSliderConstraintDriveVelocityStaticVsDynamic")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const float cMotorAcceleration = 2.0f;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
+		s.mMotorSettings = MotorSettings(0.0f, 0.0f, mass * cMotorAcceleration, 0.0f);
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+		constraint.SetMotorState(EMotorState::Velocity);
+		constraint.SetTargetVelocity(1.5f * cMotorAcceleration);
+
+		// Simulate
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		Vec3 expected_vel = cMotorAcceleration * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Simulate (after 0.5 seconds it should reach the target velocity)
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		expected_vel = 1.5f * cMotorAcceleration * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position (1.5s of acceleration + 0.5s of constant speed)
+		RVec3 expected_pos = c.PredictPosition(cInitialPos, Vec3::sZero(), cMotorAcceleration * s.mSliderAxis1, 1.5f) + 0.5f * expected_vel;
+		CHECK_APPROX_EQUAL(expected_pos, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test 2 dynamic boxes attached to a slider constraint, test that a motor can drive it to a specific velocity
+	TEST_CASE("TestSliderConstraintDriveVelocityDynamicVsDynamic")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const float cMotorAcceleration = 2.0f;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		c.ZeroGravity();
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
+		s.mMotorSettings = MotorSettings(0.0f, 0.0f, mass * cMotorAcceleration, 0.0f);
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+		constraint.SetMotorState(EMotorState::Velocity);
+		constraint.SetTargetVelocity(3.0f * cMotorAcceleration);
+
+		// Simulate
+		c.Simulate(1.0f);
+
+		// Test resulting velocity (both boxes move in opposite directions with the same force, so the resulting velocity difference is 2x as big as the previous test)
+		Vec3 expected_vel = cMotorAcceleration * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(-expected_vel, body1.GetLinearVelocity(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Simulate (after 0.5 seconds it should reach the target velocity)
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		expected_vel = 1.5f * cMotorAcceleration * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(-expected_vel, body1.GetLinearVelocity(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position (1.5s of acceleration + 0.5s of constant speed)
+		RVec3 expected_pos1 = c.PredictPosition(RVec3::sZero(), Vec3::sZero(), -cMotorAcceleration * s.mSliderAxis1, 1.5f) - 0.5f * expected_vel;
+		RVec3 expected_pos2 = c.PredictPosition(cInitialPos, Vec3::sZero(), cMotorAcceleration * s.mSliderAxis1, 1.5f) + 0.5f * expected_vel;
+		CHECK_APPROX_EQUAL(expected_pos1, body1.GetPosition(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(expected_pos2, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test a box attached to a slider constraint, test that a motor can drive it to a specific position
+	TEST_CASE("TestSliderConstraintDrivePosition")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const RVec3 cMotorPos(10.0f, 0, 0);
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+		constraint.SetMotorState(EMotorState::Position);
+		constraint.SetTargetPosition(Vec3(cMotorPos - cInitialPos).Dot(s.mSliderAxis1));
+
+		// Simulate
+		c.Simulate(2.0f);
+
+		// Test resulting velocity
+		CHECK_APPROX_EQUAL(Vec3::sZero(), body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position
+		CHECK_APPROX_EQUAL(cMotorPos, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test a box attached to a slider constraint, give it initial velocity and test that the friction provides the correct deceleration
+	TEST_CASE("TestSliderConstraintFriction")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const Vec3 cInitialVelocity(10.0f, 0, 0);
+		const float cFrictionAcceleration = 2.0f;
+		const float cSimulationTime = 2.0f;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+		body2.SetLinearVelocity(cInitialVelocity);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
+		s.mMaxFrictionForce = mass * cFrictionAcceleration;
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Simulate while applying friction
+		c.Simulate(cSimulationTime);
+
+		// Test resulting velocity
+		Vec3 expected_vel = cInitialVelocity - cFrictionAcceleration * cSimulationTime * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position
+		RVec3 expected_pos = c.PredictPosition(cInitialPos, cInitialVelocity, -cFrictionAcceleration * s.mSliderAxis1, cSimulationTime);
+		CHECK_APPROX_EQUAL(expected_pos, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderStaticVsKinematic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the kinematic body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body2.GetID());
+		CHECK(!body1.IsActive());
+		CHECK(body2.IsActive());
+
+		// The static body should not become active (it can't)
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(body2.IsActive());
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderStaticVsDynamic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the dynamic body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body2.GetID());
+		CHECK(!body1.IsActive());
+		CHECK(body2.IsActive());
+
+		// The static body should not become active (it can't)
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(body2.IsActive());
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderKinematicVsDynamic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the keyframed body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body1.GetID());
+		CHECK(body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, both bodies should be active now
+		c.SimulateSingleStep();
+		CHECK(body1.IsActive());
+		CHECK(body2.IsActive());
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderKinematicVsKinematic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the first keyframed body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body1.GetID());
+		CHECK(body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the second keyframed body should not be woken up
+		c.SimulateSingleStep();
+		CHECK(body1.IsActive());
+		CHECK(!body2.IsActive());
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderDynamicVsDynamic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the first dynamic body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body1.GetID());
+		CHECK(body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, both bodies should be active now
+		c.SimulateSingleStep();
+		CHECK(body1.IsActive());
+		CHECK(body2.IsActive());
+	}
+
+	// Test that when a reference frame is provided, the slider constraint is correctly constructed
+	TEST_CASE("TestSliderReferenceFrame")
+	{
+		// Create two boxes in semi random position/orientation
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3(1, 2, 3), Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.1f * JPH_PI), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::Activate);
+		Body &body2 = c.CreateBox(RVec3(-3, -2, -1), Quat::sRotation(Vec3(1, 0, 1).Normalized(), 0.2f * JPH_PI), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::Activate);
+
+		// Disable collision between the boxes
+		GroupFilterTable *group_filter = new GroupFilterTable(2);
+		group_filter->DisableCollision(0, 1);
+		body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+
+		// Get their transforms
+		RMat44 t1 = body1.GetCenterOfMassTransform();
+		RMat44 t2 = body2.GetCenterOfMassTransform();
+
+		// Create slider constraint so that slider connects the bodies at their center of mass and rotated XY -> YZ
+		SliderConstraintSettings s;
+		s.mPoint1 = t1.GetTranslation();
+		s.mSliderAxis1 = t1.GetColumn3(0);
+		s.mNormalAxis1 = t1.GetColumn3(1);
+		s.mPoint2 = t2.GetTranslation();
+		s.mSliderAxis2 = t2.GetColumn3(1);
+		s.mNormalAxis2 = t2.GetColumn3(2);
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Activate the motor to drive to 0
+		constraint.SetMotorState(EMotorState::Position);
+		constraint.SetTargetPosition(0);
+
+		// Simulate for a second
+		c.Simulate(1.0f);
+
+		// Now the bodies should have aligned so their COM is at the same position and they're rotated XY -> YZ
+		t1 = body1.GetCenterOfMassTransform();
+		t2 = body2.GetCenterOfMassTransform();
+		CHECK_APPROX_EQUAL(t1.GetColumn3(0), t2.GetColumn3(1), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetColumn3(1), t2.GetColumn3(2), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetColumn3(2), t2.GetColumn3(0), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetTranslation(), t2.GetTranslation(), 1.0e-2f);
+	}
+
+	// Test if the slider constraint can be used to create a spring
+	TEST_CASE("TestSliderSpring")
+	{
+		// Configuration of the spring
+		const RVec3 cInitialPosition(10, 0, 0);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			Body &body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+			body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+			// Calculate stiffness and damping of spring
+			float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = m * Square(omega);
+			float c = 2.0f * m * cDamping * omega;
+
+			// Create spring
+			SliderConstraintSettings constraint;
+			constraint.mPoint2 = cInitialPosition;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mLimitsMin = constraint.mLimitsMax = 0.0f;
+			context.CreateConstraint<SliderConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Simulate spring
+			Real x = cInitialPosition.GetX();
+			float v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				v = (v - dt * k / m * float(x)) / (1.0f + dt * c / m + Square(dt) * k / m);
+				x += v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(x, body.GetPosition().GetX(), 5.0e-6_r);
+				CHECK_APPROX_EQUAL(body.GetPosition().GetY(), 0);
+				CHECK_APPROX_EQUAL(body.GetPosition().GetZ(), 0);
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SoftBodyTests.cpp

@@ -0,0 +1,148 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+
+TEST_SUITE("SoftBodyTests")
+{
+	TEST_CASE("TestBendConstraint")
+	{
+		// Possible values for x3
+		const Float3 x3_values[] = {
+			Float3(0, 0, 1),	// forming flat plane
+			Float3(0, 0, -1),	// overlapping
+			Float3(0, 1, 0),	// 90 degrees concave
+			Float3(0, -1, 0),	// 90 degrees convex
+			Float3(0, 1, 1),	// 45 degrees concave
+			Float3(0, -1, -1)	// 135 degrees convex
+		};
+
+		for (const Float3 &x3 : x3_values)
+		{
+			PhysicsTestContext c;
+			PhysicsSystem *s = c.GetSystem();
+			BodyInterface &bi = s->GetBodyInterface();
+
+			// Create settings
+			Ref<SoftBodySharedSettings> shared_settings = new SoftBodySharedSettings;
+
+			/* Create two triangles with a shared edge, x3 = free, the rest is locked
+			   x2
+			e1/  \e3
+			 /    \
+			x0----x1
+			 \ e0 /
+			e2\  /e4
+			   x3
+			*/
+			SoftBodySharedSettings::Vertex v;
+			v.mPosition = Float3(-1, 0, 0);
+			v.mInvMass = 0;
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = Float3(1, 0, 0);
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = Float3(0, 0, -1);
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = x3;
+			v.mInvMass = 1;
+			shared_settings->mVertices.push_back(v);
+
+			// Create the 2 triangles
+			shared_settings->AddFace(SoftBodySharedSettings::Face(0, 1, 2));
+			shared_settings->AddFace(SoftBodySharedSettings::Face(0, 3, 1));
+
+			// Create edge and dihedral constraints
+			SoftBodySharedSettings::VertexAttributes va;
+			va.mShearCompliance = FLT_MAX;
+			va.mBendCompliance = 0;
+			shared_settings->CreateConstraints(&va, 1, SoftBodySharedSettings::EBendType::Dihedral);
+
+			// Optimize the settings
+			shared_settings->Optimize();
+
+			// Create the soft body
+			SoftBodyCreationSettings sb_settings(shared_settings, RVec3::sZero(), Quat::sIdentity(), Layers::MOVING);
+			sb_settings.mGravityFactor = 0.0f;
+			sb_settings.mAllowSleeping = false;
+			sb_settings.mUpdatePosition = false;
+			Body &body = *bi.CreateSoftBody(sb_settings);
+			bi.AddBody(body.GetID(), EActivation::Activate);
+			SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
+
+			// Test 4 angles to see if there are singularities (the dot product between the triangles has the same value for 2 configurations)
+			for (float angle : { 0.0f, 90.0f, 180.0f, 270.0f })
+			{
+				// Perturb x3
+				Vec3 perturbed_x3(x3);
+				mp->GetVertex(3).mPosition = 0.5f * (Mat44::sRotationX(DegreesToRadians(angle)) * perturbed_x3);
+
+				// Simulate
+				c.Simulate(0.25f);
+
+				// Should return to the original position
+				CHECK_APPROX_EQUAL(mp->GetVertex(3).mPosition, Vec3(x3), 1.0e-3f);
+			}
+		}
+	}
+
+	// Test that applying a force to a soft body and rigid body of the same mass has the same effect
+	TEST_CASE("TestApplyForce")
+	{
+		PhysicsTestContext c;
+		PhysicsSystem *s = c.GetSystem();
+		BodyInterface &bi = s->GetBodyInterface();
+
+		// Soft body cube
+		SoftBodyCreationSettings sb_box_settings(SoftBodySharedSettings::sCreateCube(6, 0.2f), RVec3::sZero(), Quat::sIdentity(), Layers::MOVING);
+		sb_box_settings.mGravityFactor = 0.0f;
+		sb_box_settings.mLinearDamping = 0.0f;
+		Body &sb_box = *bi.CreateSoftBody(sb_box_settings);
+		BodyID sb_id = sb_box.GetID();
+		bi.AddBody(sb_id, EActivation::Activate);
+		constexpr float cMass = 216; // 6 * 6 * 6 * 1 kg
+		CHECK_APPROX_EQUAL(sb_box.GetMotionProperties()->GetInverseMass(), 1.0f / cMass);
+
+		// Rigid body cube of same size and mass
+		const RVec3 cRBBoxPos(0, 2, 0);
+		BodyCreationSettings rb_box_settings(new BoxShape(Vec3::sReplicate(0.5f)), cRBBoxPos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		rb_box_settings.mGravityFactor = 0.0f;
+		rb_box_settings.mLinearDamping = 0.0f;
+		rb_box_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+		rb_box_settings.mMassPropertiesOverride.mMass = cMass;
+		Body &rb_box = *bi.CreateBody(rb_box_settings);
+		BodyID rb_id = rb_box.GetID();
+		bi.AddBody(rb_id, EActivation::Activate);
+
+		// Simulate for 3 seconds while applying the same force
+		constexpr int cNumSteps = 180;
+		const Vec3 cForce(10000.0f, 0, 0);
+		for (int i = 0; i < cNumSteps; ++i)
+		{
+			bi.AddForce(sb_id, cForce, EActivation::Activate);
+			bi.AddForce(rb_id, cForce, EActivation::Activate);
+			c.SimulateSingleStep();
+		}
+
+		// Check that the rigid body moved as expected
+		const float cTotalTime = cNumSteps * c.GetStepDeltaTime();
+		const Vec3 cAcceleration = cForce / cMass;
+		const RVec3 cExpectedPos = c.PredictPosition(cRBBoxPos, Vec3::sZero(), cAcceleration, cTotalTime);
+		CHECK_APPROX_EQUAL(rb_box.GetPosition(), cExpectedPos);
+		const Vec3 cExpectedVel = cAcceleration * cTotalTime;
+		CHECK_APPROX_EQUAL(rb_box.GetLinearVelocity(), cExpectedVel, 1.0e-3f);
+		CHECK_APPROX_EQUAL(rb_box.GetAngularVelocity(), Vec3::sZero());
+
+		// Check that the soft body moved within 1% of that
+		const RVec3 cExpectedPosSB = cExpectedPos - cRBBoxPos;
+		CHECK_APPROX_EQUAL(sb_box.GetPosition(), cExpectedPosSB, 0.01f * cExpectedPosSB.Length());
+		CHECK_APPROX_EQUAL(sb_box.GetLinearVelocity(), cExpectedVel, 2.0e-3f);
+		CHECK_APPROX_EQUAL(sb_box.GetAngularVelocity(), Vec3::sZero(), 0.01f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SubShapeIDTest.cpp

@@ -0,0 +1,75 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+TEST_SUITE("SubShapeIDTest")
+{
+	struct SSPair
+	{
+		uint32		mValue;
+		uint		mNumBits;
+	};
+
+	using SSPairs = Array<SSPair>;
+
+	// Helper function that pushes sub shape ID's on the creator and checks that they come out again
+	static void TestPushPop(const SSPairs &inPairs)
+	{
+		// Push all id's on the creator
+		SubShapeIDCreator creator;
+		int total_bits = 0;
+		for (const SSPair &p : inPairs)
+		{
+			creator = creator.PushID(p.mValue, p.mNumBits);
+			total_bits += p.mNumBits;
+		}
+		CHECK(creator.GetNumBitsWritten() == total_bits);
+
+		// Now pop all parts
+		SubShapeID id = creator.GetID();
+		for (const SSPair &p : inPairs)
+		{
+			// There should be data (note there is a possibility of a false positive if the bit pattern is all 1's)
+			CHECK(!id.IsEmpty());
+
+			// Pop the part
+			SubShapeID remainder;
+			uint32 value = id.PopID(p.mNumBits, remainder);
+
+			// Check value
+			CHECK(value == p.mValue);
+
+			// Continue with the remainder
+			id = remainder;
+		}
+
+		CHECK(id.IsEmpty());
+	}
+
+	TEST_CASE("SubShapeIDTest")
+	{
+		// Test storing some values
+		TestPushPop({ { 0b110101010, 9 }, { 0b0101010101, 10 }, { 0b10110101010, 11 } });
+
+		// Test storing some values with a different pattern
+		TestPushPop({ { 0b001010101, 9 }, { 0b1010101010, 10 }, { 0b01001010101, 11 } });
+
+		// Test storing up to 32 bits
+		TestPushPop({ { 0b10, 2 }, { 0b1110101010, 10 }, { 0b0101010101, 10 }, { 0b1010101010, 10 } });
+
+		// Test storing up to 32 bits with a different pattern
+		TestPushPop({ { 0b0001010101, 10 }, { 0b1010101010, 10 }, { 0b0101010101, 10 }, { 0b01, 2 } });
+
+		// Test storing 0 bits
+		TestPushPop({ { 0b10, 2 }, { 0b1110101010, 10 }, { 0, 0 }, { 0b0101010101, 10 }, { 0, 0 }, { 0b1010101010, 10 } });
+
+		// Test 32 bits at once
+		TestPushPop({ { 0b10101010101010101010101010101010, 32 } });
+
+		// Test 32 bits at once with a different pattern
+		TestPushPop({ { 0b01010101010101010101010101010101, 32 } });
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/TaperedCylinderShapeTests.cpp

@@ -0,0 +1,104 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+
+TEST_SUITE("TaperedCylinderShapeTests")
+{
+	TEST_CASE("TestMassAndInertia")
+	{
+		const float cDensity = 3.0f;
+		const float cRadius = 5.0f;
+		const float cHeight = 7.0f;
+
+		TaperedCylinderShapeSettings settings1(0.5f * cHeight, cRadius, 0.0f, 0.0f);
+		settings1.SetDensity(cDensity);
+
+		TaperedCylinderShapeSettings settings2(0.5f * cHeight, 0.0f, cRadius, 0.0f);
+		settings2.SetDensity(cDensity);
+
+		RefConst<TaperedCylinderShape> cylinder1 = StaticCast<TaperedCylinderShape>(settings1.Create().Get());
+		RefConst<TaperedCylinderShape> cylinder2 = StaticCast<TaperedCylinderShape>(settings2.Create().Get());
+
+		// Check accessors
+		CHECK(cylinder1->GetTopRadius() == cRadius);
+		CHECK(cylinder1->GetBottomRadius() == 0.0f);
+		CHECK(cylinder1->GetConvexRadius() == 0.0f);
+		CHECK_APPROX_EQUAL(cylinder1->GetHalfHeight(), 0.5f * cHeight);
+
+		MassProperties m1 = cylinder1->GetMassProperties();
+		MassProperties m2 = cylinder2->GetMassProperties();
+
+		// Mass/inertia is the same for both shapes because they are mirrored versions (inertia is calculated from COM)
+		CHECK_APPROX_EQUAL(m1.mMass, m2.mMass);
+		CHECK_APPROX_EQUAL(m1.mInertia, m2.mInertia);
+
+		// Center of mass for a cone is at 1/4 h (if cone runs from -h/2 to h/2)
+		// See: https://www.miniphysics.com/uy1-centre-of-mass-of-a-cone.html
+		Vec3 expected_com1(0, cHeight / 4.0f, 0);
+		Vec3 expected_com2 = -expected_com1;
+		CHECK_APPROX_EQUAL(cylinder1->GetCenterOfMass(), expected_com1);
+		CHECK_APPROX_EQUAL(cylinder2->GetCenterOfMass(), expected_com2);
+
+		// Mass of cone
+		float expected_mass = cDensity * JPH_PI * Square(cRadius) * cHeight / 3.0f;
+		CHECK_APPROX_EQUAL(expected_mass, m1.mMass);
+
+		// Inertia of cone (according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia)
+		float expected_inertia_xx = expected_mass * (3.0f / 20.0f * Square(cRadius) + 3.0f / 80.0f * Square(cHeight));
+		float expected_inertia_yy = expected_mass * (3.0f / 10.0f * Square(cRadius));
+		CHECK_APPROX_EQUAL(expected_inertia_xx, m1.mInertia(0, 0), 1.0e-3f);
+		CHECK_APPROX_EQUAL(expected_inertia_yy, m1.mInertia(1, 1), 1.0e-3f);
+		CHECK_APPROX_EQUAL(expected_inertia_xx, m1.mInertia(2, 2), 1.0e-3f);
+	}
+
+	TEST_CASE("TestCollidePoint")
+	{
+		const float cTopRadius = 3.0f;
+		const float cBottomRadius = 5.0f;
+		const float cHalfHeight = 3.5f;
+
+		RefConst<Shape> shape = TaperedCylinderShapeSettings(cHalfHeight, cTopRadius, cBottomRadius).Create().Get();
+
+		auto test_inside = [shape](Vec3Arg inPoint)
+		{
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPoint - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+		};
+
+		auto test_outside = [shape](Vec3Arg inPoint)
+		{
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPoint - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 0);
+		};
+
+		constexpr float cEpsilon = 1.0e-3f;
+
+		test_inside(Vec3::sZero());
+
+		// Top plane
+		test_inside(Vec3(0, cHalfHeight - cEpsilon, 0));
+		test_outside(Vec3(0, cHalfHeight + cEpsilon, 0));
+
+		// Bottom plane
+		test_inside(Vec3(0, -cHalfHeight + cEpsilon, 0));
+		test_outside(Vec3(0, -cHalfHeight - cEpsilon, 0));
+
+		// COM plane
+		test_inside(Vec3(0.5f * (cTopRadius + cBottomRadius) - cEpsilon, 0, 0));
+		test_outside(Vec3(0.5f * (cTopRadius + cBottomRadius) + cEpsilon, 0, 0));
+
+		// At quarter h above COM plane
+		float h = 0.5f * cHalfHeight;
+		float r = cBottomRadius + (cTopRadius - cBottomRadius) * (h + cHalfHeight) / (2.0f * cHalfHeight);
+		test_inside(Vec3(0, h, r - cEpsilon));
+		test_outside(Vec3(0, h, r + cEpsilon));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/TransformedShapeTests.cpp

@@ -0,0 +1,105 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+
+TEST_SUITE("TransformedShapeTests")
+{
+	TEST_CASE("TestTransformedShape")
+	{
+		const Vec3 half_extents(0.5f, 1.0f, 1.5f);
+		const Vec3 scale(-2, 3, 4);
+		const Vec3 rtshape_translation(1, 3, 5);
+		const Quat rtshape_rotation = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.25f * JPH_PI);
+		const RVec3 translation(13, 9, 7);
+		const Quat rotation = Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI); // A rotation of 90 degrees in order to not shear the shape
+
+		PhysicsMaterialSimple *material = new PhysicsMaterialSimple("Test Material", Color::sRed);
+
+		// Create a scaled, rotated and translated box
+		BoxShapeSettings box_settings(half_extents, 0.0f, material);
+		box_settings.SetEmbedded();
+		ScaledShapeSettings scale_settings(&box_settings, scale);
+		scale_settings.SetEmbedded();
+		RotatedTranslatedShapeSettings rtshape_settings(rtshape_translation, rtshape_rotation, &scale_settings);
+		rtshape_settings.SetEmbedded();
+
+		// Create a body with this shape
+		PhysicsTestContext c;
+		Body &body = c.CreateBody(&rtshape_settings, translation, rotation, EMotionType::Static, EMotionQuality::Discrete, 0, EActivation::DontActivate);
+
+		// Collect the leaf shape transform
+		AllHitCollisionCollector<TransformedShapeCollector> collector;
+		c.GetSystem()->GetNarrowPhaseQuery().CollectTransformedShapes(AABox::sBiggest(), collector);
+
+		// Check that there is exactly 1 shape
+		CHECK(collector.mHits.size() == 1);
+		TransformedShape &ts = collector.mHits.front();
+
+		// Check that we got the leaf shape: box
+		CHECK(ts.mShape == box_settings.Create().Get());
+
+		// Check that its transform matches the transform that we provided
+		RMat44 calc_transform = RMat44::sRotationTranslation(rotation, translation) * Mat44::sRotationTranslation(rtshape_rotation, rtshape_translation) * RMat44::sScale(scale);
+		CHECK_APPROX_EQUAL(calc_transform, ts.GetWorldTransform());
+
+		// Check that all corner points are in the bounding box
+		AABox aabox = ts.GetWorldSpaceBounds();
+		Vec3 corners[] = {
+			Vec3(-0.99f, -0.99f, -0.99f) * half_extents,
+			Vec3( 0.99f, -0.99f, -0.99f) * half_extents,
+			Vec3(-0.99f,  0.99f, -0.99f) * half_extents,
+			Vec3( 0.99f,  0.99f, -0.99f) * half_extents,
+			Vec3(-0.99f, -0.99f,  0.99f) * half_extents,
+			Vec3( 0.99f, -0.99f,  0.99f) * half_extents,
+			Vec3(-0.99f,  0.99f,  0.99f) * half_extents,
+			Vec3( 0.99f,  0.99f,  0.99f) * half_extents
+		};
+		for (Vec3 corner : corners)
+		{
+			CHECK(aabox.Contains(calc_transform * corner));
+			CHECK(!aabox.Contains(calc_transform * (2 * corner))); // Check that points twice as far away are not in the box
+		}
+
+		// Now pick a point on the box near the edge in local space, determine a raycast that hits it
+		const Vec3 point_on_box(half_extents.GetX() - 0.01f, half_extents.GetY() - 0.01f, half_extents.GetZ());
+		const Vec3 normal_on_box(0, 0, 1);
+		const Vec3 ray_direction_local(1, 1, -1);
+
+		// Transform to world space and do the raycast
+		Vec3 ray_start_local = point_on_box - ray_direction_local;
+		Vec3 ray_end_local = point_on_box + ray_direction_local;
+		RVec3 ray_start_world = calc_transform * ray_start_local;
+		RVec3 ray_end_world = calc_transform * ray_end_local;
+		Vec3 ray_direction_world = Vec3(ray_end_world - ray_start_world);
+		RRayCast ray_in_world { ray_start_world, ray_direction_world };
+		RayCastResult hit;
+		ts.CastRay(ray_in_world, hit);
+
+		// Check the hit result
+		CHECK_APPROX_EQUAL(hit.mFraction, 0.5f);
+		CHECK(hit.mBodyID == body.GetID());
+		CHECK(ts.GetMaterial(hit.mSubShapeID2) == material);
+		Vec3 world_space_normal = ts.GetWorldSpaceSurfaceNormal(hit.mSubShapeID2, ray_in_world.GetPointOnRay(hit.mFraction));
+		Vec3 expected_normal = (calc_transform.GetDirectionPreservingMatrix() * normal_on_box).Normalized();
+		CHECK_APPROX_EQUAL(world_space_normal, expected_normal);
+
+		// Reset the transform to identity and check that it worked
+		ts.SetWorldTransform(RMat44::sIdentity());
+		CHECK_APPROX_EQUAL(ts.GetWorldTransform(), RMat44::sIdentity());
+
+		// Set the calculated world transform again to see if getting/setting a transform is symmetric
+		ts.SetWorldTransform(calc_transform);
+		CHECK_APPROX_EQUAL(calc_transform, ts.GetWorldTransform());
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/WheeledVehicleTests.cpp

@@ -0,0 +1,347 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include "Layers.h"
+
+TEST_SUITE("WheeledVehicleTests")
+{
+	enum
+	{
+		FL_WHEEL,
+		FR_WHEEL,
+		BL_WHEEL,
+		BR_WHEEL
+	};
+
+	// Simplified vehicle settings
+	struct VehicleSettings
+	{
+		RVec3		mPosition { 0, 2, 0 };
+		bool		mUseCastSphere = true;
+		float		mWheelRadius = 0.3f;
+		float		mWheelWidth = 0.1f;
+		float		mHalfVehicleLength = 2.0f;
+		float		mHalfVehicleWidth = 0.9f;
+		float		mHalfVehicleHeight = 0.2f;
+		float		mWheelOffsetHorizontal = 1.4f;
+		float		mWheelOffsetVertical = 0.18f;
+		float		mSuspensionMinLength = 0.3f;
+		float		mSuspensionMaxLength = 0.5f;
+		float		mMaxSteeringAngle = DegreesToRadians(30);
+		bool		mFourWheelDrive = false;
+		float		mFrontBackLimitedSlipRatio = 1.4f;
+		float		mLeftRightLimitedSlipRatio = 1.4f;
+		bool		mAntiRollbar = true;
+	};
+
+	// Helper function to create a vehicle
+	static VehicleConstraint *AddVehicle(PhysicsTestContext &inContext, VehicleSettings &inSettings)
+	{
+		// Create vehicle body
+		RefConst<Shape> car_shape = OffsetCenterOfMassShapeSettings(Vec3(0, -inSettings.mHalfVehicleHeight, 0), new BoxShape(Vec3(inSettings.mHalfVehicleWidth, inSettings.mHalfVehicleHeight, inSettings.mHalfVehicleLength))).Create().Get();
+		BodyCreationSettings car_body_settings(car_shape, inSettings.mPosition, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		car_body_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+		car_body_settings.mMassPropertiesOverride.mMass = 1500.0f;
+		Body *car_body = inContext.GetBodyInterface().CreateBody(car_body_settings);
+		inContext.GetBodyInterface().AddBody(car_body->GetID(), EActivation::Activate);
+
+		// Create vehicle constraint
+		VehicleConstraintSettings vehicle;
+		vehicle.mDrawConstraintSize = 0.1f;
+		vehicle.mMaxPitchRollAngle = DegreesToRadians(60.0f);
+
+		// Wheels
+		WheelSettingsWV *fl = new WheelSettingsWV;
+		fl->mPosition = Vec3(inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, inSettings.mWheelOffsetHorizontal);
+		fl->mMaxSteerAngle = inSettings.mMaxSteeringAngle;
+		fl->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
+
+		WheelSettingsWV *fr = new WheelSettingsWV;
+		fr->mPosition = Vec3(-inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, inSettings.mWheelOffsetHorizontal);
+		fr->mMaxSteerAngle = inSettings.mMaxSteeringAngle;
+		fr->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
+
+		WheelSettingsWV *bl = new WheelSettingsWV;
+		bl->mPosition = Vec3(inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, -inSettings.mWheelOffsetHorizontal);
+		bl->mMaxSteerAngle = 0.0f;
+
+		WheelSettingsWV *br = new WheelSettingsWV;
+		br->mPosition = Vec3(-inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, -inSettings.mWheelOffsetHorizontal);
+		br->mMaxSteerAngle = 0.0f;
+
+		vehicle.mWheels.resize(4);
+		vehicle.mWheels[FL_WHEEL] = fl;
+		vehicle.mWheels[FR_WHEEL] = fr;
+		vehicle.mWheels[BL_WHEEL] = bl;
+		vehicle.mWheels[BR_WHEEL] = br;
+
+		for (WheelSettings *w : vehicle.mWheels)
+		{
+			w->mRadius = inSettings.mWheelRadius;
+			w->mWidth = inSettings.mWheelWidth;
+			w->mSuspensionMinLength = inSettings.mSuspensionMinLength;
+			w->mSuspensionMaxLength = inSettings.mSuspensionMaxLength;
+		}
+
+		WheeledVehicleControllerSettings *controller = new WheeledVehicleControllerSettings;
+		vehicle.mController = controller;
+
+		// Differential
+		controller->mDifferentials.resize(inSettings.mFourWheelDrive? 2 : 1);
+		controller->mDifferentials[0].mLeftWheel = FL_WHEEL;
+		controller->mDifferentials[0].mRightWheel = FR_WHEEL;
+		controller->mDifferentials[0].mLimitedSlipRatio = inSettings.mLeftRightLimitedSlipRatio;
+		controller->mDifferentialLimitedSlipRatio = inSettings.mFrontBackLimitedSlipRatio;
+		if (inSettings.mFourWheelDrive)
+		{
+			controller->mDifferentials[1].mLeftWheel = BL_WHEEL;
+			controller->mDifferentials[1].mRightWheel = BR_WHEEL;
+			controller->mDifferentials[1].mLimitedSlipRatio = inSettings.mLeftRightLimitedSlipRatio;
+
+			// Split engine torque
+			controller->mDifferentials[0].mEngineTorqueRatio = controller->mDifferentials[1].mEngineTorqueRatio = 0.5f;
+		}
+
+		// Anti rollbars
+		if (inSettings.mAntiRollbar)
+		{
+			vehicle.mAntiRollBars.resize(2);
+			vehicle.mAntiRollBars[0].mLeftWheel = FL_WHEEL;
+			vehicle.mAntiRollBars[0].mRightWheel = FR_WHEEL;
+			vehicle.mAntiRollBars[1].mLeftWheel = BL_WHEEL;
+			vehicle.mAntiRollBars[1].mRightWheel = BR_WHEEL;
+		}
+
+		// Create the constraint
+		VehicleConstraint *constraint = new VehicleConstraint(*car_body, vehicle);
+
+		// Create collision tester
+		RefConst<VehicleCollisionTester> tester;
+		if (inSettings.mUseCastSphere)
+			tester = new VehicleCollisionTesterCastSphere(Layers::MOVING, 0.5f * inSettings.mWheelWidth);
+		else
+			tester = new VehicleCollisionTesterRay(Layers::MOVING);
+		constraint->SetVehicleCollisionTester(tester);
+
+		// Add to the world
+		inContext.GetSystem()->AddConstraint(constraint);
+		inContext.GetSystem()->AddStepListener(constraint);
+		return constraint;
+	}
+
+	static void CheckOnGround(VehicleConstraint *inConstraint, const VehicleSettings &inSettings, const BodyID &inGroundID)
+	{
+		// Between min and max suspension length
+		RVec3 pos = inConstraint->GetVehicleBody()->GetPosition();
+		CHECK(pos.GetY() > inSettings.mSuspensionMinLength + inSettings.mWheelOffsetVertical + inSettings.mHalfVehicleHeight);
+		CHECK(pos.GetY() < inSettings.mSuspensionMaxLength + inSettings.mWheelOffsetVertical + inSettings.mHalfVehicleHeight);
+
+		// Wheels touching ground
+		for (const Wheel *w : inConstraint->GetWheels())
+			CHECK(w->GetContactBodyID() == inGroundID);
+	}
+
+	TEST_CASE("TestBasicWheeledVehicle")
+	{
+		PhysicsTestContext c;
+		BodyID floor_id = c.CreateFloor().GetID();
+
+		VehicleSettings settings;
+		VehicleConstraint *constraint = AddVehicle(c, settings);
+		Body *body = constraint->GetVehicleBody();
+		WheeledVehicleController *controller = static_cast<WheeledVehicleController *>(constraint->GetController());
+
+		// Should start at specified position
+		CHECK_APPROX_EQUAL(body->GetPosition(), settings.mPosition);
+
+		// After 1 step we should not be at ground yet
+		c.SimulateSingleStep();
+		for (const Wheel *w : constraint->GetWheels())
+			CHECK(w->GetContactBodyID().IsInvalid());
+		CHECK(controller->GetTransmission().GetCurrentGear() == 0);
+
+		// After 1 second we should be on ground but not moving horizontally
+		c.Simulate(1.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		RVec3 pos1 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos1.GetX(), 0); // Not moving horizontally
+		CHECK_APPROX_EQUAL(pos1.GetZ(), 0);
+		CHECK(controller->GetTransmission().GetCurrentGear() == 0);
+
+		// Start driving forward
+		controller->SetDriverInput(1.0f, 0.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		RVec3 pos2 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos2.GetX(), 0, 1.0e-2_r); // Not moving left/right
+		CHECK(pos2.GetZ() > pos1.GetZ() + 1.0f); // Moving in Z direction
+		Vec3 vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel.GetX(), 0, 2.0e-2f); // Not moving left/right
+		CHECK(vel.GetZ() > 1.0f); // Moving in Z direction
+		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
+
+		// Brake
+		controller->SetDriverInput(0.0f, 0.0f, 1.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(5.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone to sleep
+		RVec3 pos3 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos3.GetX(), 0, 2.0e-2_r); // Not moving left/right
+		CHECK(pos3.GetZ() > pos2.GetZ() + 1.0f); // Moving in Z direction while braking
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
+
+		// Start driving backwards
+		controller->SetDriverInput(-1.0f, 0.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		RVec3 pos4 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos4.GetX(), 0, 3.0e-2_r); // Not moving left/right
+		CHECK(pos4.GetZ() < pos3.GetZ() - 1.0f); // Moving in -Z direction
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel.GetX(), 0, 5.0e-2f); // Not moving left/right
+		CHECK(vel.GetZ() < -1.0f); // Moving in -Z direction
+		CHECK(controller->GetTransmission().GetCurrentGear() < 0);
+
+		// Brake
+		controller->SetDriverInput(0.0f, 0.0f, 1.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(5.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone to sleep
+		RVec3 pos5 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos5.GetX(), 0, 7.0e-2_r); // Not moving left/right
+		CHECK(pos5.GetZ() < pos4.GetZ() - 1.0f); // Moving in -Z direction while braking
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
+
+		// Turn right
+		controller->SetDriverInput(1.0f, 1.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		Vec3 omega = body->GetAngularVelocity();
+		CHECK(omega.GetY() < -0.4f); // Rotating right
+		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
+
+		// Hand brake
+		controller->SetDriverInput(0.0f, 0.0f, 0.0f, 1.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(7.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone to sleep
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
+
+		// Turn left
+		controller->SetDriverInput(1.0f, -1.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		omega = body->GetAngularVelocity();
+		CHECK(omega.GetY() > 0.4f); // Rotating left
+		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
+	}
+
+	TEST_CASE("TestLSDifferential")
+	{
+		struct Test
+		{
+			RVec3	mBlockPosition;		// Location of the box under the vehicle
+			bool	mFourWheelDrive;	// 4WD or not
+			float	mFBLSRatio;			// Limited slip ratio front-back
+			float	mLRLSRatio;			// Limited slip ratio left-right
+			bool	mFLHasContactPre;	// Which wheels should be in contact with the ground prior to the test
+			bool	mFRHasContactPre;
+			bool	mBLHasContactPre;
+			bool	mBRHasContactPre;
+			bool	mShouldMove;		// If the vehicle should be able to drive off the block
+		};
+
+		Test tests[] = {
+			// Block Position,			4WD,	FBSlip,		LRSlip		FLPre,	FRPre,	BLPre,	BRPre,	ShouldMove
+			{ RVec3(1, 0.5f, 0),		true,	FLT_MAX,	FLT_MAX,	false,	true,	false,	true,	false	},		// Block left, no limited slip -> vehicle can't move
+			{ RVec3(1, 0.5f, 0),		true,	1.4f,		FLT_MAX,	false,	true,	false,	true,	false	},		// Block left, only FB limited slip -> vehicle can't move
+			{ RVec3(1, 0.5f, 0),		true,	1.4f,		1.4f,		false,	true,	false,	true,	true	},		// Block left, limited slip -> vehicle drives off
+			{ RVec3(-1, 0.5f, 0),		true,	FLT_MAX,	FLT_MAX,	true,	false,	true,	false,	false	},		// Block right, no limited slip -> vehicle can't move
+			{ RVec3(-1, 0.5f, 0),		true,	1.4f,		FLT_MAX,	true,	false,	true,	false,	false	},		// Block right, only FB limited slip -> vehicle can't move
+			{ RVec3(-1, 0.5f, 0),		true,	1.4f,		1.4f,		true,	false,	true,	false,	true	},		// Block right, limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, 1.5f),		true,	FLT_MAX,	FLT_MAX,	false,	false,	true,	true,	false	},		// Block front, no limited slip -> vehicle can't move
+			{ RVec3(0, 0.5f, 1.5f),		true,	1.4f,		FLT_MAX,	false,	false,	true,	true,	true	},		// Block front, only FB limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, 1.5f),		true,	1.4f,		1.4f,		false,	false,	true,	true,	true	},		// Block front, limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, 1.5f),		false,	1.4f,		1.4f,		false,	false,	true,	true,	false	},		// Block front, limited slip, 2WD -> vehicle can't move
+			{ RVec3(0, 0.5f, -1.5f),	true,	FLT_MAX,	FLT_MAX,	true,	true,	false,	false,	false	},		// Block back, no limited slip -> vehicle can't move
+			{ RVec3(0, 0.5f, -1.5f),	true,	1.4f,		FLT_MAX,	true,	true,	false,	false,	true	},		// Block back, only FB limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, -1.5f),	true,	1.4f,		1.4f,		true,	true,	false,	false,	true	},		// Block back, limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, -1.5f),	false,	1.4f,		1.4f,		true,	true,	false,	false,	true	},		// Block back, limited slip, 2WD -> vehicle drives off
+		};
+
+		for (Test &t : tests)
+		{
+			PhysicsTestContext c;
+			BodyID floor_id = c.CreateFloor().GetID();
+
+			// Box under left side of the vehicle, left wheels won't be touching the ground
+			Body &box = c.CreateBox(t.mBlockPosition, Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3::sReplicate(0.5f));
+			box.SetFriction(1.0f);
+
+			// Create vehicle
+			VehicleSettings settings;
+			settings.mFourWheelDrive = t.mFourWheelDrive;
+			settings.mFrontBackLimitedSlipRatio = t.mFBLSRatio;
+			settings.mLeftRightLimitedSlipRatio = t.mLRLSRatio;
+
+			VehicleConstraint *constraint = AddVehicle(c, settings);
+			Body *body = constraint->GetVehicleBody();
+			WheeledVehicleController *controller = static_cast<WheeledVehicleController *>(constraint->GetController());
+
+			// Give the wheels extra grip
+			controller->SetTireMaxImpulseCallback(
+				[](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float)
+				{
+					outLongitudinalImpulse = 10.0f * inLongitudinalFriction * inSuspensionImpulse;
+					outLateralImpulse = inLateralFriction * inSuspensionImpulse;
+				});
+
+			// Simulate till vehicle rests on block
+			bool vehicle_on_floor = false;
+			for (float time = 0; time < 2.0f; time += c.GetDeltaTime())
+			{
+				c.SimulateSingleStep();
+
+				// Check pre condition
+				if ((constraint->GetWheel(FL_WHEEL)->GetContactBodyID() == (t.mFLHasContactPre? floor_id : BodyID()))
+					&& (constraint->GetWheel(FR_WHEEL)->GetContactBodyID() == (t.mFRHasContactPre? floor_id : BodyID()))
+					&& (constraint->GetWheel(BL_WHEEL)->GetContactBodyID() == (t.mBLHasContactPre? floor_id : BodyID()))
+					&& (constraint->GetWheel(BR_WHEEL)->GetContactBodyID() == (t.mBRHasContactPre? floor_id : BodyID())))
+				{
+					vehicle_on_floor = true;
+					break;
+				}
+			}
+			CHECK(vehicle_on_floor);
+			CHECK_APPROX_EQUAL(body->GetPosition().GetZ(), 0, 0.03_r);
+
+			// Start driving
+			controller->SetDriverInput(1.0f, 0, 0, 0);
+			c.GetBodyInterface().ActivateBody(body->GetID());
+			c.Simulate(2.0f);
+
+			// Check if vehicle had traction
+			if (t.mShouldMove)
+				CHECK(body->GetPosition().GetZ() > 0.5f);
+			else
+				CHECK_APPROX_EQUAL(body->GetPosition().GetZ(), 0, 0.06_r);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/PhysicsTestContext.cpp

@@ -0,0 +1,187 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/ContactConstraintManager.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Core/JobSystemThreadPool.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/StreamWrapper.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRendererRecorder.h>
+#endif
+
+PhysicsTestContext::PhysicsTestContext(float inDeltaTime, int inCollisionSteps, int inWorkerThreads, uint inMaxBodies, uint inMaxBodyPairs, uint inMaxContactConstraints) :
+#ifdef JPH_DISABLE_TEMP_ALLOCATOR
+	mTempAllocator(new TempAllocatorMalloc()),
+#else
+	mTempAllocator(new TempAllocatorImpl(4 * 1024 * 1024)),
+#endif
+	mJobSystem(new JobSystemThreadPool(cMaxPhysicsJobs, cMaxPhysicsBarriers, inWorkerThreads)),
+	mDeltaTime(inDeltaTime),
+	mCollisionSteps(inCollisionSteps)
+{
+	// Create physics system
+	mSystem = new PhysicsSystem();
+	mSystem->Init(inMaxBodies, 0, inMaxBodyPairs, inMaxContactConstraints, mBroadPhaseLayerInterface, mObjectVsBroadPhaseLayerFilter, mObjectVsObjectLayerFilter);
+}
+
+PhysicsTestContext::~PhysicsTestContext()
+{
+#ifdef JPH_DEBUG_RENDERER
+	delete mDebugRenderer;
+	delete mStreamWrapper;
+	delete mStream;
+#endif // JPH_DEBUG_RENDERER
+	delete mSystem;
+	delete mJobSystem;
+	delete mTempAllocator;
+}
+
+void PhysicsTestContext::ZeroGravity()
+{
+	mSystem->SetGravity(Vec3::sZero());
+}
+
+Body &PhysicsTestContext::CreateFloor()
+{
+	BodyCreationSettings settings;
+	settings.SetShape(new BoxShape(Vec3(100.0f, 1.0f, 100.0f), 0.0f));
+	settings.mPosition = RVec3(0.0f, -1.0f, 0.0f);
+	settings.mMotionType = EMotionType::Static;
+	settings.mObjectLayer = Layers::NON_MOVING;
+
+	Body &floor = *mSystem->GetBodyInterface().CreateBody(settings);
+	mSystem->GetBodyInterface().AddBody(floor.GetID(), EActivation::DontActivate);
+	return floor;
+}
+
+Body &PhysicsTestContext::CreateBody(const BodyCreationSettings &inSettings, EActivation inActivation)
+{
+	Body &body = *mSystem->GetBodyInterface().CreateBody(inSettings);
+	mSystem->GetBodyInterface().AddBody(body.GetID(), inActivation);
+	return body;
+}
+
+Body &PhysicsTestContext::CreateBody(const ShapeSettings *inShapeSettings, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, EMotionQuality inMotionQuality, ObjectLayer inLayer, EActivation inActivation)
+{
+	BodyCreationSettings settings;
+	settings.SetShapeSettings(inShapeSettings);
+	settings.mPosition = inPosition;
+	settings.mRotation = inRotation;
+	settings.mMotionType = inMotionType;
+	settings.mMotionQuality = inMotionQuality;
+	settings.mObjectLayer = inLayer;
+	settings.mLinearDamping = 0.0f;
+	settings.mAngularDamping = 0.0f;
+	settings.mCollisionGroup.SetGroupID(0);
+	return CreateBody(settings, inActivation);
+}
+
+Body &PhysicsTestContext::CreateBox(RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, EMotionQuality inMotionQuality, ObjectLayer inLayer, Vec3Arg inHalfExtent, EActivation inActivation)
+{
+	return CreateBody(new BoxShapeSettings(inHalfExtent), inPosition, inRotation, inMotionType, inMotionQuality, inLayer, inActivation);
+}
+
+Body &PhysicsTestContext::CreateSphere(RVec3Arg inPosition, float inRadius, EMotionType inMotionType, EMotionQuality inMotionQuality, ObjectLayer inLayer, EActivation inActivation)
+{
+	return CreateBody(new SphereShapeSettings(inRadius), inPosition, Quat::sIdentity(), inMotionType, inMotionQuality, inLayer, inActivation);
+}
+
+EPhysicsUpdateError PhysicsTestContext::SimulateNoDeltaTime()
+{
+	EPhysicsUpdateError errors = mSystem->Update(0.0f, mCollisionSteps, mTempAllocator, mJobSystem);
+#ifndef JPH_DISABLE_TEMP_ALLOCATOR
+	JPH_ASSERT(static_cast<TempAllocatorImpl *>(mTempAllocator)->IsEmpty());
+#endif // JPH_DISABLE_TEMP_ALLOCATOR
+	return errors;
+}
+
+EPhysicsUpdateError PhysicsTestContext::SimulateSingleStep()
+{
+	EPhysicsUpdateError errors = mSystem->Update(mDeltaTime, mCollisionSteps, mTempAllocator, mJobSystem);
+#ifndef JPH_DISABLE_TEMP_ALLOCATOR
+	JPH_ASSERT(static_cast<TempAllocatorImpl *>(mTempAllocator)->IsEmpty());
+#endif // JPH_DISABLE_TEMP_ALLOCATOR
+#ifdef JPH_DEBUG_RENDERER
+	if (mDebugRenderer != nullptr)
+	{
+		mSystem->DrawBodies(BodyManager::DrawSettings(), mDebugRenderer);
+		mSystem->DrawConstraints(mDebugRenderer);
+		mDebugRenderer->EndFrame();
+	}
+#endif // JPH_DEBUG_RENDERER
+	return errors;
+}
+
+EPhysicsUpdateError PhysicsTestContext::Simulate(float inTotalTime, function<void()> inPreStepCallback)
+{
+	EPhysicsUpdateError errors = EPhysicsUpdateError::None;
+
+	const int cNumSteps = int(round(inTotalTime / mDeltaTime));
+	for (int s = 0; s < cNumSteps; ++s)
+	{
+		inPreStepCallback();
+		errors |= SimulateSingleStep();
+	}
+
+	return errors;
+}
+
+RVec3 PhysicsTestContext::PredictPosition(RVec3Arg inPosition, Vec3Arg inVelocity, Vec3Arg inAcceleration, float inTotalTime) const
+{
+	// Integrate position using a Symplectic Euler step (just like the PhysicsSystem)
+	RVec3 pos = inPosition;
+	Vec3 vel = inVelocity;
+
+	const float delta_time = GetStepDeltaTime();
+	const int cNumSteps = int(round(inTotalTime / delta_time));
+	for (int s = 0; s < cNumSteps; ++s)
+	{
+		vel += inAcceleration * delta_time;
+		pos += vel * delta_time;
+	}
+	return pos;
+}
+
+// Predict rotation assuming ballistic motion using initial orientation, angular velocity angular acceleration and time
+Quat PhysicsTestContext::PredictOrientation(QuatArg inRotation, Vec3Arg inAngularVelocity, Vec3Arg inAngularAcceleration, float inTotalTime) const
+{
+	// Integrate position using a Symplectic Euler step (just like the PhysicsSystem)
+	Quat rot = inRotation;
+	Vec3 vel = inAngularVelocity;
+
+	const float delta_time = GetStepDeltaTime();
+	const int cNumSteps = int(round(inTotalTime / delta_time));
+	for (int s = 0; s < cNumSteps; ++s)
+	{
+		vel += inAngularAcceleration * delta_time;
+		float vel_len = vel.Length();
+		if (vel_len != 0.0f)
+			rot = Quat::sRotation(vel / vel_len, vel_len * delta_time) * rot;
+	}
+	return rot;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void PhysicsTestContext::RecordDebugOutput(const char *inFileName)
+{
+	mStream = new ofstream;
+	mStream->open(inFileName, ofstream::out | ofstream::binary | ofstream::trunc);
+	if (mStream->is_open())
+	{
+		mStreamWrapper = new StreamOutWrapper(*mStream);
+		mDebugRenderer = new DebugRendererRecorder(*mStreamWrapper);
+	}
+	else
+	{
+		delete mStream;
+		mStream = nullptr;
+	}
+}
+
+#endif // JPH_DEBUG_RENDERER

lib/All/JoltPhysics/UnitTests/PhysicsTestContext.h

@@ -0,0 +1,128 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include "Layers.h"
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <fstream>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+namespace JPH {
+	class TempAllocator;
+	class JobSystem;
+	class DebugRendererRecorder;
+	class StreamOutWrapper;
+};
+
+// Helper class used in test cases for creating and manipulating physics objects
+class PhysicsTestContext
+{
+public:
+	// Constructor / destructor
+						PhysicsTestContext(float inDeltaTime = 1.0f / 60.0f, int inCollisionSteps = 1, int inWorkerThreads = 0, uint inMaxBodies = 1024, uint inMaxBodyPairs = 4096, uint inMaxContactConstraints = 1024);
+						~PhysicsTestContext();
+
+	// Set the gravity to zero
+	void				ZeroGravity();
+
+	// Create a floor at Y = 0
+	Body &				CreateFloor();
+
+	/// Create a body and add it to the world
+	Body &				CreateBody(const BodyCreationSettings &inSettings, EActivation inActivation);
+
+	/// Create a body and add it to the world
+	Body &				CreateBody(const ShapeSettings *inShapeSettings, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, EMotionQuality inMotionQuality, ObjectLayer inLayer, EActivation inActivation);
+
+	// Create a box and add it to the world
+	Body &				CreateBox(RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, EMotionQuality inMotionQuality, ObjectLayer inLayer, Vec3Arg inHalfExtent, EActivation inActivation = EActivation::Activate);
+
+	// Create a sphere and add it to the world
+	Body &				CreateSphere(RVec3Arg inPosition, float inRadius, EMotionType inMotionType, EMotionQuality inMotionQuality, ObjectLayer inLayer, EActivation inActivation = EActivation::Activate);
+
+	// Create a constraint and add it to the world
+	template <typename T>
+	T &					CreateConstraint(Body &inBody1, Body &inBody2, const TwoBodyConstraintSettings &inSettings)
+	{
+		T *constraint = static_cast<T *>(inSettings.Create(inBody1, inBody2));
+		mSystem->AddConstraint(constraint);
+		return *constraint;
+	}
+
+	// Call the update with zero delta time
+	EPhysicsUpdateError SimulateNoDeltaTime();
+
+	// Simulate only for one delta time step
+	EPhysicsUpdateError	SimulateSingleStep();
+
+	// Simulate the world for inTotalTime time
+	EPhysicsUpdateError	Simulate(float inTotalTime, function<void()> inPreStepCallback = []() { });
+
+	// Predict position assuming ballistic motion using initial position, velocity acceleration and time
+	RVec3				PredictPosition(RVec3Arg inPosition, Vec3Arg inVelocity, Vec3Arg inAcceleration, float inTotalTime) const;
+
+	// Predict rotation assuming ballistic motion using initial orientation, angular velocity angular acceleration and time
+	Quat				PredictOrientation(QuatArg inRotation, Vec3Arg inAngularVelocity, Vec3Arg inAngularAcceleration, float inTotalTime) const;
+
+	// Access to the physics system
+	PhysicsSystem *		GetSystem() const
+	{
+		return mSystem;
+	}
+
+	// Access to the body interface
+	BodyInterface &		GetBodyInterface() const
+	{
+		return mSystem->GetBodyInterface();
+	}
+
+	// Get delta time for simulation step
+	inline float		GetDeltaTime() const
+	{
+		return mDeltaTime;
+	}
+
+	// Get delta time for a simulation collision step
+	inline float		GetStepDeltaTime() const
+	{
+		return mDeltaTime / mCollisionSteps;
+	}
+
+	// Get the temporary allocator
+	TempAllocator *		GetTempAllocator() const
+	{
+		return mTempAllocator;
+	}
+
+	// Get the job system
+	JobSystem *			GetJobSystem() const
+	{
+		return mJobSystem;
+	}
+
+#ifdef JPH_DEBUG_RENDERER
+	// Write the debug output to a file to be able to replay it with JoltViewer
+	void				RecordDebugOutput(const char *inFileName);
+#endif // JPH_DEBUG_RENDERER
+
+private:
+	TempAllocator *		mTempAllocator;
+	JobSystem *			mJobSystem;
+	BPLayerInterfaceImpl mBroadPhaseLayerInterface;
+	ObjectVsBroadPhaseLayerFilterImpl mObjectVsBroadPhaseLayerFilter;
+	ObjectLayerPairFilterImpl mObjectVsObjectLayerFilter;
+	PhysicsSystem *		mSystem;
+#ifdef JPH_DEBUG_RENDERER
+	DebugRendererRecorder *mDebugRenderer = nullptr;
+	ofstream *			mStream = nullptr;
+	StreamOutWrapper *	mStreamWrapper = nullptr;
+#endif // JPH_DEBUG_RENDERER
+	float				mDeltaTime;
+	int					mCollisionSteps;
+};

lib/All/JoltPhysics/UnitTests/UnitTestFramework.cpp

@@ -0,0 +1,360 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+#include <Jolt/ConfigurationString.h>
+#include <Jolt/Core/FPException.h>
+#include <Jolt/Core/Factory.h>
+#include <Jolt/RegisterTypes.h>
+#ifdef JPH_PLATFORM_WINDOWS
+#include <crtdbg.h>
+#endif // JPH_PLATFORM_WINDOWS
+#ifdef JPH_PLATFORM_ANDROID
+#include <Jolt/Core/Color.h>
+#include <android/log.h>
+#include <android_native_app_glue.h>
+#endif // JPH_PLATFORM_ANDROID
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <cstdarg>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+using namespace JPH;
+
+// Emit everything needed for the main function
+#define DOCTEST_CONFIG_IMPLEMENT
+#define DOCTEST_CONFIG_NO_WINDOWS_SEH
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+JPH_CLANG_16_PLUS_SUPPRESS_WARNING("-Wunsafe-buffer-usage")
+JPH_MSVC2026_PLUS_SUPPRESS_WARNING(4865) // wingdi.h(2806,1): '<unnamed-enum-DISPLAYCONFIG_OUTPUT_TECHNOLOGY_OTHER>': the underlying type will change from 'int' to '__int64' when '/Zc:enumTypes' is specified on the command line
+#include "doctest.h"
+JPH_SUPPRESS_WARNINGS_STD_END
+
+using namespace doctest;
+
+// Disable common warnings triggered by Jolt
+JPH_SUPPRESS_WARNINGS
+
+// Callback for traces
+static void TraceImpl(const char *inFMT, ...)
+{
+	// Format the message
+	va_list list;
+	va_start(list, inFMT);
+	char buffer[1024];
+	vsnprintf(buffer, sizeof(buffer), inFMT, list);
+	va_end(list);
+
+	// Forward to doctest
+	MESSAGE(buffer);
+}
+
+#ifdef JPH_ENABLE_ASSERTS
+
+// Callback for asserts
+static bool AssertFailedImpl(const char *inExpression, const char *inMessage, const char *inFile, uint inLine)
+{
+	// Format message
+	char buffer[1024];
+	snprintf(buffer, sizeof(buffer), "%s:%u: (%s) %s", inFile, inLine, inExpression, inMessage != nullptr? inMessage : "");
+
+	// Forward to doctest
+	FAIL_CHECK(buffer);
+
+	// No breakpoint
+	return false;
+}
+
+#endif // JPH_ENABLE_ASSERTS
+
+#ifdef JPH_PLATFORM_WINDOWS_UWP
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_MSVC_SUPPRESS_WARNING(4265) // warning C4265: 'winrt::impl::implements_delegate<winrt::Windows::UI::Core::DispatchedHandler,H>': class has virtual functions, but its non-trivial destructor is not virtual; instances of this class may not be destructed correctly
+JPH_MSVC_SUPPRESS_WARNING(4668) // warning C4668: '_MANAGED' is not defined as a preprocessor macro, replacing with '0' for '#if/#elif'
+JPH_MSVC_SUPPRESS_WARNING(4946) // warning C4946: reinterpret_cast used between related classes: 'winrt::impl::abi<winrt::Windows::ApplicationModel::Core::IFrameworkViewSource,void>::type' and 'winrt::impl::abi<winrt::Windows::Foundation::IUnknown,void>::type'
+JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception.
+JPH_MSVC_SUPPRESS_WARNING(5204) // warning C5204: 'winrt::impl::produce_base<D,winrt::Windows::ApplicationModel::Core::IFrameworkViewSource,void>': class has virtual functions, but its trivial destructor is not virtual; instances of objects derived from this class may not be destructed correctly
+JPH_MSVC_SUPPRESS_WARNING(5246) // warning C5246: '_Elems': the initialization of a subobject should be wrapped in braces
+#include <winrt/Windows.Foundation.h>
+#include <winrt/Windows.Foundation.Collections.h>
+#include <winrt/Windows.ApplicationModel.Core.h>
+#include <winrt/Windows.UI.Core.h>
+#include <winrt/Windows.UI.Composition.h>
+#include <winrt/Windows.UI.Input.h>
+JPH_SUPPRESS_WARNING_POP
+
+using namespace winrt;
+using namespace Windows;
+using namespace Windows::ApplicationModel::Core;
+using namespace Windows::UI;
+using namespace Windows::UI::Core;
+using namespace Windows::UI::Composition;
+
+struct App : implements<App, IFrameworkViewSource, IFrameworkView>
+{
+	CompositionTarget mTarget { nullptr };
+
+	IFrameworkView CreateView()
+	{
+		return *this;
+	}
+
+	void Initialize(CoreApplicationView const&)
+	{
+	}
+
+	void Load(hstring const&)
+	{
+	}
+
+	void Uninitialize()
+	{
+	}
+
+	void Run()
+	{
+		CoreWindow window = CoreWindow::GetForCurrentThread();
+		window.Activate();
+
+		CoreDispatcher dispatcher = window.Dispatcher();
+		dispatcher.ProcessEvents(CoreProcessEventsOption::ProcessUntilQuit);
+	}
+
+	void SetWindow(CoreWindow const& inWindow)
+	{
+		// Register allocation hook
+		RegisterDefaultAllocator();
+
+		// Install callbacks
+		Trace = TraceImpl;
+		JPH_IF_ENABLE_ASSERTS(AssertFailed = AssertFailedImpl;)
+
+#ifdef _DEBUG
+		// Enable leak detection
+		_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
+#endif
+
+		// Enable floating point exceptions
+		FPExceptionsEnable enable_exceptions;
+		JPH_UNUSED(enable_exceptions);
+
+		// Create a factory
+		Factory::sInstance = new Factory();
+
+		// Register physics types
+		RegisterTypes();
+
+		// Run the tests
+		int rv = Context().run();
+
+		// Unregisters all types with the factory and cleans up the default material
+		UnregisterTypes();
+
+		// Destroy the factory
+		delete Factory::sInstance;
+		Factory::sInstance = nullptr;
+
+		// Color the screen according to the result
+		Compositor compositor;
+		ContainerVisual root = compositor.CreateContainerVisual();
+		mTarget = compositor.CreateTargetForCurrentView();
+		mTarget.Root(root);
+		SpriteVisual visual = compositor.CreateSpriteVisual();
+		visual.Brush(compositor.CreateColorBrush(rv != 0 ? Windows::UI::Color { 0xff, 0xff, 0x00, 0x00 } : Windows::UI::Color { 0xff, 0x00, 0xff, 0x00 }));
+		visual.Size({ inWindow.Bounds().Width, inWindow.Bounds().Height });
+		visual.Offset({ 0, 0, 0, });
+		root.Children().InsertAtTop(visual);
+	}
+};
+
+int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, PWSTR pCmdLine, int nCmdShow)
+{
+	CoreApplication::Run(make<App>());
+}
+
+#elif !defined(JPH_PLATFORM_ANDROID)
+
+// Generic entry point
+int main(int argc, char** argv)
+{
+	// Show used instruction sets
+	std::cout << GetConfigurationString() << std::endl;
+
+	// Register allocation hook
+	RegisterDefaultAllocator();
+
+	// Install callbacks
+	Trace = TraceImpl;
+	JPH_IF_ENABLE_ASSERTS(AssertFailed = AssertFailedImpl;)
+
+#if defined(JPH_PLATFORM_WINDOWS) && defined(_DEBUG)
+	// Enable leak detection
+	_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
+#endif
+
+	// Enable floating point exceptions
+	FPExceptionsEnable enable_exceptions;
+	JPH_UNUSED(enable_exceptions);
+
+	// Create a factory
+	Factory::sInstance = new Factory();
+
+	// Register physics types
+	RegisterTypes();
+
+	int rv = Context(argc, argv).run();
+
+	// Unregisters all types with the factory and cleans up the default material
+	UnregisterTypes();
+
+	// Destroy the factory
+	delete Factory::sInstance;
+	Factory::sInstance = nullptr;
+
+	return rv;
+}
+
+#else // !JPH_PLATFORM_ANDROID
+
+// Reporter that writes logs to the Android log
+struct LogReporter : public ConsoleReporter
+{
+	LogReporter(const ContextOptions &inOptions) :
+		ConsoleReporter(inOptions, mStream)
+	{
+	}
+
+#define REPORTER_OVERRIDE(func, type, arg)						\
+	void func(type arg) override								\
+	{															\
+		ConsoleReporter::func(arg);								\
+		std::string str = mStream.str();						\
+		if (!str.empty())										\
+			__android_log_write(ANDROID_LOG_INFO, "Jolt", str.c_str());	\
+		mStream.str("");										\
+	}
+
+	REPORTER_OVERRIDE(test_run_start, DOCTEST_EMPTY, DOCTEST_EMPTY)
+	REPORTER_OVERRIDE(test_run_end, const TestRunStats &, in)
+	REPORTER_OVERRIDE(test_case_start, const TestCaseData &, in)
+	REPORTER_OVERRIDE(test_case_reenter, const TestCaseData &, in)
+	REPORTER_OVERRIDE(test_case_end, const CurrentTestCaseStats &, in)
+	REPORTER_OVERRIDE(test_case_exception, const TestCaseException &, in)
+	REPORTER_OVERRIDE(subcase_start, const SubcaseSignature &, in)
+	REPORTER_OVERRIDE(subcase_end, DOCTEST_EMPTY, DOCTEST_EMPTY)
+	REPORTER_OVERRIDE(log_assert, const AssertData &, in)
+	REPORTER_OVERRIDE(log_message, const MessageData &, in)
+	REPORTER_OVERRIDE(test_case_skipped, const TestCaseData &, in)
+
+private:
+	thread_local static std::ostringstream mStream;
+};
+
+thread_local std::ostringstream LogReporter::mStream;
+
+DOCTEST_REGISTER_REPORTER("android_log", 0, LogReporter);
+
+void AndroidInitialize(android_app *inApp)
+{
+	// Log configuration
+	__android_log_write(ANDROID_LOG_INFO, "Jolt", GetConfigurationString());
+
+	// Register allocation hook
+	RegisterDefaultAllocator();
+
+	// Install callbacks
+	Trace = TraceImpl;
+	JPH_IF_ENABLE_ASSERTS(AssertFailed = AssertFailedImpl;)
+
+	// Enable floating point exceptions
+	FPExceptionsEnable enable_exceptions;
+	JPH_UNUSED(enable_exceptions);
+
+	// Create a factory
+	Factory::sInstance = new Factory();
+
+	// Register physics types
+	RegisterTypes();
+
+	// Run all tests
+	Context context;
+	context.addFilter("reporters", "android_log");
+	int return_value = context.run();
+
+	// Color the screen according to the test result
+	JPH::Color color = return_value == 0? JPH::Color::sGreen : JPH::Color::sRed;
+	ANativeWindow_acquire(inApp->window);
+	ANativeWindow_Buffer buffer;
+	ARect bounds;
+	ANativeWindow_lock(inApp->window, &buffer, &bounds);
+	switch (buffer.format)
+	{
+		case AHARDWAREBUFFER_FORMAT_R8G8B8A8_UNORM:
+		case AHARDWAREBUFFER_FORMAT_R8G8B8X8_UNORM:
+		{
+			uint32 color_u32 = color.GetUInt32();
+			for (int y = 0; y < buffer.height; ++y)
+			{
+				uint32 *dest = (uint32 *)((uint8 *)buffer.bits + y * buffer.stride * sizeof(uint32));
+				for (int x = 0; x < buffer.width; ++x)
+					*dest++ = color_u32;
+			}
+			break;
+		}
+
+		case AHARDWAREBUFFER_FORMAT_R5G6B5_UNORM:
+		{
+			uint16 color_u16 = (color.b >> 3) + ((color.g >> 2) << 5) + ((color.r >> 3) << 11);
+			for (int y = 0; y < buffer.height; ++y)
+			{
+				uint16 *dest = (uint16 *) ((uint8 *) buffer.bits + y * buffer.stride * sizeof(uint16));
+				for (int x = 0; x < buffer.width; ++x)
+					*dest++ = color_u16;
+			}
+			break;
+		}
+
+		default:
+			// TODO implement
+			break;
+	}
+	ANativeWindow_unlockAndPost(inApp->window);
+	ANativeWindow_release(inApp->window);
+
+	// Unregisters all types with the factory and cleans up the default material
+	UnregisterTypes();
+
+	// Destroy the factory
+	delete Factory::sInstance;
+	Factory::sInstance = nullptr;
+}
+
+// Handle callback from Android
+void AndroidHandleCommand(android_app *inApp, int32_t inCmd)
+{
+	switch (inCmd)
+	{
+	case APP_CMD_INIT_WINDOW:
+		AndroidInitialize(inApp);
+		break;
+	}
+}
+
+// Main entry point for android
+void android_main(struct android_app *ioApp)
+{
+	ioApp->onAppCmd = AndroidHandleCommand;
+
+	int events;
+	android_poll_source *source;
+	do
+	{
+		if (ALooper_pollAll(1, nullptr, &events, (void **)&source) >= 0 && source != nullptr)
+			source->process(ioApp, source);
+	} while (ioApp->destroyRequested == 0);
+}
+
+#endif // JPH_PLATFORM_ANDROID

lib/All/JoltPhysics/UnitTests/UnitTestFramework.h

@@ -0,0 +1,110 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Core/Atomics.h>
+#include <Jolt/Math/DVec3.h>
+#include <Jolt/Math/Float2.h>
+
+// Disable common warnings
+JPH_SUPPRESS_WARNINGS
+JPH_CLANG_SUPPRESS_WARNING("-Wheader-hygiene")
+#ifdef JPH_DOUBLE_PRECISION
+JPH_CLANG_SUPPRESS_WARNING("-Wdouble-promotion")
+#endif // JPH_DOUBLE_PRECISION
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include "doctest.h"
+JPH_SUPPRESS_WARNINGS_STD_END
+
+using namespace JPH;
+using namespace JPH::literals;
+using namespace std;
+
+inline void CHECK_APPROX_EQUAL(float inLHS, float inRHS, float inTolerance = 1.0e-6f)
+{
+	CHECK(abs(inRHS - inLHS) <= inTolerance);
+}
+
+inline void CHECK_APPROX_EQUAL(double inLHS, double inRHS, double inTolerance = 1.0e-6)
+{
+	CHECK(abs(inRHS - inLHS) <= inTolerance);
+}
+
+inline void CHECK_APPROX_EQUAL(Vec3Arg inLHS, Vec3Arg inRHS, float inTolerance = 1.0e-6f)
+{
+	CHECK(inLHS.IsClose(inRHS, inTolerance * inTolerance));
+}
+
+inline void CHECK_APPROX_EQUAL(Vec4Arg inLHS, Vec4Arg inRHS, float inTolerance = 1.0e-6f)
+{
+	CHECK(inLHS.IsClose(inRHS, inTolerance * inTolerance));
+}
+
+inline void CHECK_APPROX_EQUAL(Mat44Arg inLHS, Mat44Arg inRHS, float inTolerance = 1.0e-6f)
+{
+	CHECK(inLHS.IsClose(inRHS, inTolerance * inTolerance));
+}
+
+inline void CHECK_APPROX_EQUAL(DMat44Arg inLHS, DMat44Arg inRHS, float inTolerance = 1.0e-6f)
+{
+	CHECK(inLHS.IsClose(inRHS, inTolerance * inTolerance));
+}
+
+inline void CHECK_APPROX_EQUAL(QuatArg inLHS, QuatArg inRHS, float inTolerance = 1.0e-6f)
+{
+	bool close = inLHS.IsClose(inRHS, inTolerance * inTolerance) || inLHS.IsClose(-inRHS, inTolerance * inTolerance);
+	CHECK(close);
+}
+
+inline void CHECK_APPROX_EQUAL(DVec3Arg inLHS, DVec3Arg inRHS, double inTolerance = 1.0e-6)
+{
+	CHECK(inLHS.IsClose(inRHS, inTolerance * inTolerance));
+}
+
+inline void CHECK_APPROX_EQUAL(const Float2 &inLHS, const Float2 &inRHS, float inTolerance = 1.0e-6f)
+{
+	Float2 diff(inLHS.x - inRHS.x, inLHS.y - inRHS.y);
+	CHECK(Square(diff.x) + Square(diff.y) < inTolerance * inTolerance);
+}
+
+// Define the exact random number generator we want to use across platforms for consistency (default_random_engine's implementation is platform specific)
+using UnitTestRandom = mt19937;
+
+#ifdef JPH_ENABLE_ASSERTS
+
+// Stack based object that tests for an assert
+class ExpectAssert
+{
+public:
+	/// Expect inCount asserts
+	explicit					ExpectAssert(int inCount)
+	{
+		CHECK(sCount == 0);
+		sCount = inCount;
+
+		mPrevAssertFailed = AssertFailed;
+		AssertFailed = [](const char*, const char*, const char*, uint)
+		{
+			--sCount;
+			return false;
+		};
+	}
+
+	/// Verifies that the expected number of asserts were triggered
+								~ExpectAssert()
+	{
+		AssertFailed = mPrevAssertFailed;
+		CHECK(sCount == 0);
+	}
+
+private:
+	// Keeps track of number of asserts that are expected
+	inline static atomic<int>	sCount { 0 };
+
+	// Previous assert function
+	AssertFailedFunction		mPrevAssertFailed;
+};
+
+#endif // JPH_ENABLE_ASSERTS

lib/All/JoltPhysics/UnitTests/UnitTests.cmake

@@ -0,0 +1,93 @@
+# Root
+set(UNIT_TESTS_ROOT ${PHYSICS_REPO_ROOT}/UnitTests)
+
+# Source files
+set(UNIT_TESTS_SRC_FILES
+	${UNIT_TESTS_ROOT}/Core/ArrayTest.cpp
+	${UNIT_TESTS_ROOT}/Core/BinaryHeapTest.cpp
+	${UNIT_TESTS_ROOT}/Core/FPFlushDenormalsTest.cpp
+	${UNIT_TESTS_ROOT}/Core/HashCombineTest.cpp
+	${UNIT_TESTS_ROOT}/Core/InsertionSortTest.cpp
+	${UNIT_TESTS_ROOT}/Core/JobSystemTest.cpp
+	${UNIT_TESTS_ROOT}/Core/LinearCurveTest.cpp
+	${UNIT_TESTS_ROOT}/Core/PreciseMathTest.cpp
+	${UNIT_TESTS_ROOT}/Core/ScopeExitTest.cpp
+	${UNIT_TESTS_ROOT}/Core/STLLocalAllocatorTest.cpp
+	${UNIT_TESTS_ROOT}/Core/StringToolsTest.cpp
+	${UNIT_TESTS_ROOT}/Core/QuickSortTest.cpp
+	${UNIT_TESTS_ROOT}/Core/UnorderedSetTest.cpp
+	${UNIT_TESTS_ROOT}/Core/UnorderedMapTest.cpp
+	${UNIT_TESTS_ROOT}/doctest.h
+	${UNIT_TESTS_ROOT}/Geometry/ClosestPointTests.cpp
+	${UNIT_TESTS_ROOT}/Geometry/ConvexHullBuilderTest.cpp
+	${UNIT_TESTS_ROOT}/Geometry/EllipseTest.cpp
+	${UNIT_TESTS_ROOT}/Geometry/EPATests.cpp
+	${UNIT_TESTS_ROOT}/Geometry/GJKTests.cpp
+	${UNIT_TESTS_ROOT}/Geometry/PlaneTests.cpp
+	${UNIT_TESTS_ROOT}/Geometry/RayAABoxTests.cpp
+	${UNIT_TESTS_ROOT}/Layers.h
+	${UNIT_TESTS_ROOT}/LoggingBodyActivationListener.h
+	${UNIT_TESTS_ROOT}/LoggingContactListener.h
+	${UNIT_TESTS_ROOT}/Math/BVec16Tests.cpp
+	${UNIT_TESTS_ROOT}/Math/DMat44Tests.cpp
+	${UNIT_TESTS_ROOT}/Math/DVec3Tests.cpp
+	${UNIT_TESTS_ROOT}/Math/EigenValueSymmetricTests.cpp
+	${UNIT_TESTS_ROOT}/Math/HalfFloatTests.cpp
+	${UNIT_TESTS_ROOT}/Math/Mat44Tests.cpp
+	${UNIT_TESTS_ROOT}/Math/MathTests.cpp
+	${UNIT_TESTS_ROOT}/Math/MatrixTests.cpp
+	${UNIT_TESTS_ROOT}/Math/QuatTests.cpp
+	${UNIT_TESTS_ROOT}/Math/UVec4Tests.cpp
+	${UNIT_TESTS_ROOT}/Math/TrigonometryTests.cpp
+	${UNIT_TESTS_ROOT}/Math/Vec3Tests.cpp
+	${UNIT_TESTS_ROOT}/Math/Vec4Tests.cpp
+	${UNIT_TESTS_ROOT}/Math/VectorTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/ActiveEdgesTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/BroadPhaseTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/CastShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/CharacterVirtualTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/CollideShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/CollidePointTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/CollisionGroupTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/ContactListenerTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/ConvexVsTrianglesTest.cpp
+	${UNIT_TESTS_ROOT}/Physics/DistanceConstraintTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/EstimateCollisionResponseTest.cpp
+	${UNIT_TESTS_ROOT}/Physics/HeightFieldShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/HingeConstraintTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/MotionQualityLinearCastTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/MutableCompoundShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/ObjectLayerPairFilterTableTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/ObjectLayerPairFilterMaskTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/OffsetCenterOfMassShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/PathConstraintTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/PhysicsDeterminismTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/PhysicsStepListenerTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/PhysicsTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/RayShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/SensorTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/ShapeFilterTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/ShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/SixDOFConstraintTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/SliderConstraintTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/SoftBodyTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/SubShapeIDTest.cpp
+	${UNIT_TESTS_ROOT}/Physics/TaperedCylinderShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/TransformedShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/WheeledVehicleTests.cpp
+	${UNIT_TESTS_ROOT}/PhysicsTestContext.cpp
+	${UNIT_TESTS_ROOT}/PhysicsTestContext.h
+	${UNIT_TESTS_ROOT}/UnitTestFramework.cpp
+	${UNIT_TESTS_ROOT}/UnitTestFramework.h
+	${UNIT_TESTS_ROOT}/UnitTests.cmake
+)
+
+if (ENABLE_OBJECT_STREAM)
+	set(UNIT_TESTS_SRC_FILES
+		${UNIT_TESTS_SRC_FILES}
+		${UNIT_TESTS_ROOT}/ObjectStream/ObjectStreamTest.cpp
+	)
+endif()
+
+# Group source files
+source_group(TREE ${UNIT_TESTS_ROOT} FILES ${UNIT_TESTS_SRC_FILES})