Implemented better algorithm to split soft body constraints into parallel groups (#1042)

This makes the soft body simulation 10-20% faster and also enables multithreading LRA, bend and volume constraints.

Jolt/Physics/Body/BodyManager.cpp

@@ -1083,19 +1083,19 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 					mp->DrawVertexVelocities(inRenderer, com);
 
 				if (inDrawSettings.mDrawSoftBodyEdgeConstraints)
-					mp->DrawEdgeConstraints(inRenderer, com);
+					mp->DrawEdgeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor == BodyManager::ESoftBodyConstraintColor::ConstraintGroup);
 
 				if (inDrawSettings.mDrawSoftBodyBendConstraints)
-					mp->DrawBendConstraints(inRenderer, com);
+					mp->DrawBendConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor == BodyManager::ESoftBodyConstraintColor::ConstraintGroup);
 
 				if (inDrawSettings.mDrawSoftBodyVolumeConstraints)
-					mp->DrawVolumeConstraints(inRenderer, com);
+					mp->DrawVolumeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor == BodyManager::ESoftBodyConstraintColor::ConstraintGroup);
 
 				if (inDrawSettings.mDrawSoftBodySkinConstraints)
 					mp->DrawSkinConstraints(inRenderer, com);
 
 				if (inDrawSettings.mDrawSoftBodyLRAConstraints)
-					mp->DrawLRAConstraints(inRenderer, com);
+					mp->DrawLRAConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor == BodyManager::ESoftBodyConstraintColor::ConstraintGroup);
 
 				if (inDrawSettings.mDrawSoftBodyPredictedBounds)
 					mp->DrawPredictedBounds(inRenderer, com);

Jolt/Physics/Body/BodyManager.h

@@ -203,6 +203,7 @@ public:
 	/// Save the state of a single body for replay
 	void							RestoreBodyState(Body &inBody, StateRecorder &inStream);
 
+#ifdef JPH_DEBUG_RENDERER
 	enum class EShapeColor
 	{
 		InstanceColor,				///< Random color per instance
@@ -213,7 +214,12 @@ public:
 		MaterialColor,				///< Color as defined by the PhysicsMaterial of the shape
 	};
 
-#ifdef JPH_DEBUG_RENDERER
+	enum class ESoftBodyConstraintColor
+	{
+		ConstraintType,				/// Draw different types of constraints in different colors
+		ConstraintGroup,			/// Draw constraints in the same group in the same color, non-parallel group will be red
+	};
+
 	/// Draw settings
 	struct DrawSettings
 	{
@@ -237,6 +243,7 @@ public:
 		bool						mDrawSoftBodySkinConstraints = false;			///< Draw the skin constraints of soft bodies
 		bool						mDrawSoftBodyLRAConstraints = false;			///< Draw the LRA constraints of soft bodies
 		bool						mDrawSoftBodyPredictedBounds = false;			///< Draw the predicted bounds of soft bodies
+		ESoftBodyConstraintColor	mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints
 	};
 
 	/// Draw the state of the bodies (debugging purposes)

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -248,18 +248,18 @@ void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &i
 		}
 }
 
-void SoftBodyMotionProperties::ApplyBendConstraints(const SoftBodyUpdateContext &inContext)
+void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
 {
 	JPH_PROFILE_FUNCTION();
 
 	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);
 
