diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h
index e24e17f..a84cf1a 100644
--- a/include/bounce/cloth/cloth.h
+++ b/include/bounce/cloth/cloth.h
@@ -19,11 +19,11 @@
 #ifndef B3_CLOTH_H
 #define B3_CLOTH_H
 
-#include <bounce/common/math/transform.h>
 #include <bounce/common/template/list.h>
 #include <bounce/common/memory/stack_allocator.h>
 #include <bounce/common/memory/block_pool.h>
-#include <bounce/collision/trees/dynamic_tree.h>
+#include <bounce/common/math/transform.h>
+#include <bounce/cloth/cloth_contact_manager.h>
 
 class b3World;
 class b3Shape;
@@ -42,6 +42,8 @@ class b3RayCastListener;
 struct b3RayCastInput;
 struct b3RayCastOutput;
 
+struct b3ClothAABBProxy;
+
 struct b3ClothRayCastSingleOutput
 {
 	u32 triangle;
@@ -140,16 +142,23 @@ public:
 	void Draw() const;
 private:
 	friend class b3Particle;
+	friend class b3ClothContactManager;
 
 	// Compute mass of each particle.
 	void ComputeMass();
 
-	// Update contacts
-	void UpdateContacts();
+	// Update particle-body contacts
+	void UpdateParticleBodyContacts();
 
 	// Solve
 	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
 
+	// Synchronize triangle AABB.
+	void SynchronizeTriangle(u32 triangleIndex);
+
+	// Time-step
+	float32 m_dt;
+
 	// Stack allocator
 	b3StackAllocator m_stackAllocator;
 
@@ -165,6 +174,9 @@ private:
 	// Vertex particles
 	b3Particle** m_vertexParticles;
 
+	// Triangle proxies
+	b3ClothAABBProxy* m_triangleProxies;
+
 	// Cloth density
 	float32 m_density;
 
@@ -174,11 +186,11 @@ private:
 	// List of particles
 	b3List2<b3Particle> m_particleList;
 
-	// Particle tree
-	b3DynamicTree m_particleTree;
-
 	// List of forces
 	b3List2<b3Force> m_forceList;
+
+	// Contact manager
+	b3ClothContactManager m_contactManager;
 };
 
 inline void b3Cloth::SetGravity(const b3Vec3& gravity)
diff --git a/include/bounce/cloth/cloth_contact_manager.h b/include/bounce/cloth/cloth_contact_manager.h
new file mode 100644
index 0000000..3e67e2f
--- /dev/null
+++ b/include/bounce/cloth/cloth_contact_manager.h
@@ -0,0 +1,81 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CLOTH_CONTACT_MANAGER_H
+#define B3_CLOTH_CONTACT_MANAGER_H
+
+#include <bounce/common/memory/block_pool.h>
+#include <bounce/common/template/list.h>
+#include <bounce/collision/broad_phase.h>
+
+class b3Cloth;
+class b3Particle;
+struct b3ClothMeshTriangle;
+struct b3ClothAABBProxy;
+
+// Contact between particle and a triangle
+class b3ParticleTriangleContact
+{
+public:
+	b3Particle* m_p1;
+	
+	b3ClothMeshTriangle* m_triangle;
+	b3ClothAABBProxy* m_triangleProxy;
+	b3Particle* m_p2;
+	b3Particle* m_p3;
+	b3Particle* m_p4;
+
+	bool m_front;
+	
+	bool m_active;
+
+	float32 m_normalImpulse;
+
+	b3ParticleTriangleContact* m_prev;
+	b3ParticleTriangleContact* m_next;
+};
+
+// Contact delegator for b3Cloth.
+class b3ClothContactManager
+{
+public:
+	b3ClothContactManager();
+
+	// The broad-phase callback.
+	void AddPair(void* data1, void* data2);
+
+	void FindNewContacts();
+
+	void UpdateContacts();
+	
+	b3ParticleTriangleContact* Create();
+	
+	void Destroy(b3ParticleTriangleContact* c);
+
+	void Update(b3ParticleTriangleContact* c);
+
+	b3Cloth* m_cloth;
+
+	b3BlockPool m_particleTriangleContactBlocks;
+	
+	b3BroadPhase m_broadPhase;
+
+	b3List2<b3ParticleTriangleContact> m_particleTriangleContactList;
+};
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/cloth/cloth_contact_solver.h b/include/bounce/cloth/cloth_contact_solver.h
index a945d03..3005084 100644
--- a/include/bounce/cloth/cloth_contact_solver.h
+++ b/include/bounce/cloth/cloth_contact_solver.h
@@ -28,6 +28,7 @@ class b3Particle;
 class b3Body;
 
 struct b3ParticleBodyContact;
+class b3ParticleTriangleContact;
 
 struct b3DenseVec3;
 
@@ -80,6 +81,44 @@ struct b3ClothSolverBodyContactPositionConstraint
 	b3Vec3 localPointB;
 };
 
+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;
+};
+
 struct b3ClothContactSolverDef
 {
 	b3StackAllocator* allocator;
@@ -89,6 +128,9 @@ struct b3ClothContactSolverDef
 	
 	u32 bodyContactCount;
 	b3ParticleBodyContact** bodyContacts;
+
+	u32 triangleContactCount;
+	b3ParticleTriangleContact** triangleContacts;
 };
 
 inline float32 b3MixFriction(float32 u1, float32 u2)
