Soft body skinning constraints (#947)

This can be used to limit the movement of soft body vertices based on a skinned mesh. You can specify a 'backstop' which is the max distance behind the plane formed by the skinned vertex and the averaged normal based on adjacent faces and a 'max distance' which stops the vertex when it moves more than this distance away from the vertex. This is mainly suitable for simulating clothing where you don't want to use highly detailed collision volumes to limit the movement of the soft body.

Jolt/Physics/Body/BodyManager.cpp

@@ -1085,6 +1085,9 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 				if (inDrawSettings.mDrawSoftBodyVolumeConstraints)
 					mp->DrawVolumeConstraints(inRenderer, com);
 
+				if (inDrawSettings.mDrawSoftBodySkinConstraints)
+					mp->DrawSkinConstraints(inRenderer);
+
 				if (inDrawSettings.mDrawSoftBodyPredictedBounds)
 					mp->DrawPredictedBounds(inRenderer, com);
 			}

Jolt/Physics/Body/BodyManager.h

@@ -232,6 +232,7 @@ class JPH_EXPORT BodyManager : public NonCopyable
 		bool						mDrawSoftBodyVertices = false;					///< Draw the vertices of soft bodies
 		bool						mDrawSoftBodyEdgeConstraints = false;			///< Draw the edge constraints of soft bodies
 		bool						mDrawSoftBodyVolumeConstraints = false;			///< Draw the volume constraints of soft bodies
+		bool						mDrawSoftBodySkinConstraints = false;			///< Draw the skin constraints of soft bodies
 		bool						mDrawSoftBodyPredictedBounds = false;			///< Draw the predicted bounds of soft bodies
 	};
 

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -74,6 +74,10 @@ void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSett
 		mLocalBounds.Encapsulate(out_vertex.mPosition);
 	}
 
+	// Allocate space for skinned vertices
+	if (!inSettings.mSettings->mSkinnedConstraints.empty())
+		mSkinState.resize(mVertices.size());
+
 	// We don't know delta time yet, so we can't predict the bounds and use the local bounds as the predicted bounds
 	mLocalPredictedBounds = mLocalBounds;
 
@@ -288,6 +292,50 @@ void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContex
 	}
 }
 
+void SoftBodyMotionProperties::ApplySkinConstraints([[maybe_unused]] const SoftBodyUpdateContext &inContext)
+{
+	// Early out if nothing to do
+	if (mSettings->mSkinnedConstraints.empty())
+		return;
+
+	JPH_ASSERT(mSkinStateTransform == inContext.mCenterOfMassTransform, "Skinning state is stale, artifacts will show!");
+
+	// Apply the constraints
+	Vertex *vertices = mVertices.data();
+	const SkinState *skin_states = mSkinState.data();
+	for (const Skinned &s : mSettings->mSkinnedConstraints)
+	{
+		Vertex &vertex = vertices[s.mVertex];
+		const SkinState &skin_state = skin_states[s.mVertex];
+		if (vertex.mInvMass > 0.0f)
+		{
+			// Clamp vertex distance to max distance from skinned position
+			if (s.mMaxDistance < FLT_MAX)
+			{
+				Vec3 delta = vertex.mPosition - skin_state.mPosition;
+				float delta_len_sq = delta.LengthSq();
+				float max_distance_sq = Square(s.mMaxDistance);
+				if (delta_len_sq > max_distance_sq)
+					vertex.mPosition = skin_state.mPosition + delta * sqrt(max_distance_sq / delta_len_sq);
+			}
+
+			// Move position if it violated the back stop
+			if (s.mBackStop < s.mMaxDistance)
+			{
+				Vec3 delta = vertex.mPosition - skin_state.mPosition;
+				float violation = -s.mBackStop - skin_state.mNormal.Dot(delta);
+				if (violation > 0.0f)
+					vertex.mPosition += violation * skin_state.mNormal;
+			}
+		}
+		else
+		{
+			// Kinematic: Just update the vertex position
+			vertex.mPosition = skin_state.mPosition;
+		}
+	}
+}
+
 void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
 {
 	JPH_PROFILE_FUNCTION();
@@ -624,6 +672,8 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyEdgeCon
 							// Finish the iteration
 							ApplyCollisionConstraintsAndUpdateVelocities(ioContext);
 
+							ApplySkinConstraints(ioContext);
+
 							uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
 							if (iteration < mNumIterations)
 							{
@@ -677,6 +727,76 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftB
 	}
 }
 
