From 93ac1faa7c3b6e296a5e604c2583fdbd6982b7a6 Mon Sep 17 00:00:00 2001
From: Irlan <->
Date: Sun, 24 Jun 2018 21:25:41 -0300
Subject: [PATCH] bugfix

---
 .../bounce/dynamics/cloth/sparse_sym_mat33.h  | 17 +--------
 src/bounce/dynamics/cloth/cloth_solver.cpp    | 38 ++++++++++++++-----
 2 files changed, 30 insertions(+), 25 deletions(-)

diff --git a/include/bounce/dynamics/cloth/sparse_sym_mat33.h b/include/bounce/dynamics/cloth/sparse_sym_mat33.h
index 3cd571a..e4e06aa 100644
--- a/include/bounce/dynamics/cloth/sparse_sym_mat33.h
+++ b/include/bounce/dynamics/cloth/sparse_sym_mat33.h
@@ -40,9 +40,6 @@ struct b3SparseSymMat33
 	// 
 	void Diagonal(b3DiagMat33& out) const;
 
-	// 
-	void SetZero();
-
 	u32 M;
 	u32* row_ptrs; 
 	u32 value_capacity;
@@ -137,6 +134,7 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j)
 	}
 
 	// Shift the values
+	B3_ASSERT(value_count < value_capacity);
 	for (u32 row_value = value_count; row_value > row_value_begin + row_value_k; --row_value)
 	{
 		values[row_value] = values[row_value - 1];
@@ -145,6 +143,7 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j)
 
 	// Insert the value
 	value_columns[row_value_begin + row_value_k] = j;
+	values[row_value_begin + row_value_k].SetZero();
 	++value_count;
 
 	// Shift the row pointers 
@@ -182,17 +181,6 @@ inline void b3SparseSymMat33::Diagonal(b3DiagMat33& out) const
 	}
 }
 
-inline void b3SparseSymMat33::SetZero()
-{
-	for (u32 i = 0; i < M; ++i)
-	{
-		for (u32 j = i; j < M; ++j)
-		{
-			(*this)(i, j).SetZero();
-		}
-	}
-}
-
 inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3& v)
 {
 	B3_ASSERT(A.M == out.n);
@@ -207,7 +195,6 @@ inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3
 		for (u32 row_value = 0; row_value < row_value_count; ++row_value)
 		{
 			u32 row_value_index = row_value_begin + row_value;
-
 			u32 row_value_column = A.value_columns[row_value_index];
 
 			out[row] += A.values[row_value_index] * v[row_value_column];
diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp
index 1afeeb6..2c4fc32 100644
--- a/src/bounce/dynamics/cloth/cloth_solver.cpp
+++ b/src/bounce/dynamics/cloth/cloth_solver.cpp
@@ -150,12 +150,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	b3DenseVec3 sf(m_particleCount);
 	b3DenseVec3 sy(m_particleCount);
 	b3DenseVec3 sx0(m_particleCount);
-
 	b3SparseSymMat33 dfdx(m_particleCount);
-	dfdx.SetZero();
-
 	b3SparseSymMat33 dfdv(m_particleCount);
-	dfdv.SetZero();
 
 	m_solverData.x = &sx;
 	m_solverData.v = &sv;
@@ -284,7 +280,7 @@ void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const
 	// A = M - h * dfdv - h * h * dfdx
 
 	// A = 0
-	A.SetZero();
+	//A.SetZero();
 
 	// A += M
 	for (u32 i = 0; i < m_particleCount; ++i)
@@ -293,15 +289,37 @@ void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const
 	}
 
 	// A += - h * dfdv - h * h * dfdx
-	// Set the upper triangle
-	for (u32 i = 0; i < m_particleCount; ++i)
+	
+	// A += - h * dfdv 
+	for (u32 row = 0; row < dfdv.M; ++row)
 	{
-		for (u32 j = i; j < m_particleCount; ++j)
+		u32 row_value_begin = dfdv.row_ptrs[row];
+		u32 row_value_count = dfdv.row_ptrs[row + 1] - row_value_begin;
+
+		for (u32 row_value = 0; row_value < row_value_count; ++row_value)
 		{
-			A(i, j) += (-h * dfdv(i, j)) + (-h * h * dfdx(i, j));
+			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];
 		}
 	}
 
+	// A += - h * h * dfdx
+	for (u32 row = 0; row < dfdx.M; ++row)
+	{
+		u32 row_value_begin = dfdx.row_ptrs[row];
+		u32 row_value_count = dfdx.row_ptrs[row + 1] - row_value_begin;
+
+		for (u32 row_value = 0; row_value < row_value_count; ++row_value)
+		{
+			u32 row_value_index = row_value_begin + row_value;
+			u32 row_value_column = dfdx.value_columns[row_value_index];
+
+			A(row, row_value_column) += -h * h * dfdx.values[row_value_index];
+		}
+	}
+	
 	// Compute b
 
 	// b = h * (f0 + h * dfdx * v + dfdx * y)
@@ -401,7 +419,7 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
 
 	// Maximum number of iterations.
 	// Stop at this iteration if diverged.
-	const u32 max_iterations = 100;
+	const u32 max_iterations = 20;
 
 	u32 iteration = 0;