-	for (const DihedralBend &b : mSettings->mDihedralBendConstraints)
+	for (const DihedralBend *b = mSettings->mDihedralBendConstraints.data() + inStartIndex; b < mSettings->mDihedralBendConstraints.data() + inEndIndex; ++b)
 	{
-		Vertex &v0 = mVertices[b.mVertex[0]];
-		Vertex &v1 = mVertices[b.mVertex[1]];
-		Vertex &v2 = mVertices[b.mVertex[2]];
-		Vertex &v3 = mVertices[b.mVertex[3]];
+		Vertex &v0 = mVertices[b->mVertex[0]];
+		Vertex &v1 = mVertices[b->mVertex[1]];
+		Vertex &v2 = mVertices[b->mVertex[2]];
+		Vertex &v3 = mVertices[b->mVertex[3]];
 
 		// Get positions
 		Vec3 x0 = v0.mPosition;
@@ -298,7 +298,7 @@ void SoftBodyMotionProperties::ApplyBendConstraints(const SoftBodyUpdateContext
 		// As per "Strain Based Dynamics" Appendix A we need to negate the gradients when (n1 x n2) . e > 0, instead we make sure that the sign of the constraint equation is correct
 		float sign = Sign(n2.Cross(n1).Dot(e));
 		float d = n1.Dot(n2) / sqrt(n1_len_sq_n2_len_sq);
-		float c = sign * ACos(d) - b.mInitialAngle;
+		float c = sign * ACos(d) - b->mInitialAngle;
 
 		// Ensure the range is -PI to PI
 		if (c > JPH_PI)
@@ -326,7 +326,7 @@ void SoftBodyMotionProperties::ApplyBendConstraints(const SoftBodyUpdateContext
 		float w3 = v3.mInvMass;
 
 		// Calculate -lambda
-		float denom = w0 * d0c.LengthSq() + w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + b.mCompliance * inv_dt_sq;
+		float denom = w0 * d0c.LengthSq() + w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + b->mCompliance * inv_dt_sq;
 		if (denom < 1.0e-12f)
 			continue;
 		float minus_lambda = c / denom;
@@ -339,19 +339,19 @@ void SoftBodyMotionProperties::ApplyBendConstraints(const SoftBodyUpdateContext
 	}
 }
 
-void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext)
+void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
 {
 	JPH_PROFILE_FUNCTION();
 
 	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);
 
 	// Satisfy volume constraints
-	for (const Volume &v : mSettings->mVolumeConstraints)
+	for (const Volume *v = mSettings->mVolumeConstraints.data() + inStartIndex; v < mSettings->mVolumeConstraints.data() + inEndIndex; ++v)
 	{
-		Vertex &v1 = mVertices[v.mVertex[0]];
-		Vertex &v2 = mVertices[v.mVertex[1]];
-		Vertex &v3 = mVertices[v.mVertex[2]];
-		Vertex &v4 = mVertices[v.mVertex[3]];
+		Vertex &v1 = mVertices[v->mVertex[0]];
+		Vertex &v2 = mVertices[v->mVertex[1]];
+		Vertex &v3 = mVertices[v->mVertex[2]];
+		Vertex &v4 = mVertices[v->mVertex[3]];
 
 		Vec3 x1 = v1.mPosition;
 		Vec3 x2 = v2.mPosition;
@@ -362,7 +362,7 @@ void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContex
 		Vec3 x1x2 = x2 - x1;
 		Vec3 x1x3 = x3 - x1;
 		Vec3 x1x4 = x4 - x1;
-		float c = abs(x1x2.Cross(x1x3).Dot(x1x4)) - v.mSixRestVolume;
+		float c = abs(x1x2.Cross(x1x3).Dot(x1x4)) - v->mSixRestVolume;
 
 		// Calculate gradient of constraint equation
 		Vec3 d1c = (x4 - x2).Cross(x3 - x2);
@@ -377,7 +377,7 @@ void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContex
 		float w4 = v4.mInvMass;
 
 		// Calculate -lambda
-		float denom = w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v.mCompliance * inv_dt_sq;
+		float denom = w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v->mCompliance * inv_dt_sq;
 		if (denom < 1.0e-12f)
 			continue;
 		float minus_lambda = c / denom;
@@ -452,12 +452,10 @@ void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext
 	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);
 
 	// Satisfy edge constraints
-	const Array<Edge> &edge_constraints = mSettings->mEdgeConstraints;
-	for (uint i = inStartIndex; i < inEndIndex; ++i)
+	for (const Edge *e = mSettings->mEdgeConstraints.data() + inStartIndex; e < mSettings->mEdgeConstraints.data() + inEndIndex; ++e)
 	{
-		const Edge &e = edge_constraints[i];
-		Vertex &v0 = mVertices[e.mVertex[0]];
-		Vertex &v1 = mVertices[e.mVertex[1]];
+		Vertex &v0 = mVertices[e->mVertex[0]];
+		Vertex &v1 = mVertices[e->mVertex[1]];
 
 		// Get positions
 		Vec3 x0 = v0.mPosition;
@@ -468,33 +466,33 @@ void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext
 		float length = delta.Length();
 
 		// Apply correction
-		float denom = length * (v0.mInvMass + v1.mInvMass + e.mCompliance * inv_dt_sq);
+		float denom = length * (v0.mInvMass + v1.mInvMass + e->mCompliance * inv_dt_sq);
 		if (denom < 1.0e-12f)
 			continue;
-		Vec3 correction = delta * (length - e.mRestLength) / denom;
+		Vec3 correction = delta * (length - e->mRestLength) / denom;
 		v0.mPosition = x0 + v0.mInvMass * correction;
 		v1.mPosition = x1 - v1.mInvMass * correction;
 	}
 }
 
-void SoftBodyMotionProperties::ApplyLRAConstraints()
+void SoftBodyMotionProperties::ApplyLRAConstraints(uint inStartIndex, uint inEndIndex)
 {
 	JPH_PROFILE_FUNCTION();
 
 	// Satisfy LRA constraints
 	Vertex *vertices = mVertices.data();
-	for (const LRA &lra : mSettings->mLRAConstraints)
+	for (const LRA *lra = mSettings->mLRAConstraints.data() + inStartIndex; lra < mSettings->mLRAConstraints.data() + inEndIndex; ++lra)
 	{
-		JPH_ASSERT(lra.mVertex[0] < mVertices.size());
-		JPH_ASSERT(lra.mVertex[1] < mVertices.size());
-		const Vertex &vertex0 = vertices[lra.mVertex[0]];
-		Vertex &vertex1 = vertices[lra.mVertex[1]];
+		JPH_ASSERT(lra->mVertex[0] < mVertices.size());
+		JPH_ASSERT(lra->mVertex[1] < mVertices.size());
+		const Vertex &vertex0 = vertices[lra->mVertex[0]];
+		Vertex &vertex1 = vertices[lra->mVertex[1]];
 
 		Vec3 x0 = vertex0.mPosition;
 		Vec3 delta = vertex1.mPosition - x0;
 		float delta_len_sq = delta.LengthSq();
-		if (delta_len_sq > Square(lra.mMaxDistance))
-			vertex1.mPosition = x0 + delta * lra.mMaxDistance / sqrt(delta_len_sq);
+		if (delta_len_sq > Square(lra->mMaxDistance))
+			vertex1.mPosition = x0 + delta * lra->mMaxDistance / sqrt(delta_len_sq);
 	}
 }
 
@@ -737,10 +735,6 @@ void SoftBodyMotionProperties::StartNextIteration(const SoftBodyUpdateContext &i
 	ApplyPressure(ioContext);
 
 	IntegratePositions(ioContext);
-
-	ApplyBendConstraints(ioContext);
-
-	ApplyVolumeConstraints(ioContext);
 }
 
 SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext)
