Made Mat44::sRotation and QuickSort platform independent (#196)

* Mat44::sRotation did not return the same value for ARM vs x86
* Added implementation of QuickSort because std::sort is not cross platform deterministic
* Added implementation of InsertionSort

Jolt/Core/Core.h

@@ -223,7 +223,6 @@
 // Standard C++ includes
 JPH_SUPPRESS_WARNINGS_STD_BEGIN
 #include <vector>
-#include <algorithm>
 #include <utility>
 #include <cmath>
 #include <sstream>

Jolt/Core/InsertionSort.h

@@ -0,0 +1,57 @@
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of the insertion sort algorithm.
+template <typename Iterator, typename Compare>
+inline void InsertionSort(Iterator inBegin, Iterator inEnd, Compare inCompare)
+{
+	// Empty arrays don't need to be sorted
+	if (inBegin != inEnd)
+	{
+		// Start at the second element
+		for (Iterator i = inBegin + 1; i != inEnd; ++i)
+		{
+			// Move this element to a temporary value
+			auto x = move(*i);
+
+			// Check if the element goes before inBegin (we can't decrement the iterator before inBegin so this needs to be a separate branch)
+			if (inCompare(x, *inBegin))
+			{
+				// Move all elements to the right to make space for x
+				Iterator prev;
+				for (Iterator j = i; j != inBegin; j = prev)
+				{
+					prev = j - 1;
+					*j = *prev;
+				}
+
+				// Move x to the first place
+				*inBegin = move(x);
+			}
+			else
+			{
+				// Move elements to the right as long as they are bigger than x
+				Iterator j = i;
+				for (Iterator prev = j - 1; inCompare(x, *prev); j = prev, --prev)
+					*j = move(*prev);
+
+				// Move x into place
+				*j = move(x);
+			}
+		}
+	}
+}
+
+/// Implementation of insertion sort algorithm without comparator.
+template <typename Iterator>
+inline void InsertionSort(Iterator inBegin, Iterator inEnd)
+{
+	less<> compare;
+	InsertionSort(inBegin, inEnd, compare);
+}
+
+JPH_NAMESPACE_END

Jolt/Core/LinearCurve.h

@@ -4,6 +4,7 @@
 #pragma once
 
 #include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -38,7 +39,7 @@ public:
 	void				AddPoint(float inX, float inY)					{ mPoints.push_back({ inX, inY }); }
 
 	/// Sort the points on X ascending
-	void				Sort()											{ sort(mPoints.begin(), mPoints.end(), [](const Point &inLHS, const Point &inRHS) { return inLHS.mX < inRHS.mX; }); }
+	void				Sort()											{ QuickSort(mPoints.begin(), mPoints.end(), [](const Point &inLHS, const Point &inRHS) { return inLHS.mX < inRHS.mX; }); }
 
 	/// Get the lowest X value
 	float				GetMinX() const									{ return mPoints.empty()? 0.0f : mPoints.front().mX; }

Jolt/Core/Profiler.cpp

@@ -6,6 +6,7 @@
 #include <Jolt/Core/Profiler.h>
 #include <Jolt/Core/Color.h>
 #include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_SUPPRESS_WARNINGS_STD_BEGIN
 #include <fstream>
@@ -221,7 +222,7 @@ void Profiler::DumpList(const char *inTag, const Aggregators &inAggregators)
 	
 	// Sort the list
 	Aggregators aggregators = inAggregators;
-	sort(aggregators.begin(), aggregators.end());
+	QuickSort(aggregators.begin(), aggregators.end());
 	
 	// Write all aggregators
 	for (const Aggregator &item : aggregators)

Jolt/Core/QuickSort.h

@@ -0,0 +1,136 @@
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/InsertionSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper function for QuickSort, will move the pivot element to inMiddle.
+template <typename Iterator, typename Compare>
+inline void QuickSortMedianOfThree(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare)
+{
+	// This should be guaranteed because we switch over to insertion sort when there's 32 or less elements
+	JPH_ASSERT(inFirst != inMiddle && inMiddle != inLast); 
+
+	if (inCompare(*inMiddle, *inFirst))
+		swap(*inFirst, *inMiddle);
+	
+	if (inCompare(*inLast, *inFirst))
+		swap(*inFirst, *inLast);
+
+	if (inCompare(*inLast, *inMiddle))
+		swap(*inMiddle, *inLast);
+}
+
+/// Helper function for QuickSort using the Ninther method, will move the pivot element to inMiddle.
+template <typename Iterator, typename Compare>
+inline void QuickSortNinther(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare)
+{
+	// Divide the range in 8 equal parts (this means there are 9 points)
+	auto diff = (inLast - inFirst) >> 3;
+	auto two_diff = diff << 1;
+
+	// Median of first 3 points
+	Iterator mid1 = inFirst + diff;
+	QuickSortMedianOfThree(inFirst, mid1, inFirst + two_diff, inCompare);
+
+	// Median of second 3 points
+	QuickSortMedianOfThree(inMiddle - diff, inMiddle, inMiddle + diff, inCompare);
+
+	// Median of third 3 points
+	Iterator mid3 = inLast - diff;
+	QuickSortMedianOfThree(inLast - two_diff, mid3, inLast, inCompare);
+
+	// Determine the median of the 3 medians
+	QuickSortMedianOfThree(mid1, inMiddle, mid3, inCompare);
+}
+
+/// Implementation of the quick sort algorithm. The STL version implementation is not consistent across platforms.
+template <typename Iterator, typename Compare>
+inline void QuickSort(Iterator inBegin, Iterator inEnd, Compare inCompare)
+{
+	// Implementation based on https://en.wikipedia.org/wiki/Quicksort using Hoare's partition scheme
+
+	// Loop so that we only need to do 1 recursive call instead of 2.
+	for (;;)
+	{
+		// If there's less than 2 elements we're done
+		auto num_elements = inEnd - inBegin;
+		if (num_elements < 2)
+			return;
+
+		// Fall back to insertion sort if there are too few elements
+		if (num_elements <= 32)
+		{
+			InsertionSort(inBegin, inEnd, inCompare);
+			return;
+		}
+
+		// Determine pivot
+		Iterator pivot_iterator = inBegin + ((num_elements - 1) >> 1);
+		QuickSortNinther(inBegin, pivot_iterator, inEnd - 1, inCompare);
+		auto pivot = *pivot_iterator;
+
+		// Left and right iterators
+		Iterator i = inBegin;
+		Iterator j = inEnd;
+
+		for (;;)
+		{
+			// Find the first element that is bigger than the pivot
+			while (inCompare(*i, pivot))
+				i++;
+
+			// Find the last element that is smaller than the pivot
+			do
+				--j;
+			while (inCompare(pivot, *j));
+
+			// If the two iterators crossed, we're done
+			if (i >= j)
+				break;
+
+			// Swap the elements
+			swap(*i, *j);
+
+			// Note that the first while loop in this function should
+			// have been do i++ while (...) but since we cannot decrement
+			// the iterator from inBegin we left that out, so we need to do
+			// it here.
+			++i;
+		}
+
+		// Include the middle element on the left side
+		j++;
+
+		// Check which partition is smaller
+		if (j - inBegin < inEnd - j)
+		{
+			// Left side is smaller, recurse to left first
+			QuickSort(inBegin, j, inCompare);
+
+			// Loop again with the right side to avoid a call
+			inBegin = j;
+		}
+		else
+		{
+			// Right side is smaller, recurse to right first
+			QuickSort(j, inEnd, inCompare);
+
+			// Loop again with the left side to avoid a call
+			inEnd = j;
+		}
+	}
+}
+
+/// Implementation of quick sort algorithm without comparator.
+template <typename Iterator>
+inline void QuickSort(Iterator inBegin, Iterator inEnd)
+{
+	less<> compare;
+	QuickSort(inBegin, inEnd, compare);
+}
+
+JPH_NAMESPACE_END

Jolt/Jolt.cmake

@@ -21,6 +21,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Core/FPException.h
 	${JOLT_PHYSICS_ROOT}/Core/FPFlushDenormals.h
 	${JOLT_PHYSICS_ROOT}/Core/HashCombine.h
+	${JOLT_PHYSICS_ROOT}/Core/InsertionSort.h
 	${JOLT_PHYSICS_ROOT}/Core/IssueReporting.cpp
 	${JOLT_PHYSICS_ROOT}/Core/IssueReporting.h
 	${JOLT_PHYSICS_ROOT}/Core/JobSystem.h
@@ -39,6 +40,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Core/Profiler.cpp
 	${JOLT_PHYSICS_ROOT}/Core/Profiler.h
 	${JOLT_PHYSICS_ROOT}/Core/Profiler.inl
+	${JOLT_PHYSICS_ROOT}/Core/QuickSort.h
 	${JOLT_PHYSICS_ROOT}/Core/Reference.h
 	${JOLT_PHYSICS_ROOT}/Core/Result.h
 	${JOLT_PHYSICS_ROOT}/Core/RTTI.cpp

Jolt/Math/Mat44.inl

@@ -107,9 +107,9 @@ Mat44 Mat44::sRotation(QuatArg inQuat)
 	float z = inQuat.GetZ();
 	float w = inQuat.GetW();
 
-	float tx = 2.0f * x;
-	float ty = 2.0f * y;
-	float tz = 2.0f * z;
+	float tx = x + x; // Note: Using x + x instead of 2.0f * x to force this function to return the same value as the SSE4.1 version across platforms.
+	float ty = y + y;
+	float tz = z + z;
 
 	float xx = tx * x;
 	float yy = ty * y;
@@ -121,9 +121,9 @@ Mat44 Mat44::sRotation(QuatArg inQuat)
 	float yw = ty * w;
 	float zw = tz * w;
 
-	return Mat44(Vec4(1.0f - yy - zz, xy + zw, xz - yw, 0.0f),
-				 Vec4(xy - zw, 1.0f - xx - zz, yz + xw, 0.0f),
-				 Vec4(xz + yw, yz - xw, 1.0f - xx - yy, 0.0f),
+	return Mat44(Vec4((1.0f - yy) - zz, xy + zw, xz - yw, 0.0f), // Note: Added extra brackets to force this function to return the same value as the SSE4.1 version across platforms.
+				 Vec4(xy - zw, (1.0f - zz) - xx, yz + xw, 0.0f),
+				 Vec4(xz + yw, yz - xw, (1.0f - xx) - yy, 0.0f),
 				 Vec4(0.0f, 0.0f, 0.0f, 1.0f));
 #endif
 }

Jolt/Physics/Body/BodyManager.cpp

@@ -10,6 +10,7 @@
 #include <Jolt/Physics/Body/BodyActivationListener.h>
 #include <Jolt/Physics/StateRecorder.h>
 #include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/QuickSort.h>
 #ifdef JPH_DEBUG_RENDERER
 	#include <Jolt/Renderer/DebugRenderer.h>
 #endif // JPH_DEBUG_RENDERER
@@ -489,7 +490,7 @@ void BodyManager::SaveState(StateRecorder &inStream) const
 		// Write active bodies, sort because activation can come from multiple threads, so order is not deterministic
 		inStream.Write(mNumActiveBodies);
 		BodyIDVector sorted_active_bodies(mActiveBodies, mActiveBodies + mNumActiveBodies);
-		sort(sorted_active_bodies.begin(), sorted_active_bodies.end());
+		QuickSort(sorted_active_bodies.begin(), sorted_active_bodies.end());
 		for (const BodyID &id : sorted_active_bodies)
 			inStream.Write(id);
 
@@ -540,7 +541,7 @@ bool BodyManager::RestoreState(StateRecorder &inStream)
 		for (const BodyID *id = mActiveBodies, *id_end = mActiveBodies + mNumActiveBodies; id < id_end; ++id)
 			mBodies[id->GetIndex()]->mMotionProperties->mIndexInActiveBodies = Body::cInactiveIndex;
 
-		sort(mActiveBodies, mActiveBodies + mNumActiveBodies); // Sort for validation
+		QuickSort(mActiveBodies, mActiveBodies + mNumActiveBodies); // Sort for validation
 
 		// Read active bodies
 		inStream.Read(mNumActiveBodies);

Jolt/Physics/Body/MassProperties.cpp

@@ -10,6 +10,7 @@
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/InsertionSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -32,7 +33,7 @@ bool MassProperties::DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3
 
 	// Sort so that the biggest value goes first
 	int indices[] = { 0, 1, 2 };
-	sort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; });
+	InsertionSort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; });
 		
 	// Convert to a regular Mat44 and Vec3
 	outRotation = Mat44::sIdentity();

