correctly initiate/terminate contact constraints

include/bounce/dynamics/cloth/spring_solver.h

@@ -22,6 +22,8 @@
 #include <bounce/common/math/vec3.h>
 #include <bounce/common/math/mat33.h>
 
+class b3Shape;
+
 class b3SpringCloth;
 class b3StackAllocator;
 
@@ -56,15 +58,21 @@ private:
 	// Compute A and b in Ax = b
 	void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b) const;
 
+	// Compute the initial guess for the iterative solver.
+	void Compute_x0(b3DenseVec3& x0);
+
+	// Compute the constraint projection matrix S.
+	void Compute_S(b3Mat33* S);
+	
 	// Solve Ax = b using the Modified Conjugate Gradient (MCG). 
 	// Output x and the residual error f.
-	void Solve_MCG(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const;
+	void Solve_MCG(b3DenseVec3& x0, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const;
 
 	// Solve Ax = b using MCG with Jacobi preconditioning.
 	// Output x and the residual error f.
 	// This method is slower than MCG because we have to compute the preconditioning 
 	// matrix P, but it can improve convergence.
-	void Solve_MPCG(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const;
+	void Solve_MPCG(b3DenseVec3& x0, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const;
 
 	b3SpringCloth * m_cloth;
 	float32 m_h;
@@ -87,6 +95,8 @@ private:
 	
 	b3Spring* m_springs;
 	u32 m_springCount;
+	
+	b3Shape** m_shapes;
 };
 
 inline u32 b3SpringSolver::GetIterations() const

src/bounce/dynamics/cloth/spring_cloth.cpp

@@ -25,7 +25,7 @@
 #include <bounce/common/memory/stack_allocator.h>
 #include <bounce/common/draw.h>
 
-#define B3_FORCE_THRESHOLD (0.1f)
+#define B3_FORCE_THRESHOLD 0.0f
 
 #define B3_CLOTH_BENDING 0
 
@@ -344,7 +344,7 @@ void b3SpringCloth::GetTension(b3Array<b3Vec3>& T) const
 	}
 }
 
-static B3_FORCE_INLINE void b3MakeTangents(b3Vec3& t1, b3Vec3& t2, const b3Vec3& dv, const b3Vec3& n)
+static B3_FORCE_INLINE void b3CreateTangents(b3Vec3& t1, b3Vec3& t2, const b3Vec3& dv, const b3Vec3& n)
 {
 	t1 = dv - b3Dot(dv, n) * n;
 	if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON)
@@ -369,9 +369,18 @@ void b3SpringCloth::UpdateContacts()
 			continue;
 		}
 
+		// Relative velocity
+		b3Vec3 dv = m_v[i];
+
 		b3MassContact* c = m_contacts + i;
 
-		bool wasLockedN = c->lockN;
+		// Save the old contact
+		b3MassContact c0 = *c;
+
+		// Create a new contact
+		c->lockN = false;
+		c->lockT1 = false;
+		c->lockT2 = false;
 
 		b3Sphere s1;
 		s1.vertex = m_x[i];
@@ -384,13 +393,13 @@ void b3SpringCloth::UpdateContacts()
 
 		for (u32 j = 0; j < m_shapeCount; ++j)
 		{
-			b3Shape* shape = m_shapes[j];
+			b3Shape* s2 = m_shapes[j];
 
 			b3Transform xf2;
 			xf2.SetIdentity();
 
 			b3TestSphereOutput output;
-			if (shape->TestSphere(&output, s1, xf2) == false)
+			if (s2->TestSphere(&output, s1, xf2) == false)
 			{
 				continue;
 			}
@@ -403,123 +412,121 @@ void b3SpringCloth::UpdateContacts()
 			}
 		}
 
-		if (bestIndex == ~0)
+		if (bestIndex != ~0)
 		{
-			c->Fn = 0.0f;
+			B3_ASSERT(bestSeparation <= 0.0f);
-			c->Ft1 = 0.0f;
+
-			c->Ft2 = 0.0f;
+			b3Shape* shape = m_shapes[bestIndex];
-			c->lockN = false;
+			float32 s = bestSeparation;
-			c->lockT1 = false;
+			b3Vec3 n = bestNormal;
-			c->lockT2 = false;
+
-			continue;
+			// Update contact manifold
+			// Here the normal orientation is from the shape 2 (mass) to shape 1
+			c->j = bestIndex;
+			c->n = n;
+			c->lockN = true;
+
+			// Apply position correction
+			m_y[i] -= s * n;
 		}
 
-		B3_ASSERT(bestSeparation <= 0.0f);
-
-		b3Shape* shape = m_shapes[bestIndex];
-		float32 s = bestSeparation;
-		b3Vec3 n = bestNormal;
-
-		// Apply position correction
-		m_y[i] -= s * n;
-
 		// Update contact state
