Move files around. Centralize sparsity and pattern structures. Add a view for b3SparseMat33.

include/bounce/cloth/cloth_solver.h

@@ -87,7 +87,9 @@ private:
 	void ApplyConstraints();
 
 	// Solve Ax = b.
-	void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33View& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
+	void SolveMPCG(b3DenseVec3& x, 
+		const b3SparseSymMat33View& A, const b3DenseVec3& b, 
+		const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y, u32 maxIterations = 30) const;
 
 	b3StackAllocator* m_allocator;
 

include/bounce/cloth/sparse_mat33.h

@@ -22,7 +22,36 @@
 #include <bounce/common/math/mat33.h>
 #include <bounce/cloth/diag_mat33.h>
 #include <bounce/cloth/dense_vec3.h>
-#include <bounce/cloth/sparse_sym_mat33.h>
+
+// An element in a sparse matrix.
+struct b3RowValue
+{
+	u32 column;
+	b3Mat33 value;
+	b3RowValue* next;
+};
+
+// Singly linked list of row elements.
+struct b3RowValueList
+{
+	b3RowValueList()
+	{
+		head = nullptr;
+		count = 0;
+	}
+
+	~b3RowValueList() { }
+
+	void PushFront(b3RowValue* link)
+	{
+		link->next = head;
+		head = link;
+		++count;
+	}
+
+	b3RowValue* head;
+	u32 count;
+};
 
 // A sparse matrix.
 // Each row is a list of non-zero elements in the row.

include/bounce/cloth/sparse_mat33_view.h