@@ -763,7 +757,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCol
 				JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
 				JPH_ASSERT(iteration == 0);
 				StartNextIteration(ioContext);
-				ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyEdgeConstraints, memory_order_release);
+				ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release);
 			}
 			return EStatus::DidWork;
 		}
@@ -771,76 +765,83 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCol
 	return EStatus::NoWork;
 }
 
-SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyEdgeConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
+void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex)
 {
-	// Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it)
-	uint num_groups = (uint)mSettings->mEdgeGroupEndIndices.size();
+	// Determine start and end
+	SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0 };
+	const SoftBodySharedSettings::UpdateGroup &prev = inGroupIndex > 0? mSettings->mUpdateGroups[inGroupIndex - 1] : start;
+	const SoftBodySharedSettings::UpdateGroup &current = mSettings->mUpdateGroups[inGroupIndex];
+
+	// Process volume constraints
+	ApplyVolumeConstraints(ioContext, prev.mVolumeEndIndex, current.mVolumeEndIndex);
+
+	// Process bend constraints
+	ApplyDihedralBendConstraints(ioContext, prev.mDihedralBendEndIndex, current.mDihedralBendEndIndex);
+
+	// Process edges
+	ApplyEdgeConstraints(ioContext, prev.mEdgeEndIndex, current.mEdgeEndIndex);
+
+	// Process LRA constraints
+	ApplyLRAConstraints(prev.mLRAEndIndex, current.mLRAEndIndex);
+}
+
+SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
+{
+	uint num_groups = (uint)mSettings->mUpdateGroups.size();
 	JPH_ASSERT(num_groups > 0, "SoftBodySharedSettings::Optimize should have been called!");
-	uint32 edge_group, edge_start_idx;
-	SoftBodyUpdateContext::sGetEdgeGroupAndStartIdx(ioContext.mNextEdgeConstraint.load(memory_order_relaxed), edge_group, edge_start_idx);
-	if (edge_group < num_groups)
+	--num_groups; // Last group is the non-parallel group, we don't want to execute it in parallel
+		
+	// Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it)
+	uint next_group = ioContext.mNextConstraintGroup.load(memory_order_relaxed);
+	if (next_group < num_groups || (num_groups == 0 && next_group == 0))
 	{
-		uint edge_group_size = mSettings->GetEdgeGroupSize(edge_group);
-		if (edge_start_idx < edge_group_size || edge_group_size == 0)
+		// Fetch the next group process
+		next_group = ioContext.mNextConstraintGroup.fetch_add(1, memory_order_acquire);
+		if (next_group < num_groups || (num_groups == 0 && next_group == 0))
 		{
-			// Fetch the next batch of edges to process
-			uint64 next_edge_batch = ioContext.mNextEdgeConstraint.fetch_add(SoftBodyUpdateContext::cEdgeConstraintBatch, memory_order_acquire);
-			SoftBodyUpdateContext::sGetEdgeGroupAndStartIdx(next_edge_batch, edge_group, edge_start_idx);
-			if (edge_group < num_groups)
+			uint num_groups_processed = 0;
+			if (num_groups > 0)
 			{
-				bool non_parallel_group = edge_group == num_groups - 1; // Last group is the non-parallel group and goes as a whole
-				edge_group_size = mSettings->GetEdgeGroupSize(edge_group);
-				if (non_parallel_group? edge_start_idx == 0 : edge_start_idx < edge_group_size)
-				{
-					// Process edges
-					uint num_edges_to_process = non_parallel_group? edge_group_size : min(SoftBodyUpdateContext::cEdgeConstraintBatch, edge_group_size - edge_start_idx);
-					if (edge_group > 0)
-						edge_start_idx += mSettings->mEdgeGroupEndIndices[edge_group - 1];
-					ApplyEdgeConstraints(ioContext, edge_start_idx, edge_start_idx + num_edges_to_process);
-
-					// Test if we're at the end of this group
-					uint edge_constraints_processed = ioContext.mNumEdgeConstraintsProcessed.fetch_add(num_edges_to_process, memory_order_relaxed) + num_edges_to_process;
-					if (edge_constraints_processed >= edge_group_size)
-					{
-						// Non parallel group is the last group (which is also the only group that can be empty)
-						if (non_parallel_group || mSettings->GetEdgeGroupSize(edge_group + 1) == 0)
-						{
-							// Finish the iteration
-							ApplyLRAConstraints();
+				// Process this group
+				ProcessGroup(ioContext, next_group);
 
-							ApplyCollisionConstraintsAndUpdateVelocities(ioContext);
+				// Increment total number of groups processed
+				num_groups_processed = ioContext.mNumConstraintGroupsProcessed.fetch_add(1, memory_order_relaxed) + 1;
+			}
 
-							ApplySkinConstraints(ioContext);
+			if (num_groups_processed >= num_groups)
+			{
+				// Finish the iteration
+				JPH_PROFILE("FinishIteration");
 
-							uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
-							if (iteration < mNumIterations)
-							{
-								// Start a new iteration
-								StartNextIteration(ioContext);
+				// Process non-parallel group
+				ProcessGroup(ioContext, num_groups);
 
-								// Reset next edge to process
-								ioContext.mNumEdgeConstraintsProcessed.store(0, memory_order_relaxed);
-								ioContext.mNextEdgeConstraint.store(0, memory_order_release);
-							}
-							else
-							{
-								// On final iteration we update the state
-								UpdateSoftBodyState(ioContext, inPhysicsSettings);
+				ApplyCollisionConstraintsAndUpdateVelocities(ioContext);
 
-								ioContext.mState.store(SoftBodyUpdateContext::EState::Done, memory_order_release);
-								return EStatus::Done;
-							}
-						}
-						else
-						{
-							// Next group
-							ioContext.mNumEdgeConstraintsProcessed.store(0, memory_order_relaxed);
-							ioContext.mNextEdgeConstraint.store(SoftBodyUpdateContext::sGetEdgeGroupStart(edge_group + 1), memory_order_release);
-						}
-					}
-					return EStatus::DidWork;
+				ApplySkinConstraints(ioContext);
+
+				uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
+				if (iteration < mNumIterations)
+				{
+					// Start a new iteration
+					StartNextIteration(ioContext);
+
+					// Reset group logic
+					ioContext.mNumConstraintGroupsProcessed.store(0, memory_order_relaxed);
+					ioContext.mNextConstraintGroup.store(0, memory_order_release);
+				}
+				else
+				{
+					// On final iteration we update the state
+					UpdateSoftBodyState(ioContext, inPhysicsSettings);
+
+					ioContext.mState.store(SoftBodyUpdateContext::EState::Done, memory_order_release);
+					return EStatus::Done;
 				}
 			}
+
+			return EStatus::DidWork;
 		}
 	}
 	return EStatus::NoWork;
@@ -853,8 +854,8 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftB
 	case SoftBodyUpdateContext::EState::DetermineCollisionPlanes:
 		return ParallelDetermineCollisionPlanes(ioContext);
 
-	case SoftBodyUpdateContext::EState::ApplyEdgeConstraints:
-		return ParallelApplyEdgeConstraints(ioContext, inPhysicsSettings);
+	case SoftBodyUpdateContext::EState::ApplyConstraints:
+		return ParallelApplyConstraints(ioContext, inPhysicsSettings);
 
 	case SoftBodyUpdateContext::EState::Done:
 		return EStatus::Done;
@@ -989,43 +990,86 @@ void SoftBodyMotionProperties::DrawVertexVelocities(DebugRenderer *inRenderer, R
 		inRenderer->DrawArrow(inCenterOfMassTransform * v.mPosition, inCenterOfMassTransform * (v.mPosition + v.mVelocity), Color::sYellow, 0.01f);
 }
 
-void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, bool inDrawConstraintGroupColor) const
 {
-	for (const Edge &e : mSettings->mEdgeConstraints)
-		inRenderer->DrawLine(inCenterOfMassTransform * mVertices[e.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[e.mVertex[1]].mPosition, Color::sWhite);
+	uint idx = 0;
+	for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i)
+	{
+		uint end = mSettings->mUpdateGroups[i].mEdgeEndIndex;
+
+		Color color;
+		if (inDrawConstraintGroupColor)
+			color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group
+		else
+			color = Color::sWhite;
+
+		for (; idx < end; ++idx)
+		{
+			const Edge &e = mSettings->mEdgeConstraints[idx];
+			inRenderer->DrawLine(inCenterOfMassTransform * mVertices[e.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[e.mVertex[1]].mPosition, color);
+		}
+	}
 }
 
-void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, bool inDrawConstraintGroupColor) const
 {
-	for (const DihedralBend &b : mSettings->mDihedralBendConstraints)
+	uint idx = 0;
+	for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i)
 	{
-		RVec3 x0 = inCenterOfMassTransform * mVertices[b.mVertex[0]].mPosition;
-		RVec3 x1 = inCenterOfMassTransform * mVertices[b.mVertex[1]].mPosition;
-		RVec3 x2 = inCenterOfMassTransform * mVertices[b.mVertex[2]].mPosition;
-		RVec3 x3 = inCenterOfMassTransform * mVertices[b.mVertex[3]].mPosition;
-		RVec3 c_edge = 0.5_r * (x0 + x1);
-		RVec3 c0 = (x0 + x1 + x2) / 3.0_r;
-		RVec3 c1 = (x0 + x1 + x3) / 3.0_r;
-
-		inRenderer->DrawArrow(0.9_r * x0 + 0.1_r * x1, 0.1_r * x0 + 0.9_r * x1, Color::sDarkGreen, 0.01f);
-		inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c0, Color::sGreen);
-		inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c1, Color::sGreen);
+		uint end = mSettings->mUpdateGroups[i].mDihedralBendEndIndex;
+
+		Color color;
+		if (inDrawConstraintGroupColor)
+			color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group
+		else
+			color = Color::sGreen;
+
+		for (; idx < end; ++idx)
+		{
+			const DihedralBend &b = mSettings->mDihedralBendConstraints[idx];
+
+			RVec3 x0 = inCenterOfMassTransform * mVertices[b.mVertex[0]].mPosition;
+			RVec3 x1 = inCenterOfMassTransform * mVertices[b.mVertex[1]].mPosition;
+			RVec3 x2 = inCenterOfMassTransform * mVertices[b.mVertex[2]].mPosition;
+			RVec3 x3 = inCenterOfMassTransform * mVertices[b.mVertex[3]].mPosition;
+			RVec3 c_edge = 0.5_r * (x0 + x1);
+			RVec3 c0 = (x0 + x1 + x2) / 3.0_r;
+			RVec3 c1 = (x0 + x1 + x3) / 3.0_r;
+
+			inRenderer->DrawArrow(0.9_r * x0 + 0.1_r * x1, 0.1_r * x0 + 0.9_r * x1, color, 0.01f);
+			inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c0, color);
+			inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c1, color);
+		}
 	}
 }
 
