@bloomengine/engine 0.4.0 → 0.4.2

package/native/third_party/JoltPhysics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -1,1487 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
-#include <Jolt/Physics/SoftBody/SoftBodyUpdateContext.h>
-#include <Jolt/ObjectStream/TypeDeclarations.h>
-#include <Jolt/Core/StreamIn.h>
-#include <Jolt/Core/StreamOut.h>
-#include <Jolt/Core/QuickSort.h>
-#include <Jolt/Core/UnorderedMap.h>
-#include <Jolt/Core/UnorderedSet.h>
-#include <Jolt/Core/BinaryHeap.h>
-
-JPH_NAMESPACE_BEGIN
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Vertex)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mPosition)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mVelocity)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mInvMass)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Face)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mVertex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mMaterialIndex)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Edge)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mVertex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mRestLength)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mCompliance)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::RodStretchShear)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mVertex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mLength)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mInvMass)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mCompliance)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mBishop)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::RodBendTwist)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mRod)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mCompliance)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mOmega0)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::DihedralBend)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mVertex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mCompliance)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mInitialAngle)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Volume)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mVertex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mSixRestVolume)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mCompliance)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::InvBind)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mJointIndex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mInvBind)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::SkinWeight)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mInvBindIndex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mWeight)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Skinned)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mVertex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mWeights)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mMaxDistance)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopDistance)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopRadius)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::LRA)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mVertex)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mMaxDistance)
-}
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings)
-{
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertices)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mFaces)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeConstraints)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mDihedralBendConstraints)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mLRAConstraints)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mRodStretchShearConstraints)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mRodBendTwistConstraints)
-	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials)
-}
-
-void SoftBodySharedSettings::CalculateClosestKinematic()
-{
-	// Check if we already calculated this
-	if (!mClosestKinematic.empty())
-		return;
-
-	// Reserve output size
-	mClosestKinematic.resize(mVertices.size());
-
-	// Create a list of connected vertices
-	Array<Array<uint32>> connectivity;
-	connectivity.resize(mVertices.size());
-	for (const Edge &e : mEdgeConstraints)
-	{
-		connectivity[e.mVertex[0]].push_back(e.mVertex[1]);
-		connectivity[e.mVertex[1]].push_back(e.mVertex[0]);
-	}
-	for (const RodStretchShear &r : mRodStretchShearConstraints)
-	{
-		connectivity[r.mVertex[0]].push_back(r.mVertex[1]);
-		connectivity[r.mVertex[1]].push_back(r.mVertex[0]);
-	}
-
-	// Use Dijkstra's algorithm to find the closest kinematic vertex for each vertex
-	// See: https://en.wikipedia.org/wiki/Dijkstra's_algorithm
-	//
-	// An element in the open list
-	struct Open
-	{
-		// Order so that we get the shortest distance first
-		bool	operator < (const Open &inRHS) const
-		{
-			return mDistance > inRHS.mDistance;
-		}
-
-		uint32	mVertex;
-		float	mDistance;
-	};
-
-	// Start with all kinematic elements
-	Array<Open> to_visit;
-	for (uint32 v = 0; v < mVertices.size(); ++v)
-		if (mVertices[v].mInvMass == 0.0f)
-		{
-			mClosestKinematic[v].mVertex = v;
-			mClosestKinematic[v].mHops = 0;
-			mClosestKinematic[v].mDistance = 0.0f;
-			to_visit.push_back({ v, 0.0f });
-			BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less<Open> { });
-		}
-
-	// Visit all vertices remembering the closest kinematic vertex and its distance
-	JPH_IF_ENABLE_ASSERTS(float last_closest = 0.0f;)
-	while (!to_visit.empty())
-	{
-		// Pop element from the open list
-		BinaryHeapPop(to_visit.begin(), to_visit.end(), std::less<Open> { });
-		Open current = to_visit.back();
-		to_visit.pop_back();
-		JPH_ASSERT(current.mDistance >= last_closest);
-		JPH_IF_ENABLE_ASSERTS(last_closest = current.mDistance;)
-
-		// Loop through all of its connected vertices
-		for (uint32 v : connectivity[current.mVertex])
-		{
-			// Calculate distance from the current vertex to this target vertex and check if it is smaller
-			float new_distance = current.mDistance + (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current.mVertex].mPosition)).Length();
-			if (new_distance < mClosestKinematic[v].mDistance)
-			{
-				// Remember new closest vertex
-				mClosestKinematic[v].mVertex = mClosestKinematic[current.mVertex].mVertex;
-				mClosestKinematic[v].mHops = mClosestKinematic[current.mVertex].mHops + 1;
-				mClosestKinematic[v].mDistance = new_distance;
-				to_visit.push_back({ v, new_distance });
-				BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less<Open> { });
-			}
-		}
-	}
-}
-
-void SoftBodySharedSettings::CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType, float inAngleTolerance)
-{
-	struct EdgeHelper
-	{
-		uint32	mVertex[2];
-		uint32	mEdgeIdx;
-	};
-
-	// Create list of all edges
-	Array<EdgeHelper> edges;
-	edges.reserve(mFaces.size() * 3);
-	for (const Face &f : mFaces)
-		for (int i = 0; i < 3; ++i)
-		{
-			uint32 v0 = f.mVertex[i];
-			uint32 v1 = f.mVertex[(i + 1) % 3];
-
-			EdgeHelper e;
-			e.mVertex[0] = min(v0, v1);
-			e.mVertex[1] = max(v0, v1);
-			e.mEdgeIdx = uint32(&f - mFaces.data()) * 3 + i;
-			edges.push_back(e);
-		}
-
-	// Sort the edges
-	QuickSort(edges.begin(), edges.end(), [](const EdgeHelper &inLHS, const EdgeHelper &inRHS) { return inLHS.mVertex[0] < inRHS.mVertex[0] || (inLHS.mVertex[0] == inRHS.mVertex[0] && inLHS.mVertex[1] < inRHS.mVertex[1]); });
-
-	// Only add edges if one of the vertices is movable
-	auto add_edge = [this](uint32 inVtx1, uint32 inVtx2, float inCompliance1, float inCompliance2) {
-		if ((mVertices[inVtx1].mInvMass > 0.0f || mVertices[inVtx2].mInvMass > 0.0f)
-			&& inCompliance1 < FLT_MAX && inCompliance2 < FLT_MAX)
-		{
-			Edge temp_edge;
-			temp_edge.mVertex[0] = inVtx1;
-			temp_edge.mVertex[1] = inVtx2;
-			temp_edge.mCompliance = 0.5f * (inCompliance1 + inCompliance2);
-			temp_edge.mRestLength = (Vec3(mVertices[inVtx2].mPosition) - Vec3(mVertices[inVtx1].mPosition)).Length();
-			JPH_ASSERT(temp_edge.mRestLength > 0.0f);
-			mEdgeConstraints.push_back(temp_edge);
-		}
-	};
-
-	// Helper function to get the attributes of a vertex
-	auto attr = [inVertexAttributes, inVertexAttributesLength](uint32 inVertex) {
-		return inVertexAttributes[min(inVertex, inVertexAttributesLength - 1)];
-	};
-
-	// Create the constraints
-	float sq_sin_tolerance = Square(Sin(inAngleTolerance));
-	float sq_cos_tolerance = Square(Cos(inAngleTolerance));
-	mEdgeConstraints.clear();
-	mEdgeConstraints.reserve(edges.size());
-	for (Array<EdgeHelper>::size_type i = 0; i < edges.size(); ++i)
-	{
-		const EdgeHelper &e0 = edges[i];
-
-		// Get attributes for the vertices of the edge
-		const VertexAttributes &a0 = attr(e0.mVertex[0]);
-		const VertexAttributes &a1 = attr(e0.mVertex[1]);
-
-		// Flag that indicates if this edge is a shear edge (if 2 triangles form a quad-like shape and this edge is on the diagonal)
-		bool is_shear = false;
-
-		// Test if there are any shared edges
-		for (Array<EdgeHelper>::size_type j = i + 1; j < edges.size(); ++j)
-		{
-			const EdgeHelper &e1 = edges[j];
-			if (e0.mVertex[0] == e1.mVertex[0] && e0.mVertex[1] == e1.mVertex[1])
-			{
-				// Get opposing vertices
-				const Face &f0 = mFaces[e0.mEdgeIdx / 3];
-				const Face &f1 = mFaces[e1.mEdgeIdx / 3];
-				uint32 vopposite0 = f0.mVertex[(e0.mEdgeIdx + 2) % 3];
-				uint32 vopposite1 = f1.mVertex[(e1.mEdgeIdx + 2) % 3];
-				const VertexAttributes &a_opposite0 = attr(vopposite0);
-				const VertexAttributes &a_opposite1 = attr(vopposite1);
-
-				// If the opposite vertices happen to be the same vertex then we have 2 triangles back to back and we skip creating shear / bend constraints
-				if (vopposite0 == vopposite1)
-					continue;
-
-				// Faces should be roughly in a plane
-				Vec3 n0 = (Vec3(mVertices[f0.mVertex[2]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f0.mVertex[1]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition));
-				Vec3 n1 = (Vec3(mVertices[f1.mVertex[2]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f1.mVertex[1]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition));
-				float n0_dot_n1 = n0.Dot(n1);
-				if (n0_dot_n1 > 0.0f
-					&& Square(n0_dot_n1) > sq_cos_tolerance * n0.LengthSq() * n1.LengthSq())
-				{
-					// Faces should approximately form a quad
-					Vec3 e0_dir = Vec3(mVertices[vopposite0].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition);
-					Vec3 e1_dir = Vec3(mVertices[vopposite1].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition);
-					if (Square(e0_dir.Dot(e1_dir)) < sq_sin_tolerance * e0_dir.LengthSq() * e1_dir.LengthSq())
-					{
-						// Shear constraint
-						add_edge(vopposite0, vopposite1, a_opposite0.mShearCompliance, a_opposite1.mShearCompliance);
-						is_shear = true;
-					}
-				}
-
-				// Bend constraint
-				switch (inBendType)
-				{
-				case EBendType::None:
-					// Do nothing
-					break;
-
-				case EBendType::Distance:
-					// Create an edge constraint to represent the bend constraint
-					// Use the bend compliance of the shared edge
-					if (!is_shear)
-						add_edge(vopposite0, vopposite1, a0.mBendCompliance, a1.mBendCompliance);
-					break;
-
-				case EBendType::Dihedral:
-					// Test if both opposite vertices are free to move
-					if ((mVertices[vopposite0].mInvMass > 0.0f || mVertices[vopposite1].mInvMass > 0.0f)
-						&& a0.mBendCompliance < FLT_MAX && a1.mBendCompliance < FLT_MAX)
-					{
-						// Create a bend constraint
-						// Use the bend compliance of the shared edge
-						mDihedralBendConstraints.emplace_back(e0.mVertex[0], e0.mVertex[1], vopposite0, vopposite1, 0.5f * (a0.mBendCompliance + a1.mBendCompliance));
-					}
-					break;
-				}
-			}
-			else
-			{
-				// Start iterating from the first non-shared edge
-				i = j - 1;
-				break;
-			}
-		}
-
-		// Create a edge constraint for the current edge
-		add_edge(e0.mVertex[0], e0.mVertex[1], is_shear? a0.mShearCompliance : a0.mCompliance, is_shear? a1.mShearCompliance : a1.mCompliance);
-	}
-	mEdgeConstraints.shrink_to_fit();
-
-	// Calculate the initial angle for all bend constraints
-	CalculateBendConstraintConstants();
-
-	// Check if any vertices have LRA constraints
-	bool has_lra_constraints = false;
-	for (const VertexAttributes *va = inVertexAttributes; va < inVertexAttributes + inVertexAttributesLength; ++va)
-		if (va->mLRAType != ELRAType::None)
-		{
-			has_lra_constraints = true;
-			break;
-		}
-	if (has_lra_constraints)
-	{
-		// Ensure we have calculated the closest kinematic vertex for each vertex
-		CalculateClosestKinematic();
-
-		// Find non-kinematic vertices
-		for (uint32 v = 0; v < (uint32)mVertices.size(); ++v)
-			if (mVertices[v].mInvMass > 0.0f)
-			{
-				// Check if a closest vertex was found
-				uint32 closest = mClosestKinematic[v].mVertex;
-				if (closest != 0xffffffff)
-				{
-					// Check which LRA constraint to create
-					const VertexAttributes &va = attr(v);
-					switch (va.mLRAType)
-					{
-					case ELRAType::None:
-						break;
-
-					case ELRAType::EuclideanDistance:
-						mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * (Vec3(mVertices[closest].mPosition) - Vec3(mVertices[v].mPosition)).Length());
-						break;
-
-					case ELRAType::GeodesicDistance:
-						mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * mClosestKinematic[v].mDistance);
-						break;
-					}
-				}
-			}
-	}
-}
-
-void SoftBodySharedSettings::CalculateEdgeLengths()
-{
-	for (Edge &e : mEdgeConstraints)
-	{
-		JPH_ASSERT(e.mVertex[0] != e.mVertex[1], "Edges need to connect 2 different vertices");
-		e.mRestLength = (Vec3(mVertices[e.mVertex[1]].mPosition) - Vec3(mVertices[e.mVertex[0]].mPosition)).Length();
-		JPH_ASSERT(e.mRestLength > 0.0f);
-	}
-}
-
-void SoftBodySharedSettings::CalculateRodProperties()
-{
-	// Mark connections through bend twist constraints
-	Array<Array<uint32>> connections;
-	connections.resize(mRodStretchShearConstraints.size());
-	for (const RodBendTwist &c : mRodBendTwistConstraints)
-	{
-		JPH_ASSERT(c.mRod[0] != c.mRod[1], "A bend twist constraint needs to be attached to different rods");
-		connections[c.mRod[1]].push_back(c.mRod[0]);
-		connections[c.mRod[0]].push_back(c.mRod[1]);
-	}
-
-	// Now calculate the Bishop frames for all rods
-	struct Entry
-	{
-		uint32	mFrom;	// Rod we're coming from
-		uint32	mTo;	// Rod we're going to
-	};
-	Array<Entry> stack;
-	stack.reserve(mRodStretchShearConstraints.size());
-	for (uint32 r0_idx = 0; r0_idx < mRodStretchShearConstraints.size(); ++r0_idx)
-	{
-		RodStretchShear &r0 = mRodStretchShearConstraints[r0_idx];
-
-		// Do not calculate a 2nd time
-		if (r0.mBishop == Quat::sZero())
-		{
-			// Calculate the frame for this rod
-			{
-				Vec3 tangent = Vec3(mVertices[r0.mVertex[1]].mPosition) - Vec3(mVertices[r0.mVertex[0]].mPosition);
-				r0.mLength = tangent.Length();
-				JPH_ASSERT(r0.mLength > 0.0f, "Rods of zero length are not supported!");
-				tangent /= r0.mLength;
-				Vec3 normal = tangent.GetNormalizedPerpendicular();
-				Vec3 binormal = tangent.Cross(normal);
-				r0.mBishop = Mat44(Vec4(normal, 0), Vec4(binormal, 0), Vec4(tangent, 0), Vec4(0, 0, 0, 1)).GetQuaternion().Normalized();
-			}
-
-			// Add connected rods to the stack if they haven't been calculated yet
-			for (uint32 r1_idx : connections[r0_idx])
-				if (mRodStretchShearConstraints[r1_idx].mBishop == Quat::sZero())
-					stack.push_back({ r0_idx, r1_idx });
-
-			// Now connect the bishop frame for all connected rods on the stack
-			// This follows the procedure outlined in "Discrete Elastic Rods" - M. Bergou et al.
-			// See: https://www.cs.columbia.edu/cg/pdfs/143-rods.pdf
-			while (!stack.empty())
-			{
-				uint32 r1_idx = stack.back().mFrom;
-				uint32 r2_idx = stack.back().mTo;
-				stack.pop_back();
-
-				const RodStretchShear &r1 = mRodStretchShearConstraints[r1_idx];
-				RodStretchShear &r2 = mRodStretchShearConstraints[r2_idx];
-
-				// Get the normal and tangent of the first rod's Bishop frame (that was already calculated)
-				Mat44 r1_frame = Mat44::sRotation(r1.mBishop);
-				Vec3 tangent1 = r1_frame.GetAxisZ();
-				Vec3 normal1 = r1_frame.GetAxisX();
-
-				// Calculate the Bishop frame for the 2nd rod
-				Vec3 tangent2 = Vec3(mVertices[r2.mVertex[1]].mPosition) - Vec3(mVertices[r2.mVertex[0]].mPosition);
-				if (tangent1.Dot(tangent2) < 0.0f)
-				{
-					// Edge is oriented in the opposite direction of the previous edge, flip it
-					std::swap(r2.mVertex[0], r2.mVertex[1]);
-					tangent2 = -tangent2;
-				}
-				r2.mLength = tangent2.Length();
-				JPH_ASSERT(r2.mLength > 0.0f, "Rods of zero length are not supported!");
-				tangent2 /= r2.mLength;
-				Vec3 t1_cross_t2 = tangent1.Cross(tangent2);
-				float sin_angle = t1_cross_t2.Length();
-				Vec3 normal2 = normal1;
-				if (sin_angle > 1.0e-6f)
-				{
-					t1_cross_t2 /= sin_angle;
-					normal2 = Quat::sRotation(t1_cross_t2, ASin(sin_angle)) * normal2;
-				}
-				Vec3 binormal2 = tangent2.Cross(normal2);
-				r2.mBishop = Mat44(Vec4(normal2, 0), Vec4(binormal2, 0), Vec4(tangent2, 0), Vec4(0, 0, 0, 1)).GetQuaternion().Normalized();
-
-				// Add connected rods to the stack if they haven't been calculated yet
-				for (uint32 r3_idx : connections[r2_idx])
-					if (mRodStretchShearConstraints[r3_idx].mBishop == Quat::sZero())
-						stack.push_back({ r2_idx, r3_idx });
-			}
-		}
-	}
-
-	// Calculate inverse mass for all rods by taking the minimum inverse mass (aka the heaviest vertex) of both vertices
-	for (RodStretchShear &r : mRodStretchShearConstraints)
-	{
-		JPH_ASSERT(r.mVertex[0] != r.mVertex[1], "A rod stretch shear constraint requires two different vertices");
-		r.mInvMass = min(mVertices[r.mVertex[0]].mInvMass, mVertices[r.mVertex[1]].mInvMass);
-	}
-
-	// Calculate the initial rotation between the rods
-	for (RodBendTwist &r : mRodBendTwistConstraints)
-		r.mOmega0 = (mRodStretchShearConstraints[r.mRod[0]].mBishop.Conjugated() * mRodStretchShearConstraints[r.mRod[1]].mBishop).Normalized();
-}
-
-void SoftBodySharedSettings::CalculateLRALengths(float inMaxDistanceMultiplier)
-{
-	for (LRA &l : mLRAConstraints)
-	{
-		JPH_ASSERT(l.mVertex[0] != l.mVertex[1], "LRA constraints need to connect 2 different vertices");
-		l.mMaxDistance = inMaxDistanceMultiplier * (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length();
-		JPH_ASSERT(l.mMaxDistance > 0.0f);
-	}
-}
-
-void SoftBodySharedSettings::CalculateBendConstraintConstants()
-{
-	for (DihedralBend &b : mDihedralBendConstraints)
-	{
-		JPH_ASSERT(b.mVertex[0] != b.mVertex[1] && b.mVertex[0] != b.mVertex[2] && b.mVertex[0] != b.mVertex[3]
-			&& b.mVertex[1] != b.mVertex[2] && b.mVertex[1] != b.mVertex[3]
-			&& b.mVertex[2] != b.mVertex[3], "Bend constraints need 4 different vertices");
-
-		// Get positions
-		Vec3 x0 = Vec3(mVertices[b.mVertex[0]].mPosition);
-		Vec3 x1 = Vec3(mVertices[b.mVertex[1]].mPosition);
-		Vec3 x2 = Vec3(mVertices[b.mVertex[2]].mPosition);
-		Vec3 x3 = Vec3(mVertices[b.mVertex[3]].mPosition);
-
-		/*
-		   x2
-		e1/  \e3
-		 /    \
-		x0----x1
-		 \ e0 /
-		e2\  /e4
-		   x3
-		*/
-
-		// Calculate edges
-		Vec3 e0 = x1 - x0;
-		Vec3 e1 = x2 - x0;
-		Vec3 e2 = x3 - x0;
-
-		// Normals of both triangles
-		Vec3 n1 = e0.Cross(e1);
-		Vec3 n2 = e2.Cross(e0);
-		float denom = sqrt(n1.LengthSq() * n2.LengthSq());
-		if (denom < 1.0e-12f)
-			b.mInitialAngle = 0.0f;
-		else
-		{
-			float sign = Sign(n2.Cross(n1).Dot(e0));
-			b.mInitialAngle = sign * ACosApproximate(n1.Dot(n2) / denom); // Runtime uses the approximation too
-		}
-	}
-}
-
-void SoftBodySharedSettings::CalculateVolumeConstraintVolumes()
-{
-	for (Volume &v : mVolumeConstraints)
-	{
-		JPH_ASSERT(v.mVertex[0] != v.mVertex[1] && v.mVertex[0] != v.mVertex[2] && v.mVertex[0] != v.mVertex[3]
-			&& v.mVertex[1] != v.mVertex[2] && v.mVertex[1] != v.mVertex[3]
-			&& v.mVertex[2] != v.mVertex[3], "Volume constraints need 4 different vertices");
-
-		Vec3 x1(mVertices[v.mVertex[0]].mPosition);
-		Vec3 x2(mVertices[v.mVertex[1]].mPosition);
-		Vec3 x3(mVertices[v.mVertex[2]].mPosition);
-		Vec3 x4(mVertices[v.mVertex[3]].mPosition);
-
-		Vec3 x1x2 = x2 - x1;
-		Vec3 x1x3 = x3 - x1;
-		Vec3 x1x4 = x4 - x1;
-
-		v.mSixRestVolume = abs(x1x2.Cross(x1x3).Dot(x1x4));
-	}
-}
-
-void SoftBodySharedSettings::CalculateSkinnedConstraintNormals()
-{
-	// Clear any previous results
-	mSkinnedConstraintNormals.clear();
-
-	// If there are no skinned constraints, we're done
-	if (mSkinnedConstraints.empty())
-		return;
-
-	// First collect all vertices that are skinned
-	using VertexIndexSet = UnorderedSet<uint32>;
-	VertexIndexSet skinned_vertices;
-	skinned_vertices.reserve(VertexIndexSet::size_type(mSkinnedConstraints.size()));
-	for (const Skinned &s : mSkinnedConstraints)
-		skinned_vertices.insert(s.mVertex);
-
-	// Now collect all faces that connect only to skinned vertices
-	using ConnectedFacesMap = UnorderedMap<uint32, VertexIndexSet>;
-	ConnectedFacesMap connected_faces;
-	connected_faces.reserve(ConnectedFacesMap::size_type(mVertices.size()));
-	for (const Face &f : mFaces)
-	{
-		// Must connect to only skinned vertices
-		bool valid = true;
-		for (uint32 v : f.mVertex)
-			valid &= skinned_vertices.find(v) != skinned_vertices.end();
-		if (!valid)
-			continue;
-
-		// Store faces that connect to vertices
-		for (uint32 v : f.mVertex)
-			connected_faces[v].insert(uint32(&f - mFaces.data()));
-	}
-
-	// Populate the list of connecting faces per skinned vertex
-	mSkinnedConstraintNormals.reserve(mFaces.size());
-	for (Skinned &s : mSkinnedConstraints)
-	{
-		uint32 start = uint32(mSkinnedConstraintNormals.size());
-		JPH_ASSERT((start >> 24) == 0);
-		ConnectedFacesMap::const_iterator connected_faces_it = connected_faces.find(s.mVertex);
-		if (connected_faces_it != connected_faces.cend())
-		{
-			const VertexIndexSet &faces = connected_faces_it->second;
-			uint32 num = uint32(faces.size());
-			JPH_ASSERT(num < 256);
-			mSkinnedConstraintNormals.insert(mSkinnedConstraintNormals.end(), faces.begin(), faces.end());
-			QuickSort(mSkinnedConstraintNormals.begin() + start, mSkinnedConstraintNormals.begin() + start + num);
-			s.mNormalInfo = start + (num << 24);
-		}
-		else
-			s.mNormalInfo = 0;
-	}
-	mSkinnedConstraintNormals.shrink_to_fit();
-}
-
-void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
-{
-	// Clear any previous results
-	mUpdateGroups.clear();
-
-	// Create a list of connected vertices
-	struct Connection
-	{
-		uint32	mVertex;
-		uint32	mCount;
-	};
-	Array<Array<Connection>> connectivity;
-	connectivity.resize(mVertices.size());
-	auto add_connection = [&connectivity](uint inV1, uint inV2) {
-			for (int i = 0; i < 2; ++i)
-			{
-				bool found = false;
-				for (Connection &c : connectivity[inV1])
-					if (c.mVertex == inV2)
-					{
-						c.mCount++;
-						found = true;
-						break;
-					}
-				if (!found)
-					connectivity[inV1].push_back({ inV2, 1 });
-
-				std::swap(inV1, inV2);
-			}
-		};
-	for (const Edge &c : mEdgeConstraints)
-		add_connection(c.mVertex[0], c.mVertex[1]);
-	for (const LRA &c : mLRAConstraints)
-		add_connection(c.mVertex[0], c.mVertex[1]);
-	for (const RodStretchShear &c : mRodStretchShearConstraints)
-		add_connection(c.mVertex[0], c.mVertex[1]);
-	for (const RodBendTwist &c : mRodBendTwistConstraints)
-	{
-		add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[0], mRodStretchShearConstraints[c.mRod[1]].mVertex[0]);
-		add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[1], mRodStretchShearConstraints[c.mRod[1]].mVertex[0]);
-		add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[0], mRodStretchShearConstraints[c.mRod[1]].mVertex[1]);
-		add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[1], mRodStretchShearConstraints[c.mRod[1]].mVertex[1]);
-	}
-	for (const DihedralBend &c : mDihedralBendConstraints)
-	{
-		add_connection(c.mVertex[0], c.mVertex[1]);
-		add_connection(c.mVertex[0], c.mVertex[2]);
-		add_connection(c.mVertex[0], c.mVertex[3]);
-		add_connection(c.mVertex[1], c.mVertex[2]);
-		add_connection(c.mVertex[1], c.mVertex[3]);
-		add_connection(c.mVertex[2], c.mVertex[3]);
-	}
-	for (const Volume &c : mVolumeConstraints)
-	{
-		add_connection(c.mVertex[0], c.mVertex[1]);
-		add_connection(c.mVertex[0], c.mVertex[2]);
-		add_connection(c.mVertex[0], c.mVertex[3]);
-		add_connection(c.mVertex[1], c.mVertex[2]);
-		add_connection(c.mVertex[1], c.mVertex[3]);
-		add_connection(c.mVertex[2], c.mVertex[3]);
-	}
-	// Skinned constraints only update 1 vertex, so we don't need special logic here
-
-	// Maps each of the vertices to a group index
-	Array<int> group_idx;
-	group_idx.resize(mVertices.size(), -1);
-
-	// Which group we are currently filling and its vertices
-	int current_group_idx = 0;
-	Array<uint> current_group;
-
-	// Start greedy algorithm to group vertices
-	for (;;)
-	{
-		// Find the bounding box of the ungrouped vertices
-		AABox bounds;
-		for (uint i = 0; i < (uint)mVertices.size(); ++i)
-			if (group_idx[i] == -1)
-				bounds.Encapsulate(Vec3(mVertices[i].mPosition));
-
-		// If the bounds are invalid, it means that there were no ungrouped vertices
-		if (!bounds.IsValid())
-			break;
-
-		// Determine longest and shortest axis
-		Vec3 bounds_size = bounds.GetSize();
-		uint max_axis = bounds_size.GetHighestComponentIndex();
-		uint min_axis = bounds_size.GetLowestComponentIndex();
-		if (min_axis == max_axis)
-			min_axis = (min_axis + 1) % 3;
-		uint mid_axis = 3 - min_axis - max_axis;
-
-		// Find the vertex that has the lowest value on the axis with the largest extent
-		uint current_vertex = UINT_MAX;
-		Float3 current_vertex_position { FLT_MAX, FLT_MAX, FLT_MAX };
-		for (uint i = 0; i < (uint)mVertices.size(); ++i)
-			if (group_idx[i] == -1)
-			{
-				const Float3 &vertex_position = mVertices[i].mPosition;
-				float max_axis_value = vertex_position[max_axis];
-				float mid_axis_value = vertex_position[mid_axis];
-				float min_axis_value = vertex_position[min_axis];
-
-				if (max_axis_value < current_vertex_position[max_axis]
-					|| (max_axis_value == current_vertex_position[max_axis]
-						&& (mid_axis_value < current_vertex_position[mid_axis]
-							|| (mid_axis_value == current_vertex_position[mid_axis]
-								&& min_axis_value < current_vertex_position[min_axis]))))
-				{
-					current_vertex_position = mVertices[i].mPosition;
-					current_vertex = i;
-				}
-			}
-		if (current_vertex == UINT_MAX)
-			break;
-
-		// Initialize the current group with 1 vertex
-		current_group.push_back(current_vertex);
-		group_idx[current_vertex] = current_group_idx;
-
-		// Fill up the group
-		for (;;)
-		{
-			// Find the vertex that is most connected to the current group
-			uint best_vertex = UINT_MAX;
-			uint best_num_connections = 0;
-			float best_dist_sq = FLT_MAX;
-			for (uint i = 0; i < (uint)current_group.size(); ++i) // For all vertices in the current group
-				for (const Connection &c : connectivity[current_group[i]]) // For all connections to other vertices
-				{
-					uint v = c.mVertex;
-					if (group_idx[v] == -1) // Ungrouped vertices only
-					{
-						// Count the number of connections to this group
-						uint num_connections = 0;
-						for (const Connection &v2 : connectivity[v])
-							if (group_idx[v2.mVertex] == current_group_idx)
-								num_connections += v2.mCount;
-
-						// Calculate distance to group centroid
-						float dist_sq = (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current_group.front()].mPosition)).LengthSq();
-
-						if (best_vertex == UINT_MAX
-							|| num_connections > best_num_connections
-							|| (num_connections == best_num_connections && dist_sq < best_dist_sq))
-						{
-							best_vertex = v;
-							best_num_connections = num_connections;
-							best_dist_sq = dist_sq;
-						}
-					}
-				}
-
-			// Add the best vertex to the current group
-			if (best_vertex != UINT_MAX)
-			{
-				current_group.push_back(best_vertex);
-				group_idx[best_vertex] = current_group_idx;
-			}
-
-			// Create a new group?
-			if (current_group.size() >= SoftBodyUpdateContext::cVertexConstraintBatch // If full, yes
-				|| (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2 && best_vertex == UINT_MAX)) // If half full and we found no connected vertex, yes
-			{
-				current_group.clear();
-				current_group_idx++;
-				break;
-			}
-
-			// If we didn't find a connected vertex, we need to find a new starting vertex
-			if (best_vertex == UINT_MAX)
-				break;
-		}
-	}
-
-	// If the last group is more than half full, we'll keep it as a separate group, otherwise we merge it with the 'non parallel' group
-	if (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2)
-		++current_group_idx;
-
-	// We no longer need the current group array, free the memory
-	current_group.clear();
-	current_group.shrink_to_fit();
-
-	// We're done with the connectivity list, free the memory
-	connectivity.clear();
-	connectivity.shrink_to_fit();
-
-	// Assign the constraints to their groups
-	struct Group
-	{
-		uint			GetSize() const
-		{
-			return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mRodStretchShearConstraints.size() + (uint)mRodBendTwistConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size();
-		}
-
-		Array<uint>		mEdgeConstraints;
-		Array<uint>		mLRAConstraints;
-		Array<uint>		mRodStretchShearConstraints;
-		Array<uint>		mRodBendTwistConstraints;
-		Array<uint>		mDihedralBendConstraints;
-		Array<uint>		mVolumeConstraints;
-		Array<uint>		mSkinnedConstraints;
-	};
-	Array<Group> groups;
-	groups.resize(current_group_idx + 1); // + non parallel group
-	for (const Edge &e : mEdgeConstraints)
-	{
-		int g1 = group_idx[e.mVertex[0]];
-		int g2 = group_idx[e.mVertex[1]];
-		JPH_ASSERT(g1 >= 0 && g2 >= 0);
-		if (g1 == g2) // In the same group
-			groups[g1].mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data()));
-		else // In different groups -> parallel group
-			groups.back().mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data()));
-	}
-	for (const LRA &l : mLRAConstraints)
-	{
-		int g1 = group_idx[l.mVertex[0]];
-		int g2 = group_idx[l.mVertex[1]];
-		JPH_ASSERT(g1 >= 0 && g2 >= 0);
-		if (g1 == g2) // In the same group
-			groups[g1].mLRAConstraints.push_back(uint(&l - mLRAConstraints.data()));
-		else // In different groups -> parallel group
-			groups.back().mLRAConstraints.push_back(uint(&l - mLRAConstraints.data()));
-	}
-	for (const RodStretchShear &r : mRodStretchShearConstraints)
-	{
-		int g1 = group_idx[r.mVertex[0]];
-		int g2 = group_idx[r.mVertex[1]];
-		JPH_ASSERT(g1 >= 0 && g2 >= 0);
-		if (g1 == g2) // In the same group
-			groups[g1].mRodStretchShearConstraints.push_back(uint(&r - mRodStretchShearConstraints.data()));
-		else // In different groups -> parallel group
-			groups.back().mRodStretchShearConstraints.push_back(uint(&r - mRodStretchShearConstraints.data()));
-	}
-	for (const RodBendTwist &r : mRodBendTwistConstraints)
-	{
-		int g1 = group_idx[mRodStretchShearConstraints[r.mRod[0]].mVertex[0]];
-		int g2 = group_idx[mRodStretchShearConstraints[r.mRod[0]].mVertex[1]];
-		int g3 = group_idx[mRodStretchShearConstraints[r.mRod[1]].mVertex[0]];
-		int g4 = group_idx[mRodStretchShearConstraints[r.mRod[1]].mVertex[1]];
-		JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0);
-		if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group
-			groups[g1].mRodBendTwistConstraints.push_back(uint(&r - mRodBendTwistConstraints.data()));
-		else // In different groups -> parallel group
-			groups.back().mRodBendTwistConstraints.push_back(uint(&r - mRodBendTwistConstraints.data()));
-	}
-	for (const DihedralBend &d : mDihedralBendConstraints)
-	{
-		int g1 = group_idx[d.mVertex[0]];
-		int g2 = group_idx[d.mVertex[1]];
-		int g3 = group_idx[d.mVertex[2]];
-		int g4 = group_idx[d.mVertex[3]];
-		JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0);
-		if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group
-			groups[g1].mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data()));
-		else // In different groups -> parallel group
-			groups.back().mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data()));
-	}
-	for (const Volume &v : mVolumeConstraints)
-	{
-		int g1 = group_idx[v.mVertex[0]];
-		int g2 = group_idx[v.mVertex[1]];
-		int g3 = group_idx[v.mVertex[2]];
-		int g4 = group_idx[v.mVertex[3]];
-		JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0);
-		if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group
-			groups[g1].mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data()));
-		else // In different groups -> parallel group
-			groups.back().mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data()));
-	}
-	for (const Skinned &s : mSkinnedConstraints)
-	{
-		int g1 = group_idx[s.mVertex];
-		JPH_ASSERT(g1 >= 0);
-		groups[g1].mSkinnedConstraints.push_back(uint(&s - mSkinnedConstraints.data()));
-	}
-
-	// Sort the parallel groups from big to small (this means the big groups will be scheduled first and have more time to complete)
-	QuickSort(groups.begin(), groups.end() - 1, [](const Group &inLHS, const Group &inRHS) { return inLHS.GetSize() > inRHS.GetSize(); });
-
-	// Make sure we know the closest kinematic vertex so we can sort
-	CalculateClosestKinematic();
-
-	// Sort within each group
-	for (Group &group : groups)
-	{
-		// Sort the edge constraints
-		QuickSort(group.mEdgeConstraints.begin(), group.mEdgeConstraints.end(), [this](uint inLHS, uint inRHS)
-			{
-				const Edge &e1 = mEdgeConstraints[inLHS];
-				const Edge &e2 = mEdgeConstraints[inRHS];
-
-				// First sort so that the edge with the smallest distance to a kinematic vertex comes first
-				float d1 = min(mClosestKinematic[e1.mVertex[0]].mDistance, mClosestKinematic[e1.mVertex[1]].mDistance);
-				float d2 = min(mClosestKinematic[e2.mVertex[0]].mDistance, mClosestKinematic[e2.mVertex[1]].mDistance);
-				if (d1 != d2)
-					return d1 < d2;
-
-				// Order the edges so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges).
-				// Note we could also re-order the vertices but that would be much more of a burden to the end user
-				uint32 m1 = e1.GetMinVertexIndex();
-				uint32 m2 = e2.GetMinVertexIndex();
-				if (m1 != m2)
-					return m1 < m2;
-
-				return inLHS < inRHS;
-			});
-
-		// Sort the LRA constraints
-		QuickSort(group.mLRAConstraints.begin(), group.mLRAConstraints.end(), [this](uint inLHS, uint inRHS)
-			{
-				const LRA &l1 = mLRAConstraints[inLHS];
-				const LRA &l2 = mLRAConstraints[inRHS];
-
-				// First sort so that the longest constraint comes first (meaning the shortest constraint has the most influence on the end result)
-				// Most of the time there will be a single LRA constraint per vertex and since the LRA constraint only modifies a single vertex,
-				// updating one constraint will not violate another constraint.
-				if (l1.mMaxDistance != l2.mMaxDistance)
-					return l1.mMaxDistance > l2.mMaxDistance;
-
-				// Order constraints so that the ones with the smallest index go first
-				uint32 m1 = l1.GetMinVertexIndex();
-				uint32 m2 = l2.GetMinVertexIndex();
-				if (m1 != m2)
-					return m1 < m2;
-
-				return inLHS < inRHS;
-			});
-
-		// Sort the rod stretch shear constraints
-		QuickSort(group.mRodStretchShearConstraints.begin(), group.mRodStretchShearConstraints.end(), [this](uint inLHS, uint inRHS)
-			{
-				const RodStretchShear &r1 = mRodStretchShearConstraints[inLHS];
-				const RodStretchShear &r2 = mRodStretchShearConstraints[inRHS];
-
-				// First sort so that the rod with the smallest distance to a kinematic vertex comes first
-				float d1 = min(mClosestKinematic[r1.mVertex[0]].mDistance, mClosestKinematic[r1.mVertex[1]].mDistance);
-				float d2 = min(mClosestKinematic[r2.mVertex[0]].mDistance, mClosestKinematic[r2.mVertex[1]].mDistance);
-				if (d1 != d2)
-					return d1 < d2;
-
-				// Then sort on the rod that connects to the smallest kinematic vertex
-				uint32 m1 = min(mClosestKinematic[r1.mVertex[0]].mVertex, mClosestKinematic[r1.mVertex[1]].mVertex);
-				uint32 m2 = min(mClosestKinematic[r2.mVertex[0]].mVertex, mClosestKinematic[r2.mVertex[1]].mVertex);
-				if (m1 != m2)
-					return m1 < m2;
-
-				// Order the rods so that the ones with the smallest index go first (hoping to get better cache locality when we process the rods).
-				m1 = r1.GetMinVertexIndex();
-				m2 = r2.GetMinVertexIndex();
-				if (m1 != m2)
-					return m1 < m2;
-
-				return inLHS < inRHS;
-			});
-
-		// Sort the rod bend twist constraints
-		QuickSort(group.mRodBendTwistConstraints.begin(), group.mRodBendTwistConstraints.end(), [this](uint inLHS, uint inRHS)
-			{
-				const RodBendTwist &b1 = mRodBendTwistConstraints[inLHS];
-				const RodStretchShear &b1_r1 = mRodStretchShearConstraints[b1.mRod[0]];
-				const RodStretchShear &b1_r2 = mRodStretchShearConstraints[b1.mRod[1]];
-
-				const RodBendTwist &b2 = mRodBendTwistConstraints[inRHS];
-				const RodStretchShear &b2_r1 = mRodStretchShearConstraints[b2.mRod[0]];
-				const RodStretchShear &b2_r2 = mRodStretchShearConstraints[b2.mRod[1]];
-
-				// First sort so that the rod with the smallest number of hops to a kinematic vertex comes first.
-				// Note that we don't use distance because of the bilateral interleaving below.
-				uint32 m1 = min(
-							min(mClosestKinematic[b1_r1.mVertex[0]].mHops, mClosestKinematic[b1_r1.mVertex[1]].mHops),
-							min(mClosestKinematic[b1_r2.mVertex[0]].mHops, mClosestKinematic[b1_r2.mVertex[1]].mHops));
-				uint32 m2 = min(
-							min(mClosestKinematic[b2_r1.mVertex[0]].mHops, mClosestKinematic[b2_r1.mVertex[1]].mHops),
-							min(mClosestKinematic[b2_r2.mVertex[0]].mHops, mClosestKinematic[b2_r2.mVertex[1]].mHops));
-				if (m1 != m2)
-					return m1 < m2;
-
-				// Then sort on the rod that connects to the kinematic vertex with lowest index.
-				// This ensures that we consistently order the rods that are attached to other kinematic constraints.
-				// Again, this helps bilateral interleaving below.
-				m1 = min(
-							min(mClosestKinematic[b1_r1.mVertex[0]].mVertex, mClosestKinematic[b1_r1.mVertex[1]].mVertex),
-							min(mClosestKinematic[b1_r2.mVertex[0]].mVertex, mClosestKinematic[b1_r2.mVertex[1]].mVertex));
-				m2 = min(
-							min(mClosestKinematic[b2_r1.mVertex[0]].mVertex, mClosestKinematic[b2_r1.mVertex[1]].mVertex),
-							min(mClosestKinematic[b2_r2.mVertex[0]].mVertex, mClosestKinematic[b2_r2.mVertex[1]].mVertex));
-				if (m1 != m2)
-					return m1 < m2;
-
-				// Finally order so that the smallest vertex index goes first
-				m1 = min(b1_r1.GetMinVertexIndex(), b1_r2.GetMinVertexIndex());
-				m2 = min(b2_r1.GetMinVertexIndex(), b2_r2.GetMinVertexIndex());
-				if (m1 != m2)
-					return m1 < m2;
-
-				return inLHS < inRHS;
-			});
-
-		// Bilateral interleaving, see figure 4 of "Position and Orientation Based Cosserat Rods" - Kugelstadt and Schoemer - SIGGRAPH 2016
-		// Keeping the twist constraints sorted often results in an unstable simulation
-		for (Array<uint>::size_type i = 1, s = group.mRodBendTwistConstraints.size(), s2 = s >> 1; i < s2; i += 2)
-			std::swap(group.mRodBendTwistConstraints[i], group.mRodBendTwistConstraints[s - i]);
-
-		// Sort the dihedral bend constraints
-		QuickSort(group.mDihedralBendConstraints.begin(), group.mDihedralBendConstraints.end(), [this](uint inLHS, uint inRHS)
-		{
-			const DihedralBend &b1 = mDihedralBendConstraints[inLHS];
-			const DihedralBend &b2 = mDihedralBendConstraints[inRHS];
-
-			// First sort so that the constraint with the smallest distance to a kinematic vertex comes first
-			float d1 = min(
-						min(mClosestKinematic[b1.mVertex[0]].mDistance, mClosestKinematic[b1.mVertex[1]].mDistance),
-						min(mClosestKinematic[b1.mVertex[2]].mDistance, mClosestKinematic[b1.mVertex[3]].mDistance));
-			float d2 = min(
-						min(mClosestKinematic[b2.mVertex[0]].mDistance, mClosestKinematic[b2.mVertex[1]].mDistance),
-						min(mClosestKinematic[b2.mVertex[2]].mDistance, mClosestKinematic[b2.mVertex[3]].mDistance));
-			if (d1 != d2)
-				return d1 < d2;
-
-			// Finally order so that the smallest vertex index goes first
-			uint32 m1 = b1.GetMinVertexIndex();
-			uint32 m2 = b2.GetMinVertexIndex();
-			if (m1 != m2)
-				return m1 < m2;
-
-			return inLHS < inRHS;
-		});
-
-		// Sort the volume constraints
-		QuickSort(group.mVolumeConstraints.begin(), group.mVolumeConstraints.end(), [this](uint inLHS, uint inRHS)
-		{
-			const Volume &v1 = mVolumeConstraints[inLHS];
-			const Volume &v2 = mVolumeConstraints[inRHS];
-
-			// First sort so that the constraint with the smallest distance to a kinematic vertex comes first
-			float d1 = min(
-						min(mClosestKinematic[v1.mVertex[0]].mDistance, mClosestKinematic[v1.mVertex[1]].mDistance),
-						min(mClosestKinematic[v1.mVertex[2]].mDistance, mClosestKinematic[v1.mVertex[3]].mDistance));
-			float d2 = min(
-						min(mClosestKinematic[v2.mVertex[0]].mDistance, mClosestKinematic[v2.mVertex[1]].mDistance),
-						min(mClosestKinematic[v2.mVertex[2]].mDistance, mClosestKinematic[v2.mVertex[3]].mDistance));
-			if (d1 != d2)
-				return d1 < d2;
-
-			// Order constraints so that the ones with the smallest index go first
-			uint32 m1 = v1.GetMinVertexIndex();
-			uint32 m2 = v2.GetMinVertexIndex();
-			if (m1 != m2)
-				return m1 < m2;
-
-			return inLHS < inRHS;
-		});
-
-		// Sort the skinned constraints
-		QuickSort(group.mSkinnedConstraints.begin(), group.mSkinnedConstraints.end(), [this](uint inLHS, uint inRHS)
-			{
-				const Skinned &s1 = mSkinnedConstraints[inLHS];
-				const Skinned &s2 = mSkinnedConstraints[inRHS];
-
-				// Order the skinned constraints so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges).
-				if (s1.mVertex != s2.mVertex)
-					return s1.mVertex < s2.mVertex;
-
-				return inLHS < inRHS;
-			});
-	}
-
-	// Temporary store constraints as we reorder them
-	Array<Edge> temp_edges;
-	temp_edges.swap(mEdgeConstraints);
-	mEdgeConstraints.reserve(temp_edges.size());
-	outResults.mEdgeRemap.resize(temp_edges.size(), ~uint(0));
-
-	Array<LRA> temp_lra;
-	temp_lra.swap(mLRAConstraints);
-	mLRAConstraints.reserve(temp_lra.size());
-	outResults.mLRARemap.resize(temp_lra.size(), ~uint(0));
-
-	Array<RodStretchShear> temp_rod_stretch_shear;
-	temp_rod_stretch_shear.swap(mRodStretchShearConstraints);
-	mRodStretchShearConstraints.reserve(temp_rod_stretch_shear.size());
-	outResults.mRodStretchShearConstraintRemap.resize(temp_rod_stretch_shear.size(), ~uint(0));
-
-	Array<RodBendTwist> temp_rod_bend_twist;
-	temp_rod_bend_twist.swap(mRodBendTwistConstraints);
-	mRodBendTwistConstraints.reserve(temp_rod_bend_twist.size());
-	outResults.mRodBendTwistConstraintRemap.resize(temp_rod_bend_twist.size(), ~uint(0));
-
-	Array<DihedralBend> temp_dihedral_bend;
-	temp_dihedral_bend.swap(mDihedralBendConstraints);
-	mDihedralBendConstraints.reserve(temp_dihedral_bend.size());
-	outResults.mDihedralBendRemap.resize(temp_dihedral_bend.size(), ~uint(0));
-
-	Array<Volume> temp_volume;
-	temp_volume.swap(mVolumeConstraints);
-	mVolumeConstraints.reserve(temp_volume.size());
-	outResults.mVolumeRemap.resize(temp_volume.size(), ~uint(0));
-
-	Array<Skinned> temp_skinned;
-	temp_skinned.swap(mSkinnedConstraints);
-	mSkinnedConstraints.reserve(temp_skinned.size());
-	outResults.mSkinnedRemap.resize(temp_skinned.size(), ~uint(0));
-
-	// Finalize update groups
-	for (const Group &group : groups)
-	{
-		// Reorder edge constraints for this group
-		for (uint idx : group.mEdgeConstraints)
-		{
-			outResults.mEdgeRemap[idx] = (uint)mEdgeConstraints.size();
-			mEdgeConstraints.push_back(temp_edges[idx]);
-		}
-
-		// Reorder LRA constraints for this group
-		for (uint idx : group.mLRAConstraints)
-		{
-			outResults.mLRARemap[idx] = (uint)mLRAConstraints.size();
-			mLRAConstraints.push_back(temp_lra[idx]);
-		}
-
-		// Reorder rod stretch shear constraints for this group
-		for (uint idx : group.mRodStretchShearConstraints)
-		{
-			outResults.mRodStretchShearConstraintRemap[idx] = (uint)mRodStretchShearConstraints.size();
-			mRodStretchShearConstraints.push_back(temp_rod_stretch_shear[idx]);
-		}
-
-		// Reorder rod bend twist constraints for this group
-		for (uint idx : group.mRodBendTwistConstraints)
-		{
-			outResults.mRodBendTwistConstraintRemap[idx] = (uint)mRodBendTwistConstraints.size();
-			mRodBendTwistConstraints.push_back(temp_rod_bend_twist[idx]);
-		}
-
-		// Reorder dihedral bend constraints for this group
-		for (uint idx : group.mDihedralBendConstraints)
-		{
-			outResults.mDihedralBendRemap[idx] = (uint)mDihedralBendConstraints.size();
-			mDihedralBendConstraints.push_back(temp_dihedral_bend[idx]);
-		}
-
-		// Reorder volume constraints for this group
-		for (uint idx : group.mVolumeConstraints)
-		{
-			outResults.mVolumeRemap[idx] = (uint)mVolumeConstraints.size();
-			mVolumeConstraints.push_back(temp_volume[idx]);
-		}
-
-		// Reorder skinned constraints for this group
-		for (uint idx : group.mSkinnedConstraints)
-		{
-			outResults.mSkinnedRemap[idx] = (uint)mSkinnedConstraints.size();
-			mSkinnedConstraints.push_back(temp_skinned[idx]);
-		}
-
-		// Store end indices
-		mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mRodStretchShearConstraints.size(), (uint)mRodBendTwistConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() });
-	}
-
-	// Remap bend twist indices because mRodStretchShearConstraints has been reordered
-	for (RodBendTwist &r : mRodBendTwistConstraints)
-		for (int i = 0; i < 2; ++i)
-			r.mRod[i] = outResults.mRodStretchShearConstraintRemap[r.mRod[i]];
-
-	// Free closest kinematic buffer
-	mClosestKinematic.clear();
-	mClosestKinematic.shrink_to_fit();
-}
-
-Ref<SoftBodySharedSettings> SoftBodySharedSettings::Clone() const
-{
-	Ref<SoftBodySharedSettings> clone = new SoftBodySharedSettings;
-	clone->mVertices = mVertices;
-	clone->mFaces = mFaces;
-	clone->mEdgeConstraints = mEdgeConstraints;
-	clone->mDihedralBendConstraints = mDihedralBendConstraints;
-	clone->mVolumeConstraints = mVolumeConstraints;
-	clone->mSkinnedConstraints = mSkinnedConstraints;
-	clone->mSkinnedConstraintNormals = mSkinnedConstraintNormals;
-	clone->mInvBindMatrices = mInvBindMatrices;
-	clone->mLRAConstraints = mLRAConstraints;
-	clone->mRodStretchShearConstraints = mRodStretchShearConstraints;
-	clone->mRodBendTwistConstraints = mRodBendTwistConstraints;
-	clone->mMaterials = mMaterials;
-	clone->mUpdateGroups = mUpdateGroups;
-	return clone;
-}
-
-void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const
-{
-	inStream.Write(mVertices);
-	inStream.Write(mFaces);
-	inStream.Write(mEdgeConstraints);
-	inStream.Write(mDihedralBendConstraints);
-	inStream.Write(mVolumeConstraints);
-	inStream.Write(mSkinnedConstraints);
-	inStream.Write(mSkinnedConstraintNormals);
-	inStream.Write(mLRAConstraints);
-	inStream.Write(mUpdateGroups);
-
-	// Can't write mRodStretchShearConstraints directly because the class contains padding
-	inStream.Write(mRodStretchShearConstraints, [](const RodStretchShear &inElement, StreamOut &inS) {
-		inS.Write(inElement.mVertex);
-		inS.Write(inElement.mLength);
-		inS.Write(inElement.mInvMass);
-		inS.Write(inElement.mCompliance);
-		inS.Write(inElement.mBishop);
-	});
-
-	// Can't write mRodBendTwistConstraints directly because the class contains padding
-	inStream.Write(mRodBendTwistConstraints, [](const RodBendTwist &inElement, StreamOut &inS) {
-		inS.Write(inElement.mRod);
-		inS.Write(inElement.mCompliance);
-		inS.Write(inElement.mOmega0);
-	});
-
-	// Can't write mInvBindMatrices directly because the class contains padding
-	inStream.Write(mInvBindMatrices, [](const InvBind &inElement, StreamOut &inS) {
-		inS.Write(inElement.mJointIndex);
-		inS.Write(inElement.mInvBind);
-	});
-}
-
-void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
-{
-	inStream.Read(mVertices);
-	inStream.Read(mFaces);
-	inStream.Read(mEdgeConstraints);
-	inStream.Read(mDihedralBendConstraints);
-	inStream.Read(mVolumeConstraints);
-	inStream.Read(mSkinnedConstraints);
-	inStream.Read(mSkinnedConstraintNormals);
-	inStream.Read(mLRAConstraints);
-	inStream.Read(mUpdateGroups);
-
-	inStream.Read(mRodStretchShearConstraints, [](StreamIn &inS, RodStretchShear &outElement) {
-		inS.Read(outElement.mVertex);
-		inS.Read(outElement.mLength);
-		inS.Read(outElement.mInvMass);
-		inS.Read(outElement.mCompliance);
-		inS.Read(outElement.mBishop);
-	});
-
-	inStream.Read(mRodBendTwistConstraints, [](StreamIn &inS, RodBendTwist &outElement) {
-		inS.Read(outElement.mRod);
-		inS.Read(outElement.mCompliance);
-		inS.Read(outElement.mOmega0);
-	});
-
-	inStream.Read(mInvBindMatrices, [](StreamIn &inS, InvBind &outElement) {
-		inS.Read(outElement.mJointIndex);
-		inS.Read(outElement.mInvBind);
-	});
-}
-
-void SoftBodySharedSettings::SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const
-{
-	SharedSettingsToIDMap::const_iterator settings_iter = ioSettingsMap.find(this);
-	if (settings_iter == ioSettingsMap.end())
-	{
-		// Write settings ID
-		uint32 settings_id = ioSettingsMap.size();
-		ioSettingsMap[this] = settings_id;
-		inStream.Write(settings_id);
-
-		// Write the settings
-		SaveBinaryState(inStream);
-
-		// Write materials
-		StreamUtils::SaveObjectArray(inStream, mMaterials, &ioMaterialMap);
-	}
-	else
-	{
-		// Known settings, just write the ID
-		inStream.Write(settings_iter->second);
-	}
-}
-
-SoftBodySharedSettings::SettingsResult SoftBodySharedSettings::sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap)
-{
-	SettingsResult result;
-
-	// Read settings id
-	uint32 settings_id;
-	inStream.Read(settings_id);
-	if (inStream.IsEOF() || inStream.IsFailed())
-	{
-		result.SetError("Failed to read settings id");
-		return result;
-	}
-
-	// Check nullptr settings
-	if (settings_id == ~uint32(0))
-	{
-		result.Set(nullptr);
-		return result;
-	}
-
-	// Check if we already read this settings
-	if (settings_id < ioSettingsMap.size())
-	{
-		result.Set(ioSettingsMap[settings_id]);
-		return result;
-	}
-
-	// Create new object
-	Ref<SoftBodySharedSettings> settings = new SoftBodySharedSettings;
-
-	// Read state
-	settings->RestoreBinaryState(inStream);
-
-	// Read materials
-	Result mlresult = StreamUtils::RestoreObjectArray<PhysicsMaterialList>(inStream, ioMaterialMap);
-	if (mlresult.HasError())
-	{
-		result.SetError(mlresult.GetError());
-		return result;
-	}
-	settings->mMaterials = mlresult.Get();
-
-	// Add the settings to the map
-	ioSettingsMap.push_back(settings);
-
-	result.Set(settings);
-	return result;
-}
-
-Ref<SoftBodySharedSettings> SoftBodySharedSettings::sCreateCube(uint inGridSize, float inGridSpacing)
-{
-	const Vec3 cOffset = Vec3::sReplicate(-0.5f * inGridSpacing * (inGridSize - 1));
-
-	// Create settings
-	SoftBodySharedSettings *settings = new SoftBodySharedSettings;
-	for (uint z = 0; z < inGridSize; ++z)
-		for (uint y = 0; y < inGridSize; ++y)
-			for (uint x = 0; x < inGridSize; ++x)
-			{
-				SoftBodySharedSettings::Vertex v;
-				(cOffset + Vec3::sReplicate(inGridSpacing) * Vec3(float(x), float(y), float(z))).StoreFloat3(&v.mPosition);
-				settings->mVertices.push_back(v);
-			}
-
-	// Function to get the vertex index of a point on the cube
-	auto vertex_index = [inGridSize](uint inX, uint inY, uint inZ)
-	{
-		return inX + inY * inGridSize + inZ * inGridSize * inGridSize;
-	};
-
-	// Create edges
-	for (uint z = 0; z < inGridSize; ++z)
-		for (uint y = 0; y < inGridSize; ++y)
-			for (uint x = 0; x < inGridSize; ++x)
-			{
-				SoftBodySharedSettings::Edge e;
-				e.mVertex[0] = vertex_index(x, y, z);
-				if (x < inGridSize - 1)
-				{
-					e.mVertex[1] = vertex_index(x + 1, y, z);
-					settings->mEdgeConstraints.push_back(e);
-				}
-				if (y < inGridSize - 1)
-				{
-					e.mVertex[1] = vertex_index(x, y + 1, z);
-					settings->mEdgeConstraints.push_back(e);
-				}
-				if (z < inGridSize - 1)
-				{
-					e.mVertex[1] = vertex_index(x, y, z + 1);
-					settings->mEdgeConstraints.push_back(e);
-				}
-			}
-	settings->CalculateEdgeLengths();
-
-	// Tetrahedrons to fill a cube
-	const int tetra_indices[6][4][3] = {
-		{ {0, 0, 0}, {0, 1, 1}, {0, 0, 1}, {1, 1, 1} },
-		{ {0, 0, 0}, {0, 1, 0}, {0, 1, 1}, {1, 1, 1} },
-		{ {0, 0, 0}, {0, 0, 1}, {1, 0, 1}, {1, 1, 1} },
-		{ {0, 0, 0}, {1, 0, 1}, {1, 0, 0}, {1, 1, 1} },
-		{ {0, 0, 0}, {1, 1, 0}, {0, 1, 0}, {1, 1, 1} },
-		{ {0, 0, 0}, {1, 0, 0}, {1, 1, 0}, {1, 1, 1} }
-	};
-
-	// Create volume constraints
-	for (uint z = 0; z < inGridSize - 1; ++z)
-		for (uint y = 0; y < inGridSize - 1; ++y)
-			for (uint x = 0; x < inGridSize - 1; ++x)
-				for (uint t = 0; t < 6; ++t)
-				{
-					SoftBodySharedSettings::Volume v;
-					for (uint i = 0; i < 4; ++i)
-						v.mVertex[i] = vertex_index(x + tetra_indices[t][i][0], y + tetra_indices[t][i][1], z + tetra_indices[t][i][2]);
-					settings->mVolumeConstraints.push_back(v);
-				}
-
-	settings->CalculateVolumeConstraintVolumes();
-
-	// Create faces
-	for (uint y = 0; y < inGridSize - 1; ++y)
-		for (uint x = 0; x < inGridSize - 1; ++x)
-		{
-			SoftBodySharedSettings::Face f;
-
-			// Face 1
-			f.mVertex[0] = vertex_index(x, y, 0);
-			f.mVertex[1] = vertex_index(x, y + 1, 0);
-			f.mVertex[2] = vertex_index(x + 1, y + 1, 0);
-			settings->AddFace(f);
-
-			f.mVertex[1] = vertex_index(x + 1, y + 1, 0);
-			f.mVertex[2] = vertex_index(x + 1, y, 0);
-			settings->AddFace(f);
-
-			// Face 2
-			f.mVertex[0] = vertex_index(x, y, inGridSize - 1);
-			f.mVertex[1] = vertex_index(x + 1, y + 1, inGridSize - 1);
-			f.mVertex[2] = vertex_index(x, y + 1, inGridSize - 1);
-			settings->AddFace(f);
-
-			f.mVertex[1] = vertex_index(x + 1, y, inGridSize - 1);
-			f.mVertex[2] = vertex_index(x + 1, y + 1, inGridSize - 1);
-			settings->AddFace(f);
-
-			// Face 3
-			f.mVertex[0] = vertex_index(x, 0, y);
-			f.mVertex[1] = vertex_index(x + 1, 0, y + 1);
-			f.mVertex[2] = vertex_index(x, 0, y + 1);
-			settings->AddFace(f);
-
-			f.mVertex[1] = vertex_index(x + 1, 0, y);
-			f.mVertex[2] = vertex_index(x + 1, 0, y + 1);
-			settings->AddFace(f);
-
-			// Face 4
-			f.mVertex[0] = vertex_index(x, inGridSize - 1, y);
-			f.mVertex[1] = vertex_index(x, inGridSize - 1, y + 1);
-			f.mVertex[2] = vertex_index(x + 1, inGridSize - 1, y + 1);
-			settings->AddFace(f);
-
-			f.mVertex[1] = vertex_index(x + 1, inGridSize - 1, y + 1);
-			f.mVertex[2] = vertex_index(x + 1, inGridSize - 1, y);
-			settings->AddFace(f);
-
-			// Face 5
-			f.mVertex[0] = vertex_index(0, x, y);
-			f.mVertex[1] = vertex_index(0, x, y + 1);
-			f.mVertex[2] = vertex_index(0, x + 1, y + 1);
-			settings->AddFace(f);
-
-			f.mVertex[1] = vertex_index(0, x + 1, y + 1);
-			f.mVertex[2] = vertex_index(0, x + 1, y);
-			settings->AddFace(f);
-
-			// Face 6
-			f.mVertex[0] = vertex_index(inGridSize - 1, x, y);
-			f.mVertex[1] = vertex_index(inGridSize - 1, x + 1, y + 1);
-			f.mVertex[2] = vertex_index(inGridSize - 1, x, y + 1);
-			settings->AddFace(f);
-
-			f.mVertex[1] = vertex_index(inGridSize - 1, x + 1, y);
-			f.mVertex[2] = vertex_index(inGridSize - 1, x + 1, y + 1);
-			settings->AddFace(f);
-		}
-
-	// Optimize the settings
-	settings->Optimize();
-
-	return settings;
-}
-
-JPH_NAMESPACE_END