Caching impulses for b3BodyContact has been added. Also added some code for completeness.

include/bounce/cloth/cloth.h

@@ -183,9 +183,6 @@ private:
 	// Pool of particles
 	b3BlockPool m_particleBlocks;
 
-	// Pool of body contacts
-	b3BlockPool m_bodyContactBlocks;
-
 	// Pool of particle contacts
 	b3BlockPool m_particleContactBlocks;
 
@@ -195,9 +192,6 @@ private:
 	// List of forces
 	b3List2<b3Force> m_forceList;
 
-	// List of particle contacts
-	b3List2<b3BodyContact> m_bodyContactList;
-	
 	// List of particle contacts
 	b3List2<b3ParticleContact> m_particleContactList;
 };

include/bounce/cloth/particle.h

@@ -71,6 +71,16 @@ public:
 	b3Particle* p1;
 	b3Shape* s2;
 
+	// Contact constraint
+	float32 s;
+	b3Vec3 p;
+	b3Vec3 n;
+	float32 fn0;
+	float32 fn;
+	float32 ft1, ft2;
+	bool t1Active;
+	bool t2Active;
+
 	// Contact constraint
 	b3Vec3 localPoint1;
 	b3Vec3 localPoint2;
@@ -80,8 +90,7 @@ public:
 	b3Vec3 t1, t2;
 	b3Vec2 tangentImpulse;
 
-	b3BodyContact* m_prev;
+	bool active;
-	b3BodyContact* m_next;
 };
 
 struct b3BodyContactWorldPoint
@@ -198,7 +207,7 @@ private:
 	// Applied external force
 	b3Vec3 m_force;
 
-	// Applied external translation
+	// Applied translation
 	b3Vec3 m_translation;
 
 	// Mass
@@ -230,6 +239,9 @@ private:
 	// 
 	b3Cloth* m_cloth;
 
+	// 
+	b3BodyContact m_bodyContact;
+
 	// 
 	b3Particle* m_prev;
 

src/bounce/cloth/cloth.cpp

@@ -148,7 +148,6 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
 
 b3Cloth::b3Cloth(const b3ClothDef& def) :
 	m_particleBlocks(sizeof(b3Particle)),
-	m_bodyContactBlocks(sizeof(b3BodyContact)),
 	m_particleContactBlocks(sizeof(b3ParticleContact))
 {
 	B3_ASSERT(def.mesh);
@@ -484,17 +483,6 @@ void b3Cloth::UpdateBodyContacts()
 
 	B3_PROFILE("Cloth Update Body Contacts");
 	
-	// Clear buffer
-	b3BodyContact* c = m_bodyContactList.m_head;
-	while (c)
-	{
-		b3BodyContact* c0 = c;
-		c = c->m_next;
-		m_bodyContactList.Remove(c0);
-		c0->~b3BodyContact();
-		m_bodyContactBlocks.Free(c0);
-	}
-
 	// Create contacts 
 	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
 	{
@@ -539,6 +527,7 @@ void b3Cloth::UpdateBodyContacts()
 
 		if (bestShape == nullptr)
 		{
+			p->m_bodyContact.active = false;
 			continue;
 		}
 
@@ -549,9 +538,22 @@ void b3Cloth::UpdateBodyContacts()
 		b3Vec3 point = bestPoint;
 		b3Vec3 normal = -bestNormal;
 
-		b3BodyContact* c = (b3BodyContact*)m_bodyContactBlocks.Allocate();
+		b3BodyContact* c = &p->m_bodyContact;
+		
+		b3BodyContact c0 = *c;
+
+		c->active = true;
 		c->p1 = p;
 		c->s2 = shape;
+		c->s = separation;
+		c->p = point;
+		c->n = normal;
+		c->fn0 = 0.0f;
+		c->fn = 0.0f;
+		c->ft1 = 0.0f;
+		c->ft2 = 0.0f;
+		c->t1Active = false;
+		c->t2Active = false;
 		c->localPoint1.SetZero();
 		c->localPoint2 = body->GetLocalPoint(point);
 		c->t1 = b3Perp(normal);
@@ -559,7 +561,85 @@ void b3Cloth::UpdateBodyContacts()
 		c->normalImpulse = 0.0f;
 		c->tangentImpulse.SetZero();
 		
-		m_bodyContactList.PushFront(c);
+#if 0
+		// Apply position correction
+		b3Vec3 dx = separation * normal;
+		p->m_translation += dx;
+#endif
+
+		// Update contact state
+		if (c0.active == true)
+		{
+			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->active = false;
+				continue;
+			}
+#endif
+		}
+#if 0
+		// A friction force requires an associated normal force.
+		if (c0.active == false)
+		{
+			continue;
+		}
+		b3Vec3 v1; v1.SetZero();
+		b3Vec3 v2 = p->m_velocity;
+		b3Vec3 dv = v2 - v1;
+
+		const float32 kVelTol = B3_EPSILON * 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
 	}
 }
 
