typo, optimization, consistency

include/bounce/dynamics/cloth/cloth.h

@@ -64,9 +64,9 @@ struct b3ClothDef
 	float32 kd;
 };
 
-// Static particles have zero mass and velocity, and therefore they can't move.
+// Static particle: Has zero mass, can be moved manually.
-// Kinematic particles are't moved by external and internal forces but can be moved by contact forces.
+// Kinematic particle: Has zero mass, non-zero velocity, can be moved by the solver.
-// Dynamic particles have non-zero mass and can move due to internal and external forces.
+// Dynamic particle: Has non-zero mass, non-zero velocity determined by force, can be moved by the solver.
 enum b3ParticleType
 {
 	e_staticParticle,
@@ -98,10 +98,10 @@ struct b3Particle
 	// Radius
 	float32 radius;
 
-	// User data
+	// User data. 
 	void* userData;
 
-	// Translation used for direct position manipulation
+	// Applied external translation
 	b3Vec3 translation;
 
 	// Solver temp
@@ -125,6 +125,8 @@ struct b3ClothSolverData;
 // Read-only spring
 struct b3Spring
 {
+	// Solver shared
+	
 	// Spring type
 	b3SpringType type;
 
@@ -151,8 +153,8 @@ struct b3Spring
 	// Jacobian (J_ii entry)
 	b3Mat33 Jx, Jv;
 
-	// Initialize solver data.
+	// Apply spring forces.
-	void Initialize(const b3ClothSolverData* data);
+	void ApplyForces(const b3ClothSolverData* data);
 };
 
 // Read-only contact
@@ -160,6 +162,7 @@ struct b3ParticleContact
 {
 	b3Particle* p1;
 	b3Shape* s2;
+	float32 s;
 	b3Vec3 n, t1, t2;
 	float32 Fn, Ft1, Ft2;
 	bool n_active, t1_active, t2_active;
@@ -254,8 +257,8 @@ protected:
 	u32 m_particleCount;
 	b3Particle* m_particles;
 
-	b3Spring* m_springs;
 	u32 m_springCount;
+	b3Spring* m_springs;
 
 	b3ParticleContact* m_contacts;
 	//u32 m_contactCount;

include/bounce/dynamics/cloth/cloth_solver.h

@@ -50,6 +50,13 @@ struct b3ClothSolverData
 	float32 invdt;
 };
 
+struct b3AccelerationConstraint
+{
+	u32 i1;
+	u32 ndof;
+	b3Vec3 p, q, z;
+};
+
 class b3ClothSolver
 {
 public:
@@ -62,17 +69,16 @@ public:
 
 	void Solve(float32 dt, const b3Vec3& gravity);
 private:
+	// 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;
 
-	// Compute S.
+	// Compute S and z.
-	void Compute_S(b3DiagMat33& S);
+	void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z);
-
-	// Compute z. 
-	void Compute_z(b3DenseVec3& z);
 
 	// Solve Ax = b.
-	// Output x and the residual error f = Ax - b ~ 0.
 	void Solve(b3DenseVec3& x, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
 
 	b3StackAllocator* m_allocator;
@@ -89,6 +95,10 @@ private:
 	u32 m_contactCount;
 	b3ParticleContact** m_contacts;
 
+	u32 m_constraintCapacity;
+	u32 m_constraintCount;
+	b3AccelerationConstraint* m_constraints;
+
 	b3ClothSolverData m_solverData;
 };
 

src/bounce/dynamics/cloth/cloth.cpp

@@ -32,7 +32,7 @@
 #define B3_CLOTH_FRICTION 0
 
 // b3Spring
-void b3Spring::Initialize(const b3ClothSolverData* data)
+void b3Spring::ApplyForces(const b3ClothSolverData* data)
 {
 	u32 i1 = p1->solverId;
 	u32 i2 = p2->solverId;
@@ -43,7 +43,7 @@ void b3Spring::Initialize(const b3ClothSolverData* data)
 	b3Vec3 x2 = data->x[i2];
 	b3Vec3 v2 = data->v[i2];
 
-	const b3Mat33 I = b3Mat33_identity;
+	b3Mat33 I; I.SetIdentity();
 
 	b3Vec3 dx = x1 - x2;
 
@@ -238,6 +238,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 
 	const b3ClothMesh* m = m_mesh;
 
+	// Create particles
 	m_particleCount = m->vertexCount;
 	m_particles = (b3Particle*)b3Alloc(m_particleCount * sizeof(b3Particle));
 	m_contacts = (b3ParticleContact*)b3Alloc(m_particleCount * sizeof(b3ParticleContact));
@@ -266,7 +267,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 	// Compute mass
 	ResetMass();
 
-	// Initialize springs
+	// Create springs
 	u32 edgeCount = 3 * m->triangleCount;
 
 	b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_allocator.Allocate(edgeCount * sizeof(b3UniqueEdge));
@@ -368,7 +369,7 @@ void b3Cloth::ResetMass()
 	const float32 inv3 = 1.0f / 3.0f;
 	const float32 rho = m_density;
 
-	// Accumulate the mass about the body origin of all triangles.
+	// Accumulate the mass about the mesh origin of all triangles.
 	for (u32 i = 0; i < m_mesh->triangleCount; ++i)
 	{
 		b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
@@ -477,16 +478,14 @@ void b3Cloth::UpdateContacts()
 			b3Vec3 n = bestNormal;
 
 			// Update contact manifold
-			// Remember the normal orientation is from shape 2 to shape 1 (mass)
+			// Remember the normal points from shape 2 to shape 1 (mass)
 			c->n_active = true;
 			c->p1 = p;
 			c->s2 = shape;
+			c->s = s;
 			c->n = n;
 			c->t1 = b3Perp(n);
 			c->t2 = b3Cross(c->t1, n);
-
-			// Apply position correction
-			p->translation -= s * n;
 		}
 
 		// Update contact state

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -47,11 +47,16 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 
 	m_contactCapacity = def.contactCapacity;
 	m_contactCount = 0;
-	m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*));;
+	m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*));
+	
+	m_constraintCapacity = def.particleCapacity;
+	m_constraintCount = 0;
+	m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
 }
 
 b3ClothSolver::~b3ClothSolver()
 {
+	m_allocator->Free(m_constraints);
 	m_allocator->Free(m_contacts);
 	m_allocator->Free(m_springs);
 	m_allocator->Free(m_particles);
@@ -73,6 +78,56 @@ void b3ClothSolver::Add(b3Spring* s)
 	m_springs[m_springCount++] = s;
 }
 
