More consistency and pass velocity and position iterations to cloth constraint solver

examples/testbed/tests/cloth_capsule.h

@@ -86,7 +86,7 @@ public:
 	{
 		Test::Step();
 
-		m_cloth->Step(g_testSettings->inv_hertz);
+		m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
 
 		m_cloth->Draw();
 

examples/testbed/tests/pinned_cloth.h

@@ -84,7 +84,7 @@ public:
 	{
 		Test::Step();
 
-		m_cloth->Step(g_testSettings->inv_hertz);
+		m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
 
 		m_cloth->Draw();
 

examples/testbed/tests/table_cloth.h

@@ -88,7 +88,7 @@ public:
 	{
 		Test::Step();
 
-		m_cloth->Step(g_testSettings->inv_hertz);
+		m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
 
 		m_cloth->Draw();
 

examples/testbed/tests/tension_mapping.h

@@ -110,7 +110,7 @@ public:
 	{
 		Test::Step();
 
-		m_cloth->Step(g_testSettings->inv_hertz);
+		m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
 
 		const b3ClothMesh* mesh = m_cloth->GetMesh();
 

include/bounce/cloth/cloth.h

@@ -29,8 +29,7 @@ class b3Shape;
 
 class b3Particle;
 class b3Force;
-class b3BodyContact;
+struct b3ParticleBodyContact;
-class b3ParticleContact;
 
 struct b3ParticleDef;
 struct b3ForceDef;
@@ -134,7 +133,7 @@ public:
 	float32 GetEnergy() const;
 
 	// Perform a time step. 
-	void Step(float32 dt);
+	void Step(float32 dt, u32 velocityIterations, u32 positionIterations);
 
 	// Debug draw the cloth using the associated cloth mesh.
 	void Draw() const;
@@ -149,7 +148,7 @@ private:
 	void UpdateContacts();
 
 	// Solve
-	void Solve(float32 dt, const b3Vec3& gravity);
+	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
 
 	// Stack allocator
 	b3StackAllocator m_stackAllocator;

include/bounce/cloth/cloth_contact_solver.h

@@ -27,7 +27,7 @@ class b3StackAllocator;
 class b3Particle;
 class b3Body;
 
-class b3BodyContact;
+struct b3ParticleBodyContact;
 
 struct b3DenseVec3;
 
@@ -88,7 +88,7 @@ struct b3ClothContactSolverDef
 	b3DenseVec3* velocities;
 	
 	u32 bodyContactCount;
-	b3BodyContact** bodyContacts;
+	b3ParticleBodyContact** bodyContacts;
 };
 
 inline float32 b3MixFriction(float32 u1, float32 u2)
@@ -118,7 +118,7 @@ protected:
 	b3DenseVec3* m_velocities;
 
 	u32 m_bodyContactCount;
-	b3BodyContact** m_bodyContacts;
+	b3ParticleBodyContact** m_bodyContacts;
 	b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
 	b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints;
 };

include/bounce/cloth/cloth_solver.h

@@ -33,8 +33,7 @@ struct b3DiagMat33;
 struct b3SparseSymMat33;
 struct b3SparseSymMat33View;
 
-class b3BodyContact;
+struct b3ParticleBodyContact;
-class b3ParticleContact;
 
 struct b3ClothSolverDef
 {
@@ -75,10 +74,9 @@ public:
 	
 	void Add(b3Particle* p);
 	void Add(b3Force* f);
-	void Add(b3BodyContact* c);
+	void Add(b3ParticleBodyContact* c);
-	void Add(b3ParticleContact* c);
 
-	void Solve(float32 dt, const b3Vec3& gravity);
+	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
 private:
 	// Apply forces.
 	void ApplyForces();
@@ -107,11 +105,7 @@ private:
 	
 	u32 m_bodyContactCapacity;
 	u32 m_bodyContactCount;
-	b3BodyContact** m_bodyContacts;
+	b3ParticleBodyContact** m_bodyContacts;
-
-	u32 m_particleContactCapacity;
-	u32 m_particleContactCount;
-	b3ParticleContact** m_particleContacts;
 
 	b3ClothSolverData m_solverData;
 };

include/bounce/cloth/particle.h

@@ -62,27 +62,13 @@ struct b3ParticleDef
 };
 
 // A contact between a particle and a solid
-class b3BodyContact
+struct b3ParticleBodyContact
 {
-public:
-	b3BodyContact() { }
-	~b3BodyContact() { }
-
 	b3Particle* p1;
 	b3Shape* s2;
 
 	// Contact constraint
-	float32 s;
+	b3Vec3 normal1;
-	b3Vec3 p;
-	b3Vec3 n;
-	float32 fn0;
-	float32 fn;
-	float32 ft1, ft2;
-	bool nActive;
-	bool t1Active;
-	bool t2Active;
-
-	// Contact constraint
 	b3Vec3 localPoint1;
 	b3Vec3 localPoint2;
 	float32 normalImpulse;
@@ -94,9 +80,9 @@ public:
 	bool active;
 };
 
-struct b3BodyContactWorldPoint
+struct b3ParticleBodyContactWorldPoint
 {
-	void Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB);
+	void Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB);
 
 	b3Vec3 point;
 	b3Vec3 normal;
