bugfix, particle friction, test code

examples/testbed/tests/self_collision.h

@@ -30,9 +30,10 @@ public:
 		// Create 3D mesh
 		m_rectangleClothMesh.Set(&m_rectangleGarmentMesh);
 
-		//  
+		b3Mat33 Rx = b3Mat33RotationX(0.5f * B3_PI);
 		for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i)
 		{
+			m_rectangleClothMesh.vertices[i] = Rx * m_rectangleClothMesh.vertices[i];
 			m_rectangleClothMesh.vertices[i].y += 10.0f;
 		}
 
@@ -45,7 +46,8 @@ public:
 
 		for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
 		{
-			p->SetRadius(0.25f);
+			p->SetRadius(0.2f);
+			p->SetFriction(0.2f);
 		}
 
 		{
@@ -54,14 +56,13 @@ public:
 
 			b3Body* b = m_world.CreateBody(bd);
 
-			m_boxHull.Set(50.0f, 1.0f, 50.0f);
-
-			b3HullShape boxShape;
-			boxShape.m_hull = &m_boxHull;
-			boxShape.m_radius = 0.2f;
+			b3CapsuleShape capsuleShape;
+			capsuleShape.m_centers[0].Set(0.0f, 5.0f, -5.0f);
+			capsuleShape.m_centers[1].Set(0.0f, 5.0f, 5.0f);
+			capsuleShape.m_radius = 1.0f;;
 
 			b3ShapeDef sd;
-			sd.shape = &boxShape;
+			sd.shape = &capsuleShape;
 			sd.friction = 1.0f;
 
 			b->CreateShape(sd);

include/bounce/dynamics/cloth/cloth.h

@@ -30,6 +30,7 @@ class b3Particle;
 class b3Force;
 class b3BodyContact;
 class b3ParticleContact;
+class b3TriangleContact;
 
 struct b3ParticleDef;
 struct b3ForceDef;
@@ -151,6 +152,9 @@ private:
 	// Update particle contacts. 
 	void UpdateParticleContacts();
 	
+	// Update triangle contacts. 
+	void UpdateTriangleContacts();
+	
 	// Solve
 	void Solve(float32 dt, const b3Vec3& gravity);
 
@@ -169,6 +173,9 @@ private:
 	// Pool of particle contacts
 	b3BlockPool m_particleContactBlocks;
 	
+	// Pool of triangle contacts
+	b3BlockPool m_triangleContactBlocks;
+	
 	// List of particles
 	b3List2<b3Particle> m_particleList;
 	
@@ -178,6 +185,9 @@ private:
 	// List of particle contacts
 	b3List2<b3ParticleContact> m_particleContactList;
 	
+	// List of triangle contacts
+	b3List2<b3TriangleContact> m_triangleContactList;
+	
 	// The parent world of this cloth.
 	b3World* m_world;
 	

include/bounce/dynamics/cloth/cloth_solver.h

@@ -29,6 +29,7 @@ class b3Body;
 class b3Force;
 class b3BodyContact;
 class b3ParticleContact;
+class b3TriangleContact;
 
 struct b3DenseVec3;
 struct b3DiagMat33;
@@ -41,6 +42,7 @@ struct b3ClothSolverDef
 	u32 forceCapacity;
 	u32 bodyContactCapacity;
 	u32 particleContactCapacity;
+	u32 triangleContactCapacity;
 };
 
 struct b3ClothSolverData
@@ -149,6 +151,44 @@ struct b3ClothSolverParticleContactPositionConstraint
 	float32 radiusB;
 };
 
