Split the solvers into force solver and contact solver

include/bounce/cloth/cloth_contact_solver.h

@@ -30,8 +30,6 @@ class b3Body;
 struct b3ParticleBodyContact;
 class b3ParticleTriangleContact;
 
-struct b3DenseVec3;
-
 struct b3ClothSolverBodyContactVelocityConstraint
 {
 	u32 indexA;
@@ -130,8 +128,8 @@ struct b3ClothContactSolverDef
 {
 	b3StackAllocator* allocator;
 	
-	b3DenseVec3* positions;
+	b3Vec3* positions;
-	b3DenseVec3* velocities;
+	b3Vec3* velocities;
 	
 	u32 bodyContactCount;
 	b3ParticleBodyContact** bodyContacts;
@@ -167,8 +165,8 @@ public:
 protected:
 	b3StackAllocator* m_allocator;
 
-	b3DenseVec3* m_positions;
+	b3Vec3* m_positions;
-	b3DenseVec3* m_velocities;
+	b3Vec3* m_velocities;
 
 	u32 m_bodyContactCount;
 	b3ParticleBodyContact** m_bodyContacts;

include/bounce/cloth/cloth_force_solver.h

@@ -0,0 +1,89 @@
+/*
+* 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_FORCE_SOLVER_H
+#define B3_CLOTH_FORCE_SOLVER_H
+
+#include <bounce/common/math/mat22.h>
+#include <bounce/common/math/mat33.h>
+
+class b3StackAllocator;
+
+class b3Particle;
+class b3Force;
+
+struct b3DenseVec3;
+struct b3DiagMat33;
+struct b3SparseSymMat33;
+struct b3SparseSymMat33View;
+
+struct b3ClothForceSolverDef
+{
+	b3StackAllocator* stack;
+	u32 particleCount;
+	b3Particle** particles;
+	u32 forceCount;
+	b3Force** forces;
+};
+
+struct b3ClothForceSolverData
+{
+	b3DenseVec3* x;
+	b3DenseVec3* v;
+	b3DenseVec3* f;
+	b3DenseVec3* y;
+	b3SparseSymMat33* dfdx;
+	b3SparseSymMat33* dfdv;
+	b3DiagMat33* S;
+	b3DenseVec3* z;
+};
+
+struct b3AccelerationConstraint
+{
+	u32 i1;
+	u32 ndof;
+	b3Vec3 p, q, z;
+
+	void Apply(const b3ClothForceSolverData* data);
+};
+
+class b3ClothForceSolver
+{
+public:
+	b3ClothForceSolver(const b3ClothForceSolverDef& def);
+	~b3ClothForceSolver();
+
+	void Solve(float32 dt, const b3Vec3& gravity);
+private:
+	void ApplyForces();
+	void ApplyConstraints();
+
+	b3StackAllocator* m_allocator;
+
+	u32 m_particleCount;
+	b3Particle** m_particles;
+
+	u32 m_forceCount;
+	b3Force** m_forces;
+
+	b3AccelerationConstraint* m_constraints;
+
+	b3ClothForceSolverData m_solverData;
+};
+
+#endif

include/bounce/cloth/cloth_solver.h

@@ -25,14 +25,7 @@
 class b3StackAllocator;
 
 class b3Particle;
-class b3Body;
 class b3Force;
-
-struct b3DenseVec3;
-struct b3DiagMat33;
-struct b3SparseSymMat33;
-struct b3SparseSymMat33View;
-
 struct b3ParticleBodyContact;
 class b3ParticleTriangleContact;
 
@@ -45,29 +38,6 @@ struct b3ClothSolverDef
 	u32 triangleContactCapacity;
 };
 
-struct b3ClothSolverData
-{
-	b3DenseVec3* x;
-	b3DenseVec3* v;
-	b3DenseVec3* f;
-	b3DenseVec3* y;
-	b3SparseSymMat33* dfdx;
-	b3SparseSymMat33* dfdv;
-	b3DiagMat33* S;
-	b3DenseVec3* z;
-	float32 dt;
-	float32 invdt;
-};
-
-struct b3AccelerationConstraint
-{
-	u32 i1;
-	u32 ndof;
-	b3Vec3 p, q, z;
-
-	void Apply(const b3ClothSolverData* data);
-};
-
 class b3ClothSolver
 {
 public:
@@ -81,9 +51,6 @@ public:
 
 	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
 private:
-	void ApplyForces();
-	void ApplyConstraints();
-
 	b3StackAllocator* m_allocator;
 
 	u32 m_particleCapacity;
@@ -93,11 +60,7 @@ private:
 	u32 m_forceCapacity;
 	u32 m_forceCount;
 	b3Force** m_forces;
-	
+
-	u32 m_constraintCapacity;
-	u32 m_constraintCount;
-	b3AccelerationConstraint* m_constraints;
-	
 	u32 m_bodyContactCapacity;
 	u32 m_bodyContactCount;
 	b3ParticleBodyContact** m_bodyContacts;
@@ -105,8 +68,6 @@ private:
 	u32 m_triangleContactCapacity;
 	u32 m_triangleContactCount;
 	b3ParticleTriangleContact** m_triangleContacts;
-	
-	b3ClothSolverData m_solverData;
 };
 
 #endif

include/bounce/cloth/force.h

@@ -22,7 +22,7 @@
 #include <bounce/common/math/transform.h>
 #include <bounce/common/template/list.h>
 
-struct b3ClothSolverData;
+struct b3ClothForceSolverData;
 
 class b3Particle;
 
@@ -52,7 +52,7 @@ public:
 protected:
 	friend class b3List2<b3Force>;
 	friend class b3Cloth;
-	friend class b3ClothSolver;
+	friend class b3ClothForceSolver;
 	friend class b3Particle;
 
 	static b3Force* Create(const b3ForceDef* def);
@@ -61,7 +61,7 @@ protected:
 	b3Force() { }
 	virtual ~b3Force() { }
 
-	virtual void Apply(const b3ClothSolverData* data) = 0;
+	virtual void Apply(const b3ClothForceSolverData* data) = 0;
 
 	// Force type
 	b3ForceType m_type;

include/bounce/cloth/particle.h

@@ -160,6 +160,7 @@ private:
 	friend class b3Cloth;
 	friend class b3ClothTriangle;
 	friend class b3ClothSolver;
+	friend class b3ClothForceSolver;
 	friend class b3ClothContactManager;
 	friend class b3ParticleTriangleContact;
 	friend class b3ClothContactSolver;

include/bounce/cloth/spring_force.h

@@ -76,7 +76,7 @@ private:
 	b3SpringForce(const b3SpringForceDef* def);
 	~b3SpringForce();
 
-	void Apply(const b3ClothSolverData* data);
+	void Apply(const b3ClothForceSolverData* data);
 
 	// Solver shared
 

src/bounce/cloth/cloth_contact_solver.cpp

@@ -20,7 +20,6 @@
 #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>
 #include <bounce/dynamics/body.h>
@@ -54,9 +53,6 @@ b3ClothContactSolver::~b3ClothContactSolver()
 
 void b3ClothContactSolver::InitializeBodyContactConstraints()
 {
-	b3DenseVec3& x = *m_positions;
-	b3DenseVec3& v = *m_velocities;
-
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
 	{
 		b3ParticleBodyContact* c = m_bodyContacts[i];
@@ -109,7 +105,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
 		b3Mat33 iA = vc->invIA;
 		b3Mat33 iB = vc->invIB;
 
-		b3Vec3 xA = x[indexA];
+		b3Vec3 xA = m_positions[indexA];
 		b3Vec3 xB = bodyB->m_sweep.worldCenter;
 
 		b3Quat qA; qA.SetIdentity();
@@ -182,8 +178,6 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
 }
 void b3ClothContactSolver::InitializeTriangleContactConstraints()
 {
-	b3DenseVec3& x = *m_positions;
-
 	for (u32 i = 0; i < m_triangleContactCount; ++i)
 	{
 		b3ParticleTriangleContact* c = m_triangleContacts[i];
@@ -233,10 +227,10 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints()
 		u32 indexD = pc->indexD;
 		float32 mD = pc->invMassD;
 
-		b3Vec3 xA = x[indexA];
+		b3Vec3 xA = m_positions[indexA];
-		b3Vec3 xB = x[indexB];
+		b3Vec3 xB = m_positions[indexB];
-		b3Vec3 xC = x[indexC];
+		b3Vec3 xC = m_positions[indexC];
-		b3Vec3 xD = x[indexD];
+		b3Vec3 xD = m_positions[indexD];
 
 		float32 wB = pc->wB;
 		float32 wC = pc->wC;
@@ -269,8 +263,6 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints()
 
 void b3ClothContactSolver::WarmStartBodyContactConstraints()
 {
-	b3DenseVec3& v = *m_velocities;
-
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
 	{
 		b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
@@ -278,7 +270,7 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints()
 		u32 indexA = vc->indexA;
 		b3Body* bodyB = vc->bodyB;
 
-		b3Vec3 vA = v[indexA];
+		b3Vec3 vA = m_velocities[indexA];
 		b3Vec3 vB = bodyB->GetLinearVelocity();
 
 		b3Vec3 wA; wA.SetZero();
@@ -307,7 +299,7 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints()
 		vB += mB * (P1 + P2);
 		wB += iB * b3Cross(vc->rB, P1 + P2);
 
-		v[indexA] = vA;
+		m_velocities[indexA] = vA;
 
 		bodyB->SetLinearVelocity(vB);
 		bodyB->SetAngularVelocity(wB);
@@ -316,8 +308,6 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints()
 
 void b3ClothContactSolver::WarmStartTriangleContactConstraints()
 {
-	b3DenseVec3& v = *m_velocities;
-	
 	for (u32 i = 0; i < m_triangleContactCount; ++i)
 	{
 		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
@@ -327,10 +317,10 @@ void b3ClothContactSolver::WarmStartTriangleContactConstraints()
 		u32 indexC = vc->indexC;
 		u32 indexD = vc->indexD;
 
-		b3Vec3 vA = v[indexA];
+		b3Vec3 vA = m_velocities[indexA];
-		b3Vec3 vB = v[indexB];
+		b3Vec3 vB = m_velocities[indexB];
-		b3Vec3 vC = v[indexC];
+		b3Vec3 vC = m_velocities[indexC];
-		b3Vec3 vD = v[indexD];
+		b3Vec3 vD = m_velocities[indexD];
 
 		float32 mA = vc->invMassA;
 		float32 mB = vc->invMassB;
@@ -388,17 +378,15 @@ void b3ClothContactSolver::WarmStartTriangleContactConstraints()
 			vD += mD * PD;
 		}
 		
-		v[indexA] = vA;
+		m_velocities[indexA] = vA;
-		v[indexB] = vB;
+		m_velocities[indexB] = vB;
-		v[indexC] = vC;
+		m_velocities[indexC] = vC;
-		v[indexD] = vD;
+		m_velocities[indexD] = vD;
 	}
 }
 
 void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
 {
-	b3DenseVec3& v = *m_velocities;
-
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
 	{
 		b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
@@ -406,7 +394,7 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
 		u32 indexA = vc->indexA;
 		b3Body* bodyB = vc->bodyB;
 
-		b3Vec3 vA = v[indexA];
+		b3Vec3 vA = m_velocities[indexA];
 		b3Vec3 vB = bodyB->GetLinearVelocity();
 
 		b3Vec3 wA; wA.SetZero();
@@ -476,7 +464,7 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
 			wB += iB * b3Cross(rB, P);
 		}
 
-		v[indexA] = vA;
+		m_velocities[indexA] = vA;
 
 		bodyB->SetLinearVelocity(vB);
 		bodyB->SetAngularVelocity(wB);
@@ -485,8 +473,6 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
 
 void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
 {
-	b3DenseVec3& v = *m_velocities;
-
 	for (u32 i = 0; i < m_triangleContactCount; ++i)
 	{
 		b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
@@ -507,10 +493,10 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
 		float32 wC = vc->wC;
 		float32 wD = vc->wD;
 
-		b3Vec3 vA = v[indexA];
+		b3Vec3 vA = m_velocities[indexA];
-		b3Vec3 vB = v[indexB];
+		b3Vec3 vB = m_velocities[indexB];
-		b3Vec3 vC = v[indexC];
+		b3Vec3 vC = m_velocities[indexC];
-		b3Vec3 vD = v[indexD];
+		b3Vec3 vD = m_velocities[indexD];
 
 		// Solve normal constraint.
 		{
@@ -602,10 +588,10 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
 			vD += mD * PD;
 		}
 
-		v[indexA] = vA;
+		m_velocities[indexA] = vA;
-		v[indexB] = vB;
+		m_velocities[indexB] = vB;
-		v[indexC] = vC;
+		m_velocities[indexC] = vC;
-		v[indexD] = vD;
+		m_velocities[indexD] = vD;
 	}
 }
 
@@ -658,8 +644,6 @@ struct b3ClothSolverBodyContactSolverPoint
 
 bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
 {
-	b3DenseVec3& x = *m_positions;
-
 	float32 minSeparation = 0.0f;
 
 	for (u32 i = 0; i < m_bodyContactCount; ++i)
@@ -676,7 +660,7 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
 		b3Mat33 iB = pc->invIB;
 		b3Vec3 localCenterB = pc->localCenterB;
 
-		b3Vec3 cA = x[indexA];
+		b3Vec3 cA = m_positions[indexA];
 		b3Quat qA; qA.SetIdentity();
 
 		b3Vec3 cB = bodyB->m_sweep.worldCenter;
@@ -724,7 +708,7 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
 		qB += b3Derivative(qB, iB * b3Cross(rB, P));
 		qB.Normalize();
 
-		x[indexA] = cA;
+		m_positions[indexA] = cA;
 
 		bodyB->m_sweep.worldCenter = cB;
 		bodyB->m_sweep.orientation = qB;
@@ -735,8 +719,6 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
 
 bool b3ClothContactSolver::SolveTriangleContactPositionConstraints()
 {
-	b3DenseVec3& x = *m_positions;
-
 	float32 minSeparation = 0.0f;
 
 	for (u32 i = 0; i < m_triangleContactCount; ++i)
@@ -762,10 +744,10 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints()
 		float32 wC = pc->wC;
 		float32 wD = pc->wD;
 
-		b3Vec3 xA = x[indexA];
+		b3Vec3 xA = m_positions[indexA];
-		b3Vec3 xB = x[indexB];
+		b3Vec3 xB = m_positions[indexB];
-		b3Vec3 xC = x[indexC];
+		b3Vec3 xC = m_positions[indexC];
-		b3Vec3 xD = x[indexD];
+		b3Vec3 xD = m_positions[indexD];
 
 		b3Vec3 cB = wB * xB + wC * xC + wD * xD;
 
@@ -802,10 +784,10 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints()
 		xC += mC * PC;
 		xD += mD * PD;
 
-		x[indexA] = xA;
+		m_positions[indexA] = xA;
-		x[indexB] = xB;
+		m_positions[indexB] = xB;
-		x[indexC] = xC;
+		m_positions[indexC] = xC;
-		x[indexD] = xD;
+		m_positions[indexD] = xD;
 	}
 
 	return minSeparation >= -3.0f * B3_LINEAR_SLOP;

src/bounce/cloth/cloth_force_solver.cpp

@@ -0,0 +1,293 @@
+/*
+* 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_force_solver.h>
+#include <bounce/cloth/particle.h>
+#include <bounce/cloth/force.h>
+#include <bounce/cloth/dense_vec3.h>
+#include <bounce/cloth/diag_mat33.h>
+#include <bounce/cloth/sparse_sym_mat33.h>
+#include <bounce/cloth/sparse_sym_mat33_view.h>
+#include <bounce/common/memory/stack_allocator.h>
+
+// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
+// described in the paper:
+// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
+
+// Some improvements for the original MPCG algorithm are described in the paper:
+// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman".
+
+u32 b3_clothSolverIterations = 0;
+
+b3ClothForceSolver::b3ClothForceSolver(const b3ClothForceSolverDef& def)
+{
+	m_allocator = def.stack;
+
+	m_particleCount = def.particleCount;
+	m_particles = def.particles;
+
+	m_forceCount = def.forceCount;
+	m_forces = def.forces;
+
+	m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_particleCount * sizeof(b3AccelerationConstraint));
+}
+
+b3ClothForceSolver::~b3ClothForceSolver()
+{
+	m_allocator->Free(m_constraints);
+}
+
+void b3ClothForceSolver::ApplyForces()
+{
+	for (u32 i = 0; i < m_forceCount; ++i)
+	{
+		m_forces[i]->Apply(&m_solverData);
+	}
+}
+
+void b3AccelerationConstraint::Apply(const b3ClothForceSolverData* data)
+{
+	b3DiagMat33& sS = *data->S;
+	b3DenseVec3& sz = *data->z;
+
+	sz[i1] = z;
+
+	b3Mat33 I; I.SetIdentity();
+
+	switch (ndof)
+	{
+	case 3:
+	{
+		sS[i1] = I;
+		break;
+	}
+	case 2:
+	{
+		sS[i1] = I - b3Outer(p, p);
+		break;
+	}
+	case 1:
+	{
+		sS[i1] = I - b3Outer(p, p) - b3Outer(q, q);
+		break;
+	}
+	case 0:
+	{
+		sS[i1].SetZero();
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		break;
+	}
+	}
+}
+
+void b3ClothForceSolver::ApplyConstraints()
+{
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles[i];
+		b3AccelerationConstraint* c = m_constraints + i;
+
+		c->i1 = i;
+
+		if (p->m_type != e_dynamicParticle)
+		{
+			c->ndof = 0;
+			c->z.SetZero();
+		}
+		else
+		{
+			c->ndof = 3;
+			c->z.SetZero();
+		}
+	}
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_constraints[i].Apply(&m_solverData);
+	}
+}
+
+// Solve Ax = b
+static void b3SolveMPCG(b3DenseVec3& x,
+	const b3SparseSymMat33View& A, const b3DenseVec3& b,
+	const b3DiagMat33& S, const b3DenseVec3& z,
+	const b3DenseVec3& y, const b3DiagMat33& I, u32 maxIterations = 20)
+{
+	B3_PROFILE("Cloth Solve MPCG");
+
+	// Jacobi preconditioner 
+	// P = diag(A)
+	b3DiagMat33 inv_P(A.rowCount);
+	for (u32 i = 0; i < A.rowCount; ++i)
+	{
+		b3Mat33 a = A(i, i);
+
+		// Sylvester Criterion to ensure PD-ness
+		B3_ASSERT(b3Det(a.x, a.y, a.z) > 0.0f);
+
+		B3_ASSERT(a.x.x > 0.0f);
+		float32 xx = 1.0f / a.x.x;
+
+		B3_ASSERT(a.y.y > 0.0f);
+		float32 yy = 1.0f / a.y.y;
+
+		B3_ASSERT(a.z.z > 0.0f);
+		float32 zz = 1.0f / a.z.z;
+
+		inv_P[i] = b3Diagonal(xx, yy, zz);
+	}
+
+	x = (S * y) + (I - S) * z;
+
+	b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
+
+	float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
+
+	b3DenseVec3 r = S * (b - A * x);
+
+	b3DenseVec3 p = S * (inv_P * r);
+
+	float32 delta_new = b3Dot(r, p);
+
+	u32 iteration = 0;
+	for (;;)
+	{
+		if (iteration == maxIterations)
+		{
+			break;
+		}
+
+		if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta)
+		{
+			break;
+		}
+
+		b3DenseVec3 s = S * (A * p);
+
+		float32 alpha = delta_new / b3Dot(p, s);
+
+		x = x + alpha * p;
+		r = r - alpha * s;
+
+		b3DenseVec3 h = inv_P * r;
+
+		float32 delta_old = delta_new;
+
+		delta_new = b3Dot(r, h);
+
+		float32 beta = delta_new / delta_old;
+
+		p = S * (h + beta * p);
+
+		++iteration;
+	}
+
+	b3_clothSolverIterations = iteration;
+}
+
+void b3ClothForceSolver::Solve(float32 dt, const b3Vec3& gravity)
+{
+	float32 h = dt;
+
+	b3DenseVec3 sx(m_particleCount);
+	b3DenseVec3 sv(m_particleCount);
+	b3DenseVec3 sf(m_particleCount);
+	b3DenseVec3 sy(m_particleCount);
+	b3DenseVec3 sz(m_particleCount);
+	b3DenseVec3 sx0(m_particleCount);
+	b3SparseSymMat33 dfdx(m_particleCount);
+	b3SparseSymMat33 dfdv(m_particleCount);
+	b3DiagMat33 S(m_particleCount);
+
+	m_solverData.x = &sx;
+	m_solverData.v = &sv;
+	m_solverData.f = &sf;
+	m_solverData.y = &sy;
+	m_solverData.z = &sz;
+	m_solverData.dfdx = &dfdx;
+	m_solverData.dfdv = &dfdv;
+	m_solverData.S = &S;
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles[i];
+
+		sx[i] = p->m_position;
+		sv[i] = p->m_velocity;
+		sf[i] = p->m_force;
+
+		if (p->m_type == e_dynamicParticle)
+		{
+			// Apply weight
+			sf[i] += p->m_mass * gravity;
+		}
+
+		sy[i] = p->m_translation;
+		sx0[i] = p->m_x;
+	}
+
+	// Apply internal forces
+	ApplyForces();
+
+	// Apply constraints
+	ApplyConstraints();
+
+	// Solve Ax = b, where
+	// A = M - h * dfdv - h * h * dfdx
+	// b = h * (f0 + h * dfdx * v0 + dfdx * y) 
+	b3SparseSymMat33 M(m_particleCount);
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		M(i, i) = b3Diagonal(m_particles[i]->m_mass);
+	}
+
+	// A
+	b3SparseSymMat33 A = M - h * dfdv - h * h * dfdx;
+
+	// View for A
+	b3SparseSymMat33View viewA(A);
+
+	// b
+	b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy);
+
+	// I
+	b3DiagMat33 I(m_particleCount);
+	I.SetIdentity();
+
+	// x
+	b3DenseVec3 x(m_particleCount);
+	b3SolveMPCG(x, viewA, b, S, sz, sx0, I);
+
+	// Velocity update
+	sv = sv + x;
+	sx = sx + sy;
+
+	// Copy state buffers back to the particle
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles[i];
+
+		p->m_x = x[i];
+		p->m_position = sx[i];
+		p->m_velocity = sv[i];
+	}
+}

src/bounce/cloth/cloth_solver.cpp

@@ -17,27 +17,13 @@
 */
 
 #include <bounce/cloth/cloth_solver.h>
+#include <bounce/cloth/cloth_force_solver.h>
 #include <bounce/cloth/cloth_contact_solver.h>
 #include <bounce/cloth/cloth.h>
 #include <bounce/cloth/particle.h>
-#include <bounce/cloth/force.h>
-#include <bounce/cloth/dense_vec3.h>
-#include <bounce/cloth/diag_mat33.h>
-#include <bounce/cloth/sparse_sym_mat33.h>
-#include <bounce/cloth/sparse_sym_mat33_view.h>
-#include <bounce/common/memory/stack_allocator.h>
-#include <bounce/common/math/mat.h>
 #include <bounce/dynamics/shapes/shape.h>
 #include <bounce/dynamics/body.h>
-
+#include <bounce/common/memory/stack_allocator.h>
-// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
-// described in the paper:
-// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
-
-// Some improvements for the original MPCG algorithm are described in the paper:
-// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman".
-
-u32 b3_clothSolverIterations = 0;
 
 b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 {
@@ -51,10 +37,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 	m_forceCount = 0;
 	m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));;
 
-	m_constraintCapacity = def.particleCapacity;
-	m_constraintCount = 0;
-	m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
-
 	m_bodyContactCapacity = def.bodyContactCapacity;
 	m_bodyContactCount = 0;
 	m_bodyContacts = (b3ParticleBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ParticleBodyContact*));;
