SoftBodySharedSettings::CreateConstraints can now also create LRA constraints (#1026)

* Added option to multiply max distance for LRA constraint with a constant factor

Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -13,8 +13,14 @@
 #include <Jolt/Core/UnorderedMap.h>
 #include <Jolt/Core/UnorderedSet.h>
 
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <queue>
+JPH_SUPPRESS_WARNINGS_STD_END
+
 JPH_NAMESPACE_BEGIN
 
+template<class T, class Container = Array<T>, class Compare = std::less<typename Container::value_type>> using PriorityQueue = std::priority_queue<T, Container, Compare>;
+
 JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Vertex)
 {
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mPosition)
@@ -91,6 +97,73 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius)
 }
 
+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]);
+	}
+
+	// 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
+	PriorityQueue<Open> to_visit;
+	for (uint32 v = 0; v < mVertices.size(); ++v)
+		if (mVertices[v].mInvMass == 0.0f)
+		{
+			mClosestKinematic[v].mVertex = v;
+			mClosestKinematic[v].mDistance = 0.0f;
+			to_visit.push({ v, 0.0f });
+		}
+
+	// Visit all vertices remembering the closest kinematic vertex and its distance
+	while (!to_visit.empty())
+	{
+		// Pop element from the open list
+		Open current = to_visit.top();
+		to_visit.pop();
+
+		// 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].mDistance = new_distance;
+				to_visit.push({ v, new_distance });
+			}
+		}
+	}
+}
+
 void SoftBodySharedSettings::CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType, float inAngleTolerance)
 {
 	struct EdgeHelper
@@ -225,7 +298,48 @@ void SoftBodySharedSettings::CreateConstraints(const VertexAttributes *inVertexA
 	}
 	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()
@@ -237,11 +351,11 @@ void SoftBodySharedSettings::CalculateEdgeLengths()
 	}
 }
 
-void SoftBodySharedSettings::CalculateLRALengths()
+void SoftBodySharedSettings::CalculateLRALengths(float inMaxDistanceMultiplier)
 {
 	for (LRA &l : mLRAConstraints)
 	{
-		l.mMaxDistance = (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length();
+		l.mMaxDistance = inMaxDistanceMultiplier * (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length();
 		JPH_ASSERT(l.mMaxDistance > 0.0f);
 	}
 }
@@ -407,6 +521,10 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 	// 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());
+
+	// Free closest kinematic buffer
+	mClosestKinematic.clear();
+	mClosestKinematic.shrink_to_fit();
 }
 
 Ref<SoftBodySharedSettings> SoftBodySharedSettings::Clone() const

Jolt/Physics/SoftBody/SoftBodySharedSettings.h

@@ -25,30 +25,43 @@ public:
 		Dihedral,													///< A dihedral bend constraint (most expensive, but also supports triangles that are initially not in the same plane)
 	};
 
-	/// Per vertex attributes used during the CreateConstraints function
+	/// The type of long range attachment constraint to create
+	enum class ELRAType
+	{
+		None,														///< Don't create a LRA constraint
+		EuclideanDistance,											///< Create a LRA constraint based on Euclidean distance between the closest kinematic vertex and this vertex
+		GeodesicDistance,											///< Create a LRA constraint based on the geodesic distance between the closest kinematic vertex and this vertex (follows the edge constraints)
+	};
+
+	/// Per vertex attributes used during the CreateConstraints function.
+	/// When a constraint is created, the compliance of the two attached vertices is averaged
+	/// to get the compliance for the constraint (for a bend constraint the vertices that are not on the shared edge are used).
 	struct JPH_EXPORT VertexAttributes
 	{
 		/// Constructor
 						VertexAttributes() = default;
-						VertexAttributes(float inCompliance, float inShearCompliance, float inBendCompliance) : mCompliance(inCompliance), mShearCompliance(inShearCompliance), mBendCompliance(inBendCompliance) { }
+						VertexAttributes(float inCompliance, float inShearCompliance, float inBendCompliance, ELRAType inLRAType = ELRAType::None, float inLRAMaxDistanceMultiplier = 1.0f) : mCompliance(inCompliance), mShearCompliance(inShearCompliance), mBendCompliance(inBendCompliance), mLRAType(inLRAType), mLRAMaxDistanceMultiplier(inLRAMaxDistanceMultiplier) { }
 
-		float			mCompliance = 0.0f;							///< The compliance of the normal edges. Set to FLT_MAX to disable regular edges.
-		float			mShearCompliance = 0.0f;					///< The compliance of the shear edges. Set to FLT_MAX to disable shear edges.
-		float			mBendCompliance = FLT_MAX;					///< The compliance of the bend edges. Set to FLT_MAX to disable bend edges.
+		float			mCompliance = 0.0f;							///< The compliance of the normal edges. Set to FLT_MAX to disable regular edges for any edge involving this vertex.
+		float			mShearCompliance = 0.0f;					///< The compliance of the shear edges. Set to FLT_MAX to disable shear edges for any edge involving this vertex.
+		float			mBendCompliance = FLT_MAX;					///< The compliance of the bend edges. Set to FLT_MAX to disable bend edges for any bend constraint involving this vertex.
+		ELRAType		mLRAType = ELRAType::None;					///< The type of long range attachment constraint to create.
+		float			mLRAMaxDistanceMultiplier = 1.0f;			///< Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose.
 	};
 
 	/// Automatically create constraints based on the faces of the soft body
 	/// @param inVertexAttributes A list of attributes for each vertex (1-on-1 with mVertices, note that if the list is smaller than mVertices the last element will be repeated). This defines the properties of the constraints that are created.
 	/// @param inVertexAttributesLength The length of inVertexAttributes