Jolt/Physics/Character/CharacterVirtual.cpp

@@ -8,6 +8,7 @@
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/Collision/ShapeCast.h>
 #include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Core/QuickSort.h>
 #ifdef JPH_DEBUG_RENDERER
 	#include <Jolt/Renderer/DebugRenderer.h>
 #endif // JPH_DEBUG_RENDERER
@@ -197,7 +198,7 @@ bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDis
 		return false;
 
 	// Sort the contacts on fraction
-	sort(contacts.begin(), contacts.end(), [](const Contact &inLHS, const Contact &inRHS) { return inLHS.mFraction < inRHS.mFraction; });
+	QuickSort(contacts.begin(), contacts.end(), [](const Contact &inLHS, const Contact &inRHS) { return inLHS.mFraction < inRHS.mFraction; });
 
 	// Check the first contact that will make us penetrate more than the allowed tolerance
 	bool valid_contact = false;
@@ -385,7 +386,7 @@ void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, Vec3Arg inGravity, f
 		}
 				
 		// Sort constraints on proximity
-		sort(sorted_constraints.begin(), sorted_constraints.end(), [](const Constraint *inLHS, const Constraint *inRHS) {
+		QuickSort(sorted_constraints.begin(), sorted_constraints.end(), [](const Constraint *inLHS, const Constraint *inRHS) {
 				// If both constraints hit at t = 0 then order the one that will push the character furthest first
 				// Note that because we add velocity to penetrating contacts, this will also resolve contacts that penetrate the most
 				if (inLHS->mTOI <= 0.0f && inRHS->mTOI <= 0.0f)

Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp

@@ -10,6 +10,7 @@
 #include <Jolt/Physics/Body/BodyPair.h>
 #include <Jolt/Geometry/RayAABox.h>
 #include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 	
@@ -41,7 +42,7 @@ void BroadPhaseBruteForce::AddBodiesFinalize(BodyID *ioBodies, int inNumber, Add
 	}
 
 	// Resort
-	sort(mBodyIDs.begin(), mBodyIDs.end());
+	QuickSort(mBodyIDs.begin(), mBodyIDs.end());
 }
 	
 void BroadPhaseBruteForce::RemoveBodies(BodyID *ioBodies, int inNumber) 

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -7,6 +7,7 @@
 #include <Jolt/Physics/Collision/AABoxCast.h>
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/PhysicsLock.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -158,7 +159,7 @@ BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int
 
 	// Sort bodies on layer
 	Body * const * const bodies_ptr = bodies.data(); // C pointer or else sort is incredibly slow in debug mode
-	sort(ioBodies, ioBodies + inNumber, [bodies_ptr](BodyID inLHS, BodyID inRHS) { return bodies_ptr[inLHS.GetIndex()]->GetBroadPhaseLayer() < bodies_ptr[inRHS.GetIndex()]->GetBroadPhaseLayer(); });
+	QuickSort(ioBodies, ioBodies + inNumber, [bodies_ptr](BodyID inLHS, BodyID inRHS) { return bodies_ptr[inLHS.GetIndex()]->GetBroadPhaseLayer() < bodies_ptr[inRHS.GetIndex()]->GetBroadPhaseLayer(); });
 
 	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
 	while (b_start < b_end)
@@ -282,7 +283,7 @@ void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber)
 
 	// Sort bodies on layer
 	Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
-	sort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+	QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
 
 	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
 	while (b_start < b_end)
@@ -332,7 +333,7 @@ void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber,
 
 	// Sort bodies on layer
 	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
-	sort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+	QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
 
 	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
 	while (b_start < b_end)
@@ -545,7 +546,7 @@ void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumAct
 
 	// Sort bodies on layer
 	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
-	sort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mObjectLayer < tracking[inRHS.GetIndex()].mObjectLayer; });
+	QuickSort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mObjectLayer < tracking[inRHS.GetIndex()].mObjectLayer; });
 
 	BodyID *b_start = ioActiveBodies, *b_end = ioActiveBodies + inNumActiveBodies;
 	while (b_start < b_end)

