Fixed bend constraint having 2 stable configurations instead of 1 (#1021)

* All constraints deal with the situation that all of the connected vertices are kinematic
* Added soft body bend unit test

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -290,12 +290,21 @@ void SoftBodyMotionProperties::ApplyBendConstraints(const SoftBodyUpdateContext
 		Vec3 n2 = x1x3.Cross(x3 - x0);
 		float n1_len_sq = n1.LengthSq();
 		float n2_len_sq = n2.LengthSq();
-		if (n1_len_sq < 1.0e-12f || n2_len_sq < 1.0e-12f)
+		float n1_len_sq_n2_len_sq = n1_len_sq * n2_len_sq;
+		if (n1_len_sq_n2_len_sq < 1.0e-24f)
 			continue;
 
 		// Calculate constraint equation
-		float d = n1.Dot(n2) / sqrt(n1_len_sq * n2_len_sq);
-		float c = ACos(d) - b.mInitialAngle;
+		// 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;
+
+		// Ensure the range is -PI to PI
+		if (c > JPH_PI)
+			c -= 2.0f * JPH_PI;
+		else if (c < -JPH_PI)
+			c += 2.0f * JPH_PI;
 
 		// Calculate gradient of constraint equation
 		// Taken from "Strain Based Dynamics" - Matthias Muller et al. (Appendix A)
@@ -315,7 +324,6 @@ void SoftBodyMotionProperties::ApplyBendConstraints(const SoftBodyUpdateContext
 		float w1 = v1.mInvMass;
 		float w2 = v2.mInvMass;
 		float w3 = v3.mInvMass;
-		JPH_ASSERT(w0 > 0.0f || w1 > 0.0f || w2 > 0.0f || w3 > 0.0f);
 
 		// Calculate -lambda
 		float denom = w0 * d0c.LengthSq() + w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + b.mCompliance * inv_dt_sq;
@@ -323,15 +331,11 @@ void SoftBodyMotionProperties::ApplyBendConstraints(const SoftBodyUpdateContext
 			continue;
 		float minus_lambda = c / denom;
 
-		// Negate the gradients (in this case we apply the sign flip in lambda) as per "Strain Based Dynamics" Appendix A
-		if (n1.Cross(n2).Dot(e) > 0.0f)
-			minus_lambda = -minus_lambda;
-
 		// Apply correction
-		v0.mPosition -= minus_lambda * w0 * d0c;
-		v1.mPosition -= minus_lambda * w1 * d1c;
-		v2.mPosition -= minus_lambda * w2 * d2c;
-		v3.mPosition -= minus_lambda * w3 * d3c;
+		v0.mPosition = x0 - minus_lambda * w0 * d0c;
+		v1.mPosition = x1 - minus_lambda * w1 * d1c;
+		v2.mPosition = x2 - minus_lambda * w2 * d2c;
+		v3.mPosition = x3 - minus_lambda * w3 * d3c;
 	}
 }
 
@@ -371,14 +375,18 @@ void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContex
 		float w2 = v2.mInvMass;
 		float w3 = v3.mInvMass;
 		float w4 = v4.mInvMass;
-		JPH_ASSERT(w1 > 0.0f || w2 > 0.0f || w3 > 0.0f || w4 > 0.0f);
+
+		// Calculate -lambda
+		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;
 
 		// Apply correction
-		float lambda = -c / (w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v.mCompliance * inv_dt_sq);
-		v1.mPosition += lambda * w1 * d1c;
-		v2.mPosition += lambda * w2 * d2c;
-		v3.mPosition += lambda * w3 * d3c;
-		v4.mPosition += lambda * w4 * d4c;
+		v1.mPosition = x1 - minus_lambda * w1 * d1c;
+		v2.mPosition = x2 - minus_lambda * w2 * d2c;
+		v3.mPosition = x3 - minus_lambda * w3 * d3c;
+		v4.mPosition = x4 - minus_lambda * w4 * d4c;
 	}
 }
 