+void SoftBodyMotionProperties::SkinVertices(RMat44Arg inRootTransform, const Mat44 *inJointMatrices, [[maybe_unused]] uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator)
+{
+	// Calculate the skin matrices
+	uint num_skin_matrices = uint(mSettings->mInvBindMatrices.size());
+	uint skin_matrices_size = num_skin_matrices * sizeof(Mat44);
+	Mat44 *skin_matrices = (Mat44 *)ioTempAllocator.Allocate(skin_matrices_size);
+	const Mat44 *skin_matrices_end = skin_matrices + num_skin_matrices;
+	const InvBind *inv_bind_matrix = mSettings->mInvBindMatrices.data();
+	for (Mat44 *s = skin_matrices; s < skin_matrices_end; ++s, ++inv_bind_matrix)
+		*s = inJointMatrices[inv_bind_matrix->mJointIndex] * inv_bind_matrix->mInvBind;
+
+	// Skin the vertices
+	mSkinStateTransform = inRootTransform;
+	JPH_IF_ENABLE_ASSERTS(uint num_vertices = uint(mSettings->mVertices.size());)
+	JPH_ASSERT(mSkinState.size() == num_vertices);
+	const SoftBodySharedSettings::Vertex *in_vertices = mSettings->mVertices.data();
+	for (const Skinned &s : mSettings->mSkinnedConstraints)
+	{
+		// Get bind pose
+		JPH_ASSERT(s.mVertex < num_vertices);
+		Vec3 bind_pos = Vec3::sLoadFloat3Unsafe(in_vertices[s.mVertex].mPosition);
+
+		// Skin vertex
+		Vec3 pos = Vec3::sZero();
+		for (const SkinWeight &w : s.mWeights)
+		{
+			JPH_ASSERT(w.mInvBindIndex < num_skin_matrices);
+			pos += w.mWeight * (skin_matrices[w.mInvBindIndex] * bind_pos);
+		}
+		mSkinState[s.mVertex].mPosition = pos;
+	}
+
+	// Calculate the normals
+	for (const Skinned &s : mSettings->mSkinnedConstraints)
+	{
+		Vec3 normal = Vec3::sZero();
+		uint32 num_faces = s.mNormalInfo >> 24;
+		if (num_faces > 0)
+		{
+			// Calculate normal
+			const uint32 *f = &mSettings->mSkinnedConstraintNormals[s.mNormalInfo & 0xffffff];
+			const uint32 *f_end = f + num_faces;
+			while (f < f_end)
+			{
+				const Face &face = mSettings->mFaces[*f];
+				Vec3 v0 = mSkinState[face.mVertex[0]].mPosition;
+				Vec3 v1 = mSkinState[face.mVertex[1]].mPosition;
+				Vec3 v2 = mSkinState[face.mVertex[2]].mPosition;
+				normal += (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sZero());
+				++f;
+			}
+			normal /= float(num_faces);
+		}
+		mSkinState[s.mVertex].mNormal = normal;
+	}
+
+	ioTempAllocator.Free(skin_matrices, skin_matrices_size);
+
+	if (inHardSkinAll)
+	{
+		// Hard skin all vertices and reset their velocities
+		for (const Skinned &s : mSettings->mSkinnedConstraints)
+		{
+			Vertex &vertex = mVertices[s.mVertex];
+			vertex.mPosition = mSkinState[s.mVertex].mPosition;
+			vertex.mVelocity = Vec3::sZero();
+		}
+	}
+}
+
 void SoftBodyMotionProperties::CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem)
 {
 	JPH_PROFILE_FUNCTION();
@@ -733,6 +853,15 @@ void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer,
 	}
 }
 
+void SoftBodyMotionProperties::DrawSkinConstraints(DebugRenderer *inRenderer) const
+{
+	for (const Skinned &s : mSettings->mSkinnedConstraints)
+	{
+		const SkinState &skin_state = mSkinState[s.mVertex];
+		DebugRenderer::sInstance->DrawArrow(mSkinStateTransform * skin_state.mPosition, mSkinStateTransform * (skin_state.mPosition + 0.1f * skin_state.mNormal), Color::sOrange, 0.01f);
+	}
+}
+
 void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
 {
 	inRenderer->DrawWireBox(inCenterOfMassTransform, mLocalPredictedBounds, Color::sRed);

Jolt/Physics/SoftBody/SoftBodyMotionProperties.h

@@ -20,6 +20,7 @@ struct PhysicsSettings;
 class Body;
 class Shape;
 class SoftBodyCreationSettings;
+class TempAllocator;
 #ifdef JPH_DEBUG_RENDERER
 class DebugRenderer;
 #endif // JPH_DEBUG_RENDERER
@@ -35,6 +36,9 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
 	using Edge = SoftBodySharedSettings::Edge;
 	using Face = SoftBodySharedSettings::Face;
 	using Volume = SoftBodySharedSettings::Volume;
+	using InvBind = SoftBodySharedSettings::InvBind;
+	using SkinWeight = SoftBodySharedSettings::SkinWeight;
+	using Skinned = SoftBodySharedSettings::Skinned;
 
 	/// Initialize the soft body motion properties
 	void								Initialize(const SoftBodyCreationSettings &inSettings);
@@ -85,6 +89,7 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
 	void								DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
 	void								DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
 	void								DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+	void								DrawSkinConstraints(DebugRenderer *inRenderer) const;
 	void								DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
 #endif // JPH_DEBUG_RENDERER
 
@@ -94,6 +99,14 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
 	/// Restoring state for replay
 	void								RestoreState(StateRecorder &inStream);
 
+	/// Skin vertices to supplied joints, information is used by the skinned constraints.
+	/// @param inRootTransform Value of Body::GetCenterOfMassTransform().
+	/// @param inJointMatrices The joint matrices must be expressed relative to inRootTransform.
+	/// @param inNumJoints Indicates how large the inJointMatrices array is (used only for validating out of bounds).
+	/// @param inHardSkinAll Can be used to position all vertices on the skinned vertices and can be used to hard reset the soft body.
+	/// @param ioTempAllocator Allocator.
+	void								SkinVertices(RMat44Arg inRootTransform, const Mat44 *inJointMatrices, uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator);
+
 	/// This function allows you to update the soft body immediately without going through the PhysicsSystem.
 	/// This is useful if the soft body is teleported and needs to 'settle' or it can be used if a the soft body
 	/// is not added to the PhysicsSystem and needs to be updated manually. One reason for not adding it to the
@@ -161,6 +174,13 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
 		Vec3							mOriginalAngularVelocity;					///< Angular velocity of the body in local space to the soft body at start
 	};
 