Jolt/Physics/Collision/CollisionCollectorImpl.h

@@ -4,6 +4,7 @@
 #pragma once
 
 #include <Jolt/Physics/Collision/CollisionCollector.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -32,7 +33,7 @@ public:
 	/// Order hits on closest first
 	void				Sort()
 	{
-		sort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); });
+		QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); });
 	}
 
 	/// Check if any hits were collected

Jolt/Physics/Constraints/ConstraintManager.cpp

@@ -8,6 +8,7 @@
 #include <Jolt/Physics/StateRecorder.h>
 #include <Jolt/Physics/PhysicsLock.h>
 #include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -101,7 +102,7 @@ void ConstraintManager::sSortConstraints(Constraint **inActiveConstraints, uint3
 {
 	JPH_PROFILE_FUNCTION();
 
-	sort(inConstraintIdxBegin, inConstraintIdxEnd, [inActiveConstraints](uint32 inLHS, uint32 inRHS) { return inActiveConstraints[inLHS]->mConstraintIndex < inActiveConstraints[inRHS]->mConstraintIndex; });
+	QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [inActiveConstraints](uint32 inLHS, uint32 inRHS) { return inActiveConstraints[inLHS]->mConstraintIndex < inActiveConstraints[inRHS]->mConstraintIndex; });
 }
 
 void ConstraintManager::sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime)

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -9,6 +9,7 @@
 #include <Jolt/Physics/PhysicsSettings.h>
 #include <Jolt/Physics/IslandBuilder.h>
 #include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/QuickSort.h>
 #ifdef JPH_DEBUG_RENDERER
 	#include <Jolt/Renderer/DebugRenderer.h>
 #endif // JPH_DEBUG_RENDERER