@@ -449,18 +457,22 @@ void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext
 		const Edge &e = edge_constraints[i];
 		Vertex &v0 = mVertices[e.mVertex[0]];
 		Vertex &v1 = mVertices[e.mVertex[1]];
-		JPH_ASSERT(v0.mInvMass > 0.0f || v1.mInvMass > 0.0f);
+
+		// Get positions
+		Vec3 x0 = v0.mPosition;
+		Vec3 x1 = v1.mPosition;
 
 		// Calculate current length
-		Vec3 delta = v1.mPosition - v0.mPosition;
+		Vec3 delta = x1 - x0;
 		float length = delta.Length();
-		if (length > 0.0f)
-		{
-			// Apply correction
-			Vec3 correction = delta * (length - e.mRestLength) / (length * (v0.mInvMass + v1.mInvMass + e.mCompliance * inv_dt_sq));
-			v0.mPosition += v0.mInvMass * correction;
-			v1.mPosition -= v1.mInvMass * correction;
-		}
+
+		// Apply correction
+		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;
+		v0.mPosition = x0 + v0.mInvMass * correction;
+		v1.mPosition = x1 - v1.mInvMass * correction;
 	}
 }
 
@@ -477,10 +489,11 @@ void SoftBodyMotionProperties::ApplyLRAConstraints()
 		const Vertex &vertex0 = vertices[lra.mVertex[0]];
 		Vertex &vertex1 = vertices[lra.mVertex[1]];
 
-		Vec3 delta = vertex1.mPosition - vertex0.mPosition;
+		Vec3 x0 = vertex0.mPosition;
+		Vec3 delta = vertex1.mPosition - x0;
 		float delta_len_sq = delta.LengthSq();
 		if (delta_len_sq > Square(lra.mMaxDistance))
-			vertex1.mPosition = vertex0.mPosition + delta * lra.mMaxDistance / sqrt(delta_len_sq);
+			vertex1.mPosition = x0 + delta * lra.mMaxDistance / sqrt(delta_len_sq);
 	}
 }
 

Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -272,13 +272,16 @@ void SoftBodySharedSettings::CalculateBendConstraintConstants()
 		Vec3 e2 = x3 - x0;
 
 		// Normals of both triangles
-		Vec3 n0 = e0.Cross(e1);
-		Vec3 n1 = e2.Cross(e0);
-		float denom = sqrt(n0.LengthSq() * n1.LengthSq());
-		if (denom == 0.0f)
+		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
-			b.mInitialAngle = ACos(n0.Dot(n1) / denom);
+		{
+			float sign = Sign(n2.Cross(n1).Dot(e0));
+			b.mInitialAngle = sign * ACos(n1.Dot(n2) / denom);
+		}
 	}
 }
 

Samples/Tests/SoftBody/SoftBodyBendConstraintTest.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(SoftBodyBendConstraintTest)
 {
@@ -18,26 +19,61 @@ void SoftBodyBendConstraintTest::Initialize()
 {
 	CreateFloor();
 
+	default_random_engine random;
+	uniform_real_distribution<float> random_float(-0.1f, 0.1f);
+
 	auto inv_mass = [](uint, uint inZ) { return inZ < 2? 0.0f : 1.0f; };
+	auto perturbation = [&random, &random_float](uint, uint inZ) { return Vec3(random_float(random), (inZ & 1)? 0.1f : -0.1f, random_float(random)); };
 
 	{
+		random.seed(1234);
+
 		// Cloth without bend constraints
-		Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass, SoftBodySharedSettings::EBendType::None);
+		Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass, perturbation, SoftBodySharedSettings::EBendType::None);
 		SoftBodyCreationSettings cloth(cloth_settings, RVec3(-5.0f, 5.0f, 0), Quat::sIdentity(), Layers::MOVING);
 		mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
 	}
 
 	{
-		// Cloth with edge bend constraints
-		Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass, SoftBodySharedSettings::EBendType::Distance);
+		random.seed(1234);
+
+		// Cloth with distance bend constraints
+		Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass, perturbation, SoftBodySharedSettings::EBendType::Distance);
 		SoftBodyCreationSettings cloth(cloth_settings, RVec3(0.0f, 5.0f, 0), Quat::sIdentity(), Layers::MOVING);
 		mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
 	}
 
 	{
+		random.seed(1234);
+
 		// Cloth with dihedral bend constraints
-		Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass, SoftBodySharedSettings::EBendType::Dihedral);
+		Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass, perturbation, SoftBodySharedSettings::EBendType::Dihedral);
 		SoftBodyCreationSettings cloth(cloth_settings, RVec3(5.0f, 5.0f, 0), Quat::sIdentity(), Layers::MOVING);
 		mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
 	}