@@ -103,14 +145,18 @@ public:
 	~b3ClothContactSolver();
 
 	void InitializeBodyContactConstraints();
+	void InitializeTriangleContactConstraints();
 
-	void WarmStart();
+	void WarmStartBodyContactConstraints();
+	void WarmStartTriangleContactConstraints();
 
 	void SolveBodyContactVelocityConstraints();
+	void SolveTriangleContactVelocityConstraints();
 
 	void StoreImpulses();
 
 	bool SolveBodyContactPositionConstraints();
+	bool SolveTriangleContactPositionConstraints();
 protected:
 	b3StackAllocator* m_allocator;
 
@@ -121,6 +167,11 @@ protected:
 	b3ParticleBodyContact** m_bodyContacts;
 	b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
 	b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints;
+
+	u32 m_triangleContactCount;
+	b3ParticleTriangleContact** m_triangleContacts;
+	b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints;
+	b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints;
 };
 
 #endif
\ No newline at end of file
diff --git a/include/bounce/cloth/cloth_solver.h b/include/bounce/cloth/cloth_solver.h
index 3a582db..c39e417 100644
--- a/include/bounce/cloth/cloth_solver.h
+++ b/include/bounce/cloth/cloth_solver.h
@@ -34,6 +34,7 @@ struct b3SparseSymMat33;
 struct b3SparseSymMat33View;
 
 struct b3ParticleBodyContact;
+class b3ParticleTriangleContact;
 
 struct b3ClothSolverDef
 {
@@ -41,6 +42,7 @@ struct b3ClothSolverDef
 	u32 particleCapacity;
 	u32 forceCapacity;
 	u32 bodyContactCapacity;
+	u32 triangleContactCapacity;
 };
 
 struct b3ClothSolverData
@@ -75,6 +77,7 @@ public:
 	void Add(b3Particle* p);
 	void Add(b3Force* f);
 	void Add(b3ParticleBodyContact* c);
+	void Add(b3ParticleTriangleContact* c);
 
 	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
 private:
@@ -107,6 +110,10 @@ private:
 	u32 m_bodyContactCount;
 	b3ParticleBodyContact** m_bodyContacts;
 
+	u32 m_triangleContactCapacity;
+	u32 m_triangleContactCount;
+	b3ParticleTriangleContact** m_triangleContacts;
+	
 	b3ClothSolverData m_solverData;
 };
 
diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h
index 6d96c4a..d2d13cf 100644
--- a/include/bounce/cloth/particle.h
+++ b/include/bounce/cloth/particle.h
@@ -44,6 +44,7 @@ struct b3ParticleDef
 	b3ParticleDef()
 	{
 		type = e_staticParticle;
+		mass = 0.0f;
 		position.SetZero();
 		velocity.SetZero();
 		force.SetZero();
@@ -53,6 +54,7 @@ struct b3ParticleDef
 	}
 
 	b3ParticleType type;
+	float32 mass;
 	b3Vec3 position;
 	b3Vec3 velocity;
 	b3Vec3 force;
@@ -89,6 +91,20 @@ struct b3ParticleBodyContactWorldPoint
 	float32 separation;
 };
 
+enum b3ClothAABBProxyType
+{
+	e_particleProxy,
+	e_triangleProxy
+};
+
+struct b3ClothAABBProxy
+{
+	b3ClothAABBProxyType type;
+	void* data;
+	u32 index;
+	u32 broadPhaseId;
+};
+
 // A cloth particle.
 class b3Particle
 {
@@ -143,18 +159,23 @@ private:
 	friend class b3List2<b3Particle>;
 	friend class b3Cloth;
 	friend class b3ClothSolver;
+	friend class b3ClothContactManager;
 	friend class b3ClothContactSolver;
 	friend class b3Force;
 	friend class b3SpringForce;
-	friend class b3BendForce;
-	friend class b3FrictionForce;
 
 	b3Particle(const b3ParticleDef& def, b3Cloth* cloth);
 	~b3Particle();
 
-	// Synchronize AABB
+	// Synchronize particle AABB
 	void Synchronize();
 
+	// Synchronize triangles AABB
+	void SynchronizeTriangles();
+
+	// Destroy contacts.
+	void DestroyContacts();
+
 	// Type
 	b3ParticleType m_type;
 
@@ -202,8 +223,8 @@ private:
 	// Contact
 	b3ParticleBodyContact m_bodyContact;
 
-	// Particle tree identifier
-	u32 m_treeId;
+	// AABB Proxy
+	b3ClothAABBProxy m_aabbProxy;
 
 	// 
 	b3Particle* m_prev;
@@ -228,6 +249,7 @@ inline void b3Particle::SetPosition(const b3Vec3& position)
 	m_translation.SetZero();
 
 	Synchronize();
+	SynchronizeTriangles();
 }
 
 inline const b3Vec3& b3Particle::GetPosition() const
diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp
index 85b12dd..3c66427 100644
--- a/src/bounce/cloth/cloth.cpp
+++ b/src/bounce/cloth/cloth.cpp
@@ -32,6 +32,8 @@
 
 #include <bounce/common/draw.h>
 
