cleanup

include/bounce/dynamics/cloth/cloth_solver.h

@@ -48,6 +48,8 @@ struct b3ClothSolverData
 	b3DenseVec3* y;
 	b3SparseSymMat33* dfdx;
 	b3SparseSymMat33* dfdv;
+	b3DiagMat33* S;
+	b3DenseVec3* z;
 	float32 dt;
 	float32 invdt;
 };
@@ -57,6 +59,8 @@ struct b3AccelerationConstraint
 	u32 i1;
 	u32 ndof;
 	b3Vec3 p, q, z;
+
+	void Apply(const b3ClothSolverData* data);
 };
 
 class b3ClothSolver
@@ -71,21 +75,15 @@ public:
 
 	void Solve(float32 dt, const b3Vec3& gravity);
 private:
-	// Initialize forces.
-	void InitializeForces();
-	
 	// Apply forces.
 	void ApplyForces();
 	
-	// Initialize constraints.
+	// Apply constraints.
-	void InitializeConstraints();
+	void ApplyConstraints();
 
-	// Compute A and b in Ax = b
+	// Compute A and b in Ax = b.
 	void Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const;
 
-	// Compute S and z.
-	void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z);
-
 	// Solve Ax = b.
 	void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
 

include/bounce/dynamics/cloth/force.h

@@ -58,11 +58,8 @@ protected:
 	b3Force() { }
 	virtual ~b3Force() { }
 
-	virtual void Initialize(const b3ClothSolverData* data) = 0;
 	virtual void Apply(const b3ClothSolverData* data) = 0;
 
-	// Solver shared
-
 	// Force type
 	b3ForceType m_type;
 

include/bounce/dynamics/cloth/spring_force.h

@@ -73,9 +73,7 @@ private:
 
 	b3SpringForce(const b3SpringForceDef* def);
 	~b3SpringForce();
-	
+
-	void Initialize(const b3ClothSolverData* data);
-	
 	void Apply(const b3ClothSolverData* data);
 
 	// Solver shared
@@ -95,16 +93,8 @@ private:
 	// Damping stiffness
 	float32 m_kd;
 
-	// Solver temp
+	// Applied internal force (on particle 1)
-
-	// Force (f_1 entry)
 	b3Vec3 m_f;
-
-	// Jacobian (J_11 entry)
-	b3Mat33 m_Jx;
-	
-	// Jacobian (J_11 entry)
-	b3Mat33 m_Jv;
 };
 
 inline b3Particle * b3SpringForce::GetParticle1()

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -79,14 +79,6 @@ void b3ClothSolver::Add(b3Force* f)
 	m_forces[m_forceCount++] = f;
 }
 
-void b3ClothSolver::InitializeForces()
-{
-	for (u32 i = 0; i < m_forceCount; ++i)
-	{
-		m_forces[i]->Initialize(&m_solverData);
-	}
-}
-
 void b3ClothSolver::ApplyForces()
 {
 	for (u32 i = 0; i < m_forceCount; ++i)
@@ -95,8 +87,50 @@ void b3ClothSolver::ApplyForces()
 	}
 }
 
-void b3ClothSolver::InitializeConstraints()
+void b3AccelerationConstraint::Apply(const b3ClothSolverData* data)
 {
+	(*data->z)[i1] = z;
+
+	b3Mat33 I; I.SetIdentity();
+
+	switch (ndof)
+	{
+	case 3:
+	{
+		(*data->S)[i1] = I;
+		break;
+	}
+	case 2:
+	{
+		(*data->S)[i1] = I - b3Outer(p, p);
+		break;
+	}
+	case 1:
+	{
+		(*data->S)[i1] = I - b3Outer(p, p) - b3Outer(q, q);
+		break;
+	}
+	case 0:
+	{
+		(*data->S)[i1].SetZero();
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		break;
+	}
+	}
+}
+
+void b3ClothSolver::ApplyConstraints()
+{
+	b3DiagMat33& S = *m_solverData.S;
+	b3DenseVec3& z = *m_solverData.z;
+
+	S.SetIdentity();
+	z.SetZero();
+
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
 		b3Particle* p = m_particles[i];
@@ -141,6 +175,11 @@ void b3ClothSolver::InitializeConstraints()
 			}
 		}
 	}
+
+	for (u32 i = 0; i < m_constraintCount; ++i)
+	{
+		m_constraints[i].Apply(&m_solverData);
+	}
 }
 
 void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
@@ -152,6 +191,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	b3DenseVec3 sx0(m_particleCount);
 	b3SparseSymMat33 dfdx(m_particleCount);
 	b3SparseSymMat33 dfdv(m_particleCount);
+	b3DiagMat33 S(m_particleCount);
+	b3DenseVec3 z(m_particleCount);
 
 	m_solverData.x = &sx;
 	m_solverData.v = &sv;
@@ -159,6 +200,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	m_solverData.y = &sy;
 	m_solverData.dfdx = &dfdx;
 	m_solverData.dfdv = &dfdv;
+	m_solverData.S = &S;
+	m_solverData.z = &z;
 	m_solverData.dt = dt;
 	m_solverData.invdt = 1.0f / dt;
 
@@ -188,19 +231,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		sy[p->m_solverId] -= c->s * c->n;
 	}
 
-	// Initialize internal forces
-	InitializeForces();
-
 	// Apply internal forces
 	ApplyForces();
 