@@ -308,7 +309,7 @@ void ContactConstraintManager::ManifoldCache::GetAllBodyPairsSorted(Array<const
 	mCachedBodyPairs.GetAllKeyValues(outAll);
 
 	// Sort by key
-	sort(outAll.begin(), outAll.end(), [](const BPKeyValue *inLHS, const BPKeyValue *inRHS) {
+	QuickSort(outAll.begin(), outAll.end(), [](const BPKeyValue *inLHS, const BPKeyValue *inRHS) {
 		return inLHS->GetKey() < inRHS->GetKey();
 	});
 }
@@ -325,7 +326,7 @@ void ContactConstraintManager::ManifoldCache::GetAllManifoldsSorted(const Cached
 	}
 
 	// Sort by key
-	sort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) {
+	QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) {
 		return inLHS->GetKey() < inRHS->GetKey();
 	});
 }
@@ -342,7 +343,7 @@ void ContactConstraintManager::ManifoldCache::GetAllCCDManifoldsSorted(Array<con
 		}
 
 	// Sort by key
-	sort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) {
+	QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) {
 		return inLHS->GetKey() < inRHS->GetKey();
 	});
 }
@@ -1260,7 +1261,7 @@ void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32
 {
 	JPH_PROFILE_FUNCTION();
 
-	sort(inConstraintIdxBegin, inConstraintIdxEnd, [this](uint32 inLHS, uint32 inRHS) {
+	QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [this](uint32 inLHS, uint32 inRHS) {
 		const ContactConstraint &lhs = mConstraints[inLHS];
 		const ContactConstraint &rhs = mConstraints[inRHS];
 
@@ -1276,7 +1277,7 @@ void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32
 		if (lhs.mBody2 != rhs.mBody2)
 			return lhs.mBody2->GetID() < rhs.mBody2->GetID();
 
-		JPH_ASSERT(false, "Hash collision, ordering will be inconsistent");
+		JPH_ASSERT(inLHS == inRHS, "Hash collision, ordering will be inconsistent");
 		return false;
 	});
 }

Jolt/Physics/IslandBuilder.cpp

@@ -9,6 +9,7 @@
 #include <Jolt/Core/Profiler.h>
 #include <Jolt/Core/Atomics.h>
 #include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -360,7 +361,7 @@ void IslandBuilder::SortIslands(TempAllocator *inTempAllocator)
 
 		// Sort so the biggest islands go first, this means that the jobs that take longest will be running
 		// first which improves the chance that all jobs finish at the same time.
-		sort(mIslandsSorted, mIslandsSorted + mNumIslands, [num_constraints](uint32 inLHS, uint32 inRHS) {
+		QuickSort(mIslandsSorted, mIslandsSorted + mNumIslands, [num_constraints](uint32 inLHS, uint32 inRHS) {
 			return num_constraints[inLHS] > num_constraints[inRHS];
 		});
 

Jolt/Physics/PhysicsSystem.cpp

@@ -22,6 +22,7 @@
 #include <Jolt/Geometry/RayAABox.h>
 #include <Jolt/Core/JobSystem.h>
 #include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/QuickSort.h>
 #ifdef JPH_DEBUG_RENDERER
 	#include <Jolt/Renderer/DebugRenderer.h>
 #endif // JPH_DEBUG_RENDERER
@@ -1848,7 +1849,7 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 				*(dst_ccd_bodies++) = src_ccd_bodies++;
 
 			// Which we then sort
-			sort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2) 
+			QuickSort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2) 
 				{ 
 					if (inBody1->mFractionPlusSlop != inBody2->mFractionPlusSlop)
 						return inBody1->mFractionPlusSlop < inBody2->mFractionPlusSlop;

Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp

@@ -5,6 +5,7 @@
 
 #include <Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h>
 #include <Jolt/Geometry/MortonCode.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -56,7 +57,7 @@ void TriangleGrouperClosestCentroid::Group(const VertexList &inVertices, const I
 		Vec3 first_centroid = centroids[*batch_begin];
 
 		// Sort remaining triangles in batch on distance to first triangle
-		sort(batch_begin_plus_1, batch_end, 
+		QuickSort(batch_begin_plus_1, batch_end, 
 			[&first_centroid, &centroids](uint inLHS, uint inRHS)
 			{ 
 				return (centroids[inLHS] - first_centroid).LengthSq() < (centroids[inRHS] - first_centroid).LengthSq(); 

Jolt/TriangleGrouper/TriangleGrouperMorton.cpp

@@ -5,6 +5,7 @@
 
 #include <Jolt/TriangleGrouper/TriangleGrouperMorton.h>
 #include <Jolt/Geometry/MortonCode.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -41,7 +42,7 @@ void TriangleGrouperMorton::Group(const VertexList &inVertices, const IndexedTri
 		morton_codes[t] = MortonCode::sGetMortonCode(centroids[t], centroid_bounds);
 
 	// Sort triangles based on morton code
-	sort(outGroupedTriangleIndices.begin(), outGroupedTriangleIndices.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; });
+	QuickSort(outGroupedTriangleIndices.begin(), outGroupedTriangleIndices.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; });
 }
 
 JPH_NAMESPACE_END

Jolt/TriangleSplitter/TriangleSplitterMorton.cpp

@@ -5,6 +5,7 @@
 
 #include <Jolt/TriangleSplitter/TriangleSplitterMorton.h>
 #include <Jolt/Geometry/MortonCode.h>
+#include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -26,7 +27,7 @@ TriangleSplitterMorton::TriangleSplitterMorton(const VertexList &inVertices, con
 
 	// Sort triangles on morton code
 	const Array<uint32> &morton_codes = mMortonCodes;
-	sort(mSortedTriangleIdx.begin(), mSortedTriangleIdx.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; });
+	QuickSort(mSortedTriangleIdx.begin(), mSortedTriangleIdx.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; });
 }
 
 bool TriangleSplitterMorton::Split(const Range &inTriangles, Range &outLeft, Range &outRight)