+struct b3ClothSolverTriangleContactVelocityConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+
+	u32 indexB;
+	float32 invMassB;
+	u32 indexC;
+	float32 invMassC;
+	u32 indexD;
+	float32 invMassD;
+
+	b3Vec3 JA;
+	b3Vec3 JB;
+	b3Vec3 JC;
+	b3Vec3 JD;
+	
+	float32 normalMass;
+	float32 normalImpulse;
+};
+
+struct b3ClothSolverTriangleContactPositionConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	float32 radiusA;
+
+	u32 indexB;
+	float32 invMassB;
+	u32 indexC;
+	float32 invMassC;
+	u32 indexD;
+	float32 invMassD;
+	float32 triangleRadius;
+	
+	bool front;
+};
+
 class b3ClothSolver
 {
 public:
@@ -159,6 +199,7 @@ public:
 	void Add(b3Force* f);
 	void Add(b3BodyContact* c);
 	void Add(b3ParticleContact* c);
+	void Add(b3TriangleContact* c);
 
 	void Solve(float32 dt, const b3Vec3& gravity);
 private:
@@ -177,6 +218,9 @@ private:
 	// 
 	void InitializeParticleContactConstraints();
 	
+	// 
+	void InitializeTriangleContactConstraints();
+	
 	//
 	void WarmStart();
 
@@ -186,6 +230,9 @@ private:
 	// 
 	void SolveParticleContactVelocityConstraints();
 	
+	// 
+	void SolveTriangleContactVelocityConstraints();
+	
 	// 
 	void StoreImpulses();
 
@@ -195,6 +242,9 @@ private:
 	// 
 	bool SolveParticleContactPositionConstraints();
 	
+	// 
+	bool SolveTriangleContactPositionConstraints();
+	
 	b3StackAllocator* m_allocator;
 
 	u32 m_particleCapacity;
@@ -221,6 +271,12 @@ private:
 	b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints;
 	b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints;
 
+	u32 m_triangleContactCapacity;
+	u32 m_triangleContactCount;
+	b3TriangleContact** m_triangleContacts;
+	b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints;
+	b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints;
+	
 	b3ClothSolverData m_solverData;
 };
 

include/bounce/dynamics/cloth/particle.h

@@ -48,6 +48,7 @@ struct b3ParticleDef
 		velocity.SetZero();
 		force.SetZero();
 		radius = 0.0f;
+		friction = 0.0f;
 		userData = nullptr;
 	}
 
@@ -56,21 +57,10 @@ struct b3ParticleDef
 	b3Vec3 velocity;
 	b3Vec3 force;
 	float32 radius;
+	float32 friction;
 	void* userData;
 };
 
-//
-class b3FrictionForce : public b3Force
-{
-public:
-	b3FrictionForce() { }
-	~b3FrictionForce() { }
-
-	void Apply(const b3ClothSolverData* data);
-
-	b3Particle* m_p;
-};
-
 // A contact between a particle and a solid
 class b3BodyContact
 {
@@ -134,6 +124,27 @@ struct b3ParticleContactWorldPoint
 	float32 separation;
 };
 
+// A contact between a particle and a triangle
+class b3TriangleContact
+{
+public:
+	b3TriangleContact() { }
+	~b3TriangleContact() { }
+
+	b3Particle* p1;	
+	b3Particle* p2;
+	b3Particle* p3;
+	b3Particle* p4;
+
+	bool front;
+	
+	// Contact constraint
+	float32 normalImpulse;
+
+	b3TriangleContact* m_prev;
+	b3TriangleContact* m_next;
+};
+
 // A cloth particle.
 class b3Particle
 {
@@ -170,6 +181,12 @@ public:
 	// Get the particle radius.
 	float32 GetRadius() const;
 
+	// Set the particle coefficient of friction.
+	void SetFriction(float32 friction);
+	
+	// Get the particle coefficient of friction.
+	float32 GetFriction() const;
+	
 	// Apply a force.
 	void ApplyForce(const b3Vec3& force);
 
@@ -214,6 +231,9 @@ private:
 	// Radius
 	float32 m_radius;
 
+	// Coefficient of friction
+	float32 m_friction;
+	
 	// User data. 
 	void* m_userData;
 
@@ -291,6 +311,16 @@ inline float32 b3Particle::GetRadius() const
 	return m_radius;
 }
 
+inline void b3Particle::SetFriction(float32 friction)
+{
+	m_friction = friction;
+}
+
+inline float32 b3Particle::GetFriction() const
+{
+	return m_friction;
+}
+
 inline void b3Particle::ApplyForce(const b3Vec3& force)
 {
 	if (m_type != e_dynamicParticle)

src/bounce/dynamics/cloth/cloth.cpp

@@ -149,9 +149,10 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
 	return sharedCount;
 }
 
-b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : 
+b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) :
 	m_particleBlocks(sizeof(b3Particle)),