-void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, bool inDrawConstraintGroupColor) const
 {
-	for (const Volume &v : mSettings->mVolumeConstraints)
+	uint idx = 0;
+	for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i)
 	{
-		RVec3 x1 = inCenterOfMassTransform * mVertices[v.mVertex[0]].mPosition;
-		RVec3 x2 = inCenterOfMassTransform * mVertices[v.mVertex[1]].mPosition;
-		RVec3 x3 = inCenterOfMassTransform * mVertices[v.mVertex[2]].mPosition;
-		RVec3 x4 = inCenterOfMassTransform * mVertices[v.mVertex[3]].mPosition;
-
-		inRenderer->DrawTriangle(x1, x3, x2, Color::sYellow, DebugRenderer::ECastShadow::On);
-		inRenderer->DrawTriangle(x2, x3, x4, Color::sYellow, DebugRenderer::ECastShadow::On);
-		inRenderer->DrawTriangle(x1, x4, x3, Color::sYellow, DebugRenderer::ECastShadow::On);
-		inRenderer->DrawTriangle(x1, x2, x4, Color::sYellow, DebugRenderer::ECastShadow::On);
+		uint end = mSettings->mUpdateGroups[i].mVolumeEndIndex;
+
+		Color color;
+		if (inDrawConstraintGroupColor)
+			color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group
+		else
+			color = Color::sYellow;
+
+		for (; idx < end; ++idx)
+		{
+			const Volume &v = mSettings->mVolumeConstraints[idx];
+
+			RVec3 x1 = inCenterOfMassTransform * mVertices[v.mVertex[0]].mPosition;
+			RVec3 x2 = inCenterOfMassTransform * mVertices[v.mVertex[1]].mPosition;
+			RVec3 x3 = inCenterOfMassTransform * mVertices[v.mVertex[2]].mPosition;
+			RVec3 x4 = inCenterOfMassTransform * mVertices[v.mVertex[3]].mPosition;
+
+			inRenderer->DrawTriangle(x1, x3, x2, color, DebugRenderer::ECastShadow::On);
+			inRenderer->DrawTriangle(x2, x3, x4, color, DebugRenderer::ECastShadow::On);
+			inRenderer->DrawTriangle(x1, x4, x3, color, DebugRenderer::ECastShadow::On);
+			inRenderer->DrawTriangle(x1, x2, x4, color, DebugRenderer::ECastShadow::On);
+		}
 	}
 }
 
