through an acceleration constraint, the solver can remove acceleration from kinematic particles; consistency; in effect test update

examples/testbed/tests/cloth_test.h

@@ -118,13 +118,13 @@ public:
 		b3Vec3 dx = B - A;
 
 		b3Particle* p1 = m_cloth->GetParticle(t->v1);
-		m_cloth->Translate(p1, dx);
+		m_cloth->ApplyTranslation(p1, dx);
 		
 		b3Particle* p2 = m_cloth->GetParticle(t->v2);
-		m_cloth->Translate(p2, dx);
+		m_cloth->ApplyTranslation(p2, dx);
 		
 		b3Particle* p3 = m_cloth->GetParticle(t->v3);
-		m_cloth->Translate(p3, dx);
+		m_cloth->ApplyTranslation(p3, dx);
 	}
 
 	void StopDragging()

examples/testbed/tests/particle_types.h

@@ -96,7 +96,7 @@ public:
 			{
 				if (p->type == e_staticParticle)
 				{
-					m_cloth->Translate(p, d);
+					m_cloth->ApplyTranslation(p, d);
 				}
 
 				if (p->type == e_kinematicParticle)

examples/testbed/tests/table_cloth.h

@@ -69,6 +69,11 @@ public:
 			tableShape.m_hull = &m_tableHull;
 			tableShape.m_radius = 0.2f;
 
+			//b3CapsuleShape tableShape;
+			//tableShape.m_centers[0].Set(0.0f, 0.0f, -1.0f);
+			//tableShape.m_centers[1].Set(0.0f, 0.0f, 1.0f);
+			//tableShape.m_radius = 2.0f;
+
 			b3ShapeDef sd;
 			sd.shape = &tableShape;
 			sd.friction = 1.0f;
@@ -82,6 +87,7 @@ public:
 		return new TableCloth();
 	}
 
+	//b3GridMesh<2, 2> m_gridMesh;
 	b3GridMesh<10, 10> m_gridMesh;
 	b3ClothMeshMesh m_gridClothMeshMesh;
 	b3ClothMesh m_gridClothMesh;

examples/testbed/tests/tension_mapping.h

@@ -124,8 +124,8 @@ public:
 			u32 i1 = m_cloth->GetParticleIndex(p1);
 			u32 i2 = m_cloth->GetParticleIndex(p2);
 
-			tension[i1] += s->tension;
+			tension[i1] += s->f;
-			tension[i2] -= s->tension;
+			tension[i2] -= s->f;
 		}
 
 		for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i)

include/bounce/dynamics/cloth/cloth.h

@@ -145,14 +145,14 @@ struct b3Spring
 
 	// Solver temp
 
-	// Action tensile force (f_i entry)
+	// Force (f_i entry)
-	b3Vec3 tension;
+	b3Vec3 f;
 
 	// Jacobian (J_ii entry)
 	b3Mat33 Jx, Jv;
 
-	// Apply spring forces.
+	// Initialize forces and its derivatives.
-	void ApplyForces(const b3ClothSolverData* data);
+	void InitializeForces(const b3ClothSolverData* data);
 };
 
 // Read-only body contact between a particle and a solid
@@ -191,15 +191,15 @@ public:
 	// Set the type of a given particle.
 	void SetType(b3Particle* p, b3ParticleType type);
 
-	// Translate a given particle in the next time step.
-	void Translate(b3Particle* p, const b3Vec3& translation);
-
 	// Set the velocity of a given particle.
 	void SetVelocity(b3Particle* p, const b3Vec3& velocity);
 
 	// Apply a force to a given particle.
 	void ApplyForce(b3Particle* p, const b3Vec3& force);
 
+	// Apply a translation to a given particle.
+	void ApplyTranslation(b3Particle* p, const b3Vec3& translation);
+
 	// Return the number of springs in this cloth.
 	u32 GetSpringCount() const;
 
@@ -317,11 +317,6 @@ inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type)
 	}
 }
 