+#define B3_ENABLE_SELF_COLLISION 0
+
 static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
 {
 	return i + 1 < 3 ? i + 1 : 0;
@@ -158,6 +160,8 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 
 	m_mesh = def.mesh;
 	m_density = def.density;
+	m_contactManager.m_cloth = this;
+	m_dt = 0.0f;
 
 	const b3ClothMesh* m = m_mesh;
 
@@ -167,10 +171,12 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 	{
 		b3ParticleDef pd;
 		pd.type = e_dynamicParticle;
+		pd.mass = 1.0f;
 		pd.position = m->vertices[i];
 
 		b3Particle* p = CreateParticle(pd);
 
+		p->m_aabbProxy.index = i;
 		p->m_vertex = i;
 		m_vertexParticles[i] = p;
 	}
@@ -200,7 +206,10 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 		b3SpringForceDef fd;
 		fd.Initialize(p1, p2, def.structural, def.damping);
 
-		CreateForce(fd);
+		if (def.structural > 0.0f)
+		{
+			CreateForce(fd);
+		}
 	}
 
 	// Bending
@@ -216,9 +225,12 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 		b3SpringForceDef fd;
 		fd.Initialize(p3, p4, def.bending, def.damping);
 
-		CreateForce(fd);
+		if (def.bending > 0.0f)
+		{
+			CreateForce(fd);
+		}
 	}
-
+	
 	allocator->Free(sharedEdges);
 	allocator->Free(uniqueEdges);
 
@@ -233,7 +245,30 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 		b3SpringForceDef fd;
 		fd.Initialize(p1, p2, def.structural, def.damping);
 
-		CreateForce(fd);
+		if (def.structural > 0.0f)
+		{
+			CreateForce(fd);
+		}
+	}
+
+	// Initialize triangle proxies
+	m_triangleProxies = (b3ClothAABBProxy*)b3Alloc(m_mesh->triangleCount * sizeof(b3ClothAABBProxy));
+	for (u32 i = 0; i < m_mesh->triangleCount; ++i)
+	{
+		b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
+		b3ClothAABBProxy* triangleProxy = m_triangleProxies + i;
+
+		b3Vec3 v1 = m_mesh->vertices[triangle->v1];
+		b3Vec3 v2 = m_mesh->vertices[triangle->v2];
+		b3Vec3 v3 = m_mesh->vertices[triangle->v3];
+
+		b3AABB3 aabb;
+		aabb.Set(v1, v2, v3);
+
+		triangleProxy->type = e_triangleProxy;
+		triangleProxy->index = i;
+		triangleProxy->data = triangle;
+		triangleProxy->broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, triangleProxy);
 	}
 
 	m_gravity.SetZero();
@@ -251,6 +286,7 @@ b3Cloth::~b3Cloth()
 	}
 
 	b3Free(m_vertexParticles);
+	b3Free(m_triangleProxies);
 
 	b3Force* f = m_forceList.m_head;
 	while (f)
@@ -265,11 +301,14 @@ b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def)
 {
 	void* mem = m_particleBlocks.Allocate();
 	b3Particle* p = new(mem) b3Particle(def, this);
-	
+
 	b3AABB3 aabb;
 	aabb.Set(p->m_position, p->m_radius);
-	
-	p->m_treeId = m_particleTree.InsertNode(aabb, p);
+
+	p->m_aabbProxy.type = e_particleProxy;
+	p->m_aabbProxy.data = p;
+	p->m_aabbProxy.index = p->m_vertex;
+	p->m_aabbProxy.broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &p->m_aabbProxy);
 
 	m_particleList.PushFront(p);
 
@@ -280,6 +319,7 @@ void b3Cloth::DestroyParticle(b3Particle* particle)
 {
 	B3_ASSERT(particle->m_vertex == ~0);
 	
+	// Destroy particle forces
 	b3Force* f = m_forceList.m_head;
 	while (f)
 	{
@@ -293,7 +333,11 @@ void b3Cloth::DestroyParticle(b3Particle* particle)
 		}
 	}
 
-	m_particleTree.RemoveNode(particle->m_treeId);
+	// Destroy particle contacts
+	particle->DestroyContacts();
+
+	// Destroy AABB proxy
+	m_contactManager.m_broadPhase.DestroyProxy(particle->m_aabbProxy.broadPhaseId);
 
 	m_particleList.Remove(particle);
 	particle->~b3Particle();
@@ -370,6 +414,44 @@ void b3Cloth::ComputeMass()
 	}
 }
 