-	m_particleContactBlocks(sizeof(b3ParticleContact))
+	m_particleContactBlocks(sizeof(b3ParticleContact)),
+	m_triangleContactBlocks(sizeof(b3TriangleContact))
 {
 	B3_ASSERT(def.mesh);
 	B3_ASSERT(def.density > 0.0f);
@@ -481,8 +482,7 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32
 	float32 v = b3Dot(QC_x_QA, AB_x_AC);
 	float32 w = b3Dot(QA_x_QB, AB_x_AC);
 
-	// This tolerance helps intersections lying on  
-	// shared edges to not be missed.
+	// Characteristic length of triangle
 	const float32 kTol = -B3_EPSILON;
 
 	// Is the intersection on the triangle?
@@ -588,7 +588,7 @@ void b3Cloth::UpdateBodyContacts()
 void b3Cloth::UpdateParticleContacts()
 {
 	B3_PROFILE("Update Particle Contacts");
-	
+
 	// Clear buffer
 	b3ParticleContact* c = m_particleContactList.m_head;
 	while (c)
@@ -624,7 +624,7 @@ void b3Cloth::UpdateParticleContacts()
 			{
 				continue;
 			}
-			
+
 			b3Vec3 n(0.0f, 1.0f, 0.0f);
 			if (dd > B3_EPSILON * B3_EPSILON)
 			{
@@ -637,7 +637,7 @@ void b3Cloth::UpdateParticleContacts()
 			c->p2 = p2;
 			c->normalImpulse = 0.0f;
 			c->t1 = b3Perp(n);
-			c->t2 = b3Cross(c->t1, n); 
+			c->t2 = b3Cross(c->t1, n);
 			c->tangentImpulse.SetZero();
 
 			m_particleContactList.PushFront(c);
@@ -645,6 +645,241 @@ void b3Cloth::UpdateParticleContacts()
 	}
 }
 
+static B3_FORCE_INLINE void b3Barycentric(float32 out[3],
+	const b3Vec3& A, const b3Vec3& B,
+	const b3Vec3& Q)
+{
+	b3Vec3 AB = B - A;
+	b3Vec3 QA = A - Q;
+	b3Vec3 QB = B - Q;
+
+	//float32 divisor = b3Dot(AB, AB);
+
+	out[0] = b3Dot(QB, AB);
+	out[1] = -b3Dot(QA, AB);
+	out[2] = out[0] + out[1];
+}
+
+// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w) 
+// with respect to a triangle ABC.
+// The last output value is the divisor.
+static B3_FORCE_INLINE void b3Barycentric(float32 out[4],
+	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C,
+	const b3Vec3& Q)
+{
+	b3Vec3 AB = B - A;
+	b3Vec3 AC = C - A;
+
+	b3Vec3 QA = A - Q;
+	b3Vec3 QB = B - Q;
+	b3Vec3 QC = C - Q;
+
+	b3Vec3 QB_x_QC = b3Cross(QB, QC);
+	b3Vec3 QC_x_QA = b3Cross(QC, QA);
+	b3Vec3 QA_x_QB = b3Cross(QA, QB);
+
+	b3Vec3 AB_x_AC = b3Cross(AB, AC);
+
+	//float32 divisor = b3Dot(AB_x_AC, AB_x_AC);
+
+	out[0] = b3Dot(QB_x_QC, AB_x_AC);
+	out[1] = b3Dot(QC_x_QA, AB_x_AC);
+	out[2] = b3Dot(QA_x_QB, AB_x_AC);
+	out[3] = out[0] + out[1] + out[2];
+}
+
+static B3_FORCE_INLINE void b3Solve3(float32 out[3],
+	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, 
+	const b3Vec3& Q)
+{
+	// Test vertex regions
+	float32 wAB[3], wBC[3], wCA[3];
+	b3Barycentric(wAB, A, B, Q);
+	b3Barycentric(wBC, B, C, Q);
+	b3Barycentric(wCA, C, A, Q);
+
+	// R A
+	if (wAB[1] <= 0.0f && wCA[0] <= 0.0f)
+	{
+		out[0] = 1.0f;
+		out[1] = 0.0f;
+		out[2] = 0.0f;
+		return;
+	}
+
+	// R B
+	if (wAB[0] <= 0.0f && wBC[1] <= 0.0f)
+	{
+		out[0] = 0.0f;
+		out[1] = 1.0f;
+		out[2] = 0.0f;
+		return;
+	}
+
+	// R C
+	if (wBC[0] <= 0.0f && wCA[1] <= 0.0f)
+	{
+		out[0] = 0.0f;
+		out[1] = 0.0f;
+		out[2] = 1.0f;
+		return;
+	}
+
+	// Test edge regions		
+	float32 wABC[4];
+	b3Barycentric(wABC, A, B, C, Q);
+
+	// This is used to help testing if the face degenerates 
+	// into an edge.
+	float32 area = wABC[3];
+
+	// R AB
+	if (wAB[0] > 0.0f && wAB[1] > 0.0f && area * wABC[2] <= 0.0f)
+	{
+		float32 divisor = wAB[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		out[0] = s * wAB[0];
+		out[1] = s * wAB[1];
+		out[2] = 0.0f;
+		return;
+	}
+
+	// R BC
+	if (wBC[0] > 0.0f && wBC[1] > 0.0f && area * wABC[0] <= 0.0f)
+	{
+		float32 divisor = wBC[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		out[0] = 0.0f;
+		out[1] = s * wBC[0];
+		out[2] = s * wBC[1];
+		return;
+	}
+
+	// R CA
+	if (wCA[0] > 0.0f && wCA[1] > 0.0f && area * wABC[1] <= 0.0f)
+	{
+		float32 divisor = wCA[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		out[0] = 0.0f;
+		out[2] = s * wCA[0];
+		out[1] = s * wCA[1];
+		return;
+	}
+
+	// R ABC/ACB
+	float32 divisor = wABC[3];
+	if (divisor <= 0.0f)
+	{
+		float32 s = 1.0f / 3.0f;
+		out[0] = s;
+		out[2] = s;
+		out[1] = s;
+		return;
+	}
+
+	B3_ASSERT(divisor > 0.0f);
+	float32 s = 1.0f / divisor;
+	out[0] = s * wABC[0];
+	out[1] = s * wABC[1];
+	out[2] = s * wABC[2];
+}
+
+void b3Cloth::UpdateTriangleContacts()
+{
+	B3_PROFILE("Update Triangle Contacts");
+
+	// Clear buffer
+	b3TriangleContact* c = m_triangleContactList.m_head;
+	while (c)
+	{
+		b3TriangleContact* c0 = c;
+		c = c->m_next;
+		m_triangleContactList.Remove(c0);
+		c0->~b3TriangleContact();
+		m_triangleContactBlocks.Free(c0);
+	}
+
+	// Create triangle contacts
+	for (b3Particle* p1 = m_particleList.m_head; p1; p1 = p1->m_next)
+	{
+		for (u32 i = 0; i < m_mesh->triangleCount; ++i)
+		{
+			b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
+
+			b3Particle* p2 = m_vertexParticles[triangle->v1];
+			b3Particle* p3 = m_vertexParticles[triangle->v2];
+			b3Particle* p4 = m_vertexParticles[triangle->v3];
+
+			if (p1 == p2 || p1 == p3 || p1 == p4)
+			{
+				continue;
+			}
+
+			float32 r1 = p1->m_radius;
+			float32 r2 = 0.0f;
+
+			float32 totalRadius = r1 + r2;
+
+			b3Vec3 A = p2->m_position;
+			b3Vec3 B = p3->m_position;
+			b3Vec3 C = p4->m_position;
+			
+			b3Vec3 P = p1->m_position;
+
+			b3Vec3 n = b3Cross(B - A, C - A);
+			float32 n_len = n.Normalize();
+
+			float32 distance = b3Dot(n, P - A);
+
+			if (distance < -totalRadius)
+			{
+				continue;
+			}
+
+			if (distance > totalRadius)
+			{
+				continue;
+			}
+
+			// Project particle on triangle plane
+			b3Vec3 Q = P - distance * n;
+			
+			// Barycentric coordinates for Q
+			float32 wABC[3];
+			b3Solve3(wABC, A, B, C, Q);
+
+			b3Vec3 c1 = p1->m_position;
+			b3Vec3 c2 = wABC[0] * A + wABC[1] * B + wABC[2] * C;
+
+			if (b3DistanceSquared(c1, c2) > totalRadius * totalRadius)
+			{
+				continue;
+			}
+
+			bool front = false;
+
+			// Is the the other point in front of the triangle plane? 
+			if (distance >= 0.0f)
+			{
+				front = true;
+			}
+
+			b3TriangleContact* c = (b3TriangleContact*)m_triangleContactBlocks.Allocate();
+			c->p1 = p1;
+			c->p2 = p2;
+			c->p3 = p3;
+			c->p4 = p4;
+			c->front = front;
+			c->normalImpulse = 0.0f;
+
+			m_triangleContactList.PushFront(c);
+		}
+	}
+}
+
 void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 {
 	B3_PROFILE("Solve");
@@ -684,6 +919,11 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 		solver.Add(pc);
 	}
 
+	for (b3TriangleContact* tc = m_triangleContactList.m_head; tc; tc = tc->m_next)
+	{
+		solver.Add(tc);
+	}
+	
 	// Solve	
 	solver.Solve(dt, gravity);
 
@@ -704,6 +944,9 @@ void b3Cloth::Step(float32 dt, const b3Vec3& gravity)
 
 	// Update particle contacts
 	UpdateParticleContacts();
+
+	// Update triangle contacts
+	UpdateTriangleContacts();
 	
 	// Solve constraints, integrate state, clear forces and translations. 
 	if (dt > 0.0f)

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -63,10 +63,20 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 	m_particleContacts = (b3ParticleContact**)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ParticleContact*));
 	m_particleVelocityConstraints = (b3ClothSolverParticleContactVelocityConstraint*)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ClothSolverParticleContactVelocityConstraint));
 	m_particlePositionConstraints = (b3ClothSolverParticleContactPositionConstraint*)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ClothSolverParticleContactPositionConstraint));