-		if (wasLockedN)
+		if (c0.lockN == true && c->lockN == true)
 		{
-			// Was the contact force attractive?
+			// The contact persists
-			if (c->Fn < B3_FORCE_THRESHOLD)
-			{
-				// Terminate the contact.
-				c->Fn = 0.0f;
-				c->Ft1 = 0.0f;
-				c->Ft2 = 0.0f;
-				c->lockN = false;
-				c->lockT1 = false;
-				c->lockT2 = false;
-				continue;
-			}
 			
-			// Since the contact force was repulsive 
+			// Is the contact constraint still violated?
-			// maintain the normal acceleration constraint.
+			if (c0.Fn <= -B3_FORCE_THRESHOLD)
-			c->j = bestIndex;
+			{
-			c->n = n;
+				// Contact force is attractive.
+
+				// Terminate the contact.
+				c->lockN = false;
+			}
 		}
-		else
+
+#if 0
+		// Notify the new contact state
+		if (wasLockedN == false && c->lockN == true)
 		{
-			// The contact has began.
+			// The contact has begun
-			c->j = bestIndex;
+		}
-			c->n = n;
+
-			c->Fn = 0.0f;
+		if (wasLockedN == true && c->lockN == false)
-			c->Ft1 = 0.0f;
+		{
-			c->Ft2 = 0.0f;
+			// The contact has ended
-			c->lockN = true;
+		}
-			c->lockT1 = false;
+#endif
-			c->lockT2 = false;
+		if (c->lockN == false)
+		{
+			continue;
 		}
 
 #if B3_CLOTH_FRICTION == 1
 
-		// Apply friction impulses
+		// A friction force requires an associated normal force.
+		if (c0.lockN == false)
+		{
+			continue;
+		}
 
-		// Relative velocity
+		b3Shape* s = m_shapes[c->j];
-		b3Vec3 dv = m_v[i];
+		b3Vec3 n = c->n;
+		float32 friction = s->GetFriction();
+		float32 normalForce = c0.Fn;
 
-		b3MakeTangents(c->t1, c->t2, dv, n);
+		// Tangents
-
+		b3CreateTangents(c->t1, c->t2, dv, n);
-		// Note without a friction force, the tangential acceleration won't be 
-		// removed.
-
-		// Coefficients of friction for the solid
-		const float32 uk = shape->GetFriction();
-		const float32 us = 2.0f * uk;
-
-		float32 dvn = b3Dot(dv, n);
-		float32 normalImpulse = -m_inv_m[i] * dvn;
 
 		b3Vec3 ts[2];
 		ts[0] = c->t1;
 		ts[1] = c->t2;
 
 		bool lockT[2];
+		lockT[0] = c->lockT1;
+		lockT[1] = c->lockT2;
+
+		bool lockT0[2];
+		lockT0[0] = c0.lockT1;
+		lockT0[1] = c0.lockT2;
+
+		float32 Ft0[2];
+		Ft0[0] = c0.Ft1;
+		Ft0[1] = c0.Ft2;
 
 		for (u32 k = 0; k < 2; ++k)
 		{
 			b3Vec3 t = ts[k];
 
+			// Relative tangential velocity
 			float32 dvt = b3Dot(dv, t);
-			float32 tangentImpulse = -m_inv_m[i] * dvt;
 
-			float32 maxStaticImpulse = us * normalImpulse;
+			if (dvt * dvt <= B3_EPSILON * B3_EPSILON)
-			if (tangentImpulse * tangentImpulse > maxStaticImpulse * maxStaticImpulse)
-			{
-				lockT[k] = false;
-
-				// Dynamic friction
-				float32 maxDynamicImpulse = uk * normalImpulse;
-				if (tangentImpulse * tangentImpulse > maxDynamicImpulse * maxDynamicImpulse)
-				{
-					b3Vec3 P = tangentImpulse * t;
-
-					m_v[i] += m_m[i] * P;
-				}
-			}
-			else
 			{
+				// Lock mass on surface
 				lockT[k] = true;
+			}
 
-				// Static friction
+			if (lockT0[k] == true && lockT[k] == true)
-				b3Vec3 P = tangentImpulse * t;
+			{
+				// The contact persists
+				float32 maxForce = friction * normalForce;
 				
-				m_v[i] += m_m[i] * P;
+				if (Ft0[k] * Ft0[k] > maxForce * maxForce)
+				{
+					// Unlock mass off surface
+					lockT[k] = false;
+				}
 			}
 		}
 
 		c->lockT1 = lockT[0];
 		c->lockT2 = lockT[1];
 
-#endif // #if B3_CLOTH_FRICTION
+#endif
 
 	}
+
 }
 
 void b3SpringCloth::Step(float32 dt)
