Support cloth self friction, thickness. Small refactor

include/bounce/cloth/cloth.h

@@ -29,6 +29,8 @@ class b3World;
 class b3Shape;
 
 class b3Particle;
+class b3ClothTriangle;
+
 class b3Force;
 struct b3ParticleBodyContact;
 
@@ -62,6 +64,8 @@ struct b3ClothDef
 		structural = 0.0f;
 		bending = 0.0f;
 		damping = 0.0f;
+		thickness = 0.0f;
+		friction = 0.2f;
 	}
 
 	// Cloth mesh 
@@ -78,6 +82,12 @@ struct b3ClothDef
 
 	// Damping stiffness
 	float32 damping;
+
+	// Cloth thickness
+	float32 thickness;
+
+	// Cloth coefficient of friction
+	float32 friction;
 };
 
 // A cloth represents a deformable surface as a collection of particles.
@@ -126,6 +136,9 @@ public:
 	// Return the particle associated with the given vertex.
 	b3Particle* GetVertexParticle(u32 i);
 
+	// Return the cloth triangle given the triangle index.
+	b3ClothTriangle* GetTriangle(u32 i);
+
 	// Return the list of particles in this cloth.
 	const b3List2<b3Particle>& GetParticleList() const;
 
@@ -142,6 +155,7 @@ public:
 	void Draw() const;
 private:
 	friend class b3Particle;
+	friend class b3ClothTriangle;
 	friend class b3ClothContactManager;
 
 	// Compute mass of each particle.
@@ -153,12 +167,6 @@ private:
 	// 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;
 
@@ -174,8 +182,8 @@ private:
 	// Vertex particles
 	b3Particle** m_vertexParticles;
 
-	// Triangle proxies
+	// Triangles
-	b3ClothAABBProxy* m_triangleProxies;
+	b3ClothTriangle* m_triangles;
 
 	// Cloth density
 	float32 m_density;

include/bounce/cloth/cloth_contact.h

@@ -22,8 +22,7 @@
 #include <bounce/common/template/list.h>
 
 class b3Particle;
-struct b3ClothMeshTriangle;
+class b3ClothTriangle;
-struct b3ClothAABBProxy;
 
 // Contact between particle and a triangle
 class b3ParticleTriangleContact
@@ -32,6 +31,7 @@ public:
 private:
 	friend class b3Cloth;
 	friend class b3Particle;
+	friend class b3ClothTriangle;
 	friend class b3ClothContactManager;
 	friend class b3List2<b3ParticleTriangleContact>;
 	friend class b3ClothContactSolver;
@@ -45,16 +45,17 @@ private:
 	b3Particle* m_p1;
 
 	// Triangle
-	b3ClothMeshTriangle* m_triangle;
+	b3ClothTriangle* m_t2;
-	b3ClothAABBProxy* m_triangleProxy;
 	b3Particle* m_p2;
 	b3Particle* m_p3;
 	b3Particle* m_p4;
 
-	float32 m_normalImpulse;
-
 	float32 m_w2, m_w3, m_w4;
 
+	float32 m_normalImpulse;
+	float32 m_tangentImpulse1;
+	float32 m_tangentImpulse2;
+
 	bool m_active;
 
 	b3ParticleTriangleContact* m_prev;

include/bounce/cloth/cloth_contact_solver.h

@@ -96,9 +96,17 @@ struct b3ClothSolverTriangleContactVelocityConstraint
 	float32 wB, wC, wD;
 
 	b3Vec3 normal;
-
 	float32 normalMass;
 	float32 normalImpulse;
+
+	float32 friction;
+
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	float32 tangentMass1;
+	float32 tangentMass2;
+	float32 tangentImpulse1;
+	float32 tangentImpulse2;
 };
 
 struct b3ClothSolverTriangleContactPositionConstraint

include/bounce/cloth/cloth_triangle.h

