optimization, friction force

include/bounce/dynamics/cloth/cloth_solver.h

@@ -26,7 +26,7 @@ class b3StackAllocator;
 
 class b3Particle;
 class b3Force;
-struct b3BodyContact;
+class b3BodyContact;
 
 struct b3DenseVec3;
 struct b3DiagMat33;

include/bounce/dynamics/cloth/force.h

@@ -29,6 +29,7 @@ class b3Particle;
 // Force types
 enum b3ForceType
 {
+	e_frictionForce,
 	e_springForce,
 };
 

include/bounce/dynamics/cloth/particle.h

@@ -21,6 +21,7 @@
 
 #include <bounce/common/math/transform.h>
 #include <bounce/common/template/list.h>
+#include <bounce/dynamics/cloth/force.h>
 
 class b3Shape;
 class b3Cloth;
@@ -57,15 +58,43 @@ struct b3ParticleDef
 	void* userData;
 };
 
-// A contact between a particle and a solid
+class b3FrictionForce : public b3Force
-struct b3BodyContact
 {
+public:
+	b3FrictionForce() { }
+
+	~b3FrictionForce() { }
+	
+	void Apply(const b3ClothSolverData* data);
+
+	b3Particle* m_p;
+	float32 m_kd;
+};
+
+// A contact between a particle and a solid
+class b3BodyContact
+{
+public:
+	b3BodyContact()
+	{
+
+	}
+
+	~b3BodyContact()
+	{
+
+	}
+
 	b3Particle* p1;
 	b3Shape* s2;
 	float32 s;
+
+	bool f1_active, f2_active;
+	b3FrictionForce f1, f2;
+	
+	bool n_active, t1_active, t2_active;
 	b3Vec3 n, t1, t2;
 	float32 Fn, Ft1, Ft2;
-	bool n_active, t1_active, t2_active;
 };
 
 // A cloth particle.
@@ -115,6 +144,7 @@ private:
 	friend class b3ClothSolver;
 	friend class b3Force;
 	friend class b3SpringForce;
+	friend class b3FrictionForce;
 
 	b3Particle(const b3ParticleDef& def, b3Cloth* cloth);
 	~b3Particle();

src/bounce/dynamics/cloth/cloth.cpp

@@ -33,8 +33,6 @@
 
 #define B3_CLOTH_BENDING 0
 
-#define B3_CLOTH_FRICTION 1
-
 static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
 {
 	return i + 1 < 3 ? i + 1 : 0;
@@ -485,6 +483,8 @@ void b3Cloth::UpdateContacts()
 		b3BodyContact c0 = *c;
 
 		// Create a new contact
+		c->f1_active = false;
+		c->f2_active = false;
 		c->n_active = false;
 		c->t1_active = false;
 		c->t2_active = false;
@@ -565,8 +565,6 @@ void b3Cloth::UpdateContacts()
 			continue;
 		}
 
-#if B3_CLOTH_FRICTION == 1
-
 		// A friction force requires an associated normal force.
 		if (c0.n_active == false)
 		{
@@ -577,6 +575,7 @@ void b3Cloth::UpdateContacts()
 		b3Vec3 n = c->n;
 		float32 u = s->GetFriction();
 		float32 normalForce = c0.Fn;
+		float32 maxFrictionForce = u * normalForce;
 
 		// Relative velocity
 		b3Vec3 dv = p->m_velocity;
@@ -593,12 +592,6 @@ void b3Cloth::UpdateContacts()
 			c->t1 = t1;
 			c->t2 = t2;
 		}
-		else
-		{
-			c->t1_active = true;
-			c->t2_active = true;
-			continue;
-		}
 
 		b3Vec3 ts[2];
 		ts[0] = c->t1;
@@ -616,6 +609,18 @@ void b3Cloth::UpdateContacts()
 		Ft0[0] = c0.Ft1;
 		Ft0[1] = c0.Ft2;
 
+		bool f_active[2];
+		f_active[0] = c->f1_active;
+		f_active[1] = c->f2_active;
+
+		bool f_active0[2];
+		f_active0[0] = c0.f1_active;
+		f_active0[1] = c0.f2_active;
+
+		b3FrictionForce* sf[2];
+		sf[0] = &c->f1;
+		sf[1] = &c->f2;
+
 		for (u32 k = 0; k < 2; ++k)
 		{
 			b3Vec3 t = ts[k];
@@ -631,24 +636,30 @@ void b3Cloth::UpdateContacts()
 			
 			if (t_active0[k] == true && t_active[k] == true)
 			{
-				// The contact persists
+				float32 frictionForce = Ft0[k];
-				float32 maxForce = u * normalForce;
 
-				if (Ft0[k] * Ft0[k] > maxForce * maxForce)
+				// Dynamic friction
+				if (frictionForce * frictionForce > maxFrictionForce * maxFrictionForce)
 				{
 					// Unlock particle off surface
-					t_active[k] = false;
+					//t_active[k] = false;
+
+					// Apply dynamic friction
+					//f_active[k] = true;
+
+					//sf[k]->m_type = e_frictionForce;
+					//sf[k]->m_p = p;
+					//sf[k]->m_kd = 100.0f;
 				}
 			}
 		}
 