@@ -532,7 +539,7 @@ void b3SpringCloth::Step(float32 dt)
 	// Update contacts
 	UpdateContacts();
 
-	// Apply gravity forces
+	// Apply weights
 	for (u32 i = 0; i < m_massCount; ++i)
 	{
 		if (m_types[i] == b3MassType::e_dynamicMass)
@@ -560,14 +567,19 @@ void b3SpringCloth::Step(float32 dt)
 	// Store constraint forces for physics logic
 	for (u32 i = 0; i < m_massCount; ++i)
 	{
-		b3Vec3 force = forces[i];
-
 		b3MassContact* c = m_contacts + i;
 
+		if (c->lockN == false)
+		{
+			continue;
+		}
+
+		b3Vec3 force = forces[i];
+
 		// Signed normal force magnitude
 		c->Fn = b3Dot(force, c->n);
 
-		// Signed tangent forces magnitude
+		// Signed tangent force magnitude
 		c->Ft1 = b3Dot(force, c->t1);
 		c->Ft2 = b3Dot(force, c->t2);
 	}
@@ -599,21 +611,7 @@ void b3SpringCloth::Draw() const
 
 	for (u32 i = 0; i < m->vertexCount; ++i)
 	{
-		if (m_contacts[i].lockN)
+		b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_green);
-		{
-			if (m_contacts[i].Fn < B3_FORCE_THRESHOLD)
-			{
-				b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_yellow);
-			}
-			else
-			{
-				b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_red);
-			}
-		}
-		else
-		{
-			b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_green);
-		}
 	}
 
 	for (u32 i = 0; i < m->triangleCount; ++i)

src/bounce/dynamics/cloth/spring_solver.cpp

@@ -20,6 +20,7 @@
 #include <bounce/dynamics/cloth/spring_cloth.h>
 #include <bounce/dynamics/cloth/dense_vec3.h>
 #include <bounce/dynamics/cloth/sparse_mat33.h>
+#include <bounce/dynamics/shapes/shape.h>
 #include <bounce/common/memory/stack_allocator.h>
 
 // Here, we solve Ax = b using the Modified Conjugate Gradient method.
@@ -53,6 +54,8 @@ b3SpringSolver::b3SpringSolver(const b3SpringSolverDef& def)
 
 	m_springs = m_cloth->m_springs;
 	m_springCount = m_cloth->m_springCount;
+
+	m_shapes = m_cloth->m_shapes;
 }
 
 b3SpringSolver::~b3SpringSolver()
@@ -66,7 +69,7 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
 	m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33));
 	m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33));
 
-	// Apply spring forces. Also, store their unique derivatives.
+	// Apply internal forces. Also, store their unique derivatives.
 	ApplySpringForces();
 
 	// Integrate
@@ -87,19 +90,26 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
 		//
 		b3DenseVec3 b(m_massCount);
 
-		//
+		// A, b
 		Compute_A_b(A, b);
 
 		// x
 		b3DenseVec3 x(m_massCount);
 
+		// x0
+		Compute_x0(x);
+
+		// S
+		b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33));
+		Compute_S(S);
+
 		if (b3_enablePrecontitioning)
 		{
-			Solve_MPCG(x, f, m_iterations, A, b);
+			Solve_MPCG(x, f, m_iterations, A, b, S);
 		}
 		else
 		{
-			Solve_MCG(x, f, m_iterations, A, b);
+			Solve_MCG(x, f, m_iterations, A, b, S);
 		}
 
 		// Update state
@@ -110,6 +120,8 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
 			// dx = h * (v0 + dv) + y = h * v1 + y
 			m_x[i] += m_h * m_v[i] + m_y[i];
 		}
+
+		m_allocator->Free(S);
 	}
 
 	m_allocator->Free(rowPtrs);
@@ -176,7 +188,7 @@ void b3SpringSolver::ApplySpringForces()
 	b3SetZero_Jacobian(m_Jx, m_springCount);
 	b3SetZero_Jacobian(m_Jv, m_springCount);
 
-	// Compute forces and Jacobians
+	// Compute spring forces and Jacobians
 	for (u32 i = 0; i < m_springCount; ++i)
 	{
 		b3Spring* S = m_springs + i;
@@ -234,9 +246,9 @@ void b3SpringSolver::ApplySpringForces()
 
 static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
 {
-	bool isZeroX = b3Dot(A.x, A.x) <= B3_EPSILON * B3_EPSILON;
+	bool isZeroX = b3Dot(A.x, A.x) == 0.0f;
-	bool isZeroY = b3Dot(A.y, A.y) <= B3_EPSILON * B3_EPSILON;
+	bool isZeroY = b3Dot(A.y, A.y) == 0.0f;
-	bool isZeroZ = b3Dot(A.z, A.z) <= B3_EPSILON * B3_EPSILON;
+	bool isZeroZ = b3Dot(A.z, A.z) == 0.0f;
 
 	return isZeroX * isZeroY * isZeroZ;
 }
@@ -310,7 +322,7 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
 		{
 			++nzCount;
 		}
-}
+	}
 #endif
 
 	SA.row_ptrs[0] = 0;