@@ -1039,10 +1083,25 @@ void SoftBodyMotionProperties::DrawSkinConstraints(DebugRenderer *inRenderer, RM
 	}
 }
 
-void SoftBodyMotionProperties::DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+void SoftBodyMotionProperties::DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, bool inDrawConstraintGroupColor) const
 {
-	for (const LRA &l : mSettings->mLRAConstraints)
-		inRenderer->DrawLine(inCenterOfMassTransform * mVertices[l.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[l.mVertex[1]].mPosition, Color::sGrey);
+	uint idx = 0;
+	for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i)
+	{
+		uint end = mSettings->mUpdateGroups[i].mLRAEndIndex;
+
+		Color color;
+		if (inDrawConstraintGroupColor)
+			color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group
+		else
+			color = Color::sGrey;
+
+		for (; idx < end; ++idx)
+		{
+			const LRA &l = mSettings->mLRAConstraints[idx];
+			inRenderer->DrawLine(inCenterOfMassTransform * mVertices[l.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[l.mVertex[1]].mPosition, color);
+		}
+	}
 }
 
 void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const

Jolt/Physics/SoftBody/SoftBodyMotionProperties.h

@@ -98,11 +98,11 @@ public:
 	/// Draw the state of a soft body
 	void								DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
 	void								DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
-	void								DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
-	void								DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
-	void								DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+	void								DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, bool inDrawConstraintGroupColor) const;
+	void								DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, bool inDrawConstraintGroupColor) const;
+	void								DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, bool inDrawConstraintGroupColor) const;
 	void								DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
-	void								DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+	void								DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, bool inDrawConstraintGroupColor) const;
 	void								DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
 #endif // JPH_DEBUG_RENDERER
 
@@ -204,10 +204,10 @@ private:
 	void								IntegratePositions(const SoftBodyUpdateContext &inContext);
 
 	/// Enforce all bend constraints
-	void								ApplyBendConstraints(const SoftBodyUpdateContext &inContext);
+	void								ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
 
 	/// Enforce all volume constraints
-	void								ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext);
+	void								ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
 
 	/// Enforce all skin constraints
 	void								ApplySkinConstraints(const SoftBodyUpdateContext &inContext);
@@ -216,7 +216,7 @@ private:
 	void								ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
 
 	/// Enforce all LRA constraints
-	void								ApplyLRAConstraints();
+	void								ApplyLRAConstraints(uint inStartIndex, uint inEndIndex);
 
 	/// Enforce all collision constraints & update all velocities according the XPBD algorithm
 	void								ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext);
@@ -230,8 +230,11 @@ private:
 	/// Helper function for ParallelUpdate that works on batches of collision planes
 	EStatus								ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext);
 