@@ -210,8 +196,8 @@ private:
 	// 
 	b3Cloth* m_cloth;
 
-	// 
+	// Contact
-	b3BodyContact m_bodyContact;
+	b3ParticleBodyContact m_bodyContact;
 
 	// 
 	b3Particle* m_prev;

src/bounce/cloth/cloth.cpp

@@ -464,23 +464,14 @@ void b3Cloth::UpdateBodyContacts()
 		b3Vec3 point = bestPoint;
 		b3Vec3 normal = -bestNormal;
 
-		b3BodyContact* c = &p->m_bodyContact;
+		b3ParticleBodyContact* c = &p->m_bodyContact;
 		
-		b3BodyContact c0 = *c;
+		b3ParticleBodyContact c0 = *c;
 
 		c->active = true;
 		c->p1 = p;
 		c->s2 = shape;
-		c->s = separation;
+		c->normal1 = normal;
-		c->p = point;
-		c->n = normal;
-		c->fn0 = 0.0f;
-		c->fn = 0.0f;
-		c->ft1 = 0.0f;
-		c->ft2 = 0.0f;
-		c->nActive = true;
-		c->t1Active = false;
-		c->t2Active = false;
 		c->localPoint1.SetZero();
 		c->localPoint2 = body->GetLocalPoint(point);
 		c->t1 = b3Perp(normal);
@@ -488,97 +479,15 @@ void b3Cloth::UpdateBodyContacts()
 		c->normalImpulse = 0.0f;
 		c->tangentImpulse.SetZero();
 
-#if 0
-		// Apply position correction
-		p->m_translation += separation * normal;
-#endif
-
-		// Update contact state
 		if (c0.active == true)
 		{
-			if (c0.nActive == true)
+			c->normalImpulse = c0.normalImpulse;
-			{
+			c->tangentImpulse = c0.tangentImpulse;
-				c->fn0 = c0.fn0;
-				c->fn = c0.fn;
-				c->ft1 = c0.ft1;
-				c->ft2 = c0.ft2;
-
-				c->normalImpulse = c0.normalImpulse;
-				c->tangentImpulse = c0.tangentImpulse;
-#if 0
-				const float32 kForceTol = 0.0f;
-
-				// Allow the contact to release when the constraint force 
-				// switches from a repulsive force to an attractive one.
-				// if (c0.fn0 < kForceTol && c0.fn > kForceTol)
-				if (c0.fn > kForceTol)
-				{
-					// Contact force is attractive.
-					c->nActive = false;
-					continue;
-				}
-#endif
-			}
 		}
-#if 0
-		if (c0.active == false)
-		{
-			continue;
-		}
-		
-		// A friction force requires an associated normal force.
-		if (c0.nActive == false)
-		{
-			continue;
-		}
-		
-		b3Vec3 v1; v1.SetZero();
-		b3Vec3 v2 = p->m_velocity;
-		b3Vec3 dv = v2 - v1;
-
-		const float32 kVelTol = 2.0f * B3_EPSILON;
-
-		// Lock particle on surface
-		float32 dvt1 = b3Dot(dv, c->t1);
-		if (dvt1 * dvt1 < kVelTol * kVelTol)
-		{
-			c->t1Active = true;
-		}
-
-		float32 dvt2 = b3Dot(dv, c->t2);
-		if (dvt2 * dvt2 < kVelTol * kVelTol)
-		{
-			c->t2Active = true;
-		}
-
-		// Unlock particle off surface
-		float32 normalForce = c->fn;
-		
-		float32 friction = shape->GetFriction();
-		float32 maxFrictionForce = friction * normalForce;
-
-		if (c0.t1Active == true)
-		{
-			float32 tangentForce1 = c0.ft1;
-			if (tangentForce1 * tangentForce1 > maxFrictionForce * maxFrictionForce)
-			{
-				c->t1Active = false;
-			}
-		}
-
-		if (c0.t2Active == true)
-		{
-			float32 tangentForce2 = c0.ft2;
-			if (tangentForce2 * tangentForce2 > maxFrictionForce* maxFrictionForce)
-			{
-				c->t2Active = false;
-			}
-		}
-#endif
 	}
 }
 