-inline void b3Cloth::Translate(b3Particle* p, const b3Vec3& translation)
-{
-	p->translation += translation;
-}
-
 inline void b3Cloth::SetVelocity(b3Particle* p, const b3Vec3& velocity)
 {
 	if (p->type == e_staticParticle)
@@ -340,6 +335,11 @@ inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force)
 	p->force += force;
 }
 
+inline void b3Cloth::ApplyTranslation(b3Particle* p, const b3Vec3& translation)
+{
+	p->translation += translation;
+}
+
 inline u32 b3Cloth::GetSpringCount() const
 {
 	return m_springCount;

include/bounce/dynamics/cloth/cloth_solver.h

@@ -25,6 +25,7 @@
 struct b3DenseVec3;
 struct b3DiagMat33;
 struct b3SparseMat33;
+struct b3SolverSparseMat33;
 
 struct b3Particle;
 struct b3Spring;
@@ -50,6 +51,13 @@ struct b3ClothSolverData
 	float32 invdt;
 };
 
+struct b3SpringForce
+{
+	u32 i1, i2;
+	b3Vec3 f;
+	b3Mat33 Jx, Jv;
+};
+
 struct b3AccelerationConstraint
 {
 	u32 i1;
@@ -69,11 +77,14 @@ public:
 
 	void Solve(float32 dt, const b3Vec3& gravity);
 private:
+	// Initialize forces.
+	void InitializeForces();
+	
 	// Initialize constraints.
 	void InitializeConstraints();
-	
+
 	// Compute A and b in Ax = b
-	void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
+	void Compute_A_b(b3SolverSparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
 
 	// Compute S and z.
 	void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z);

src/bounce/dynamics/cloth/cloth.cpp

@@ -31,10 +31,10 @@
 
 #define B3_CLOTH_BENDING 0
 
-#define B3_CLOTH_FRICTION 0
+#define B3_CLOTH_FRICTION 1
 
 // b3Spring
-void b3Spring::ApplyForces(const b3ClothSolverData* data)
+void b3Spring::InitializeForces(const b3ClothSolverData* data)
 {
 	u32 i1 = p1->solverId;
 	u32 i2 = p2->solverId;
@@ -51,36 +51,25 @@ void b3Spring::ApplyForces(const b3ClothSolverData* data)
 
 	if (b3Dot(dx, dx) >= L0 * L0)
 	{
-		// Tension
 		float32 L = b3Length(dx);
 		b3Vec3 n = dx / L;
 
-		b3Vec3 sf1 = -ks * (L - L0) * n;
+		// Tension
-		b3Vec3 sf2 = -sf1;
+		f = -ks * (L - L0) * n;
-
-		tension = sf1;
-
-		data->f[i1] += sf1;
-		data->f[i2] += sf2;
 
 		// Jacobian
 		Jx = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
 	}
 	else
 	{
-		tension.SetZero();
+		f.SetZero();
 		Jx.SetZero();
 	}
 
 	// Damping
 	b3Vec3 dv = v1 - v2;
 
-	b3Vec3 df1 = -kd * dv;
+	f += -kd * dv;
-	b3Vec3 df2 = -df1;
-
-	data->f[i1] += df1;
-	data->f[i2] += df2;
-
 	Jv = -kd * I;
 }
 
@@ -238,6 +227,9 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world)
 		c->n_active = false;
 		c->t1_active = false;
 		c->t2_active = false;
+		c->Fn = 0.0f;
+		c->Ft1 = 0.0f;
+		c->Ft2 = 0.0f;
 	}
 
 	// Compute mass
@@ -284,7 +276,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world)
 		s->L0 = b3Distance(p1->position, p2->position);
 		s->ks = def.ks;
 		s->kd = def.kd;
-		s->tension.SetZero();
+		s->f.SetZero();
 	}
 
 #if B3_CLOTH_BENDING
@@ -330,7 +322,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world)
 		s->L0 = 0.0f;
 		s->ks = def.ks;
 		s->kd = def.kd;
-		s->tension.SetZero();
+		s->f.SetZero();
 	}
 
 	B3_ASSERT(m_springCount <= springCapacity);