-	/// @param inAngleTolerance Shear edges are created when two connected triangles form a quad (are roughly in the same plane and form a square with roughly 90 degree angles). This defines the tolerance (in radians).
 	/// @param inBendType The type of bend constraint to create
+	/// @param inAngleTolerance Shear edges are created when two connected triangles form a quad (are roughly in the same plane and form a square with roughly 90 degree angles). This defines the tolerance (in radians).
 	void				CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType = EBendType::Distance, float inAngleTolerance = DegreesToRadians(8.0f));
 
 	/// Calculate the initial lengths of all springs of the edges of this soft body (if you use CreateConstraint, this is already done)
 	void				CalculateEdgeLengths();
 
-	/// Calculate the max lengths for the long range attachment constraints
-	void				CalculateLRALengths();
+	/// Calculate the max lengths for the long range attachment constraints based on Euclidean distance (if you use CreateConstraints, this is already done)
+	/// @param inMaxDistanceMultiplier Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose.
+	void				CalculateLRALengths(float inMaxDistanceMultiplier = 1.0f);
 
 	/// Calculate the constants for the bend constraints (if you use CreateConstraints, this is already done)
 	void				CalculateBendConstraintConstants();
@@ -275,9 +288,20 @@ public:
 private:
 	friend class SoftBodyMotionProperties;
 
+	/// Tracks the closest kinematic vertex
+	struct ClosestKinematic
+	{
+		uint32			mVertex = 0xffffffff;						///< Vertex index of closest kinematic vertex
+		float			mDistance = FLT_MAX;						///< Distance to the closest kinematic vertex
+	};
+
+	/// Calculate te 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]; }
 
+	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<uint32>		mSkinnedConstraintNormals;					///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals()
 };

Samples/Tests/SoftBody/SoftBodyLRAConstraintTest.cpp

@@ -8,6 +8,7 @@
 #include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
 #include <Utils/SoftBodyCreator.h>
 #include <Layers.h>
+#include <Renderer/DebugRendererImp.h>
 
 JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyLRAConstraintTest)
 {
@@ -18,21 +19,24 @@ void SoftBodyLRAConstraintTest::Initialize()
 {
 	CreateFloor();
 
-	// Cloth without LRA constraints
-	auto inv_mass = [](uint, uint inZ) { return inZ == 0? 0.0f : 1.0f; };
-	Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass);
-	for (SoftBodySharedSettings::Edge &e : cloth_settings->mEdgeConstraints)
-		e.mCompliance = 1.0e-3f; // Soften the edges a bit so that the effect of the LRA constraints is more visible
-	SoftBodyCreationSettings cloth(cloth_settings, RVec3(-10.0f, 25.0f, 0), Quat::sIdentity(), Layers::MOVING);
-	mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
-
-	// Cloth with LRA constraints
-	Ref<SoftBodySharedSettings> lra_cloth_settings = cloth_settings->Clone();
-	auto get_vertex = [](uint inX, uint inZ) { return inX + inZ * cNumVerticesX; };
-	for (int z = 1; z < cNumVerticesZ; ++z)
-		for (int x = 0; x < cNumVerticesX; ++x)
-			lra_cloth_settings->mLRAConstraints.push_back(SoftBodySharedSettings::LRA(get_vertex(x, 0), get_vertex(x, z), 0.0f));
-	lra_cloth_settings->CalculateLRALengths();
-	SoftBodyCreationSettings lra_cloth(lra_cloth_settings, RVec3(10.0f, 25.0f, 0), Quat::sIdentity(), Layers::MOVING);
-	mBodyInterface->CreateAndAddSoftBody(lra_cloth, EActivation::Activate);
+	for (int i = 0; i < 2; ++i)
+	{
+		auto inv_mass = [](uint, uint inZ) { return inZ == 0? 0.0f : 1.0f; };
+		auto perturbation = [](uint, uint) { return Vec3::sZero(); };
+
+		SoftBodySharedSettings::VertexAttributes va;
+		va.mShearCompliance = va.mCompliance = 1.0e-3f; // Soften the edges a bit so that the effect of the LRA constraints is more visible
+		va.mLRAType = i == 0? SoftBodySharedSettings::ELRAType::None : SoftBodySharedSettings::ELRAType::EuclideanDistance;
+
+		Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass, perturbation, SoftBodySharedSettings::EBendType::None, va);
+
+		SoftBodyCreationSettings cloth(cloth_settings, RVec3(-10.0f + i * 20.0f, 25.0f, 0), Quat::sIdentity(), Layers::MOVING);
+		mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
+	}
+}
+
+void SoftBodyLRAConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	mDebugRenderer->DrawText3D(RVec3(-10, 26, -0.5f * cNumVerticesZ * cVertexSpacing), "Without LRA constraints", Color::sWhite);
+	mDebugRenderer->DrawText3D(RVec3(10, 26, -0.5f * cNumVerticesZ * cVertexSpacing), "With LRA constraints", Color::sWhite);
 }

Samples/Tests/SoftBody/SoftBodyLRAConstraintTest.h

@@ -14,6 +14,7 @@ public:
 
 	// See: Test
 	virtual void			Initialize() override;
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
 
 private:
 	// Size and spacing of the cloth