-	/// Helper function for ParallelUpdate that works on batches of edges
-	EStatus								ParallelApplyEdgeConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings);
+	/// Helper function for ParallelUpdate that works on batches of constraints
+	EStatus								ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings);
+
+	/// Helper function to update a single group of constraints
+	void								ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex);
 
 	/// Returns 6 times the volume of the soft body
 	float								GetVolumeTimesSix() const;

Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -87,7 +87,6 @@ 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, mEdgeGroupEndIndices)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mDihedralBendConstraints)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints)
@@ -464,39 +463,245 @@ void SoftBodySharedSettings::CalculateSkinnedConstraintNormals()
 
 void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 {
-	const uint cMaxNumGroups = 32;
-	const uint cNonParallelGroupIdx = cMaxNumGroups - 1;
-	const uint cMinimumSize = 2 * SoftBodyUpdateContext::cEdgeConstraintBatch; // There should be at least 2 batches, otherwise there's no point in parallelizing
-
-	// Assign edges to non-overlapping groups
-	Array<uint32> masks;
-	masks.resize(mVertices.size(), 0);
-	Array<uint> edge_groups[cMaxNumGroups];
-	for (const Edge &e : mEdgeConstraints)
+	// 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 });
+
+				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 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)
 	{
-		uint32 &mask1 = masks[e.mVertex[0]];
-		uint32 &mask2 = masks[e.mVertex[1]];
-		uint group = min(CountTrailingZeros((~mask1) & (~mask2)), cNonParallelGroupIdx);
-		uint32 mask = uint32(1U << group);
-		mask1 |= mask;
-		mask2 |= mask;
-		edge_groups[group].push_back(uint(&e - mEdgeConstraints.data()));
+		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]);
 	}
 
-	// Merge groups that are too small into the non-parallel group
-	for (uint i = 0; i < cNonParallelGroupIdx; ++i)
-		if (edge_groups[i].size() < cMinimumSize)
+	// 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));
+
+		// 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 (;;)
 		{
-			edge_groups[cNonParallelGroupIdx].insert(edge_groups[cNonParallelGroupIdx].end(), edge_groups[i].begin(), edge_groups[i].end());
-			edge_groups[i].clear();
+			// 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)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size();
 		}
 
+		Array<uint>		mEdgeConstraints;
+		Array<uint>		mLRAConstraints;
+		Array<uint>		mDihedralBendConstraints;
+		Array<uint>		mVolumeConstraints;
+	};
+	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 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()));
+	}
+
+	// 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 the edge constraints
-	for (Array<uint> &group : edge_groups)
-		QuickSort(group.begin(), group.end(), [this](uint inLHS, uint inRHS)
+	// 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];
@@ -509,33 +714,37 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 
 				// 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
-				return e1.GetMinVertexIndex() < e2.GetMinVertexIndex();
+				uint32 m1 = e1.GetMinVertexIndex();
+				uint32 m2 = e2.GetMinVertexIndex();
+				if (m1 != m2)
+					return m1 < m2;
+
+				return inLHS < inRHS;
 			});
 
-	// Assign the edges to groups and reorder them
-	Array<Edge> temp_edges;
-	temp_edges.swap(mEdgeConstraints);
-	mEdgeConstraints.reserve(temp_edges.size());
-	for (const Array<uint> &group : edge_groups)
-		if (!group.empty())
-		{
-			for (uint idx : group)
+		// Sort the LRA constraints
+		QuickSort(group.mLRAConstraints.begin(), group.mLRAConstraints.end(), [this](uint inLHS, uint inRHS)
 			{
-				mEdgeConstraints.push_back(temp_edges[idx]);
-				outResults.mEdgeRemap.push_back(idx);
-			}
-			mEdgeGroupEndIndices.push_back((uint)mEdgeConstraints.size());
-		}
+				const LRA &l1 = mLRAConstraints[inLHS];
+				const LRA &l2 = mLRAConstraints[inRHS];
 
-	// If there is no non-parallel group then add an empty group at the end
-	if (edge_groups[cNonParallelGroupIdx].empty())
-		mEdgeGroupEndIndices.push_back((uint)mEdgeConstraints.size());
+				// First sort so that the edge with the smallest distance to a kinematic vertex comes first
+				float d1 = min(mClosestKinematic[l1.mVertex[0]].mDistance, mClosestKinematic[l1.mVertex[1]].mDistance);
+				float d2 = min(mClosestKinematic[l2.mVertex[0]].mDistance, mClosestKinematic[l2.mVertex[1]].mDistance);
+				if (d1 != d2)
+					return d1 < d2;
+
+				// 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 bend constraints
-	outResults.mDihedralBendRemap.resize(mDihedralBendConstraints.size());
-	for (int i = 0; i < (int)mDihedralBendConstraints.size(); ++i)
-		outResults.mDihedralBendRemap[i] = i;
-	QuickSort(outResults.mDihedralBendRemap.begin(), outResults.mDihedralBendRemap.end(), [this](uint inLHS, uint inRHS)
+		// 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];
@@ -551,15 +760,95 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 				return d1 < d2;
 
 			// Order constraints so that the ones with the smallest index go first
-			return b1.GetMinVertexIndex() < b2.GetMinVertexIndex();
+			uint32 m1 = b1.GetMinVertexIndex();
+			uint32 m2 = b2.GetMinVertexIndex();
+			if (m1 != m2)
+				return m1 < m2;
+
+			return inLHS < inRHS;
 		});
 