@@ -388,21 +380,13 @@ void b3Cloth::UpdateContacts()
 {
 	B3_PROFILE("Update Contacts");
 
-	// Clear active flags
-	for (u32 i = 0; i < m_particleCount; ++i)
-	{
-		m_contacts[i].n_active = false;
-		m_contacts[i].t1_active = false;
-		m_contacts[i].t2_active = false;
-	}
-
 	// Create contacts 
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
 		b3Particle* p = m_particles + i;
 
-		// Static particles can't participate in unilateral collisions.
+		// Static and kinematic particles can't participate in unilateral collisions.
-		if (p->type == e_staticParticle)
+		if (p->type != e_dynamicParticle)
 		{
 			continue;
 		}
@@ -412,6 +396,11 @@ void b3Cloth::UpdateContacts()
 		// Save the old contact
 		b3BodyContact c0 = *c;
 
+		// Create a new contact
+		c->n_active = false;
+		c->t1_active = false;
+		c->t2_active = false;
+
 		b3Sphere s1;
 		s1.vertex = p->position;
 		s1.radius = p->radius;
@@ -456,7 +445,7 @@ void b3Cloth::UpdateContacts()
 			c->t2 = b3Cross(c->t1, n);
 		}
 
-		// Update contact state
+		// Update the contact state
 		if (c0.n_active == true && c->n_active == true)
 		{
 			// The contact persists
@@ -496,7 +485,7 @@ void b3Cloth::UpdateContacts()
 			continue;
 		}
 
-		b3Shape* s = c->s;
+		b3Shape* s = c->s2;
 		b3Vec3 n = c->n;
 		float32 u = s->GetFriction();
 		float32 normalForce = c0.Fn;

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -48,7 +48,7 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 	m_contactCapacity = def.contactCapacity;
 	m_contactCount = 0;
 	m_contacts = (b3BodyContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3BodyContact*));
-	
+
 	m_constraintCapacity = def.particleCapacity;
 	m_constraintCount = 0;
 	m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
@@ -78,12 +78,20 @@ void b3ClothSolver::Add(b3Spring* s)
 	m_springs[m_springCount++] = s;
 }
 
+void b3ClothSolver::InitializeForces()
+{
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		m_springs[i]->InitializeForces(&m_solverData);
+	}
+}
+
 void b3ClothSolver::InitializeConstraints()
 {
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
 		b3Particle* p = m_particles[i];
-		if (p->type == e_staticParticle)
+		if (p->type != e_dynamicParticle)
 		{
 			b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
 			++m_constraintCount;
@@ -98,8 +106,6 @@ void b3ClothSolver::InitializeConstraints()
 		b3BodyContact* pc = m_contacts[i];
 		b3Particle* p = pc->p1;
 
-		B3_ASSERT(p->type != e_staticParticle);
-
 		b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
 		++m_constraintCount;
 		ac->i1 = p->solverId;
@@ -118,7 +124,7 @@ void b3ClothSolver::InitializeConstraints()
 				ac->ndof = 1;
 				ac->q = pc->t1;
 			}
-			
+
 			if (pc->t2_active)
 			{
 				ac->ndof = 1;
@@ -128,22 +134,28 @@ void b3ClothSolver::InitializeConstraints()
 	}
 }
 
-static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N)
+struct b3SolverSparseMat33 : public b3SparseMat33
 {
-	u32 size = M * N;
+	b3SolverSparseMat33(b3StackAllocator* a, u32 m, u32 n)
-	b3Mat33* elements = (b3Mat33*)allocator->Allocate(size * sizeof(b3Mat33));
+	{
-	u32* cols = (u32*)allocator->Allocate(size * sizeof(u32));
+		allocator = a;
-	u32* row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
+		M = m;
+		N = n;
+		valueCount = 0;
+		values = (b3Mat33*)allocator->Allocate(M * N * sizeof(b3Mat33));
+		cols = (u32*)allocator->Allocate(M * N * sizeof(u32));
+		row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
+	}
 
-	return b3SparseMat33(M, N, size, elements, row_ptrs, cols);
+	~b3SolverSparseMat33()
-}
+	{
+		allocator->Free(row_ptrs);
+		allocator->Free(cols);
+		allocator->Free(values);
+	}
 