+struct b3ClothRayCastSingleCallback
+{
+	float32 Report(const b3RayCastInput& input, u32 proxyId)
+	{
+		// Get primitive associated with the proxy.
+		void* userData = broadPhase->GetUserData(proxyId);
+		b3ClothAABBProxy* proxy = (b3ClothAABBProxy*)userData;
+
+		if (proxy->type != e_triangleProxy)
+		{
+			// Continue search from where we stopped.
+			return input.maxFraction;
+		}
+
+		u32 triangleIndex = proxy->index;
+
+		b3RayCastOutput subOutput;
+		if (cloth->RayCast(&subOutput, &input, triangleIndex))
+		{
+			// Ray hits triangle.
+			if (subOutput.fraction < output0.fraction)
+			{
+				triangle0 = proxy->index;
+				output0.fraction = subOutput.fraction;
+				output0.normal = subOutput.normal;
+			}
+		}
+
+		// Continue search from where we stopped.
+		return input.maxFraction;
+	}
+
+	const b3Cloth* cloth;
+	const b3BroadPhase* broadPhase;
+	u32 triangle0;
+	b3RayCastOutput output0;
+};
+
 bool b3Cloth::RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const
 {
 	b3RayCastInput input;
@@ -377,29 +459,19 @@ bool b3Cloth::RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1
 	input.p2 = p2;
 	input.maxFraction = 1.0f;
 
-	u32 triangle0 = ~0;
-	b3RayCastOutput output0;
-	output0.fraction = B3_MAX_FLOAT;
+	b3ClothRayCastSingleCallback callback;
+	callback.cloth = this;
+	callback.broadPhase = &m_contactManager.m_broadPhase;
+	callback.triangle0 = ~0;
+	callback.output0.fraction = B3_MAX_FLOAT;
 
-	for (u32 i = 0; i < m_mesh->triangleCount; ++i)
-	{
-		b3RayCastOutput subOutput;
-		if (RayCast(&subOutput, &input, i))
-		{
-			if (subOutput.fraction < output0.fraction)
-			{
-				triangle0 = i;
-				output0.fraction = subOutput.fraction;
-				output0.normal = subOutput.normal;
-			}
-		}
-	}
+	m_contactManager.m_broadPhase.RayCast(&callback, input);
 
-	if (triangle0 != ~0)
+	if (callback.triangle0 != ~0)
 	{
-		output->triangle = triangle0;
-		output->fraction = output0.fraction;
-		output->normal = output0.normal;
+		output->triangle = callback.triangle0;
+		output->fraction = callback.output0.fraction;
+		output->normal = callback.output0.normal;
 
 		return true;
 	}
@@ -455,9 +527,9 @@ public:
 	b3Vec3 bestNormal;
 };
 
-void b3Cloth::UpdateContacts()
+void b3Cloth::UpdateParticleBodyContacts()
 {
-	B3_PROFILE("Cloth Update Contacts");
+	B3_PROFILE("Cloth Update Particle Body Contacts");
 
 	// Is there a world attached to this cloth?
 	if (m_world == nullptr)
@@ -473,7 +545,7 @@ void b3Cloth::UpdateContacts()
 			continue;
 		}
 
-		b3AABB3 aabb = m_particleTree.GetAABB(p->m_treeId);
+		b3AABB3 aabb = m_contactManager.m_broadPhase.GetAABB(p->m_aabbProxy.broadPhaseId);
 
 		b3ClothUpdateContactsQueryListener listener;
 		listener.sphere.vertex = p->m_position;
@@ -528,6 +600,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u
 	solverDef.particleCapacity = m_particleList.m_count;
 	solverDef.forceCapacity = m_forceList.m_count;
 	solverDef.bodyContactCapacity = m_particleList.m_count;
+	solverDef.triangleContactCapacity = m_contactManager.m_particleTriangleContactList.m_count;
 
 	b3ClothSolver solver(solverDef);
 
@@ -549,6 +622,14 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u
 		}
 	}
 
+	for (b3ParticleTriangleContact* c = m_contactManager.m_particleTriangleContactList.m_head; c; c = c->m_next)
+	{
+		if (c->m_active)
+		{
+			solver.Add(c);
+		}
+	}
+
 	// Solve	
 	solver.Solve(dt, gravity, velocityIterations, positionIterations);
 }
@@ -557,8 +638,13 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
 {
 	B3_PROFILE("Cloth Step");
 
-	// Update contacts
-	UpdateContacts();
+	m_dt = dt;
+
+	// Update particle-body contacts
+	UpdateParticleBodyContacts();
+
+	// Update self-contacts
+	m_contactManager.UpdateContacts();
 
 	// Integrate state, solve constraints. 
 	if (dt > 0.0f)
@@ -576,13 +662,49 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
 	// Synchronize particles
 	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
 	{
-		if (p->m_type == e_staticParticle)
-		{
-			continue;
-		}
-
 		p->Synchronize();
 	}
+
+	// Synchronize triangles
+	for (u32 i = 0; i < m_mesh->triangleCount; ++i)
+	{
+		SynchronizeTriangle(i);
+	}
+
+#if B3_ENABLE_SELF_COLLISION
+
+	// Find new self-contacts
+	m_contactManager.FindNewContacts();
+
+#endif
+}
+
+void b3Cloth::SynchronizeTriangle(u32 triangleIndex)
+{
+	b3ClothMeshTriangle* triangle = m_mesh->triangles + triangleIndex;
+	b3ClothAABBProxy* triangleProxy = m_triangleProxies + triangleIndex;
+
+	b3Particle* p1 = m_vertexParticles[triangle->v1];
+	b3Particle* p2 = m_vertexParticles[triangle->v2];
+	b3Particle* p3 = m_vertexParticles[triangle->v3];
+
+	b3Vec3 x1 = p1->m_position;
+	b3Vec3 x2 = p2->m_position;
+	b3Vec3 x3 = p3->m_position;
+
+	b3Vec3 v1 = p1->m_velocity;
+	b3Vec3 v2 = p2->m_velocity;
+	b3Vec3 v3 = p3->m_velocity;
+
+	b3AABB3 aabb;
+	aabb.Set(x1, x2, x3);
+
+	const float32 kInv3 = 1.0f / 3.0f;
+	
+	b3Vec3 center_velocity = kInv3 * (v1 + v2 + v3);
+	b3Vec3 center_displacement = m_dt * center_velocity;
+
+	m_contactManager.m_broadPhase.MoveProxy(triangleProxy->broadPhaseId, aabb, center_displacement);
 }
 
 void b3Cloth::Draw() const