@@ -0,0 +1,137 @@
+/*
+* 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_SPARSE_MAT_33_VIEW_H
+#define B3_SPARSE_MAT_33_VIEW_H
+
+#include <bounce/cloth/sparse_mat33.h>
+
+struct b3ArrayRowValue
+{
+	u32 column;
+	b3Mat33 value;
+};
+
+struct b3RowValueArray
+{
+	u32 count;
+	b3ArrayRowValue* values;
+};
+
+// A read-only sparse matrix.
+struct b3SparseMat33View
+{
+	//
+	b3SparseMat33View(const b3SparseMat33& _m);
+
+	//
+	~b3SparseMat33View();
+
+	//
+	const b3Mat33& operator()(u32 i, u32 j) const;
+
+	u32 rowCount;
+	b3RowValueArray* rows;
+};
+
+inline b3SparseMat33View::b3SparseMat33View(const b3SparseMat33& _m)
+{
+	rowCount = _m.rowCount;
+	rows = (b3RowValueArray*)b3Alloc(rowCount * sizeof(b3RowValueArray));
+	for (u32 i = 0; i < _m.rowCount; ++i)
+	{
+		b3RowValueList* rowList = _m.rows + i;
+		b3RowValueArray* rowArray = rows + i;
+
+		rowArray->count = rowList->count;
+		rowArray->values = (b3ArrayRowValue*)b3Alloc(rowArray->count * sizeof(b3ArrayRowValue));
+
+		u32 valueIndex = 0;
+		for (b3RowValue* v = rowList->head; v; v = v->next)
+		{
+			rowArray->values[valueIndex].column = v->column;
+			rowArray->values[valueIndex].value = v->value;
+			++valueIndex;
+		}
+	}
+}
+
+inline b3SparseMat33View::~b3SparseMat33View()
+{
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		b3RowValueArray* rowArray = rows + i;
+		b3Free(rowArray->values);
+	}
+	b3Free(rows);
+}
+
+inline const b3Mat33& b3SparseMat33View::operator()(u32 i, u32 j) const
+{
+	B3_ASSERT(i < rowCount);
+	B3_ASSERT(j < rowCount);
+
+	if (i > j)
+	{
+		b3Swap(i, j);
+	}
+
+	b3RowValueArray* vs = rows + i;
+
+	for (u32 c = 0; c < vs->count; ++c)
+	{
+		b3ArrayRowValue* rv = vs->values + c;
+		if (rv->column == j)
+		{
+			return rv->value;
+		}
+	}
+
+	return b3Mat33_zero;
+}
+
+inline void b3Mul(b3DenseVec3& out, const b3SparseMat33View& A, const b3DenseVec3& v)
+{
+	B3_ASSERT(A.rowCount == out.n);
+
+	out.SetZero();
+
+	for (u32 i = 0; i < A.rowCount; ++i)
+	{
+		b3RowValueArray* rowArray = A.rows + i;
+
+		for (u32 c = 0; c < rowArray->count; ++c)
+		{
+			b3ArrayRowValue* rv = rowArray->values + c;
+
+			u32 j = rv->column;
+			b3Mat33 a = rv->value;
+
+			out[i] += a * v[j];
+		}
+	}
+}
+
+inline b3DenseVec3 operator*(const b3SparseMat33View& A, const b3DenseVec3& v)
+{
+	b3DenseVec3 result(v.n);
+	b3Mul(result, A, v);
+	return result;
+}
+
+#endif

include/bounce/cloth/sparse_sym_mat33.h

@@ -19,44 +19,14 @@
 #ifndef B3_SPARSE_SYM_MAT_33_H
 #define B3_SPARSE_SYM_MAT_33_H
 
-#include <bounce/common/math/mat33.h>
+#include <bounce/cloth/sparse_mat33.h>
-#include <bounce/cloth/diag_mat33.h>
-#include <bounce/cloth/dense_vec3.h>
-
-// An element in a sparse symmetric matrix.
-struct b3RowValue
-{
-	u32 column;
-	b3Mat33 value;
-	b3RowValue* next;
-};
-
-// Doubly linked list of row elements.
-struct b3RowValueList
-{
-	b3RowValueList()
-	{
-		head = nullptr;
-		count = 0;
-	}
-
-	~b3RowValueList() { }
-
-	void PushFront(b3RowValue* link)
-	{
-		link->next = head;
-		head = link;
-		++count;
-	}
-
-	b3RowValue* head;
-	u32 count;
-};
 
 // A sparse symmetric matrix.
 // Each row is a list of non-zero elements in the row.
 // The total matrix capacity is bounded by 
 // M * (M + 1) / 2
+// You must write only the upper triangle elements of the original
+// matrix to this matrix because a_ji = a_ij.
 struct b3SparseSymMat33
 {
 	//

include/bounce/cloth/sparse_sym_mat33_view.h

@@ -19,20 +19,9 @@
 #ifndef B3_SPARSE_SYM_MAT_33_VIEW_H
 #define B3_SPARSE_SYM_MAT_33_VIEW_H
 
+#include <bounce/cloth/sparse_mat33_view.h>
 #include <bounce/cloth/sparse_sym_mat33.h>
 
-struct b3ArrayRowValue
-{
-	u32 column;
-	b3Mat33 value;
-};
-
-struct b3RowValueArray
-{
-	u32 count;
-	b3ArrayRowValue* values;
-};
-
 // A read-only sparse symmetric matrix.
 struct b3SparseSymMat33View
 {
@@ -45,9 +34,6 @@ struct b3SparseSymMat33View
 	//
 	const b3Mat33& operator()(u32 i, u32 j) const;
 	
-	//
-	void Diagonal(b3DiagMat33& out) const;
-		
 	u32 rowCount;
 	b3RowValueArray* rows;
 };
@@ -99,6 +85,7 @@ inline const b3Mat33& b3SparseSymMat33View::operator()(u32 i, u32 j) const
 	for (u32 c = 0; c < vs->count; ++c)
 	{
 		b3ArrayRowValue* rv = vs->values + c;
+		
 		if (rv->column == j)
 		{
 			return rv->value;
@@ -108,16 +95,6 @@ inline const b3Mat33& b3SparseSymMat33View::operator()(u32 i, u32 j) const
 	return b3Mat33_zero;
 }
 
-inline void b3SparseSymMat33View::Diagonal(b3DiagMat33& out) const
-{
-	B3_ASSERT(rowCount == out.n);
-
-	for (u32 i = 0; i < rowCount; ++i)
-	{
-		out[i] = (*this)(i, i);
-	}
-}
-
 inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33View& A, const b3DenseVec3& v)
 {
 	B3_ASSERT(A.rowCount == out.n);

src/bounce/cloth/cloth_solver.cpp

@@ -287,9 +287,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 
 		// x
 		b3DenseVec3 x(m_particleCount);
-		u32 iterations = 0;
+		SolveMPCG(x, viewA, b, S, z, sx0);
-		Solve(x, iterations, viewA, b, S, z, sx0);
-		b3_clothSolverIterations = iterations;
 #if 0
 		// Copy constraint forces to the contacts
 		b3DenseVec3 f = A * x - b;
@@ -398,17 +396,21 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	}
 }
 
-void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
+void b3ClothSolver::SolveMPCG(b3DenseVec3& x,
-	const b3SparseSymMat33View& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
+	const b3SparseSymMat33View& A, const b3DenseVec3& b, 
+	const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y, u32 maxIterations) const
 {
 	B3_PROFILE("Cloth Solve Ax = b");
 
 	// P = diag(A)
-	b3DiagMat33 inv_P(m_particleCount);
+	b3DiagMat33 inv_P(A.rowCount);
-	A.Diagonal(inv_P);
+	for (u32 i = 0; i < A.rowCount; ++i)
+	{
+		inv_P[i] = A(i, i);
+	}
 
 	// Invert
-	for (u32 i = 0; i < m_particleCount; ++i)
+	for (u32 i = 0; i < inv_P.n; ++i)
 	{
 		b3Mat33& D = inv_P[i];
 
@@ -427,81 +429,53 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
 		D = b3Diagonal(xx, yy, zz);
 	}
 
-	// I
 	b3DiagMat33 I(m_particleCount);
 	I.SetIdentity();
 
-	// x = S * y + (I - S) * z 
 	x = (S * y) + (I - S) * z;
 
-	// b^ = S * (b - A * (I - S) * z)
 	b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
 
-	// b_delta = dot(b^, P^-1 * b_^)
 	float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
 
-	// r = S * (b - A * x)
 	b3DenseVec3 r = S * (b - A * x);
 
-	// p = S * (P^-1 * r)
 	b3DenseVec3 p = S * (inv_P * r);
 
-	// delta_new = dot(r, p)
 	float32 delta_new = b3Dot(r, p);
 
-	// Set the tolerance.
-	const float32 tolerance = 2.0f * B3_EPSILON;
-
-	// Maximum number of iterations.
-	// Stop at this iteration if diverged.
-	const u32 max_iterations = 50;
-
 	u32 iteration = 0;
-
-	// Main iteration loop.
 	for (;;)
 	{
-		// Divergence check.
+		if (iteration >= maxIterations)
-		if (iteration >= max_iterations)
 		{
 			break;
 		}
 		
-		// Convergence check.
+		if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta)
-		if (delta_new <= tolerance * tolerance * b_delta)
 		{
 			break;
 		}
 
-		// s = S * (A * p)
 		b3DenseVec3 s = S * (A * p);
 
-		// alpha = delta_new / dot(p, s)
 		float32 alpha = delta_new / b3Dot(p, s);
 
-		// x = x + alpha * p
 		x = x + alpha * p;
-
-		// r = r - alpha * s
 		r = r - alpha * s;
 
-		// h = inv_P * r
 		b3DenseVec3 h = inv_P * r;
 
-		// delta_old = delta_new
 		float32 delta_old = delta_new;
 
-		// delta_new = dot(r, h)
 		delta_new = b3Dot(r, h);
 
-		// beta = delta_new / delta_old
 		float32 beta = delta_new / delta_old;
 
-		// p = S * (h + beta * p)
 		p = S * (h + beta * p);
 
 		++iteration;
 	}
 
-	iterations = iteration;
+	b3_clothSolverIterations = iteration;
 }

src/bounce/softbody/softbody_solver.cpp

@@ -20,12 +20,14 @@
 #include <bounce/softbody/softbody_mesh.h>
 #include <bounce/softbody/softbody_node.h>
 #include <bounce/softbody/softbody.h>
-#include <bounce/softbody/sparse_mat33.h>
 #include <bounce/softbody/softbody_contact_solver.h>
 
 #include <bounce/dynamics/body.h>
 #include <bounce/dynamics/shapes/shape.h>
 
+#include <bounce/cloth/sparse_mat33.h>
+#include <bounce/cloth/sparse_mat33_view.h>
+
 // This work is based on the paper "Interactive Virtual Materials" written by 
 // Matthias Mueller Fischer
 // The paper is available here:
@@ -91,9 +93,11 @@ static void b3ExtractRotation(b3Mat33& out, b3Quat& q, const b3Mat33& A, u32 max
 }
 
 static void b3SolveMPCG(b3DenseVec3& x,
-	const b3SparseMat33& A, const b3DenseVec3& b,
+	const b3SparseMat33View& A, const b3DenseVec3& b,
 	const b3DenseVec3& z, const b3DiagMat33& S, u32 maxIterations = 20)
 {
+	B3_PROFILE("Soft Body Solve Ax = b");
+
 	b3DiagMat33 P(A.rowCount);
 	b3DiagMat33 invP(A.rowCount);
 	for (u32 i = 0; i < A.rowCount; ++i)
@@ -378,11 +382,13 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter
 
 	b3SparseMat33 A = M + h * C + h * h * K;
 	
+	b3SparseMat33View viewA(A);
+
 	b3DenseVec3 b = M * v - h * (K * p + f0 - (f_plastic + fe));
 
 	// Solve Ax = b
 	b3DenseVec3 sx(m_mesh->vertexCount);
-	b3SolveMPCG(sx, A, b, z, S);
+	b3SolveMPCG(sx, viewA, b, z, S);
 
 	// Solve constraints
 	b3SoftBodyContactSolverDef contactSolverDef;