early out, bugfix

include/bounce/dynamics/cloth/particle.h

@@ -89,8 +89,8 @@ public:
 	b3Shape* s2;
 	float32 s;
 
-	bool f1_active, f2_active;
+	bool f_active;
-	b3FrictionForce f1, f2;
+	b3FrictionForce f;
 	
 	bool n_active, t1_active, t2_active;
 	b3Vec3 n, t1, t2;

include/bounce/dynamics/cloth/sparse_sym_mat33.h

@@ -85,6 +85,11 @@ inline const b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) const
 		u32 row_value_index = row_value_begin + row_value;
 		u32 row_value_column = value_columns[row_value_index];
 
+		if (row_value_column < j)
+		{
+			break;
+		}
+
 		if (row_value_column == j)
 		{
 			return values[row_value_index];
@@ -112,7 +117,12 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j)
 	{
 		u32 row_value_index = row_value_begin + row_value;
 		u32 row_value_column = value_columns[row_value_index];
-		
+
+		if (row_value_column < j)
+		{
+			break;
+		}
+
 		if (row_value_column == j)
 		{
 			return values[row_value_index];
@@ -120,30 +130,30 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j)
 	}
 
 	// Find insert position
-	u32 row_value_k = 0;
+	u32 row_value_position = 0;
 	for (u32 row_value = 0; row_value < row_value_count; ++row_value)
 	{
 		u32 row_value_index = row_value_begin + row_value;
 		u32 row_value_column = value_columns[row_value_index];
 		
-		if (row_value_column >= j)
+		if (row_value_column > j)
 		{
-			row_value_k = row_value;
+			row_value_position = row_value;
 			break;
 		}
 	}
 
 	// Shift the values
 	B3_ASSERT(value_count < value_capacity);
-	for (u32 row_value = value_count; row_value > row_value_begin + row_value_k; --row_value)
+	for (u32 row_value = value_count; row_value > row_value_begin + row_value_position; --row_value)
 	{
 		values[row_value] = values[row_value - 1];
 		value_columns[row_value] = value_columns[row_value - 1];
 	}
 
 	// Insert the value
-	value_columns[row_value_begin + row_value_k] = j;
+	value_columns[row_value_begin + row_value_position] = j;
-	values[row_value_begin + row_value_k].SetZero();
+	values[row_value_begin + row_value_position].SetZero();
 	++value_count;
 
 	// Shift the row pointers 
@@ -153,7 +163,7 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j)
 	}
 
 	// Return the inserted value
-	return values[row_value_begin + row_value_k];
+	return values[row_value_begin + row_value_position];
 }
 
 inline void b3SparseSymMat33::Diagonal(b3DiagMat33& out) const

src/bounce/dynamics/cloth/cloth.cpp

@@ -483,8 +483,7 @@ void b3Cloth::UpdateContacts()
 		b3BodyContact c0 = *c;
 
 		// Create a new contact
-		c->f1_active = false;
+		c->f_active = false;
-		c->f2_active = false;
 		c->n_active = false;
 		c->t1_active = false;
 		c->t2_active = false;
@@ -591,6 +590,11 @@ void b3Cloth::UpdateContacts()
 
 			c->t1 = t1;
 			c->t2 = t2;
+
+			//c->f_active = true;
+			//c->f.m_type = e_frictionForce;
+			//c->f.m_p = p;
+			//c->f.m_kd = 10.0f;
 		}
 
 		b3Vec3 ts[2];
@@ -609,18 +613,6 @@ 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];
@@ -633,30 +625,8 @@ void b3Cloth::UpdateContacts()
 				// Lock particle on surface
 				t_active[k] = true;
 			}
-			
-			if (t_active0[k] == true && t_active[k] == true)
-			{
-				float32 frictionForce = Ft0[k];
-
-				// Dynamic friction
-				if (frictionForce * frictionForce > maxFrictionForce * maxFrictionForce)
-				{
-					// Unlock particle off surface
-					//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];
 	}
@@ -687,14 +657,9 @@ 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)
+		if (p->m_contact.f_active)
 		{
-			solver.Add(&p->m_contact.f1);
+			solver.Add(&p->m_contact.f);
-		}
-
-		if (p->m_contact.f2_active)
-		{
-			solver.Add(&p->m_contact.f2);
 		}
 
 		if (p->m_contact.n_active)
@@ -706,8 +671,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 	// Solve
 	solver.Solve(dt, gravity);
 
-	// Clear external applied forces
+	// Clear external applied forces and translations
-	// Clear translations
 	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
 	{
 		p->m_force.SetZero();

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -223,7 +223,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		sx0[i] = p->m_x;
 	}
 
-	// Apply contact position correction
+	// Apply internal translations
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
 		b3BodyContact* c = m_contacts[i];

src/bounce/dynamics/cloth/particle.cpp

@@ -57,8 +57,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
 	m_x.SetZero();
 	m_vertex = ~0;
 
-	m_contact.f1_active = false;
+	m_contact.f_active = false;
-	m_contact.f2_active = false;
 	m_contact.n_active = false;
 	m_contact.t1_active = false;
 	m_contact.t2_active = false;
@@ -87,8 +86,7 @@ void b3Particle::SetType(b3ParticleType type)
 		m_velocity.SetZero();
 		m_translation.SetZero();
 
-		m_contact.f1_active = false;
+		m_contact.f_active = false;
-		m_contact.f2_active = false;
 		m_contact.n_active = false;
 		m_contact.t1_active = false;
 		m_contact.t2_active = false;