-static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator)
+	b3StackAllocator* allocator;
-{
+};
-	allocator->Free(matrix.row_ptrs);
-	allocator->Free(matrix.cols);
-	allocator->Free(matrix.values);
-}
 
 void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 {
@@ -183,19 +195,25 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
 		b3BodyContact* c = m_contacts[i];
-		b3Particle* p = c->p1;	
+		b3Particle* p = c->p1;
 		sy[p->solverId] -= c->s * c->n;
 	}
 
-	// Apply spring forces and derivatives
+	// Initialize forces
+	InitializeForces();
+	
+	// Apply internal forces
 	for (u32 i = 0; i < m_springCount; ++i)
 	{
-		m_springs[i]->ApplyForces(&m_solverData);
+		b3Spring* s = m_springs[i];
+		sf[s->p1->solverId] += s->f;
+		sf[s->p2->solverId] -= s->f;
 	}
 
 	// Initialize constraints
 	InitializeConstraints();
 
+	// Compute S, z
 	b3DiagMat33 S(m_particleCount);
 	b3DenseVec3 z(m_particleCount);
 	Compute_S_z(S, z);
@@ -205,7 +223,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	// b = h * (f0 + h * dfdx * v0 + dfdx * y) 
 
 	// A
-	b3SparseMat33 A = b3AllocSparse(m_allocator, m_particleCount, m_particleCount);
+	b3SolverSparseMat33 A(m_allocator, m_particleCount, m_particleCount);
 
 	// b
 	b3DenseVec3 b(m_particleCount);
@@ -220,38 +238,17 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	Solve(x, iterations, A, b, S, z, sx0);
 	b3_clothSolverIterations = iterations;
 
-	// f = A * x - b
+	// Compute the new state
-	b3DenseVec3 f = A * x - b;
+	// Clamp large translations?
-
-	// Update state
 	float32 h = dt;
+	sv = sv + x;
+	sx = sx + h * sv + sy;
 
+	// Copy state buffers back to the particles
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
-		b3Particle* p = m_particles[i];
+		m_particles[i]->position = sx[i];
-		b3ParticleType type = p->type;
+		m_particles[i]->velocity = sv[i];
-
-		b3Vec3 ix0 = sx[i];
-		b3Vec3 iv0 = sv[i];
-		b3Vec3 iy = sy[i];
-
-		b3Vec3 dv = x[i];
-
-		// v1 = v0 + dv
-		b3Vec3 v1 = iv0;
-		if (type == e_dynamicParticle)
-		{
-			v1 += dv;
-		}
-
-		// dx = h * (v0 + dv) + y = h * v1 + y
-		b3Vec3 dx = h * v1 + iy;
-
-		// x1 = x0 + dx
-		b3Vec3 x1 = ix0 + dx;
-
-		sv[i] = v1;
-		sx[i] = x1;
 	}
 
 	// Cache x to improve convergence
@@ -263,6 +260,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	// Store the extra contact constraint forces that should have been 
 	// supplied to enforce the contact constraints exactly.
 	// These forces can be used in contact constraint logic.
+
+	// f = A * x - b
+	b3DenseVec3 f = A * x - b;
+
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
 		b3BodyContact* c = m_contacts[i];
@@ -277,15 +278,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		c->Ft1 = b3Dot(force, c->t1);
 		c->Ft2 = b3Dot(force, c->t2);
 	}
-
-	// Copy state buffers back to the particles
-	for (u32 i = 0; i < m_particleCount; ++i)
-	{
-		m_particles[i]->position = sx[i];
-		m_particles[i]->velocity = sv[i];
-	}
-
-	b3FreeSparse(A, m_allocator);
 }
 
 #define B3_INDEX(i, j, size) (i + j * size)