@@ -68,8 +50,6 @@ b3ClothSolver::~b3ClothSolver()
 {
 	m_allocator->Free(m_triangleContacts);
 	m_allocator->Free(m_bodyContacts);
-
-	m_allocator->Free(m_constraints);
 	m_allocator->Free(m_forces);
 	m_allocator->Free(m_particles);
 }
@@ -95,326 +75,106 @@ void b3ClothSolver::Add(b3ParticleTriangleContact* c)
 	m_triangleContacts[m_triangleContactCount++] = c;
 }
 
-void b3ClothSolver::ApplyForces()
-{
-	for (u32 i = 0; i < m_forceCount; ++i)
-	{
-		m_forces[i]->Apply(&m_solverData);
-	}
-}
-
-void b3AccelerationConstraint::Apply(const b3ClothSolverData* data)
-{
-	b3DiagMat33& sS = *data->S;
-	b3DenseVec3& sz = *data->z;
-
-	sz[i1] = z;
-
-	b3Mat33 I; I.SetIdentity();
-
-	switch (ndof)
-	{
-	case 3:
-	{
-		sS[i1] = I;
-		break;
-	}
-	case 2:
-	{
-		sS[i1] = I - b3Outer(p, p);
-		break;
-	}
-	case 1:
-	{
-		sS[i1] = I - b3Outer(p, p) - b3Outer(q, q);
-		break;
-	}
-	case 0:
-	{
-		sS[i1].SetZero();
-		break;
-	}
-	default:
-	{
-		B3_ASSERT(false);
-		break;
-	}
-	}
-}
-
-void b3ClothSolver::ApplyConstraints()
-{
-	b3DiagMat33& S = *m_solverData.S;
-	b3DenseVec3& z = *m_solverData.z;
-	b3DenseVec3& x = *m_solverData.x;
-
-	S.SetIdentity();
-	z.SetZero();
-	
-	for (u32 i = 0; i < m_particleCount; ++i)
-	{
-		b3Particle* p = m_particles[i];
-		
-		if (p->m_type != e_dynamicParticle)
-		{
-			b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
-			++m_constraintCount;
-			ac->i1 = i;
-			ac->ndof = 0;
-			ac->z.SetZero();
-		}
-	}
-
-	for (u32 i = 0; i < m_constraintCount; ++i)
-	{
-		m_constraints[i].Apply(&m_solverData);
-	}
-}
-
-// 
-static void b3SolveMPCG(b3DenseVec3& x,
-	const b3SparseSymMat33View& A, const b3DenseVec3& b,
-	const b3DiagMat33& S, const b3DenseVec3& z, 
-	const b3DenseVec3& y, const b3DiagMat33& I, u32 maxIterations = 20)
-{
-	B3_PROFILE("Cloth Solve MPCG");
-
-	// Jacobi preconditioner 
-	// P = diag(A)
-	b3DiagMat33 inv_P(A.rowCount);
-	for (u32 i = 0; i < A.rowCount; ++i)
-	{
-		b3Mat33 a = A(i, i);
-
-		// Sylvester Criterion to ensure PD-ness
-		B3_ASSERT(b3Det(a.x, a.y, a.z) > 0.0f);
-
-		B3_ASSERT(a.x.x > 0.0f);
-		float32 xx = 1.0f / a.x.x;
-
-		B3_ASSERT(a.y.y > 0.0f);
-		float32 yy = 1.0f / a.y.y;
-
-		B3_ASSERT(a.z.z > 0.0f);
-		float32 zz = 1.0f / a.z.z;
-
-		inv_P[i] = b3Diagonal(xx, yy, zz);
-	}
-
-	x = (S * y) + (I - S) * z;
-
-	b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
-
-	float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
-
-	b3DenseVec3 r = S * (b - A * x);
-
-	b3DenseVec3 p = S * (inv_P * r);
-
-	float32 delta_new = b3Dot(r, p);
-
-	u32 iteration = 0;
-	for (;;)
-	{
-		if (iteration == maxIterations)
-		{
-			break;
-		}
-
-		if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta)
-		{
-			break;
-		}
-
-		b3DenseVec3 s = S * (A * p);
-
-		float32 alpha = delta_new / b3Dot(p, s);
-
-		x = x + alpha * p;
-		r = r - alpha * s;
-
-		b3DenseVec3 h = inv_P * r;
-
-		float32 delta_old = delta_new;
-
-		delta_new = b3Dot(r, h);
-
-		float32 beta = delta_new / delta_old;
-
-		p = S * (h + beta * p);
-
-		++iteration;
-	}
-
-	b3_clothSolverIterations = iteration;
-}
-
 void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations)
 {
-	float32 h = dt;
+	{
-	float32 inv_h = 1.0f / h;
+		// Solve internal dynamics
+		b3ClothForceSolverDef forceSolverDef;
+		forceSolverDef.stack = m_allocator;
+		forceSolverDef.particleCount = m_particleCount;
+		forceSolverDef.particles = m_particles;
+		forceSolverDef.forceCount = m_forceCount;
+		forceSolverDef.forces = m_forces;
 
-	b3DenseVec3 sx(m_particleCount);
+		b3ClothForceSolver forceSolver(forceSolverDef);
-	b3DenseVec3 sv(m_particleCount);
-	b3DenseVec3 sf(m_particleCount);
 
-	m_solverData.dt = h;
+		forceSolver.Solve(dt, gravity);
-	m_solverData.invdt = inv_h;
+	}
-	m_solverData.x = &sx;
+
-	m_solverData.v = &sv;
+	// Copy particle state to state buffer
-	m_solverData.f = &sf;
+	b3Vec3* positions = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
+	b3Vec3* velocities = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
 
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
-		b3Particle* p = m_particles[i];
+		positions[i] = m_particles[i]->m_position;
-
+		velocities[i] = m_particles[i]->m_velocity;
-		sx[i] = p->m_position;
-		sv[i] = p->m_velocity;
-		sf[i] = p->m_force;
-
-		// Apply weight
-		if (p->m_type == e_dynamicParticle)
-		{
-			sf[i] += p->m_mass * gravity;
-		}
 	}
 