+
+	m_triangleContactCapacity = def.triangleContactCapacity;
+	m_triangleContactCount = 0;
+	m_triangleContacts = (b3TriangleContact**)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3TriangleContact*));
+	m_triangleVelocityConstraints = (b3ClothSolverTriangleContactVelocityConstraint*)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3ClothSolverTriangleContactVelocityConstraint));
+	m_trianglePositionConstraints = (b3ClothSolverTriangleContactPositionConstraint*)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3ClothSolverTriangleContactPositionConstraint));
 }
 
 b3ClothSolver::~b3ClothSolver()
 {
+	m_allocator->Free(m_trianglePositionConstraints);
+	m_allocator->Free(m_triangleVelocityConstraints);
+	m_allocator->Free(m_triangleContacts);
+
 	m_allocator->Free(m_particlePositionConstraints);
 	m_allocator->Free(m_particleVelocityConstraints);
 	m_allocator->Free(m_particleContacts);
@@ -101,6 +111,11 @@ void b3ClothSolver::Add(b3ParticleContact* c)
 	m_particleContacts[m_particleContactCount++] = c;
 }
 
+void b3ClothSolver::Add(b3TriangleContact* c)
+{
+	m_triangleContacts[m_triangleContactCount++] = c;
+}
+
 void b3ClothSolver::ApplyForces()
 {
 	for (u32 i = 0; i < m_forceCount; ++i)
@@ -263,6 +278,9 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	// Initialize particle contact constraints
 	InitializeParticleContactConstraints();
 
+	// Initialize triangle contact constraints
+	// InitializeTriangleContactConstraints();
+
 	// Warm start velocity constraints
 	WarmStart();
 
@@ -273,6 +291,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	{
 		SolveBodyContactVelocityConstraints();
 		SolveParticleContactVelocityConstraints();
+		// SolveTriangleContactVelocityConstraints();
 	}
 
 	// Store impulses to improve convergence
@@ -283,11 +302,13 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 
 	// Solve position constraints
 	const u32 kPositionIterations = 2;
-	
+
 	bool positionSolved = false;
 	for (u32 i = 0; i < kPositionIterations; ++i)
 	{
 		bool particleContactsSolved = SolveParticleContactPositionConstraints();
+		// bool triangleContactsSolved = SolveTriangleContactPositionConstraints();
+		//if (particleContactsSolved && triangleContactsSolved)
 		if (particleContactsSolved)
 		{
 			positionSolved = true;
@@ -559,6 +580,12 @@ void b3ClothSolver::InitializeBodyContactConstraints()
 	}
 }
 
+// 
+static B3_FORCE_INLINE float32 b3MixFriction(float32 u1, float32 u2)
+{
+	return b3Sqrt(u1 * u2);
+}
+
 void b3ClothSolver::InitializeParticleContactConstraints()
 {
 	b3DenseVec3& x = *m_solverData.x;
@@ -578,7 +605,7 @@ void b3ClothSolver::InitializeParticleContactConstraints()
 		vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
 		vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
 
-		vc->friction = 1.0f;
+		vc->friction = b3MixFriction(c->p1->m_friction, c->p2->m_friction);
 
 		pc->indexA = c->p1->m_solverId;
 		pc->indexB = c->p2->m_solverId;
@@ -624,7 +651,7 @@ void b3ClothSolver::InitializeParticleContactConstraints()
 			float32 K = mA + mB;
 
 			vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
-			
+
 			vc->velocityBias = 0.0f;
 		}
 
@@ -645,6 +672,93 @@ void b3ClothSolver::InitializeParticleContactConstraints()
 	}
 }
 