+	// Information about the state of all skinned vertices
+	struct SkinState
+	{
+		Vec3							mPosition = Vec3::sNaN();
+		Vec3							mNormal = Vec3::sNaN();
+	};
+
 	/// Do a narrow phase check and determine the closest feature that we can collide with
 	void								DetermineCollisionPlanes(const SoftBodyUpdateContext &inContext, uint inVertexStart, uint inNumVertices);
 
@@ -173,6 +193,9 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
 	/// Enforce all volume constraints
 	void								ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext);
 
+	/// Enforce all skin constraints
+	void								ApplySkinConstraints(const SoftBodyUpdateContext &inContext);
+
 	/// Enforce all edge constraints
 	void								ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
 
@@ -194,9 +217,11 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
 	/// Returns 6 times the volume of the soft body
 	float								GetVolumeTimesSix() const;
 
+	RMat44								mSkinStateTransform = RMat44::sIdentity();	///< The matrix that transforms mSkinState to world space
 	RefConst<SoftBodySharedSettings>	mSettings;									///< Configuration of the particles and constraints
 	Array<Vertex>						mVertices;									///< Current state of all vertices in the simulation
 	Array<CollidingShape>				mCollidingShapes;							///< List of colliding shapes retrieved during the last update
+	Array<SkinState>					mSkinState;									///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in)
 	AABox								mLocalBounds;								///< Bounding box of all vertices
 	AABox								mLocalPredictedBounds;						///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time
 	uint32								mNumIterations;								///< Number of solver iterations

Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -10,6 +10,8 @@
 #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>
 
 JPH_NAMESPACE_BEGIN
 
@@ -40,13 +42,35 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Volume)
 	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, mBackStop)
+}
+
 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, mVolumeConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius)
 }
@@ -77,6 +101,54 @@ void SoftBodySharedSettings::CalculateVolumeConstraintVolumes()
 	}
 }
 
+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
+	UnorderedSet<uint32> skinned_vertices;
+	skinned_vertices.reserve(mSkinnedConstraints.size());
+	for (const Skinned &s : mSkinnedConstraints)
+		skinned_vertices.insert(s.mVertex);
+
+	// Now collect all faces that connect only to skinned vertices
+	UnorderedMap<uint32, UnorderedSet<uint32>> connected_faces;
+	connected_faces.reserve(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);
+		const UnorderedSet<uint32> &faces = connected_faces[s.mVertex];
+		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);
+	}
+	mSkinnedConstraintNormals.shrink_to_fit();
+}
+
 void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 {
 	const uint cMaxNumGroups = 32;
@@ -143,7 +215,17 @@ void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mEdgeConstraints);
 	inStream.Write(mEdgeGroupEndIndices);
 	inStream.Write(mVolumeConstraints);
+	inStream.Write(mSkinnedConstraints);
+	inStream.Write(mSkinnedConstraintNormals);
 	inStream.Write(mVertexRadius);
+
+	// Can't write mInvBindMatrices directly because the class contains padding
+	inStream.Write(uint32(mInvBindMatrices.size()));
+	for (const InvBind &ib : mInvBindMatrices)
+	{
+		inStream.Write(ib.mJointIndex);
+		inStream.Write(ib.mInvBind);
+	}
 }
 
 void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
@@ -153,7 +235,18 @@ void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mEdgeConstraints);
 	inStream.Read(mEdgeGroupEndIndices);
 	inStream.Read(mVolumeConstraints);
+	inStream.Read(mSkinnedConstraints);
+	inStream.Read(mSkinnedConstraintNormals);
 	inStream.Read(mVertexRadius);
+
+	uint32 num_inv_bind_matrices = 0;
+	inStream.Read(num_inv_bind_matrices);
+	mInvBindMatrices.resize(num_inv_bind_matrices);
+	for (InvBind &ib : mInvBindMatrices)
+	{
+		inStream.Read(ib.mJointIndex);
+		inStream.Read(ib.mInvBind);
+	}
 }
 
 void SoftBodySharedSettings::SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const