-void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
+void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations)
 {
 	B3_PROFILE("Cloth Solve");
 
@@ -610,7 +519,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 	}
 
 	// Solve	
-	solver.Solve(dt, gravity);
+	solver.Solve(dt, gravity, velocityIterations, positionIterations);
 }
 
 void b3Cloth::UpdateContacts()
@@ -619,7 +528,7 @@ void b3Cloth::UpdateContacts()
 	UpdateBodyContacts();
 }
 
-void b3Cloth::Step(float32 dt)
+void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
 {
 	B3_PROFILE("Cloth Step");
 
@@ -629,7 +538,7 @@ void b3Cloth::Step(float32 dt)
 	// Solve constraints, integrate state, clear forces and translations. 
 	if (dt > 0.0f)
 	{
-		Solve(dt, m_gravity);
+		Solve(dt, m_gravity, velocityIterations, positionIterations);
 	}
 
 	// Clear external applied forces and translations

src/bounce/cloth/cloth_contact_solver.cpp

@@ -49,7 +49,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
 
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
 	{
-		b3BodyContact* c = m_bodyContacts[i];
+		b3ParticleBodyContact* c = m_bodyContacts[i];
 		b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
 		b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
 
@@ -79,14 +79,14 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
 		pc->localCenterA.SetZero();
 		pc->localCenterB = pc->bodyB->m_sweep.localCenter;
 
-		pc->normalA = c->n;
+		pc->normalA = c->normal1;
 		pc->localPointA = c->localPoint1;
 		pc->localPointB = c->localPoint2;
 	}
 
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
 	{
-		b3BodyContact* c = m_bodyContacts[i];
+		b3ParticleBodyContact* c = m_bodyContacts[i];
 		b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
 		b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
 
@@ -116,7 +116,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
 		xfB.rotation = b3QuatMat33(qB);
 		xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
 
-		b3BodyContactWorldPoint wp;
+		b3ParticleBodyContactWorldPoint wp;
 		wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB);
 
 		vc->normal = wp.normal;
@@ -310,7 +310,7 @@ void b3ClothContactSolver::StoreImpulses()
 {
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
 	{
-		b3BodyContact* c = m_bodyContacts[i];
+		b3ParticleBodyContact* c = m_bodyContacts[i];
 		b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
 
 		c->normalImpulse = vc->normalImpulse;
@@ -322,14 +322,14 @@ struct b3ClothSolverBodyContactSolverPoint
 {
 	void Initialize(const b3ClothSolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB)
 	{
+		b3Vec3 nA = pc->normalA;
+
 		b3Vec3 cA = b3Mul(xfA, pc->localPointA);
 		b3Vec3 cB = b3Mul(xfB, pc->localPointB);
 
 		float32 rA = pc->radiusA;
 		float32 rB = pc->radiusB;
 		
-		b3Vec3 nA = pc->normalA;
-
 		b3Vec3 pA = cA + rA * nA;
 		b3Vec3 pB = cB - rB * nA;
 

src/bounce/cloth/cloth_solver.cpp

@@ -57,7 +57,7 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 
 	m_bodyContactCapacity = def.bodyContactCapacity;
 	m_bodyContactCount = 0;
-	m_bodyContacts = (b3BodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3BodyContact*));;
+	m_bodyContacts = (b3ParticleBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ParticleBodyContact*));;
 }
 
 b3ClothSolver::~b3ClothSolver()
@@ -80,16 +80,11 @@ void b3ClothSolver::Add(b3Force* f)
 	m_forces[m_forceCount++] = f;
 }
 
-void b3ClothSolver::Add(b3BodyContact* c)
+void b3ClothSolver::Add(b3ParticleBodyContact* c)
 {
 	m_bodyContacts[m_bodyContactCount++] = c;
 }
 
-void b3ClothSolver::Add(b3ParticleContact* c)
-{
-	m_particleContacts[m_particleContactCount++] = c;
-}
-
 void b3ClothSolver::ApplyForces()
 {
 	for (u32 i = 0; i < m_forceCount; ++i)
@@ -149,6 +144,7 @@ void b3ClothSolver::ApplyConstraints()
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
 		b3Particle* p = m_particles[i];
+		
 		if (p->m_type != e_dynamicParticle)
 		{
 			b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
@@ -158,47 +154,6 @@ void b3ClothSolver::ApplyConstraints()
 			ac->z.SetZero();
 		}
 	}
-#if 0
-	for (u32 i = 0; i < m_bodyContactCount; ++i)
-	{
-		b3BodyContact* bc = m_bodyContacts[i];
-		
-		if (bc->nActive == false)
-		{
-			continue;
-		}
-
-		b3Particle* p1 = bc->p1;
-
-		B3_ASSERT(p1->m_type == e_dynamicParticle);
-
-		b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
-		++m_constraintCount;
-		ac->i1 = p1->m_solverId;
-		ac->ndof = 2;
-		ac->p = bc->n;
-		ac->z.SetZero();
-
-		if (bc->t1Active && bc->t2Active)
-		{
-			ac->ndof = 0;
-		}
-		else
-		{
-			if (bc->t1Active)
-			{
-				ac->ndof = 1;
-				ac->q = bc->t1;
-			}
-
-			if (bc->t2Active)
-			{
-				ac->ndof = 1;
-				ac->q = bc->t2;
-			}
-		}
-	}
-#endif
 
 	for (u32 i = 0; i < m_constraintCount; ++i)
 	{
@@ -206,7 +161,7 @@ void b3ClothSolver::ApplyConstraints()
 	}
 }
 