+void b3ClothSolver::InitializeTriangleContactConstraints()
+{
+	b3DenseVec3& x = *m_solverData.x;
+
+	for (u32 i = 0; i < m_triangleContactCount; ++i)
+	{
+		b3TriangleContact* c = m_triangleContacts[i];
+		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
+		b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i;
+
+		vc->indexA = c->p1->m_solverId;
+		vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
+
+		vc->indexB = c->p2->m_solverId;
+		vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
+
+		vc->indexC = c->p3->m_solverId;
+		vc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass;
+
+		vc->indexD = c->p4->m_solverId;
+		vc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass;
+
+		pc->indexA = c->p1->m_solverId;
+		pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
+		pc->radiusA = c->p1->m_radius;
+
+		pc->indexB = c->p2->m_solverId;
+		pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
+
+		pc->indexC = c->p3->m_solverId;
+		pc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass;
+
+		pc->indexD = c->p4->m_solverId;
+		pc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass;
+
+		pc->triangleRadius = 0.0f;
+		pc->front = c->front;
+
+		u32 indexA = pc->indexA;
+		float32 mA = pc->invMassA;
+
+		u32 indexB = pc->indexB;
+		float32 mB = pc->invMassB;
+
+		u32 indexC = pc->indexC;
+		float32 mC = pc->invMassC;
+
+		u32 indexD = pc->indexD;
+		float32 mD = pc->invMassD;
+
+		b3Vec3 xA = x[indexA];
+		b3Vec3 xB = x[indexB];
+		b3Vec3 xC = x[indexC];
+		b3Vec3 xD = x[indexD];
+
+		b3Vec3 n = b3Cross(xC - xB, xD - xB);
+
+		if (pc->front == false)
+		{
+			n = -n;
+		}
+
+		float32 n_len = n.Normalize();
+		
+		b3Mat33 I; I.SetIdentity();
+
+		b3Mat33 N = I - b3Outer(n, n);
+		if (n_len > B3_EPSILON)
+		{
+			N = (1.0f / n_len) * N;
+		}
+
+		b3Vec3 N_n = N * n;
+
+		vc->JA = n;
+		vc->JC = b3Cross(xD - xB, N_n);
+		vc->JD = b3Cross(xC - xB, N_n);
+		vc->JB = b3Cross(xC - xD, N_n) - n;
+
+		// Compute effective mass.
+		float32 K = mA + mB + mC + mD;
+
+		vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
+		vc->normalImpulse = c->normalImpulse;
+	}
+}
+
 void b3ClothSolver::WarmStart()
 {
 	b3DenseVec3& v = *m_solverData.v;
@@ -718,6 +832,41 @@ void b3ClothSolver::WarmStart()
 		v[indexA] = vA;
 		v[indexB] = vB;
 	}