+void b3ClothSolver::InitializeConstraints()
+{
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles[i];
+		if (p->type == e_staticParticle)
+		{
+			b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
+			++m_constraintCount;
+			ac->i1 = i;
+			ac->ndof = 0;
+			ac->z.SetZero();
+		}
+	}
+
+	for (u32 i = 0; i < m_contactCount; ++i)
+	{
+		b3ParticleContact* 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;
+		ac->ndof = 2;
+		ac->p = pc->n;
+		ac->z.SetZero();
+
+		if (pc->t1_active && pc->t2_active)
+		{
+			ac->ndof = 0;
+		}
+		else
+		{
+			if (pc->t1_active)
+			{
+				ac->ndof = 1;
+				ac->q = pc->t1;
+			}
+			
+			if (pc->t2_active)
+			{
+				ac->ndof = 1;
+				ac->q = pc->t2;
+			}
+		}
+	}
+}
+
 static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N)
 {
 	u32 size = M * N;
@@ -124,13 +179,27 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		sx0[i] = p->x;
 	}
 
-	// Initialize spring forces
+	// Apply contact position corrections
+	for (u32 i = 0; i < m_contactCount; ++i)
+	{
+		b3ParticleContact* c = m_contacts[i];
+		b3Particle* p = c->p1;	
+		sy[p->solverId] -= c->s * c->n;
+	}
+
+	// Apply spring forces and derivatives
 	for (u32 i = 0; i < m_springCount; ++i)
 	{
-		b3Spring* s = m_springs[i];
+		m_springs[i]->ApplyForces(&m_solverData);
-		s->Initialize(&m_solverData);
 	}
 
+	// Initialize constraints
+	InitializeConstraints();
+
+	b3DiagMat33 S(m_particleCount);
+	b3DenseVec3 z(m_particleCount);
+	Compute_S_z(S, z);
+
 	// Solve Ax = b, where
 	// A = M - h * dfdv - h * h * dfdx
 	// b = h * (f0 + h * dfdx * v0 + dfdx * y) 
@@ -143,14 +212,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 
 	Compute_A_b(A, b, sf, sx, sv, sy);
 
-	// S
-	b3DiagMat33 S(m_particleCount);
-	Compute_S(S);
-
-	// z
-	b3DenseVec3 z(m_particleCount);
-	Compute_z(z);
-
 	// x
 	b3DenseVec3 x(m_particleCount);
 
@@ -379,67 +440,47 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 	b = h * (f + h * Jx_v + Jx_y);
 }
 
-void b3ClothSolver::Compute_z(b3DenseVec3& z)
+void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z)
 {
-	// TODO
+	S.SetIdentity();
 	z.SetZero();
-}
 
-void b3ClothSolver::Compute_S(b3DiagMat33& out)
+	for (u32 i = 0; i < m_constraintCount; ++i)
-{
-	for (u32 i = 0; i < m_particleCount; ++i)
 	{
-		b3Particle* p = m_particles[i];
+		b3AccelerationConstraint* ac = m_constraints + i;
+		u32 ip = ac->i1;
+		u32 ndof = ac->ndof;
+		b3Vec3 p = ac->p;
+		b3Vec3 q = ac->q;
+		b3Vec3 cz = ac->z;
 
-		if (p->type == e_staticParticle)
+		z[ip] = cz;
+
+		if (ndof == 2)
 		{
-			out[i].SetZero();
+			b3Mat33 I; I.SetIdentity();
-			continue;
+
+			S[ip] = I - b3Outer(p, p);
 		}
 
-		out[i].SetIdentity();
+		if (ndof == 1)
+		{
+			b3Mat33 I; I.SetIdentity();
+
+			S[ip] = I - b3Outer(p, p) - b3Outer(q, q);
 		}
 
-	for (u32 i = 0; i < m_contactCount; ++i)
+		if (ndof == 0)
 		{
-		b3ParticleContact* c = m_contacts[i];
+			S[ip].SetZero();
-		
-		B3_ASSERT(c->n_active);
-
-		b3Vec3 n = c->n;
-
-		b3Mat33 S = b3Mat33_identity - b3Outer(n, n);
-
-		if (c->t1_active == true && c->t2_active == true)
-		{
-			S.SetZero();
 		}
-		else
-		{
-			if (c->t1_active == true)
-			{
-				b3Vec3 t1 = c->t1;
-
-				S -= b3Outer(t1, t1);
-			}
-
-			if (c->t2_active == true)
-			{
-				b3Vec3 t2 = c->t2;
-
-				S -= b3Outer(t2, t2);
-			}
-		}
-
-		b3Particle* p = c->p1;
-		out[p->solverId] = S;
 	}
 }
 
 void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
 	const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
 {
-	B3_PROFILE("Solve A * x = b");
+	B3_PROFILE("Solve Ax = b");
 
 	// P = diag(A)
 	b3DiagMat33 inv_P(m_particleCount);