diff --git a/src/bounce/cloth/cloth_contact_manager.cpp b/src/bounce/cloth/cloth_contact_manager.cpp
new file mode 100644
index 0000000..6f5ce47
--- /dev/null
+++ b/src/bounce/cloth/cloth_contact_manager.cpp
@@ -0,0 +1,336 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include <bounce/cloth/cloth_contact_manager.h>
+#include <bounce/cloth/cloth.h>
+#include <bounce/cloth/cloth_mesh.h>
+#include <bounce/cloth/particle.h>
+
+#include <bounce/common/draw.h>
+
+b3ClothContactManager::b3ClothContactManager() : 
+	m_particleTriangleContactBlocks(sizeof(b3ParticleTriangleContact))
+{
+
+}
+
+void b3ClothContactManager::FindNewContacts()
+{
+	B3_PROFILE("Cloth Find New Contacts");
+	
+	m_broadPhase.FindPairs(this);
+}
+
+void b3ClothContactManager::AddPair(void* data1, void* data2)
+{
+	b3ClothAABBProxy* proxy1 = (b3ClothAABBProxy*)data1;
+	b3ClothAABBProxy* proxy2 = (b3ClothAABBProxy*)data2;
+
+	if (proxy1->type == e_particleProxy && proxy2->type == e_particleProxy)
+	{
+		// Particle-particle contacts are not supported.
+		return;
+	}
+
+	if (proxy1->type == e_triangleProxy && proxy2->type == e_triangleProxy)
+	{
+		// Triangle-triangle contacts are not supported.
+		return;
+	}
+
+	if (proxy1->type == e_triangleProxy)
+	{
+		// Ensure proxy1 is a particle and proxy 2 a triangle.
+		b3Swap(proxy1, proxy2);
+	}
+
+	B3_ASSERT(proxy1->type == e_particleProxy);
+	B3_ASSERT(proxy2->type == e_triangleProxy);
+
+	b3Particle* p1 = (b3Particle*)proxy1->data;
+
+	b3ClothMeshTriangle* triangle = (b3ClothMeshTriangle*)proxy2->data;
+	b3Particle* p2 = m_cloth->m_vertexParticles[triangle->v1];
+	b3Particle* p3 = m_cloth->m_vertexParticles[triangle->v2];
+	b3Particle* p4 = m_cloth->m_vertexParticles[triangle->v3];
+
+	// Check if there is a contact between the two entities.
+	for (b3ParticleTriangleContact* c = m_particleTriangleContactList.m_head; c; c = c->m_next)
+	{
+		if (c->m_p1 == p1 && c->m_triangle == triangle)
+		{
+			// A contact already exists.
+			return;
+		}
+	}
+
+	bool isntDynamic1 = p1->m_type != e_dynamicParticle;
+	bool isntDynamic2 = p2->m_type != e_dynamicParticle && p3->m_type != e_dynamicParticle && p4->m_type != e_dynamicParticle;
+
+	if (isntDynamic1 && isntDynamic2)
+	{
+		// The entities must not collide with each other.
+		return;
+	}
+
+	if (triangle->v1 == p1->m_vertex || triangle->v2 == p1->m_vertex || triangle->v3 == p1->m_vertex)
+	{
+		// The entities must not collide with each other.
+		return;
+	}
+	
+	// Create a new contact.
+	b3ParticleTriangleContact* c = Create();
+
+	c->m_p1 = p1;
+	
+	c->m_p2 = p2;
+	c->m_p3 = p3;
+	c->m_p4 = p4;
+	c->m_triangle = triangle;
+	c->m_triangleProxy = proxy2;
+
+	c->m_front = false;
+	c->m_active = false;
+
+	c->m_normalImpulse = 0.0f;
+
+	// Add the contact to the cloth contact list.
+	m_particleTriangleContactList.PushFront(c);
+}
+
+b3ParticleTriangleContact* b3ClothContactManager::Create()
+{
+	void* block = m_particleTriangleContactBlocks.Allocate();
+	return new(block) b3ParticleTriangleContact();
+}
+
+void b3ClothContactManager::Destroy(b3ParticleTriangleContact* c)
+{
+	m_particleTriangleContactList.Remove(c);
+
+	c->~b3ParticleTriangleContact();
+	
+	m_particleTriangleContactBlocks.Free(c);
+}
+
+static 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];
+	b3BarycentricCoordinates(wAB, A, B, Q);
+	b3BarycentricCoordinates(wBC, B, C, Q);
+	b3BarycentricCoordinates(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];
+	b3BarycentricCoordinates(wABC, A, B, C, Q);
+
+	// R AB
+	if (wAB[0] > 0.0f && wAB[1] > 0.0f && wABC[3] * 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 && wABC[3] * 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 && wABC[3] * wABC[1] <= 0.0f)
+	{
+		float32 divisor = wCA[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		out[0] = s * wCA[1];
+		out[1] = 0.0f;
+		out[2] = s * wCA[0];
+		return;
+	}
+
+	// R ABC/ACB
+	float32 divisor = wABC[3];
+	if (divisor == 0.0f)
+	{
+		float32 s = 1.0f / 3.0f;
+		out[0] = s;
+		out[1] = s;
+		out[2] = 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 b3ClothContactManager::Update(b3ParticleTriangleContact* c)
+{
+	b3Particle* p1 = c->m_p1;
+	
+	b3Particle* p2 = c->m_p2;
+	b3Particle* p3 = c->m_p3;
+	b3Particle* p4 = c->m_p4;
+
+	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 n = b3Cross(B - A, C - A);
+	float32 len = n.Normalize();
+
+	// Is ABC degenerate?
+	if (len == 0.0f)
+	{
+		c->m_active = false;
+		return;
+	}
+
+	b3Vec3 P1 = p1->m_position;
+
+	float32 distance = b3Dot(n, P1 - A);
+
+	// Is P1 below the plane?
+	if (distance < -totalRadius)
+	{
+		c->m_active = false;
+		return;
+	}
+
+	// Is P1 above the plane?
+	if (distance > totalRadius)
+	{
+		c->m_active = false;
+		return;
+	}
+
+	// Closest point on ABC to P1
+	float32 wABC[3];
+	b3Solve3(wABC, A, B, C, P1);
+
+	b3Vec3 P2 = wABC[0] * A + wABC[1] * B + wABC[2] * C;
+
+	if (b3DistanceSquared(P1, P2) > totalRadius * totalRadius)
+	{
+		c->m_active = false;
+		return;
+	}
+
+	// Activate the contact
+	c->m_active = true;
+
+	// Is the the other point in front of the plane? 
+	if (distance >= 0.0f)
+	{
+		c->m_front = true;
+	}
+	else
+	{
+		c->m_front = false;
+	}
+}
+
+void b3ClothContactManager::UpdateContacts()
+{
+	B3_PROFILE("Cloth Update Contacts");
+	
+	// Update the state of all triangle contacts.
+	b3ParticleTriangleContact* c = m_particleTriangleContactList.m_head;
+	while (c)
+	{
+		bool isntDynamic1 = c->m_p1->m_type != e_dynamicParticle;
+		bool isntDynamic2 = c->m_p2->m_type != e_dynamicParticle && c->m_p3->m_type != e_dynamicParticle && c->m_p4->m_type != e_dynamicParticle;
+
+		// Destroy the contact if primitives must not collide with each other.
+		if (isntDynamic1 && isntDynamic2)
+		{
+			b3ParticleTriangleContact* quack = c;
+			c = c->m_next;
+			Destroy(quack);
+			continue;
+		}
+
+		u32 proxy1 = c->m_p1->m_aabbProxy.broadPhaseId;
+		u32 proxy2 = c->m_triangleProxy->broadPhaseId;
+
+		// Destroy the contact if primitive AABBs are not overlapping.
+		bool overlap = m_broadPhase.TestOverlap(proxy1, proxy2);
+		if (overlap == false)
+		{
+			b3ParticleTriangleContact* quack = c;
+			c = c->m_next;
+			Destroy(quack);
+			continue;
+		}
+
+		// The contact persists.
+		Update(c);
+
+		c = c->m_next;
+	}
+}
\ No newline at end of file
diff --git a/src/bounce/cloth/cloth_contact_solver.cpp b/src/bounce/cloth/cloth_contact_solver.cpp
index 46d84ad..0a7768c 100644
--- a/src/bounce/cloth/cloth_contact_solver.cpp
+++ b/src/bounce/cloth/cloth_contact_solver.cpp
@@ -17,6 +17,7 @@
 */
 
 #include <bounce/cloth/cloth_contact_solver.h>
+#include <bounce/cloth/cloth_contact_manager.h>
 #include <bounce/cloth/particle.h>
 #include <bounce/cloth/dense_vec3.h>
 #include <bounce/common/memory/stack_allocator.h>
@@ -34,10 +35,18 @@ b3ClothContactSolver::b3ClothContactSolver(const b3ClothContactSolverDef& def)
 	m_bodyContacts = def.bodyContacts;
 	m_bodyVelocityConstraints = (b3ClothSolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactVelocityConstraint));
 	m_bodyPositionConstraints = (b3ClothSolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactPositionConstraint));