+
+	{
+		// Create sphere
+		SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere(1.0f, 10, 20, SoftBodySharedSettings::EBendType::None), RVec3(-5.0f, 5.0f, 10.0f), Quat::sIdentity(), Layers::MOVING);
+		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+	}
+
+	{
+		// Create sphere with distance bend constraints
+		SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere(1.0f, 10, 20, SoftBodySharedSettings::EBendType::Distance), RVec3(0.0f, 5.0f, 10.0f), Quat::sIdentity(), Layers::MOVING);
+		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+	}
+
+	{
+		// Create sphere with dihedral bend constraints
+		SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere(1.0f, 10, 20, SoftBodySharedSettings::EBendType::Dihedral), RVec3(5.0f, 5.0f, 10.0f), Quat::sIdentity(), Layers::MOVING);
+		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+	}
+}
+
+void SoftBodyBendConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	mDebugRenderer->DrawText3D(RVec3(-5.0f, 7.5f, 0), "No bend constraints", Color::sWhite);
+	mDebugRenderer->DrawText3D(RVec3(0.0f, 7.5f, 0), "Distance bend constraints", Color::sWhite);
+	mDebugRenderer->DrawText3D(RVec3(5.0f, 7.5f, 0), "Dihedral angle bend constraints", Color::sWhite);
 }

Samples/Tests/SoftBody/SoftBodyBendConstraintTest.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

Samples/Utils/SoftBodyCreator.cpp