-	// Integrate velocities
 	{
-		b3SparseSymMat33 dfdx(m_particleCount);
+		// Solve constraints
-		b3SparseSymMat33 dfdv(m_particleCount);
+		b3ClothContactSolverDef contactSolverDef;
-		b3DiagMat33 S(m_particleCount);
+		contactSolverDef.allocator = m_allocator;
-		b3DenseVec3 z(m_particleCount);
+		contactSolverDef.positions = positions;
-		b3DenseVec3 sy(m_particleCount);
+		contactSolverDef.velocities = velocities;
-		b3DenseVec3 sx0(m_particleCount);
+		contactSolverDef.bodyContactCount = m_bodyContactCount;
+		contactSolverDef.bodyContacts = m_bodyContacts;
+		contactSolverDef.triangleContactCount = m_triangleContactCount;
+		contactSolverDef.triangleContacts = m_triangleContacts;
 
-		m_solverData.y = &sy;
+		b3ClothContactSolver contactSolver(contactSolverDef);
-		m_solverData.dfdx = &dfdx;
-		m_solverData.dfdv = &dfdv;
-		m_solverData.S = &S;
-		m_solverData.z = &z;
 
-		// Apply position correction
-		for (u32 i = 0; i < m_particleCount; ++i)
 		{
-			b3Particle* p = m_particles[i];
+			// Initialize constraints
-
+			contactSolver.InitializeBodyContactConstraints();
-			sy[i] = p->m_translation;
+			contactSolver.InitializeTriangleContactConstraints();
-			sx0[i] = p->m_x;
 		}
 
-		// Apply internal forces
-		ApplyForces();
-
-		// Apply constraints
-		ApplyConstraints();
-
-		// Solve Ax = b, where
-		// A = M - h * dfdv - h * h * dfdx
-		// b = h * (f0 + h * dfdx * v0 + dfdx * y) 
-
-		b3SparseSymMat33 M(m_particleCount);
-		for (u32 i = 0; i < m_particleCount; ++i)
 		{
-			M(i, i) = b3Diagonal(m_particles[i]->m_mass);
+			// Warm start velocity constraints
+			contactSolver.WarmStartBodyContactConstraints();
+			contactSolver.WarmStartTriangleContactConstraints();
 		}
 
-		// A
-		b3SparseSymMat33 A = M - h * dfdv - h * h * dfdx;
-		
-		// View for A
-		b3SparseSymMat33View viewA(A);
-
-		// b
-		b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy);
-
-		// I
-		b3DiagMat33 I(m_particleCount);
-		I.SetIdentity();
-
-		// x
-		b3DenseVec3 x(m_particleCount);
-		b3SolveMPCG(x, viewA, b, S, z, sx0, I);
-
-		// Velocity update
-		sv = sv + x;
-		sx = sx + sy;
-
-		// Copy state buffers back to the particle
-		for (u32 i = 0; i < m_particleCount; ++i)
 		{
-			b3Particle* p = m_particles[i];
+			// Solve velocity constraints
-
+			for (u32 i = 0; i < velocityIterations; ++i)
-			p->m_x = x[i];
-			p->m_position = sx[i];
-			p->m_velocity = sv[i];
-		}
-	}
-
-	// Solve constraints
-	b3ClothContactSolverDef contactSolverDef;
-	contactSolverDef.allocator = m_allocator;
-	contactSolverDef.positions = m_solverData.x;
-	contactSolverDef.velocities = m_solverData.v;
-	contactSolverDef.bodyContactCount = m_bodyContactCount;
-	contactSolverDef.bodyContacts = m_bodyContacts;
-	contactSolverDef.triangleContactCount = m_triangleContactCount;
-	contactSolverDef.triangleContacts = m_triangleContacts;
-
-	b3ClothContactSolver contactSolver(contactSolverDef);
-
-	{
-		contactSolver.InitializeBodyContactConstraints();
-		contactSolver.InitializeTriangleContactConstraints();
-	}
-
-	{
-		contactSolver.WarmStartBodyContactConstraints();
-		contactSolver.WarmStartTriangleContactConstraints();
-	}
-
-	{
-		for (u32 i = 0; i < velocityIterations; ++i)
-		{
-			contactSolver.SolveBodyContactVelocityConstraints();
-			contactSolver.SolveTriangleContactVelocityConstraints();
-		}
-	}
-
-	{
-		contactSolver.StoreImpulses();
-	}
-
-	// Integrate positions
-	sx = sx + h * sv;
-
-	// Solve position constraints
-	{
-		bool positionSolved = false;
-		for (u32 i = 0; i < positionIterations; ++i)
-		{
-			bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
-			bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints();
-
-			if (bodyContactsSolved && triangleContactsSolved)
 			{
-				// Early out if the position errors are small.
+				contactSolver.SolveBodyContactVelocityConstraints();
-				positionSolved = true;
+				contactSolver.SolveTriangleContactVelocityConstraints();
-				break;
 			}
 		}
-	}
 