+
+	for (u32 i = 0; i < m_triangleContactCount; ++i)
+	{
+		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
+
+		u32 indexA = vc->indexA;
+		u32 indexB = vc->indexB;
+		u32 indexC = vc->indexC;
+		u32 indexD = vc->indexD;
+
+		b3Vec3 vA = v[indexA];
+		b3Vec3 vB = v[indexB];
+		b3Vec3 vC = v[indexC];
+		b3Vec3 vD = v[indexD];
+
+		float32 mA = vc->invMassA;
+		float32 mB = vc->invMassB;
+		float32 mC = vc->invMassC;
+		float32 mD = vc->invMassD;
+
+		b3Vec3 PA = vc->normalImpulse * vc->JA;
+		b3Vec3 PB = vc->normalImpulse * vc->JB;
+		b3Vec3 PC = vc->normalImpulse * vc->JC;
+		b3Vec3 PD = vc->normalImpulse * vc->JD;
+
+		vA += mA * PA;
+		vB += mB * PB;
+		vC += mC * PC;
+		vD += mD * PD;
+
+		v[indexA] = vA;
+		v[indexB] = vB;
+		v[indexC] = vC;
+		v[indexD] = vD;
+	}
 }
 
 void b3ClothSolver::SolveBodyContactVelocityConstraints()