@@ -8,7 +8,7 @@
 
 namespace SoftBodyCreator {
 
-Ref<SoftBodySharedSettings> CreateCloth(uint inGridSizeX, uint inGridSizeZ, float inGridSpacing, const function<float(uint, uint)> &inVertexGetInvMass, SoftBodySharedSettings::EBendType inBendType, const SoftBodySharedSettings::VertexAttributes &inVertexAttributes)
+Ref<SoftBodySharedSettings> CreateCloth(uint inGridSizeX, uint inGridSizeZ, float inGridSpacing, const function<float(uint, uint)> &inVertexGetInvMass, const function<Vec3(uint, uint)> &inVertexPerturbation, SoftBodySharedSettings::EBendType inBendType, const SoftBodySharedSettings::VertexAttributes &inVertexAttributes)
 {
 	const float cOffsetX = -0.5f * inGridSpacing * (inGridSizeX - 1);
 	const float cOffsetZ = -0.5f * inGridSpacing * (inGridSizeZ - 1);
@@ -19,7 +19,8 @@ Ref<SoftBodySharedSettings> CreateCloth(uint inGridSizeX, uint inGridSizeZ, floa
 		for (uint x = 0; x < inGridSizeX; ++x)
 		{
 			SoftBodySharedSettings::Vertex v;
-			v.mPosition = Float3(cOffsetX + x * inGridSpacing, 0.0f, cOffsetZ + z * inGridSpacing);
+			Vec3 position = inVertexPerturbation(x, z) + Vec3(cOffsetX + x * inGridSpacing, 0.0f, cOffsetZ + z * inGridSpacing);
+			position.StoreFloat3(&v.mPosition);
 			v.mInvMass = inVertexGetInvMass(x, z);
 			settings->mVertices.push_back(v);
 		}
@@ -45,7 +46,7 @@ Ref<SoftBodySharedSettings> CreateCloth(uint inGridSizeX, uint inGridSizeZ, floa
 			settings->AddFace(f);
 		}
 
-	// Create edges
+	// Create constraints
 	settings->CreateConstraints(&inVertexAttributes, 1, inBendType);
 
 	// Optimize the settings
@@ -209,7 +210,7 @@ Ref<SoftBodySharedSettings> CreateCube(uint inGridSize, float inGridSpacing)
 	return settings;
 }
 
-Ref<SoftBodySharedSettings> CreateSphere(float inRadius, uint inNumTheta, uint inNumPhi)
+Ref<SoftBodySharedSettings> CreateSphere(float inRadius, uint inNumTheta, uint inNumPhi, SoftBodySharedSettings::EBendType inBendType, const SoftBodySharedSettings::VertexAttributes &inVertexAttributes)
 {
 	// Create settings
 	SoftBodySharedSettings *settings = new SoftBodySharedSettings;
@@ -266,10 +267,8 @@ Ref<SoftBodySharedSettings> CreateSphere(float inRadius, uint inNumTheta, uint i
 		settings->AddFace(f);
 	}
 
-	// Create edges
-	SoftBodySharedSettings::VertexAttributes va;
-	va.mCompliance = va.mShearCompliance = 1.0e-4f;
-	settings->CreateConstraints(&va, 1);
+	// Create constraints
+	settings->CreateConstraints(&inVertexAttributes, 1, inBendType);
 
 	// Optimize the settings
 	settings->Optimize();

Samples/Utils/SoftBodyCreator.h

@@ -13,7 +13,7 @@ namespace SoftBodyCreator
 	/// @param inGridSizeZ Number of points along the Z axis
 	/// @param inGridSpacing Distance between points
 	/// @param inVertexGetInvMass Function that determines the inverse mass of each vertex
-	Ref<SoftBodySharedSettings>	CreateCloth(uint inGridSizeX = 30, uint inGridSizeZ = 30, float inGridSpacing = 0.75f, const function<float(uint, uint)> &inVertexGetInvMass = [](uint, uint) { return 1.0f; }, SoftBodySharedSettings::EBendType inBendType = SoftBodySharedSettings::EBendType::None, const SoftBodySharedSettings::VertexAttributes &inVertexAttributes = { 1.0e-5f, 1.0e-5f, 1.0e-5f });
+	Ref<SoftBodySharedSettings>	CreateCloth(uint inGridSizeX = 30, uint inGridSizeZ = 30, float inGridSpacing = 0.75f, const function<float(uint, uint)> &inVertexGetInvMass = [](uint, uint) { return 1.0f; }, const function<Vec3(uint, uint)> &inVertexPerturbation = [](uint, uint) { return Vec3::sZero(); }, SoftBodySharedSettings::EBendType inBendType = SoftBodySharedSettings::EBendType::None, const SoftBodySharedSettings::VertexAttributes &inVertexAttributes = { 1.0e-5f, 1.0e-5f, 1.0e-5f });
 
 	/// Same as above but fixates the corners of the cloth
 	Ref<SoftBodySharedSettings>	CreateClothWithFixatedCorners(uint inGridSizeX = 30, uint inGridSizeZ = 30, float inGridSpacing = 0.75f);
@@ -27,5 +27,5 @@ namespace SoftBodyCreator
 	/// @param inRadius Radius of the sphere
 	/// @param inNumTheta Number of segments in the theta direction
 	/// @param inNumPhi Number of segments in the phi direction
-	Ref<SoftBodySharedSettings>	CreateSphere(float inRadius = 1.0f, uint inNumTheta = 10, uint inNumPhi = 20);
+	Ref<SoftBodySharedSettings>	CreateSphere(float inRadius = 1.0f, uint inNumTheta = 10, uint inNumPhi = 20, SoftBodySharedSettings::EBendType inBendType = SoftBodySharedSettings::EBendType::None, const SoftBodySharedSettings::VertexAttributes &inVertexAttributes = { 1.0e-4f, 1.0e-4f, 1.0e-3f });
 };

UnitTests/Physics/SoftBodyTests.cpp

@@ -0,0 +1,93 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+
+TEST_SUITE("SoftBodyTests")
+{
+	TEST_CASE("TestBendConstraint")
+	{
+		// Possible values for x3
+		const Float3 x3_values[] = {
+			Float3(0, 0, 1),	// forming flat plane
+			Float3(0, 0, -1),	// overlapping
+			Float3(0, 1, 0),	// 90 degrees concave
+			Float3(0, -1, 0),	// 90 degrees convex
+			Float3(0, 1, 1),	// 45 degrees concave
+			Float3(0, -1, -1)	// 135 degrees convex
+		};
+
+		for (const Float3 &x3 : x3_values)
+		{
+			PhysicsTestContext c;
+			PhysicsSystem *s = c.GetSystem();
+			BodyInterface &bi = s->GetBodyInterface();
+
+			// Create settings
+			Ref<SoftBodySharedSettings> shared_settings = new SoftBodySharedSettings;
+
+			/* Create two triangles with a shared edge, x3 = free, the rest is locked
+			   x2
+			e1/  \e3
+			 /    \
+			x0----x1
+			 \ e0 /
+			e2\  /e4
+			   x3
+			*/
+			SoftBodySharedSettings::Vertex v;
+			v.mPosition = Float3(-1, 0, 0);
+			v.mInvMass = 0;
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = Float3(1, 0, 0);
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = Float3(0, 0, -1);
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = x3;
+			v.mInvMass = 1;
+			shared_settings->mVertices.push_back(v);
+
+			// Create the 2 triangles
+			shared_settings->AddFace(SoftBodySharedSettings::Face(0, 1, 2));
+			shared_settings->AddFace(SoftBodySharedSettings::Face(0, 3, 1));
+
+			// Create edge and dihedral constraints
+			SoftBodySharedSettings::VertexAttributes va;
+			va.mShearCompliance = FLT_MAX;
+			va.mBendCompliance = 0;
+			shared_settings->CreateConstraints(&va, 1, SoftBodySharedSettings::EBendType::Dihedral);
+
+			// Optimize the settings
+			shared_settings->Optimize();
+
+			// Create the soft body
+			SoftBodyCreationSettings sb_settings(shared_settings, RVec3::sZero(), Quat::sIdentity(), Layers::MOVING);
+			sb_settings.mGravityFactor = 0.0f;
+			sb_settings.mAllowSleeping = false;
+			sb_settings.mUpdatePosition = false;
+			Body &body = *bi.CreateSoftBody(sb_settings);
+			bi.AddBody(body.GetID(), EActivation::Activate);
+			SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
+
+			// Test 4 angles to see if there are singularities (the dot product between the triangles has the same value for 2 configurations)
+			for (float angle : { 0.0f, 90.0f, 180.0f, 270.0f })
+			{
+				// Perturb x3
+				Vec3 perturbed_x3(x3);
+				mp->GetVertex(3).mPosition = 0.5f * (Mat44::sRotationX(DegreesToRadians(angle)) * perturbed_x3);
+
+				// Simulate
+				c.Simulate(0.25f);
+
+				// Should return to the original position
+				CHECK_APPROX_EQUAL(mp->GetVertex(3).mPosition, Vec3(x3), 1.0e-3f);
+			}
+		}
+	}
+}

UnitTests/UnitTests.cmake

@@ -59,6 +59,7 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Physics/ShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SixDOFConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SliderConstraintTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/SoftBodyTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SubShapeIDTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/TransformedShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/WheeledVehicleTests.cpp