@@ -632,7 +712,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 	solverDef.stack = &m_stackAllocator;
 	solverDef.particleCapacity = m_particleList.m_count;
 	solverDef.forceCapacity = m_forceList.m_count;
-	solverDef.bodyContactCapacity = m_bodyContactList.m_count;
+	solverDef.bodyContactCapacity = m_particleList.m_count;
 	solverDef.particleContactCapacity = m_particleContactList.m_count;
 
 	b3ClothSolver solver(solverDef);
@@ -647,9 +727,12 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 		solver.Add(f);
 	}
 
-	for (b3BodyContact* c = m_bodyContactList.m_head; c; c = c->m_next)
+	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
 	{
-		solver.Add(c);
+		if (p->m_bodyContact.active)
+		{
+			solver.Add(&p->m_bodyContact);
+		}
 	}
 
 	for (b3ParticleContact* c = m_particleContactList.m_head; c; c = c->m_next)
@@ -659,13 +742,6 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 
 	// Solve	
 	solver.Solve(dt, gravity);
-
-	// Clear external applied forces and translations
-	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
-	{
-		p->m_force.SetZero();
-		p->m_translation.SetZero();
-	}
 }
 
 void b3Cloth::UpdateContacts()
@@ -691,6 +767,13 @@ void b3Cloth::Step(float32 dt)
 	{
 		Solve(dt, m_gravity);
 	}
+
+	// Clear external applied forces and translations
+	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
+	{
+		p->m_force.SetZero();
+		p->m_translation.SetZero();
+	}
 }
 
 void b3Cloth::Draw() const

src/bounce/cloth/cloth_solver.cpp

@@ -162,7 +162,7 @@ void b3ClothSolver::ApplyConstraints()
 			ac->z.SetZero();
 		}
 	}
-
+#if 0
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
 	{
 		b3BodyContact* bc = m_bodyContacts[i];
@@ -171,24 +171,33 @@ void b3ClothSolver::ApplyConstraints()
 
 		B3_ASSERT(p1->m_type == e_dynamicParticle);
 
-		b3Transform xf1; 
-		xf1.position = x[p1->m_solverId];
-		xf1.rotation.SetIdentity();
-
-		b3Shape* s2 = bc->s2;
-		b3Body* b2 = s2->GetBody();
-		b3Transform xf2 = b2->GetTransform();
-
-		b3BodyContactWorldPoint bcwp;
-		bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2);
-		
 		b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
 		++m_constraintCount;
 		ac->i1 = p1->m_solverId;
 		ac->ndof = 2;
-		ac->p = bcwp.normal;
+		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)
 	{
@@ -250,25 +259,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 			sx0[i] = p->m_x;
 		}
 
-		for (u32 i = 0; i < m_bodyContactCount; ++i)
-		{
-			b3BodyContact* bc = m_bodyContacts[i];
-
-			b3Particle* p1 = bc->p1;
-			b3Transform xf1;
-			xf1.position = sx[p1->m_solverId];
-			xf1.rotation.SetIdentity();
-
-			b3Shape* s2 = bc->s2;
-			b3Body* b2 = s2->GetBody();
-			b3Transform xf2 = b2->GetTransform();
-
-			b3BodyContactWorldPoint bcwp;
-			bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2);
-
-			sy[p1->m_solverId] += bcwp.separation * bcwp.normal;
-		}
-
 		// Apply internal forces
 		ApplyForces();
 
@@ -296,10 +286,27 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		u32 iterations = 0;
 		Solve(x, iterations, A, b, S, z, sx0);
 		b3_clothSolverIterations = iterations;
+#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];
+
+			b3Vec3 cf = f[i];
+
+			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;
 
+		// Copy state buffers back to the particle
 		for (u32 i = 0; i < m_particleCount; ++i)
 		{
 			b3Particle* p = m_particles[i];
@@ -309,7 +316,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 			p->m_velocity = sv[i];
 		}
 	}
-
+#if 1
 	// Solve constraints
 	b3ClothContactSolverDef contactSolverDef;
 	contactSolverDef.allocator = m_allocator;
@@ -345,6 +352,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	{
 		contactSolver.StoreImpulses();
 	}
+#endif
 
 	// Integrate positions
 	sx = sx + h * sv;
@@ -366,7 +374,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 			}
 		}
 	}
-
+#if 0
 	// Synchronize bodies
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
 	{
@@ -378,7 +386,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

@@ -73,7 +73,6 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
 {
 	m_cloth = cloth;
 	m_type = def.type;
-	
 	m_position = def.position;
 	m_velocity = def.velocity;
 	m_force = def.force;
@@ -85,6 +84,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
 	m_userData = nullptr;
 	m_x.SetZero();
 	m_vertex = ~0;
+	m_bodyContact.active = false;
 }
 
 b3Particle::~b3Particle()