-void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
+void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations)
 {
 	float32 h = dt;
 	float32 inv_h = 1.0f / h;
@@ -288,23 +243,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		// x
 		b3DenseVec3 x(m_particleCount);
 		SolveMPCG(x, viewA, b, S, z, sx0);
-#if 0
-		// Copy constraint forces to the contacts
-		b3DenseVec3 f = A * x - b;
 
-		for (u32 i = 0; i < m_bodyContactCount; ++i)
-		{
-			b3BodyContact* c = m_bodyContacts[i];
-			b3Particle* p1 = c->p1;
-
-			b3Vec3 cf = f[p1->m_solverId];
-
-			c->fn0 = c->fn;
-			c->fn = b3Dot(cf, c->n);
-			c->ft1 = b3Dot(cf, c->t1);
-			c->ft2 = b3Dot(cf, c->t2);
-		}
-#endif
 		// 
 		sv = sv + x;
 		sx = sx + sy;
@@ -319,7 +258,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 			p->m_velocity = sv[i];
 		}
 	}
-#if 1
+
 	// Solve constraints
 	b3ClothContactSolverDef contactSolverDef;
 	contactSolverDef.allocator = m_allocator;
@@ -338,11 +277,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		contactSolver.WarmStart();
 	}
 
-	// Solve velocity constraints
 	{
-		const u32 kVelocityIterations = 10;
+		for (u32 i = 0; i < velocityIterations; ++i)
-
-		for (u32 i = 0; i < kVelocityIterations; ++i)
 		{
 			contactSolver.SolveBodyContactVelocityConstraints();
 		}
@@ -351,18 +287,14 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	{
 		contactSolver.StoreImpulses();
 	}
-#endif
 
 	// Integrate positions
 	sx = sx + h * sv;
 
-#if 1
 	// Solve position constraints
 	{
-		const u32 kPositionIterations = 2;
-
 		bool positionSolved = false;
-		for (u32 i = 0; i < kPositionIterations; ++i)
+		for (u32 i = 0; i < positionIterations; ++i)
 		{
 			bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
 
@@ -385,7 +317,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 
 		body->SynchronizeShapes();
 	}
-#endif
+
 	// Copy state buffers back to the particles
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{

src/bounce/cloth/particle.cpp

@@ -22,13 +22,13 @@
 #include <bounce/cloth/dense_vec3.h>
 #include <bounce/cloth/sparse_sym_mat33.h>
 
-void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
+void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
 {
+	b3Vec3 nA = c->normal1;
+
 	b3Vec3 cA = b3Mul(xfA, c->localPoint1);
 	b3Vec3 cB = b3Mul(xfB, c->localPoint2);
 
-	b3Vec3 nA = c->n;
-	
 	b3Vec3 pA = cA + rA * nA;
 	b3Vec3 pB = cB - rB * nA;