+
+	m_triangleContactCount = def.triangleContactCount;
+	m_triangleContacts = def.triangleContacts;
+	m_triangleVelocityConstraints = (b3ClothSolverTriangleContactVelocityConstraint*)m_allocator->Allocate(m_triangleContactCount * sizeof(b3ClothSolverTriangleContactVelocityConstraint));
+	m_trianglePositionConstraints = (b3ClothSolverTriangleContactPositionConstraint*)m_allocator->Allocate(m_triangleContactCount * sizeof(b3ClothSolverTriangleContactPositionConstraint));
 }
 
 b3ClothContactSolver::~b3ClothContactSolver()
 {
+	m_allocator->Free(m_trianglePositionConstraints);
+	m_allocator->Free(m_triangleVelocityConstraints);
+
 	m_allocator->Free(m_bodyPositionConstraints);
 	m_allocator->Free(m_bodyVelocityConstraints);
 }
@@ -170,8 +179,93 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
 		}
 	}
 }
+void b3ClothContactSolver::InitializeTriangleContactConstraints()
+{
+	b3DenseVec3& x = *m_positions;
 
-void b3ClothContactSolver::WarmStart()
+	for (u32 i = 0; i < m_triangleContactCount; ++i)
+	{
+		b3ParticleTriangleContact* c = m_triangleContacts[i];
+		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
+		b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i;
+
+		vc->indexA = c->m_p1->m_solverId;
+		vc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass;
+
+		vc->indexB = c->m_p2->m_solverId;
+		vc->invMassB = c->m_p2->m_type == e_staticParticle ? 0.0f : c->m_p2->m_invMass;
+
+		vc->indexC = c->m_p3->m_solverId;
+		vc->invMassC = c->m_p3->m_type == e_staticParticle ? 0.0f : c->m_p3->m_invMass;
+
+		vc->indexD = c->m_p4->m_solverId;
+		vc->invMassD = c->m_p4->m_type == e_staticParticle ? 0.0f : c->m_p4->m_invMass;
+
+		pc->indexA = c->m_p1->m_solverId;
+		pc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass;
+		pc->radiusA = c->m_p1->m_radius;
+
+		pc->indexB = c->m_p2->m_solverId;
+		pc->invMassB = c->m_p2->m_type == e_staticParticle ? 0.0f : c->m_p2->m_invMass;
+
+		pc->indexC = c->m_p3->m_solverId;
+		pc->invMassC = c->m_p3->m_type == e_staticParticle ? 0.0f : c->m_p3->m_invMass;
+
+		pc->indexD = c->m_p4->m_solverId;
+		pc->invMassD = c->m_p4->m_type == e_staticParticle ? 0.0f : c->m_p4->m_invMass;
+
+		pc->triangleRadius = 0.0f;
+		pc->front = c->m_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->JB = b3Cross(xC - xD, N_n) - n;
+		vc->JC = b3Cross(xD - xB, N_n);
+		vc->JD = b3Cross(xC - xB, N_n);
+
+		float32 K = mA + mB * b3Dot(vc->JB, vc->JB) + mC * b3Dot(vc->JC, vc->JC) + mD * b3Dot(vc->JD, vc->JD);
+
+		vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
+		vc->normalImpulse = c->m_normalImpulse;
+	}
+}
+
+void b3ClothContactSolver::WarmStartBodyContactConstraints()
 {
 	b3DenseVec3& v = *m_velocities;
 
@@ -218,6 +312,46 @@ void b3ClothContactSolver::WarmStart()
 	}
 }
 