@@ -329,7 +321,7 @@ static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
 	return isZeroX * isZeroY * isZeroZ;
 }
 
-void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
+void b3ClothSolver::Compute_A_b(b3SolverSparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
 {
 	float32 h = m_solverData.dt;
 
@@ -368,7 +360,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 	}
 
 	// Compute A
-	
+
 	// A = M - h * dfdv - h * h * dfdx
 
 	// A = 0
@@ -390,14 +382,14 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 		}
 	}
 
-	// Assembly sparsity
+	// Assembly sparsity. 
-	u32 nzCount = 0;
+	u32 valueCapacity = m_particleCapacity * m_particleCapacity;
 
 	SA.row_ptrs[0] = 0;
 
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
-		u32 rowNzCount = 0;
+		u32 rowValueCount = 0;
 
 		for (u32 j = 0; j < m_particleCount; ++j)
 		{
@@ -405,21 +397,17 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 
 			if (b3IsZero(a) == false)
 			{
-				B3_ASSERT(nzCount <= SA.valueCount);
+				SA.values[SA.valueCount] = a;
-
+				SA.cols[SA.valueCount] = j;
-				SA.values[nzCount] = a;
+				++SA.valueCount;
-				SA.cols[nzCount] = j;
+				++rowValueCount;
-
-				++nzCount;
-				++rowNzCount;
 			}
 		}
 
-		SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount;
+		SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowValueCount;
 	}
 
-	B3_ASSERT(nzCount <= SA.valueCount);
+	B3_ASSERT(SA.valueCount <= valueCapacity);
-	SA.valueCount = nzCount;
 
 	m_allocator->Free(A);
 
@@ -525,26 +513,34 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
 	// p = S * (P^-1 * r)
 	b3DenseVec3 p = S * (inv_P * r);
 
-	// deltaNew = dot(r, p)
+	// delta_new = dot(r, p)
-	float32 deltaNew = b3Dot(r, p);
+	float32 delta_new = b3Dot(r, p);
 
-	// Tolerance.
+	// Set the tolerance.
-	// This is the main stopping criteria.
+	const float32 tolerance = 1.e-4f;
-	// [0, 1]
-	const float32 tolerance = 10.0f * B3_EPSILON;
 
 	// Maximum number of iterations.
-	const u32 maxIters = 1000;
+	// Stop at this iteration if diverged.
+	const u32 max_iterations = 100;
+
+	u32 iteration = 0;
 
 	// Main iteration loop.
-	u32 iter = 0;
+	while (iteration < max_iterations)
-	while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters)
 	{
+		B3_ASSERT(b3IsValid(delta_new));
+
+		// Convergence check.
+		if (delta_new <= tolerance * tolerance * b_delta)
+		{
+			break;
+		}
+
 		// s = S * (A * p)
 		b3DenseVec3 s = S * (A * p);
 
-		// alpha = deltaNew / dot(c, q)
+		// alpha = delta_new / dot(p, s)
-		float32 alpha = deltaNew / b3Dot(p, s);
+		float32 alpha = delta_new / b3Dot(p, s);
 
 		// x = x + alpha * p
 		x = x + alpha * p;
@@ -555,21 +551,20 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
 		// h = inv_P * r
 		b3DenseVec3 h = inv_P * r;
 
-		// deltaOld = deltaNew
+		// delta_old = delta_new
-		float32 deltaOld = deltaNew;
+		float32 delta_old = delta_new;
 
-		// deltaNew = dot(r, h)
+		// delta_new = dot(r, h)
-		deltaNew = b3Dot(r, h);
+		delta_new = b3Dot(r, h);
-		//B3_ASSERT(b3IsValid(deltaNew));
 
-		// beta = deltaNew / deltaOld
+		// beta = delta_new / delta_old
-		float32 beta = deltaNew / deltaOld;
+		float32 beta = delta_new / delta_old;
 
 		// p = S * (h + beta * p)
 		p = S * (h + beta * p);
 
-		++iter;
+		++iteration;
 	}
 
-	iterations = iter;
+	iterations = iteration;
 }