@@ -879,6 +1028,59 @@ void b3ClothSolver::SolveParticleContactVelocityConstraints()
 	}
 }
 
+void b3ClothSolver::SolveTriangleContactVelocityConstraints()
+{
+	b3DenseVec3& v = *m_solverData.v;
+
+	for (u32 i = 0; i < m_triangleContactCount; ++i)
+	{
+		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
+
+		u32 indexA = vc->indexA;
+		float32 mA = vc->invMassA;
+
+		u32 indexB = vc->indexB;
+		float32 mB = vc->invMassB;
+
+		u32 indexC = vc->indexC;
+		float32 mC = vc->invMassC;
+
+		u32 indexD = vc->indexD;
+		float32 mD = vc->invMassD;
+
+		b3Vec3 vA = v[indexA];
+		b3Vec3 vB = v[indexB];
+		b3Vec3 vC = v[indexC];
+		b3Vec3 vD = v[indexD];
+
+		b3Vec3 n = vc->JA;
+
+		// Allow some slop and prevent large corrections.
+		float32 Cdot = b3Dot(n, vA - vB);
+
+		float32 impulse = -vc->normalMass * Cdot;
+		
+		float32 oldImpulse = vc->normalImpulse;
+		vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
+		impulse = vc->normalImpulse - oldImpulse;
+
+		b3Vec3 PA = impulse * vc->JA;
+		b3Vec3 PB = impulse * vc->JB;
+		b3Vec3 PC = impulse * vc->JC;
+		b3Vec3 PD = impulse * vc->JD;
+
+		vA += mA * PA;
+		vB += mB * PB;
+		vC += mC * PC;
+		vD += mD * PD;
+
+		v[indexA] = vA;
+		v[indexB] = vB;
+		v[indexC] = vC;
+		v[indexD] = vD;
+	}
+}
+
 void b3ClothSolver::StoreImpulses()
 {
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
@@ -898,6 +1100,14 @@ void b3ClothSolver::StoreImpulses()
 		c->normalImpulse = vc->normalImpulse;
 		c->tangentImpulse = vc->tangentImpulse;
 	}
+
+	for (u32 i = 0; i < m_triangleContactCount; ++i)
+	{
+		b3TriangleContact* c = m_triangleContacts[i];
+		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
+
+		c->normalImpulse = vc->normalImpulse;
+	}
 }
 
 struct b3ClothSolverContactSolverPoint