-	// Initialize constraints
+	// Apply constraints
-	InitializeConstraints();
+	ApplyConstraints();
-
-	// Compute S, z
-	b3DiagMat33 S(m_particleCount);
-	b3DenseVec3 z(m_particleCount);
-	Compute_S_z(S, z);
 
 	// Solve Ax = b, where
 	// A = M - h * dfdv - h * h * dfdx
@@ -267,7 +302,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const
 {
 	B3_PROFILE("Compute A, b");
-	
+
 	b3DenseVec3& x = *m_solverData.x;
 	b3DenseVec3& v = *m_solverData.v;
 	b3DenseVec3& f = *m_solverData.f;
@@ -289,7 +324,7 @@ void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const
 	}
 
 	// A += - h * dfdv - h * h * dfdx
-	
+
 	// A += - h * dfdv 
 	for (u32 row = 0; row < dfdv.M; ++row)
 	{
@@ -300,7 +335,7 @@ void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const
 		{
 			u32 row_value_index = row_value_begin + row_value;
 			u32 row_value_column = dfdv.value_columns[row_value_index];
-			
+
 			A(row, row_value_column) += -h * dfdv.values[row_value_index];
 		}
 	}
@@ -319,50 +354,13 @@ void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const
 			A(row, row_value_column) += -h * h * dfdx.values[row_value_index];
 		}
 	}
-	
+
 	// Compute b
 
 	// b = h * (f0 + h * dfdx * v + dfdx * y)
 	b = h * (f + h * (dfdx * v) + dfdx * y);
 }
 
-void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z)
-{
-	S.SetIdentity();
-	z.SetZero();
-
-	for (u32 i = 0; i < m_constraintCount; ++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;
-
-		z[ip] = cz;
-
-		if (ndof == 2)
-		{
-			b3Mat33 I; I.SetIdentity();
-
-			S[ip] = I - b3Outer(p, p);
-		}
-
-		if (ndof == 1)
-		{
-			b3Mat33 I; I.SetIdentity();
-
-			S[ip] = I - b3Outer(p, p) - b3Outer(q, q);
-		}
-
-		if (ndof == 0)
-		{
-			S[ip].SetZero();
-		}
-	}
-}
-
 void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
 	const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
 {
@@ -415,7 +413,7 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
 	float32 delta_new = b3Dot(r, p);
 
 	// Set the tolerance.
-	const float32 tolerance = 1.e-4f;
+	const float32 tolerance = 10.0f * B3_EPSILON;
 
 	// Maximum number of iterations.
 	// Stop at this iteration if diverged.

src/bounce/dynamics/cloth/spring_force.cpp

@@ -50,10 +50,13 @@ b3SpringForce::~b3SpringForce()
 
 }
 
-void b3SpringForce::Initialize(const b3ClothSolverData* data)
+void b3SpringForce::Apply(const b3ClothSolverData* data)
 {
 	b3DenseVec3& x = *data->x;
 	b3DenseVec3& v = *data->v;
+	b3DenseVec3& f = *data->f;
+	b3SparseSymMat33& dfdx = *data->dfdx;
+	b3SparseSymMat33& dfdv = *data->dfdv;
 
 	u32 i1 = m_p1->m_solverId;
 	u32 i2 = m_p2->m_solverId;
@@ -70,6 +73,8 @@ void b3SpringForce::Initialize(const b3ClothSolverData* data)
 
 	float32 L = b3Length(dx);
 
+	b3Mat33 Jx;
+
 	if (L >= m_L0)
 	{
 		b3Vec3 n = dx / L;
@@ -78,34 +83,24 @@ void b3SpringForce::Initialize(const b3ClothSolverData* data)
 		m_f = -m_ks * (L - m_L0) * n;
 
 		// Jacobian
-		m_Jx = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx)));
+		Jx = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx)));
 	}
 	else
 	{
 		m_f.SetZero();
-		m_Jx.SetZero();
+		Jx.SetZero();
 	}
 
 	// Damping
 	b3Vec3 dv = v1 - v2;
 
 	m_f += -m_kd * dv;
-	m_Jv = -m_kd * I;
+	b3Mat33 Jv = -m_kd * I;
-}
-
-void b3SpringForce::Apply(const b3ClothSolverData* data)
-{
-	b3DenseVec3& f = *data->f;
-	b3SparseSymMat33& dfdx = *data->dfdx;
-	b3SparseSymMat33& dfdv = *data->dfdv;
-
-	u32 i1 = m_p1->m_solverId;
-	u32 i2 = m_p2->m_solverId;
 
 	f[i1] += m_f;
 	f[i2] -= m_f;
 
-	b3Mat33 Jx11 = m_Jx;
+	b3Mat33 Jx11 = Jx;
 	b3Mat33 Jx12 = -Jx11;
 	//b3Mat33 Jx21 = Jx12;
 	b3Mat33 Jx22 = Jx11;
@@ -115,7 +110,7 @@ void b3SpringForce::Apply(const b3ClothSolverData* data)
 	//dfdx(i2, i1) += Jx21;
 	dfdx(i2, i2) += Jx22;
 
-	b3Mat33 Jv11 = m_Jv;
+	b3Mat33 Jv11 = Jv;
 	b3Mat33 Jv12 = -Jv11;
 	//b3Mat33 Jv21 = Jv12;
 	b3Mat33 Jv22 = Jv11;