Samples/Utils/ContactListenerImpl.cpp

@@ -6,6 +6,7 @@
 #include <Utils/ContactListenerImpl.h>
 #include <Renderer/DebugRendererImp.h>
 #include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Core/QuickSort.h>
 
 ValidateResult ContactListenerImpl::OnContactValidate(const Body &inBody1, const Body &inBody2, const CollideShapeResult &inCollisionResult)
 {
@@ -107,7 +108,7 @@ void ContactListenerImpl::SaveState(StateRecorder &inStream) const
 	Array<SubShapeIDPair> keys;
 	for (const StateMap::value_type &kv : mState)
 		keys.push_back(kv.first);
-	sort(keys.begin(), keys.end());
+	QuickSort(keys.begin(), keys.end());
 
 	// Write key value pairs
 	for (const SubShapeIDPair &k : keys)
@@ -144,7 +145,7 @@ void ContactListenerImpl::RestoreState(StateRecorder &inStream)
 		// Get and sort keys
 		for (const StateMap::value_type &kv : old_state)
 			keys.push_back(kv.first);
-		sort(keys.begin(), keys.end());
+		QuickSort(keys.begin(), keys.end());
 	}
 
 	// Ensure we have the corect size

UnitTests/Core/InsertionSortTest.cpp

@@ -0,0 +1,95 @@
+// 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<int> array;
+		for (int i = 0; i < 100; i++)
+		{
+			int 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);
+	}
+}

UnitTests/Core/QuickSortTest.cpp

@@ -0,0 +1,95 @@
+// 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<int> array;
+		for (int i = 0; i < 1000; i++)
+		{
+			int 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);
+	}
+}

UnitTests/UnitTests.cmake

@@ -4,9 +4,11 @@ set(UNIT_TESTS_ROOT ${PHYSICS_REPO_ROOT}/UnitTests)
 # Source files
 set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Core/FPFlushDenormalsTest.cpp
+	${UNIT_TESTS_ROOT}/Core/InsertionSortTest.cpp
 	${UNIT_TESTS_ROOT}/Core/JobSystemTest.cpp
 	${UNIT_TESTS_ROOT}/Core/LinearCurveTest.cpp
 	${UNIT_TESTS_ROOT}/Core/StringToolsTest.cpp
+	${UNIT_TESTS_ROOT}/Core/QuickSortTest.cpp
 	${UNIT_TESTS_ROOT}/doctest.h
 	${UNIT_TESTS_ROOT}/Geometry/ConvexHullBuilderTest.cpp
 	${UNIT_TESTS_ROOT}/Geometry/EllipseTest.cpp