@@ -995,14 +1205,8 @@ bool b3ClothSolver::SolveBodyContactPositionConstraints()
 
 struct b3ClothSolverParticleContactSolverPoint
 {
-	void Initialize(const b3Particle* p1, const b3Particle* p2)
+	void Initialize(const b3Vec3& cA, float32 rA, const b3Vec3& cB, float32 rB)
 	{
-		b3Vec3 cA = p1->GetPosition();
-		float32 rA = p1->GetRadius();
-
-		b3Vec3 cB = p2->GetPosition();
-		float32 rB = p2->GetRadius();
-
 		b3Vec3 d = cB - cA;
 		float32 distance = b3Length(d);
 
@@ -1037,15 +1241,17 @@ bool b3ClothSolver::SolveParticleContactPositionConstraints()
 
 		u32 indexA = pc->indexA;
 		float32 mA = pc->invMassA;
+		float32 rA = pc->radiusA;
 
 		u32 indexB = pc->indexB;
 		float32 mB = pc->invMassB;
+		float32 rB = pc->radiusB;
 
 		b3Vec3 xA = x[indexA];
 		b3Vec3 xB = x[indexB];
 
 		b3ClothSolverParticleContactSolverPoint cpcp;
-		cpcp.Initialize(m_particles[indexA], m_particles[indexB]);
+		cpcp.Initialize(xA, rA, xB, rB);
 
 		b3Vec3 normal = cpcp.normal;
 		b3Vec3 point = cpcp.point;
@@ -1071,5 +1277,92 @@ bool b3ClothSolver::SolveParticleContactPositionConstraints()
 		x[indexB] = xB;
 	}
 
+	return minSeparation >= -3.0f * B3_LINEAR_SLOP;
+}
+
+bool b3ClothSolver::SolveTriangleContactPositionConstraints()
+{
+	b3DenseVec3& x = *m_solverData.v;
+
+	float32 minSeparation = 0.0f;
+
+	for (u32 i = 0; i < m_triangleContactCount; ++i)
+	{
+		b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i;
+
+		u32 indexA = pc->indexA;
+		float32 mA = pc->invMassA;
+		float32 radiusA = pc->radiusA;
+
+		u32 indexB = pc->indexB;
+		float32 mB = pc->invMassB;
+
+		u32 indexC = pc->indexC;
+		float32 mC = pc->invMassC;
+
+		u32 indexD = pc->indexD;
+		float32 mD = pc->invMassD;
+
+		float32 triangleRadius = pc->triangleRadius;
+
+		b3Vec3 xA = x[indexA];
+		b3Vec3 xB = x[indexB];
+		b3Vec3 xC = x[indexC];
+		b3Vec3 xD = x[indexD];
+
+		b3Vec3 n = b3Cross(xC - xB, xD - xB);
+		if (pc->front == false)
+		{
+			n = -n;
+		}
+
+		float32 n_len = n.Normalize();
+
+		float32 distance = b3Dot(n, xA - xB);
+		float32 separation = distance - radiusA - triangleRadius;
+
+		// Update max constraint error.
+		minSeparation = b3Min(minSeparation, separation);
+
+		// Allow some slop and prevent large corrections.
+		float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f);
+
+		// Compute effective mass.
+		float32 K = mA + mB + mC + mD;
+
+		b3Mat33 I; I.SetIdentity();
+
+		b3Mat33 N = I - b3Outer(n, n);
+		if (n_len > B3_EPSILON)
+		{
+			N = (1.0f / n_len) * N;
+		}
+
+		b3Vec3 N_n = N * n;
+
+		b3Vec3 JA = n;
+		b3Vec3 JC = b3Cross(xD - xB, N_n);
+		b3Vec3 JD = b3Cross(xC - xB, N_n);
+		b3Vec3 JB = b3Cross(xC - xD, N_n) - n;
+
+		// Compute normal impulse.
+		float32 impulse = K > 0.0f ? -C / K : 0.0f;
+
+		b3Vec3 PA = impulse * JA;
+		b3Vec3 PB = impulse * JB;
+		b3Vec3 PC = impulse * JC;
+		b3Vec3 PD = impulse * JD;
+
+		xA += mA * PA;
+		xB += mB * PB;
+		xC += mC * PC;
+		xD += mD * PD;
+
+		x[indexA] = xA;
+		x[indexB] = xB;
+		x[indexC] = xC;
+		x[indexD] = xD;
+	}
+
 	return minSeparation >= -3.0f * B3_LINEAR_SLOP;
 }

src/bounce/dynamics/cloth/particle.cpp

@@ -73,6 +73,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
 	m_mass = 0.0f;
 	m_invMass = 0.0f;
 	m_radius = def.radius;
+	m_friction = def.friction;
 	m_userData = nullptr;
 	m_x.SetZero();
 	m_vertex = ~0;