@@ -0,0 +1,98 @@
+/*
+* 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_TRIANGLE_H
+#define B3_CLOTH_TRIANGLE_H
+
+#include <bounce/cloth/particle.h>
+
+// A cloth triangle
+class b3ClothTriangle
+{
+public:
+	// Return the triangle index.
+	u32 GetTriangle() const;
+
+	// Set the triangle radius.
+	void SetRadius(float32 radius);
+
+	// Return the triangle radius.
+	float32 GetRadius() const;
+
+	// Set the triangle coefficient of friction.
+	void SetFriction(float32 friction);
+
+	// Return the triangle coefficient of friction.
+	float32 GetFriction() const;
+private:
+	friend class b3Cloth;
+	friend class b3Particle;
+	friend class b3ClothContactManager;
+	friend class b3ParticleTriangleContact;
+	friend class b3ClothSolver;
+	friend class b3ClothContactSolver;
+
+	b3ClothTriangle() { }
+	~b3ClothTriangle() { }
+
+	// Synchronize AABB
+	void Synchronize(const b3Vec3& displacement);
+
+	// Cloth
+	b3Cloth* m_cloth;
+
+	// Triangle index
+	u32 m_triangle;
+
+	// Radius
+	float32 m_radius;
+
+	// Coefficient of friction
+	float32 m_friction;
+
+	// AABB Proxy
+	b3ClothAABBProxy m_aabbProxy;
+};
+
+inline u32 b3ClothTriangle::GetTriangle() const
+{
+	return m_triangle;
+}
+
+inline void b3ClothTriangle::SetRadius(float32 radius) 
+{
+	m_radius = radius;
+	Synchronize(b3Vec3_zero);
+}
+
+inline float32 b3ClothTriangle::GetRadius() const
+{
+	return m_radius;
+}
+
+inline void b3ClothTriangle::SetFriction(float32 friction)
+{
+	m_friction = friction;
+}
+
+inline float32 b3ClothTriangle::GetFriction() const
+{
+	return m_friction;
+}
+
+#endif

include/bounce/cloth/particle.h

@@ -103,7 +103,6 @@ struct b3ClothAABBProxy
 {
 	b3ClothAABBProxyType type;
 	void* data;
-	u32 index;
 	u32 broadPhaseId;
 };
 
@@ -160,6 +159,7 @@ public:
 private:
 	friend class b3List2<b3Particle>;
 	friend class b3Cloth;
+	friend class b3ClothTriangle;
 	friend class b3ClothSolver;
 	friend class b3ClothContactManager;
 	friend class b3ParticleTriangleContact;
@@ -171,7 +171,7 @@ private:
 	~b3Particle();
 
 	// Synchronize particle AABB
-	void Synchronize();
+	void Synchronize(const b3Vec3& displacement);
 
 	// Synchronize triangles AABB
 	void SynchronizeTriangles();
@@ -246,10 +246,12 @@ inline u32 b3Particle::GetVertex() const
 
 inline void b3Particle::SetPosition(const b3Vec3& position)
 {
+	b3Vec3 displacement = position - m_position;
+
 	m_position = position;
 	m_translation.SetZero();
 
-	Synchronize();
+	Synchronize(displacement);
 	SynchronizeTriangles();
 }
 
@@ -281,7 +283,7 @@ inline void b3Particle::SetRadius(float32 radius)
 {
 	m_radius = radius;
 	
-	Synchronize();
+	Synchronize(b3Vec3_zero);
 }
 
 inline float32 b3Particle::GetRadius() const

src/bounce/cloth/cloth.cpp

@@ -19,6 +19,7 @@
 #include <bounce/cloth/cloth.h>
 #include <bounce/cloth/cloth_mesh.h>
 #include <bounce/cloth/particle.h>
+#include <bounce/cloth/cloth_triangle.h>
 #include <bounce/cloth/force.h>
 #include <bounce/cloth/spring_force.h>
 #include <bounce/cloth/cloth_solver.h>
@@ -161,11 +162,10 @@ 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;
 
-	// Create particles
+	// Initialize particles
 	m_vertexParticles = (b3Particle**)b3Alloc(m->vertexCount * sizeof(b3Particle*));
 	for (u32 i = 0; i < m->vertexCount; ++i)
 	{
@@ -176,7 +176,6 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 
 		b3Particle* p = CreateParticle(pd);
 
-		p->m_aabbProxy.index = i;
 		p->m_vertex = i;
 		m_vertexParticles[i] = p;
 	}
@@ -184,7 +183,32 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 	// Compute mass
 	ComputeMass();
 
-	// Create forces
+	// Initialize triangles
+	m_triangles = (b3ClothTriangle*)b3Alloc(m_mesh->triangleCount * sizeof(b3ClothTriangle));
+	for (u32 i = 0; i < m_mesh->triangleCount; ++i)
+	{
+		b3ClothMeshTriangle* meshTriangle = m_mesh->triangles + i;
+		b3ClothTriangle* triangle = m_triangles + i;
+
+		triangle->m_cloth = this;
+		triangle->m_radius = def.thickness;
+		triangle->m_friction = def.friction;
+		triangle->m_triangle = i;
+
+		b3Vec3 v1 = m_mesh->vertices[meshTriangle->v1];
+		b3Vec3 v2 = m_mesh->vertices[meshTriangle->v2];
+		b3Vec3 v3 = m_mesh->vertices[meshTriangle->v3];
+
+		b3AABB3 aabb;
+		aabb.Set(v1, v2, v3);
+		aabb.Extend(triangle->m_radius);
+
+		triangle->m_aabbProxy.type = e_triangleProxy;
+		triangle->m_aabbProxy.data = triangle;
+		triangle->m_aabbProxy.broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &triangle->m_aabbProxy);
+	}
+
+	// Initialize forces
 	b3StackAllocator* allocator = &m_stackAllocator;
 
 	// Worst-case edge memory
@@ -251,32 +275,15 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 		}
 	}
 
-	// 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();
 	m_world = nullptr;
 }
 
 b3Cloth::~b3Cloth()
 {
+	b3Free(m_vertexParticles);
+	b3Free(m_triangles);
+
 	b3Particle* p = m_particleList.m_head;
 	while (p)
 	{
@@ -285,9 +292,6 @@ b3Cloth::~b3Cloth()
 		p0->~b3Particle();
 	}
 
-	b3Free(m_vertexParticles);
-	b3Free(m_triangleProxies);
-
 	b3Force* f = m_forceList.m_head;
 	while (f)
 	{
@@ -307,7 +311,6 @@ b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def)
 
 	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);
@@ -373,6 +376,12 @@ b3Particle* b3Cloth::GetVertexParticle(u32 i)
 	return m_vertexParticles[i];
 }
 
+b3ClothTriangle* b3Cloth::GetTriangle(u32 i)
+{
+	B3_ASSERT(i < m_mesh->triangleCount);
+	return m_triangles + i;
+}
+
 void b3Cloth::ComputeMass()
 {
 	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
@@ -428,7 +437,8 @@ struct b3ClothRayCastSingleCallback
 			return input.maxFraction;
 		}
 
-		u32 triangleIndex = proxy->index;
+		b3ClothTriangle* triangle = (b3ClothTriangle*)proxy->data;
+		u32 triangleIndex = triangle->GetTriangle();
 
 		b3RayCastOutput subOutput;
 		if (cloth->RayCast(&subOutput, &input, triangleIndex))
@@ -436,7 +446,7 @@ struct b3ClothRayCastSingleCallback
 			// Ray hits triangle.
 			if (subOutput.fraction < output0.fraction)
 			{
-				triangle0 = proxy->index;
+				triangle0 = triangleIndex;
 				output0.fraction = subOutput.fraction;
 				output0.normal = subOutput.normal;
 			}
@@ -638,8 +648,6 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
 {
 	B3_PROFILE("Cloth Step");
 
-	m_dt = dt;
-
 	// Update particle-body contacts
 	UpdateParticleBodyContacts();
 
@@ -662,13 +670,29 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
 	// Synchronize particles
 	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
 	{
-		p->Synchronize();
+		b3Vec3 displacement = dt * p->m_velocity;
+
+		p->Synchronize(displacement);
 	}
 
 	// Synchronize triangles
 	for (u32 i = 0; i < m_mesh->triangleCount; ++i)
 	{
-		SynchronizeTriangle(i);
+		b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
+
+		b3Particle* p1 = m_vertexParticles[triangle->v1];
+		b3Particle* p2 = m_vertexParticles[triangle->v2];
+		b3Particle* p3 = m_vertexParticles[triangle->v3];
+
+		b3Vec3 v1 = p1->m_velocity;
+		b3Vec3 v2 = p2->m_velocity;
+		b3Vec3 v3 = p3->m_velocity;
+
+		b3Vec3 velocity = (v1 + v2 + v3) / 3.0f;
+
+		b3Vec3 displacement = dt * velocity;
+
+		m_triangles[i].Synchronize(displacement);
 	}
 
 #if B3_ENABLE_SELF_COLLISION
@@ -679,34 +703,6 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
 #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
 {
 	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)

src/bounce/cloth/cloth_contact_manager.cpp

@@ -20,6 +20,7 @@
 #include <bounce/cloth/cloth.h>
 #include <bounce/cloth/cloth_mesh.h>
 #include <bounce/cloth/particle.h>
+#include <bounce/cloth/cloth_triangle.h>
 
 b3ClothContactManager::b3ClothContactManager() : 
 	m_particleTriangleContactBlocks(sizeof(b3ParticleTriangleContact))
@@ -62,7 +63,8 @@ void b3ClothContactManager::AddPair(void* data1, void* data2)
 
 	b3Particle* p1 = (b3Particle*)proxy1->data;
 
-	b3ClothMeshTriangle* triangle = (b3ClothMeshTriangle*)proxy2->data;
+	b3ClothTriangle* t2 = (b3ClothTriangle*)proxy2->data;
+	b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + t2->m_triangle;
 	b3Particle* p2 = m_cloth->m_vertexParticles[triangle->v1];
 	b3Particle* p3 = m_cloth->m_vertexParticles[triangle->v2];
 	b3Particle* p4 = m_cloth->m_vertexParticles[triangle->v3];
@@ -70,7 +72,7 @@ void b3ClothContactManager::AddPair(void* data1, void* data2)
 	// 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)
+		if (c->m_p1 == p1 && c->m_t2 == t2)
 		{
 			// A contact already exists.
 			return;
@@ -86,7 +88,7 @@ void b3ClothContactManager::AddPair(void* data1, void* data2)
 		return;
 	}
 
-	if (triangle->v1 == p1->m_vertex || triangle->v2 == p1->m_vertex || triangle->v3 == p1->m_vertex)
+	if (p1 == p2 || p1 == p3 || p1 == p4)
 	{
 		// The entities must not collide with each other.
 		return;
@@ -96,12 +98,13 @@ void b3ClothContactManager::AddPair(void* data1, void* data2)
 	b3ParticleTriangleContact* c = CreateParticleTriangleContact();
 
 	c->m_p1 = p1;
-	c->m_triangle = triangle;
+	c->m_t2 = t2;
-	c->m_triangleProxy = proxy2;
 	c->m_p2 = p2;
 	c->m_p3 = p3;
 	c->m_p4 = p4;
 	c->m_normalImpulse = 0.0f;
+	c->m_tangentImpulse1 = 0.0f;
+	c->m_tangentImpulse2 = 0.0f;
 	c->m_active = false;
 
 	// Add the contact to the cloth contact list.
@@ -144,7 +147,7 @@ void b3ClothContactManager::UpdateContacts()
 		}
 
 		u32 proxy1 = c->m_p1->m_aabbProxy.broadPhaseId;
-		u32 proxy2 = c->m_triangleProxy->broadPhaseId;
+		u32 proxy2 = c->m_t2->m_aabbProxy.broadPhaseId;
 
 		// Destroy the contact if primitive AABBs are not overlapping.
 		bool overlap = m_broadPhase.TestOverlap(proxy1, proxy2);

src/bounce/cloth/cloth_contact_solver.cpp

@@ -19,6 +19,7 @@
 #include <bounce/cloth/cloth_contact_solver.h>
 #include <bounce/cloth/cloth_contact_manager.h>
 #include <bounce/cloth/particle.h>
+#include <bounce/cloth/cloth_triangle.h>
 #include <bounce/cloth/dense_vec3.h>
 #include <bounce/common/memory/stack_allocator.h>
 #include <bounce/dynamics/shapes/shape.h>
@@ -214,7 +215,7 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints()
 		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->triangleRadius = c->m_t2->m_radius;
 
 		pc->wB = c->m_w2;
 		pc->wC = c->m_w3;
@@ -255,6 +256,14 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints()
 
 		vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
 		vc->normalImpulse = c->m_normalImpulse;
+
+		vc->friction = b3MixFriction(c->m_p1->m_friction, c->m_t2->m_friction);
+		vc->tangent1 = b3Perp(n);
+		vc->tangent2 = b3Cross(vc->tangent1, n);
+		vc->tangentMass1 = vc->normalMass;
+		vc->tangentMass2 = vc->normalMass;
+		vc->tangentImpulse1 = c->m_tangentImpulse1;
+		vc->tangentImpulse2 = c->m_tangentImpulse2;
 	}
 }
 
@@ -328,21 +337,57 @@ void b3ClothContactSolver::WarmStartTriangleContactConstraints()
 		float32 mC = vc->invMassC;
 		float32 mD = vc->invMassD;
 
-		b3Vec3 JA = -vc->normal;
+		{
-		b3Vec3 JB = vc->wB * vc->normal;
+			b3Vec3 JA = -vc->normal;
-		b3Vec3 JC = vc->wC * vc->normal;
+			b3Vec3 JB = vc->wB * vc->normal;
-		b3Vec3 JD = vc->wD * vc->normal;
+			b3Vec3 JC = vc->wC * vc->normal;
+			b3Vec3 JD = vc->wD * vc->normal;
 
-		b3Vec3 PA = vc->normalImpulse * JA;
+			b3Vec3 PA = vc->normalImpulse * JA;
-		b3Vec3 PB = vc->normalImpulse * JB;
+			b3Vec3 PB = vc->normalImpulse * JB;
-		b3Vec3 PC = vc->normalImpulse * JC;
+			b3Vec3 PC = vc->normalImpulse * JC;
-		b3Vec3 PD = vc->normalImpulse * JD;
+			b3Vec3 PD = vc->normalImpulse * JD;
 
-		vA += mA * PA;
+			vA += mA * PA;
-		vB += mB * PB;
+			vB += mB * PB;
-		vC += mC * PC;
+			vC += mC * PC;
-		vD += mD * PD;
+			vD += mD * PD; 
+		}
 
+		{
+			b3Vec3 JA = -vc->tangent1;
+			b3Vec3 JB = vc->wB * vc->tangent1;
+			b3Vec3 JC = vc->wC * vc->tangent1;
+			b3Vec3 JD = vc->wD * vc->tangent1;
+
+			b3Vec3 PA = vc->tangentImpulse1 * JA;
+			b3Vec3 PB = vc->tangentImpulse1 * JB;
+			b3Vec3 PC = vc->tangentImpulse1 * JC;
+			b3Vec3 PD = vc->tangentImpulse1 * JD;
+
+			vA += mA * PA;
+			vB += mB * PB;
+			vC += mC * PC;
+			vD += mD * PD; 
+		}
+		
+		{
+			b3Vec3 JA = -vc->tangent2;
+			b3Vec3 JB = vc->wB * vc->tangent2;
+			b3Vec3 JC = vc->wC * vc->tangent2;
+			b3Vec3 JD = vc->wD * vc->tangent2;
+
+			b3Vec3 PA = vc->tangentImpulse2 * JA;
+			b3Vec3 PB = vc->tangentImpulse2 * JB;
+			b3Vec3 PC = vc->tangentImpulse2 * JC;
+			b3Vec3 PD = vc->tangentImpulse2 * JD;
+
+			vA += mA * PA;
+			vB += mB * PB;
+			vC += mC * PC;
+			vD += mD * PD;
+		}
+		
 		v[indexA] = vA;
 		v[indexB] = vB;
 		v[indexC] = vC;
@@ -467,30 +512,95 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
 		b3Vec3 vC = v[indexC];
 		b3Vec3 vD = v[indexD];
 
-		b3Vec3 vCB = wB * vB + wC * vC + wD * vD;
+		// Solve normal constraint.
+		{
+			b3Vec3 vCB = wB * vB + wC * vC + wD * vD;
 
-		float32 Cdot = b3Dot(vCB - vA, vc->normal);
+			float32 Cdot = b3Dot(vCB - vA, vc->normal);
 
-		float32 impulse = -vc->normalMass * Cdot;
+			float32 impulse = -vc->normalMass * Cdot;
 
-		float32 oldImpulse = vc->normalImpulse;
+			float32 oldImpulse = vc->normalImpulse;
-		vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
+			vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
-		impulse = vc->normalImpulse - oldImpulse;
+			impulse = vc->normalImpulse - oldImpulse;
 
-		b3Vec3 JA = -vc->normal;
+			b3Vec3 JA = -vc->normal;
-		b3Vec3 JB = wB * vc->normal;
+			b3Vec3 JB = wB * vc->normal;
-		b3Vec3 JC = wC * vc->normal;
+			b3Vec3 JC = wC * vc->normal;
-		b3Vec3 JD = wD * vc->normal;
+			b3Vec3 JD = wD * vc->normal;
 
-		b3Vec3 PA = impulse * JA;
+			b3Vec3 PA = impulse * JA;
-		b3Vec3 PB = impulse * JB;
+			b3Vec3 PB = impulse * JB;
-		b3Vec3 PC = impulse * JC;
+			b3Vec3 PC = impulse * JC;
-		b3Vec3 PD = impulse * JD;
+			b3Vec3 PD = impulse * JD;
 
-		vA += mA * PA;
+			vA += mA * PA;
-		vB += mB * PB;
+			vB += mB * PB;
-		vC += mC * PC;
+			vC += mC * PC;
-		vD += mD * PD;
+			vD += mD * PD;
+		}
+
+		// Solve tangent constraint.
+		{
+			float32 hi = vc->friction * vc->normalImpulse;
+			float32 lo = -hi;
+
+			b3Vec3 vCB = wB * vB + wC * vC + wD * vD;
+
+			float32 Cdot = b3Dot(vCB - vA, vc->tangent1);
+
+			float32 impulse = -vc->tangentMass1 * Cdot;
+
+			float32 oldImpulse = vc->tangentImpulse1;
+			vc->tangentImpulse1 = b3Clamp(vc->tangentImpulse1 + impulse, lo, hi);
+			impulse = vc->tangentImpulse1 - oldImpulse;
+
+			b3Vec3 JA = -vc->tangent1;
+			b3Vec3 JB = wB * vc->tangent1;
+			b3Vec3 JC = wC * vc->tangent1;
+			b3Vec3 JD = wD * vc->tangent1;
+
+			b3Vec3 PA = impulse * JA;
+			b3Vec3 PB = impulse * JB;
+			b3Vec3 PC = impulse * JC;
+			b3Vec3 PD = impulse * JD;
+
+			vA += mA * PA;
+			vB += mB * PB;
+			vC += mC * PC;
+			vD += mD * PD;
+		}
+
+		// Solve bitangent constraint.
+		{
+			float32 hi = vc->friction * vc->normalImpulse;
+			float32 lo = -hi;
+
+			b3Vec3 vCB = wB * vB + wC * vC + wD * vD;
+
+			float32 Cdot = b3Dot(vCB - vA, vc->tangent2);
+
+			float32 impulse = -vc->tangentMass2 * Cdot;
+
+			float32 oldImpulse = vc->tangentImpulse2;
+			vc->tangentImpulse2 = b3Clamp(vc->tangentImpulse2 + impulse, lo, hi);
+			impulse = vc->tangentImpulse2 - oldImpulse;
+
+			b3Vec3 JA = -vc->tangent2;
+			b3Vec3 JB = wB * vc->tangent2;
+			b3Vec3 JC = wC * vc->tangent2;
+			b3Vec3 JD = wD * vc->tangent2;
+
+			b3Vec3 PA = impulse * JA;
+			b3Vec3 PB = impulse * JB;
+			b3Vec3 PC = impulse * JC;
+			b3Vec3 PD = impulse * JD;
+
+			vA += mA * PA;
+			vB += mB * PB;
+			vC += mC * PC;
+			vD += mD * PD;
+		}
 
 		v[indexA] = vA;
 		v[indexB] = vB;
@@ -516,6 +626,8 @@ void b3ClothContactSolver::StoreImpulses()
 		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
 
 		c->m_normalImpulse = vc->normalImpulse;
+		c->m_tangentImpulse1 = vc->tangentImpulse1;
+		c->m_tangentImpulse2 = vc->tangentImpulse2;
 	}
 }
 

src/bounce/cloth/cloth_triangle.cpp

@@ -0,0 +1,40 @@
+/*
+* 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_triangle.h>
+#include <bounce/cloth/cloth.h>
+#include <bounce/cloth/cloth_mesh.h>
+
+void b3ClothTriangle::Synchronize(const b3Vec3& displacement)
+{
+	b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + m_triangle;
+
+	b3Particle* p1 = m_cloth->m_vertexParticles[triangle->v1];
+	b3Particle* p2 = m_cloth->m_vertexParticles[triangle->v2];
+	b3Particle* p3 = m_cloth->m_vertexParticles[triangle->v3];
+
+	b3Vec3 x1 = p1->m_position;
+	b3Vec3 x2 = p2->m_position;
+	b3Vec3 x3 = p3->m_position;
+
+	b3AABB3 aabb;
+	aabb.Set(x1, x2, x3);
+	aabb.Extend(m_radius);
+
+	m_cloth->m_contactManager.m_broadPhase.MoveProxy(m_aabbProxy.broadPhaseId, aabb, displacement);
+}

src/bounce/cloth/particle.cpp

@@ -19,6 +19,7 @@
 #include <bounce/cloth/particle.h>
 #include <bounce/cloth/cloth.h>
 #include <bounce/cloth/cloth_mesh.h>
+#include <bounce/cloth/cloth_triangle.h>
 
 void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
 {
@@ -70,13 +71,11 @@ b3Particle::~b3Particle()
 
 }
 
-void b3Particle::Synchronize()
+void b3Particle::Synchronize(const b3Vec3& displacement)
 {
 	b3AABB3 aabb;
 	aabb.Set(m_position, m_radius);
 
-	b3Vec3 displacement = m_cloth->m_dt * m_velocity;
-
 	m_cloth->m_contactManager.m_broadPhase.MoveProxy(m_aabbProxy.broadPhaseId, aabb, displacement);
 }
 
@@ -93,7 +92,7 @@ void b3Particle::SynchronizeTriangles()
 
 		if (triangle->v1 == m_vertex || triangle->v2 == m_vertex || triangle->v3 == m_vertex)
 		{
-			m_cloth->SynchronizeTriangle(i);
+			m_cloth->GetTriangle(i)->Synchronize(b3Vec3_zero);
 		}
 	}
 }
@@ -134,7 +133,7 @@ void b3Particle::SetType(b3ParticleType type)
 		m_velocity.SetZero();
 		m_translation.SetZero();
 		
-		Synchronize();
+		Synchronize(b3Vec3_zero);
 		SynchronizeTriangles();
 	}