-	// Reorder the bend constraints
-	Array<DihedralBend> temp_bends;
-	temp_bends.swap(mDihedralBendConstraints);
-	mDihedralBendConstraints.reserve(temp_bends.size());
-	for (uint idx : outResults.mDihedralBendRemap)
-		mDihedralBendConstraints.push_back(temp_bends[idx]);
+		// 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;
+		});
+	}
+
+	// Temporary store constraints as we reorder them
+	Array<Edge> temp_edges;
+	temp_edges.swap(mEdgeConstraints);
+	mEdgeConstraints.reserve(temp_edges.size());
+	outResults.mEdgeRemap.reserve(temp_edges.size());
+
+	Array<LRA> temp_lra;
+	temp_lra.swap(mLRAConstraints);
+	mLRAConstraints.reserve(temp_lra.size());
+	outResults.mLRARemap.reserve(temp_lra.size());
+
+	Array<DihedralBend> temp_dihedral_bend;
+	temp_dihedral_bend.swap(mDihedralBendConstraints);
+	mDihedralBendConstraints.reserve(temp_dihedral_bend.size());
+	outResults.mDihedralBendRemap.reserve(temp_dihedral_bend.size());
+
+	Array<Volume> temp_volume;
+	temp_volume.swap(mVolumeConstraints);
+	mVolumeConstraints.reserve(temp_volume.size());
+	outResults.mVolumeRemap.reserve(temp_volume.size());
+
+	// Finalize update groups
+	for (const Group &group : groups)
+	{
+		// Reorder edge constraints for this group
+		for (uint idx : group.mEdgeConstraints)
+		{
+			mEdgeConstraints.push_back(temp_edges[idx]);
+			outResults.mEdgeRemap.push_back(idx);
+		}
+
+		// Reorder LRA constraints for this group
+		for (uint idx : group.mLRAConstraints)
+		{
+			mLRAConstraints.push_back(temp_lra[idx]);
+			outResults.mLRARemap.push_back(idx);
+		}
+
+		// Reorder dihedral bend constraints for this group
+		for (uint idx : group.mDihedralBendConstraints)
+		{
+			mDihedralBendConstraints.push_back(temp_dihedral_bend[idx]);
+			outResults.mDihedralBendRemap.push_back(idx);
+		}
+
+		// Reorder volume constraints for this group
+		for (uint idx : group.mVolumeConstraints)
+		{
+			mVolumeConstraints.push_back(temp_volume[idx]);
+			outResults.mVolumeRemap.push_back(idx);
+		}
+
+		// Store end indices
+		mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size() });
+	}
 
 	// Free closest kinematic buffer
 	mClosestKinematic.clear();
@@ -572,7 +861,6 @@ Ref<SoftBodySharedSettings> SoftBodySharedSettings::Clone() const
 	clone->mVertices = mVertices;
 	clone->mFaces = mFaces;
 	clone->mEdgeConstraints = mEdgeConstraints;
-	clone->mEdgeGroupEndIndices = mEdgeGroupEndIndices;
 	clone->mDihedralBendConstraints = mDihedralBendConstraints;
 	clone->mVolumeConstraints = mVolumeConstraints;
 	clone->mSkinnedConstraints = mSkinnedConstraints;
@@ -581,6 +869,7 @@ Ref<SoftBodySharedSettings> SoftBodySharedSettings::Clone() const
 	clone->mLRAConstraints = mLRAConstraints;
 	clone->mMaterials = mMaterials;
 	clone->mVertexRadius = mVertexRadius;
+	clone->mUpdateGroups = mUpdateGroups;
 	return clone;
 }
 
@@ -589,13 +878,13 @@ void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mVertices);
 	inStream.Write(mFaces);
 	inStream.Write(mEdgeConstraints);
-	inStream.Write(mEdgeGroupEndIndices);
 	inStream.Write(mDihedralBendConstraints);
 	inStream.Write(mVolumeConstraints);
 	inStream.Write(mSkinnedConstraints);
 	inStream.Write(mSkinnedConstraintNormals);
 	inStream.Write(mLRAConstraints);
 	inStream.Write(mVertexRadius);