+void b3ClothContactSolver::WarmStartTriangleContactConstraints()
+{
+	b3DenseVec3& v = *m_velocities;
+	
+	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 b3ClothContactSolver::SolveBodyContactVelocityConstraints()
 {
 	b3DenseVec3& v = *m_velocities;
@@ -306,6 +440,56 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
 	}
 }
 
+void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
+{
+	b3DenseVec3& v = *m_velocities;
+
+	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];
+
+		float32 Cdot = b3Dot(vc->JA, vA) + b3Dot(vc->JB, vB) + b3Dot(vc->JC, vC) + b3Dot(vc->JD, vD);
+
+		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 b3ClothContactSolver::StoreImpulses()
 {
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
@@ -316,6 +500,14 @@ void b3ClothContactSolver::StoreImpulses()
 		c->normalImpulse = vc->normalImpulse;
 		c->tangentImpulse = vc->tangentImpulse;
 	}
+
+	for (u32 i = 0; i < m_triangleContactCount; ++i)
+	{
+		b3ParticleTriangleContact* c = m_triangleContacts[i];
+		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
+
+		c->m_normalImpulse = vc->normalImpulse;
+	}
 }
 
 struct b3ClothSolverBodyContactSolverPoint
@@ -417,5 +609,92 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
 		bodyB->m_sweep.orientation = qB;
 	}
 
+	return minSeparation >= -3.0f * B3_LINEAR_SLOP;
+}
+
+bool b3ClothContactSolver::SolveTriangleContactPositionConstraints()
+{
+	b3DenseVec3& x = *m_positions;
+
+	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);
+
+		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 JB = b3Cross(xC - xD, N_n) - n;
+		b3Vec3 JC = b3Cross(xD - xB, N_n);
+		b3Vec3 JD = b3Cross(xC - xB, N_n);
+
+		// Compute effective mass.
+		float32 K = mA + mB * b3Dot(JB, JB) + mC * b3Dot(JC, JC) + mD * b3Dot(JD, JD);
+
+		// 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;
 }
\ No newline at end of file
diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp
index 0daf1a8..92c7530 100644
--- a/src/bounce/cloth/cloth_solver.cpp
+++ b/src/bounce/cloth/cloth_solver.cpp
@@ -58,10 +58,15 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 	m_bodyContactCapacity = def.bodyContactCapacity;
 	m_bodyContactCount = 0;
 	m_bodyContacts = (b3ParticleBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ParticleBodyContact*));;