+		c->f1_active = f_active[0];
+		c->f2_active = f_active[1];
+
 		c->t1_active = t_active[0];
 		c->t2_active = t_active[1];
-
-#endif
-
 	}
-
 }
 
 void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
@@ -659,7 +670,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 	b3ClothSolverDef solverDef;
 	solverDef.stack = &m_world->m_stackAllocator;
 	solverDef.particleCapacity = m_particleList.m_count;
-	solverDef.forceCapacity = m_forceList.m_count;
+	solverDef.forceCapacity = m_forceList.m_count + (2 * m_particleList.m_count);
 	solverDef.contactCapacity = m_particleList.m_count;
 
 	b3ClothSolver solver(solverDef);
@@ -676,6 +687,16 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 
 	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
 	{
+		if (p->m_contact.f1_active)
+		{
+			solver.Add(&p->m_contact.f1);
+		}
+
+		if (p->m_contact.f2_active)
+		{
+			solver.Add(&p->m_contact.f2);
+		}
+
 		if (p->m_contact.n_active)
 		{
 			solver.Add(&p->m_contact);

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -268,11 +268,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	{
 		m_particles[i]->m_position = sx[i];
 		m_particles[i]->m_velocity = sv[i];
-	}
 		
 		// Cache x to improve convergence
-	for (u32 i = 0; i < m_particleCount; ++i)
-	{
 		m_particles[i]->m_x = x[i];
 	}
 

src/bounce/dynamics/cloth/particle.cpp

@@ -18,6 +18,25 @@
 
 #include <bounce/dynamics/cloth/particle.h>
 #include <bounce/dynamics/cloth/cloth.h>
+#include <bounce/dynamics/cloth/cloth_solver.h>
+#include <bounce/dynamics/cloth/dense_vec3.h>
+#include <bounce/dynamics/cloth/sparse_sym_mat33.h>
+
+void b3FrictionForce::Apply(const b3ClothSolverData* data)
+{
+	b3DenseVec3& v = *data->v;
+	b3DenseVec3& f = *data->f;
+	b3SparseSymMat33& dfdv = *data->dfdv;
+
+	u32 i = m_p->m_solverId;
+
+	f[i] += -m_kd * v[i];
+	
+	b3Mat33 I; I.SetIdentity();
+	b3Mat33 Jv = -m_kd * I;
+
+	dfdv(i, i) += Jv;
+}
 
 b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
 {
@@ -34,6 +53,8 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
 	m_x.SetZero();
 	m_vertex = ~0;
 
+	m_contact.f1_active = false;
+	m_contact.f2_active = false;
 	m_contact.n_active = false;
 	m_contact.t1_active = false;
 	m_contact.t2_active = false;

src/bounce/dynamics/cloth/spring_force.cpp

@@ -69,38 +69,23 @@ void b3SpringForce::Apply(const b3ClothSolverData* data)
 
 	b3Mat33 I; I.SetIdentity();
 
+	m_f.SetZero();
+
+	if (m_ks > 0.0f)
+	{
 		b3Vec3 dx = x1 - x2;
 
 		float32 L = b3Length(dx);
 
-	b3Mat33 Jx;
-
 		if (L >= m_L0)
 		{
 			b3Vec3 n = dx / L;
 
-		// Tension
+			// Apply tension
-		m_f = -m_ks * (L - m_L0) * n;
+			m_f += -m_ks * (L - m_L0) * n;
 
 			// Jacobian
-		Jx = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx)));
+			b3Mat33 Jx11 = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx)));
-	}
-	else
-	{
-		m_f.SetZero();
-		Jx.SetZero();
-	}
-
-	// Damping
-	b3Vec3 dv = v1 - v2;
-
-	m_f += -m_kd * dv;
-	b3Mat33 Jv = -m_kd * I;
-
-	f[i1] += m_f;
-	f[i2] -= m_f;
-
-	b3Mat33 Jx11 = Jx;
 			b3Mat33 Jx12 = -Jx11;
 			//b3Mat33 Jx21 = Jx12;
 			b3Mat33 Jx22 = Jx11;
@@ -109,8 +94,17 @@ void b3SpringForce::Apply(const b3ClothSolverData* data)
 			dfdx(i1, i2) += Jx12;
 			//dfdx(i2, i1) += Jx21;
 			dfdx(i2, i2) += Jx22;
+		}
+	}
 
-	b3Mat33 Jv11 = Jv;
+	if (m_kd > 0.0f)
+	{
+		// Apply damping
+		b3Vec3 dv = v1 - v2;
+
+		m_f += -m_kd * dv;
+		
+		b3Mat33 Jv11 = -m_kd * I;
 		b3Mat33 Jv12 = -Jv11;
 		//b3Mat33 Jv21 = Jv12;
 		b3Mat33 Jv22 = Jv11;
@@ -120,3 +114,7 @@ void b3SpringForce::Apply(const b3ClothSolverData* data)
 		//dfdv(i2, i1) += Jv21;
 		dfdv(i2, i2) += Jv22;
 	}
+
+	f[i1] += m_f;
+	f[i2] -= m_f;
+}