@@ -344,7 +356,7 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
 	m_allocator->Free(A);
 
 	// Compute b
-	// b = h * (f0 + h * Jx_v + Jx_y )
+	// b = h * (f0 + h * Jx_v + Jx_y)
 
 	// Jx_v = dfdx * v
 	b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3));
@@ -366,20 +378,16 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
 	m_allocator->Free(dfdx);
 }
 
-// This outputs the desired acceleration of the masses in the constrained 
+void b3SpringSolver::Compute_x0(b3DenseVec3& x0)
-// directions.
-static void b3Compute_z(b3DenseVec3& out,
-	u32 massCount, const b3MassType* types, const b3MassContact* contacts)
 {
-	out.SetZero();
+	x0.SetZero();
 }
 
-// 
+void b3SpringSolver::Compute_S(b3Mat33* out)
-static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, const b3MassContact* contacts)
 {
-	for (u32 i = 0; i < massCount; ++i)
+	for (u32 i = 0; i < m_massCount; ++i)
 	{
-		switch (types[i])
+		switch (m_types[i])
 		{
 		case b3MassType::e_staticMass:
 		{
@@ -388,24 +396,31 @@ static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, co
 		}
 		case b3MassType::e_dynamicMass:
 		{
-			if (contacts[i].lockN == true)
+			if (m_contacts[i].lockN == true)
 			{
-				b3Vec3 n = contacts[i].n;
+				b3Vec3 n = m_contacts[i].n;
 
 				b3Mat33 S = b3Mat33_identity - b3Outer(n, n);
 
-				if (contacts[i].lockT1 == true)
+				if (m_contacts[i].lockT1 == true && m_contacts[i].lockT2 == true)
 				{
-					b3Vec3 t1 = contacts[i].t1;
+					S.SetZero();
-
-					S -= b3Outer(t1, t1);
 				}
-
+				else
-				if (contacts[i].lockT2 == true)
 				{
-					b3Vec3 t2 = contacts[i].t2;
+					if (m_contacts[i].lockT1 == true)
+					{
+						b3Vec3 t1 = m_contacts[i].t2;
 						
-					S -= b3Outer(t2, t2);
+						S -= b3Outer(t1, t1);
+					}
+
+					if (m_contacts[i].lockT2 == true)
+					{
+						b3Vec3 t2 = m_contacts[i].t2;
+						
+						S -= b3Outer(t2, t2);
+					}
 				}
 
 				out[i] = S;
@@ -434,15 +449,8 @@ static void b3Filter(b3DenseVec3& out,
 	}
 }
 
-void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const
+void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const
 {
-	//
-	b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33));
-	b3Compute_S(S, m_massCount, m_types, m_contacts);
-
-	// dv = z
-	b3Compute_z(dv, m_massCount, m_types, m_contacts);
-
 	// r = filter(b - Adv)
 	b3DenseVec3 r = b - A * dv;
 	b3Filter(r, r, S, m_massCount);
@@ -494,8 +502,6 @@ void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations,
 		++iter;
 	}
 
-	m_allocator->Free(S);
-
 	iterations = iter;
 
 	// Residual error
@@ -522,15 +528,8 @@ static bool b3IsPD(const b3Mat33* diagA, u32 n)
 	return true;
 }
 
-void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const
+void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const
 {
-	// S
-	b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33));
-	b3Compute_S(S, m_massCount, m_types, m_contacts);
-
-	// dv = z
-	b3Compute_z(dv, m_massCount, m_types, m_contacts);
-
 	// P = diag(A)^-1
 	b3DenseVec3 P(m_massCount);
 
@@ -610,6 +609,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
 		b3Filter(q, q, S, m_massCount);
 
 		// alpha = epsNew / dot(c, q)
+		B3_ASSERT(b3IsValid(b3Dot(c, q)));
 		float32 alpha = epsNew / b3Dot(c, q);
 
 		// dv = dv + alpha * c
@@ -629,9 +629,11 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
 
 		// epsOld = epsNew
 		float32 epsOld = epsNew;
+		B3_ASSERT(b3IsValid(epsOld));
 
 		// epsNew = dot(r, s)
 		epsNew = b3Dot(r, s);
+		B3_ASSERT(b3IsValid(epsNew));
 
 		// beta = epsNew / epsOld
 		float32 beta = epsNew / epsOld;
@@ -643,8 +645,6 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
 		++iter;
 	}
 
-	m_allocator->Free(S);
-
 	iterations = iter;
 
 	// Residual error