-	// Synchronize bodies
+		{
-	for (u32 i = 0; i < m_bodyContactCount; ++i)
+			// Cache impulses for warm-starting
-	{
+			contactSolver.StoreImpulses();
-		b3Body* body = m_bodyContacts[i]->s2->GetBody();
+		}
 
-		body->SynchronizeTransform();
+		// Integrate positions
+		float32 h = dt;
+		for (u32 i = 0; i < m_particleCount; ++i)
+		{
+			positions[i] += h * velocities[i];
+		}
 
-		body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
+		// Solve position constraints
+		{
+			bool positionSolved = false;
+			for (u32 i = 0; i < positionIterations; ++i)
+			{
+				bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
+				bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints();
 
-		body->SynchronizeShapes();
+				if (bodyContactsSolved && triangleContactsSolved)
+				{
+					// Early out if the position errors are small.
+					positionSolved = true;
+					break;
+				}
+			}
+		}
+
+		// Synchronize bodies
+		for (u32 i = 0; i < m_bodyContactCount; ++i)
+		{
+			b3Body* body = m_bodyContacts[i]->s2->GetBody();
+
+			body->SynchronizeTransform();
+
+			body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
+
+			body->SynchronizeShapes();
+		}
 	}
 
 	// Copy state buffers back to the particles
@@ -422,7 +182,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
 	{
 		b3Particle* p = m_particles[i];
 
-		p->m_position = sx[i];
+		p->m_position = positions[i];
-		p->m_velocity = sv[i];
+		p->m_velocity = velocities[i];
 	}
-}
+
+	m_allocator->Free(velocities);
+	m_allocator->Free(positions);
+}

src/bounce/cloth/spring_force.cpp

@@ -18,7 +18,7 @@
 
 #include <bounce/cloth/spring_force.h>
 #include <bounce/cloth/particle.h>
-#include <bounce/cloth/cloth_solver.h>
+#include <bounce/cloth/cloth_force_solver.h>
 #include <bounce/cloth/dense_vec3.h>
 #include <bounce/cloth/sparse_sym_mat33.h>
 
@@ -50,7 +50,7 @@ b3SpringForce::~b3SpringForce()
 
 }
 
-void b3SpringForce::Apply(const b3ClothSolverData* data)
+void b3SpringForce::Apply(const b3ClothForceSolverData* data)
 {
 	b3DenseVec3& x = *data->x;
 	b3DenseVec3& v = *data->v;