+	inStream.Write(mUpdateGroups);
 
 	// Can't write mInvBindMatrices directly because the class contains padding
 	inStream.Write(mInvBindMatrices, [](const InvBind &inElement, StreamOut &inS) {
@@ -609,13 +898,13 @@ void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mVertices);
 	inStream.Read(mFaces);
 	inStream.Read(mEdgeConstraints);
-	inStream.Read(mEdgeGroupEndIndices);
 	inStream.Read(mDihedralBendConstraints);
 	inStream.Read(mVolumeConstraints);
 	inStream.Read(mSkinnedConstraints);
 	inStream.Read(mSkinnedConstraintNormals);
 	inStream.Read(mLRAConstraints);
 	inStream.Read(mVertexRadius);
+	inStream.Read(mUpdateGroups);
 
 	inStream.Read(mInvBindMatrices, [](StreamIn &inS, InvBind &outElement) {
 		inS.Read(outElement.mJointIndex);

Jolt/Physics/SoftBody/SoftBodySharedSettings.h

@@ -77,7 +77,9 @@ public:
 	{
 	public:
 		Array<uint>		mEdgeRemap;									///< Maps old edge index to new edge index
+		Array<uint>		mLRARemap;									///< Maps old LRA index to new LRA index
 		Array<uint>		mDihedralBendRemap;							///< Maps old dihedral bend index to new dihedral bend index
+		Array<uint>		mVolumeRemap;								///< Maps old volume constraint index to new volume constraint index
 	};
 
 	/// Optimize the soft body settings for simulation. This will reorder constraints so they can be executed in parallel.
@@ -301,6 +303,9 @@ public:
 private:
 	friend class SoftBodyMotionProperties;
 
+	/// Calculate the closest kinematic vertex array
+	void				CalculateClosestKinematic();
+
 	/// Tracks the closest kinematic vertex
 	struct ClosestKinematic
 	{
@@ -308,14 +313,17 @@ private:
 		float			mDistance = FLT_MAX;						///< Distance to the closest kinematic vertex
 	};
 
-	/// Calculate the closest kinematic vertex array
-	void				CalculateClosestKinematic();
-
-	/// Get the size of an edge group (edge groups can run in parallel)
-	uint				GetEdgeGroupSize(uint inGroupIdx) const		{ return inGroupIdx == 0? mEdgeGroupEndIndices[0] : mEdgeGroupEndIndices[inGroupIdx] - mEdgeGroupEndIndices[inGroupIdx - 1]; }
+	/// Tracks the end indices of the various constraint groups
+	struct UpdateGroup
+	{
+		uint			mEdgeEndIndex;								///< The end index of the edge constraints in this group
+		uint			mLRAEndIndex;								///< The end index of the LRA constraints in this group
+		uint			mDihedralBendEndIndex;						///< The end index of the dihedral bend constraints in this group
+		uint			mVolumeEndIndex;							///< The end index of the volume constraints in this group
+	};
 
 	Array<ClosestKinematic> mClosestKinematic;						///< The closest kinematic vertex to each vertex in mVertices
-	Array<uint>			mEdgeGroupEndIndices;						///< The start index of each group of edges that can be solved in parallel, calculated by Optimize()
+	Array<UpdateGroup>	mUpdateGroups;								///< The end indices for each group of constraints that can be updated in parallel
 	Array<uint32>		mSkinnedConstraintNormals;					///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals()
 };
 

Jolt/Physics/SoftBody/SoftBodyUpdateContext.h

@@ -18,7 +18,7 @@ class SoftBodyUpdateContext : public NonCopyable
 {
 public:
 	static constexpr uint				cVertexCollisionBatch = 64;					///< Number of vertices to process in a batch in DetermineCollisionPlanes
-	static constexpr uint				cEdgeConstraintBatch = 256;					///< Number of edge constraints to process in a batch in ApplyEdgeConstraints
+	static constexpr uint				cVertexConstraintBatch = 256;				///< Number of vertices to group for processing batches of constraints in ApplyEdgeConstraints
 
 	// Input
 	Body *								mBody;										///< Body that is being updated
@@ -34,30 +34,17 @@ public:
 	enum class EState
 	{
 		DetermineCollisionPlanes,													///< Determine collision planes for vertices in parallel
-		ApplyEdgeConstraints,														///< Apply edge constraints in parallel
+		ApplyConstraints,															///< Apply constraints in parallel
 		Done																		///< Update is finished
 	};
 
-	/// Construct the edge constraint iterator starting at a new group
-	static inline uint64				sGetEdgeGroupStart(uint32 inGroup)
-	{
-		return uint64(inGroup) << 32;
-	}
-
-	/// Get the group and start index from the edge constraint iterator
-	static inline void					sGetEdgeGroupAndStartIdx(uint64 inNextEdgeConstraint, uint32 &outGroup, uint32 &outStartIdx)
-	{
-		outGroup = uint32(inNextEdgeConstraint >> 32);
-		outStartIdx = uint32(inNextEdgeConstraint);
-	}
-
 	// State of the update
 	atomic<EState>						mState { EState::DetermineCollisionPlanes };///< Current state of the update
 	atomic<uint>						mNextCollisionVertex { 0 };					///< Next vertex to process for DetermineCollisionPlanes
 	atomic<uint>						mNumCollisionVerticesProcessed { 0 };		///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can start simulating
 	atomic<uint>						mNextIteration { 0 };						///< Next simulation iteration to process
-	atomic<uint64>						mNextEdgeConstraint { 0 };					///< Next edge constraint group and start index to process
-	atomic<uint>						mNumEdgeConstraintsProcessed { 0 };			///< Number of edge constraints processed by ApplyEdgeConstraints, used to determine if we can go to the next group / iteration
+	atomic<uint>						mNextConstraintGroup { 0 };					///< Next constraint group to process
+	atomic<uint>						mNumConstraintGroupsProcessed { 0 };		///< Number of groups processed, used to determine if we can go to the next iteration
 
 	// Output
 	Vec3								mDeltaPosition;								///< Delta position of the body in the current time step, should be applied after the update

Samples/SamplesApp.cpp

@@ -531,6 +531,7 @@ SamplesApp::SamplesApp()
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Soft Body Skin Constraints", mBodyDrawSettings.mDrawSoftBodySkinConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodySkinConstraints = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Soft Body LRA Constraints", mBodyDrawSettings.mDrawSoftBodyLRAConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyLRAConstraints = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Soft Body Predicted Bounds", mBodyDrawSettings.mDrawSoftBodyPredictedBounds, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyPredictedBounds = inState == UICheckBox::STATE_CHECKED; });
+			mDebugUI->CreateComboBox(drawing_options, "Draw Soft Body Constraint Color", { "Constraint Type", "Constraint Group" }, (int)mBodyDrawSettings.mDrawSoftBodyConstraintColor, [this](int inItem) { mBodyDrawSettings.mDrawSoftBodyConstraintColor = (BodyManager::ESoftBodyConstraintColor)inItem; });
 			mDebugUI->ShowMenu(drawing_options);
 		});
 	#endif // JPH_DEBUG_RENDERER