+
+	m_triangleContactCapacity = def.triangleContactCapacity;
+	m_triangleContactCount = 0;
+	m_triangleContacts = (b3ParticleTriangleContact**)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3ParticleTriangleContact*));;
 }
 
 b3ClothSolver::~b3ClothSolver()
 {
+	m_allocator->Free(m_triangleContacts);
 	m_allocator->Free(m_bodyContacts);
 
 	m_allocator->Free(m_constraints);
@@ -85,6 +90,11 @@ void b3ClothSolver::Add(b3ParticleBodyContact* c)
 	m_bodyContacts[m_bodyContactCount++] = c;
 }
 
+void b3ClothSolver::Add(b3ParticleTriangleContact* c)
+{
+	m_triangleContacts[m_triangleContactCount++] = c;
+}
+
 void b3ClothSolver::ApplyForces()
 {
 	for (u32 i = 0; i < m_forceCount; ++i)
@@ -266,21 +276,26 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
 	contactSolverDef.velocities = m_solverData.v;
 	contactSolverDef.bodyContactCount = m_bodyContactCount;
 	contactSolverDef.bodyContacts = m_bodyContacts;
+	contactSolverDef.triangleContactCount = m_triangleContactCount;
+	contactSolverDef.triangleContacts = m_triangleContacts;
 
 	b3ClothContactSolver contactSolver(contactSolverDef);
 
 	{
 		contactSolver.InitializeBodyContactConstraints();
+		contactSolver.InitializeTriangleContactConstraints();
 	}
 
 	{
-		contactSolver.WarmStart();
+		contactSolver.WarmStartBodyContactConstraints();
+		contactSolver.WarmStartTriangleContactConstraints();
 	}
 
 	{
 		for (u32 i = 0; i < velocityIterations; ++i)
 		{
 			contactSolver.SolveBodyContactVelocityConstraints();
+			contactSolver.SolveTriangleContactVelocityConstraints();
 		}
 	}
 
@@ -297,9 +312,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
 		for (u32 i = 0; i < positionIterations; ++i)
 		{
 			bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
+			bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints();
 
-			if (bodyContactsSolved)
+			if (bodyContactsSolved && triangleContactsSolved)
 			{
+				// Early out if the position errors are small.
 				positionSolved = true;
 				break;
 			}
diff --git a/src/bounce/cloth/particle.cpp b/src/bounce/cloth/particle.cpp
index 3316a77..5eeddb0 100644
--- a/src/bounce/cloth/particle.cpp
+++ b/src/bounce/cloth/particle.cpp
@@ -18,6 +18,7 @@
 
 #include <bounce/cloth/particle.h>
 #include <bounce/cloth/cloth.h>
+#include <bounce/cloth/cloth_mesh.h>
 
 void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
 {
@@ -42,8 +43,20 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
 	m_velocity = def.velocity;
 	m_force = def.force;
 	m_translation.SetZero();
-	m_mass = 0.0f;
-	m_invMass = 0.0f;
+	m_mass = def.mass;
+
+	if (m_mass == 0.0f)
+	{
+		m_type = e_staticParticle;
+		m_mass = 1.0f;
+		m_invMass = 1.0f;
+	}
+	else
+	{
+		m_type = e_dynamicParticle;
+		m_invMass = 1.0f / m_mass;
+	}
+
 	m_radius = def.radius;
 	m_friction = def.friction;
 	m_userData = nullptr;
@@ -62,7 +75,48 @@ void b3Particle::Synchronize()
 	b3AABB3 aabb;
 	aabb.Set(m_position, m_radius);
 
-	m_cloth->m_particleTree.UpdateNode(m_treeId, aabb);
+	b3Vec3 displacement = m_cloth->m_dt * m_velocity;
+
+	m_cloth->m_contactManager.m_broadPhase.MoveProxy(m_aabbProxy.broadPhaseId, aabb, displacement);
+}
+
+void b3Particle::SynchronizeTriangles()
+{
+	if (m_vertex == ~0)
+	{
+		return;
+	}
+
+	for (u32 i = 0; i < m_cloth->m_mesh->triangleCount; ++i)
+	{
+		b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + i;
+
+		if (triangle->v1 == m_vertex || triangle->v2 == m_vertex || triangle->v3 == m_vertex)
+		{
+			m_cloth->SynchronizeTriangle(i);
+		}
+	}
+}
+
+void b3Particle::DestroyContacts()
+{
+	// Destroy body contacts
+	m_bodyContact.active = false;
+
+	// Destroy triangle contacts
+	b3ParticleTriangleContact* c = m_cloth->m_contactManager.m_particleTriangleContactList.m_head;
+	while (c)
+	{
+		if (c->m_p1 == this)
+		{
+			b3ParticleTriangleContact* quack = c;
+			c = c->m_next;
+			m_cloth->m_contactManager.Destroy(quack);
+			continue;
+		}
+
+		c = c->m_next;
+	}
 }
 
 void b3Particle::SetType(b3ParticleType type)
@@ -81,7 +135,11 @@ void b3Particle::SetType(b3ParticleType type)
 		m_translation.SetZero();
 		
 		Synchronize();
+		SynchronizeTriangles();
 	}
 
-	m_bodyContact.active = false;
+	DestroyContacts();
+	
+	// Move the proxy so new contacts can be created.
+	m_cloth->m_contactManager.m_broadPhase.TouchProxy(m_aabbProxy.broadPhaseId);
 }
\ No newline at end of file