gauss seidel solver

6 files changed, 196 insertions, 1 deletions

diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index 3e006f74ae3..b8f036feebb 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -39,6 +39,8 @@ set(SRC
   src/admmpd_embeddedmesh.cpp
   src/admmpd_energy.h
   src/admmpd_energy.cpp
+  src/admmpd_linsolve.h
+  src/admmpd_linsolve.cpp
   src/admmpd_math.h
   src/admmpd_math.cpp
   src/admmpd_solver.h
diff --git a/extern/softbody/src/admmpd_bvh.cpp b/extern/softbody/src/admmpd_bvh.cpp
index 97c39271199..fffc8c33a74 100644
--- a/extern/softbody/src/admmpd_bvh.cpp
+++ b/extern/softbody/src/admmpd_bvh.cpp
@@ -23,7 +23,7 @@ void AABBTree<T,DIM>::init(const std::vector<AABB> &leaves)
     if (np==0)
         return;
     std::vector<int> queue(np);
-	std::iota(queue.begin(), queue.end(), 0);
+    std::iota(queue.begin(), queue.end(), 0);
     create_children(root.get(), queue, leaves);
 }
 
diff --git a/extern/softbody/src/admmpd_linsolve.cpp b/extern/softbody/src/admmpd_linsolve.cpp
new file mode 100644
index 00000000000..5b3a62dc7bb
--- /dev/null
+++ b/extern/softbody/src/admmpd_linsolve.cpp
@@ -0,0 +1,147 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_linsolve.h"
+#include <numeric>
+#include <iostream>
+#include <unordered_set>
+#include "BLI_assert.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+void GaussSeidel::solve(
+		const Options *options,
+		SolverData *data)
+{
+	init_solve(options,data);
+	std::vector<std::vector<int> > *colors;
+	//if (data->gsdata.KtK.nonZeros()==0)
+		colors = &data->gsdata.A_colors;
+	//else...
+
+	double omega = 1.0; // over relaxation
+	int n_colors = colors->size();
+
+	// Outer iteration loop
+	int iter = 0;
+	for (; iter < options->max_gs_iters; ++iter)
+	{
+		for (int color=0; color<n_colors; ++color)
+		{
+			const std::vector<int> &inds = colors->at(color);
+			int n_inds = inds.size();
+			for (int i=0; i<n_inds; ++i)
+			{
+				int idx = inds[i];
+
+				// Special case pins TODO
+				// We can skip the usual Gauss-Seidel update
+	//			if (is_pinned[idx]) ...
+
+				RowSparseMatrix<double>::InnerIterator rit(data->A,idx);
+				Vector3d LUx(0,0,0);
+				Vector3d inv_aii(0,0,0);
+				for (; rit; ++rit)
+				{
+					int r = rit.row();
+					int c = rit.col();
+					double v = rit.value();
+					if (v==0.0)
+						continue;
+
+					if (r==c) // Diagonal
+					{
+						inv_aii.array() = 1.0/v;
+						continue;
+					}
+					Vector3d xj = data->x.row(c);
+					LUx += v*xj;
+				}
+
+				// Update x
+				Vector3d bi = data->b.row(idx);
+				Vector3d xi = data->x.row(idx);
+				Vector3d xi_new = (bi-LUx);
+
+				for (int j=0; j<3; ++j)
+					xi_new[j] *= inv_aii[j];
+				data->x.row(idx) = xi*(1.0-omega) + xi_new*omega;
+				data->gsdata.last_dx.row(idx) = data->x.row(idx)-xi.transpose();
+
+				// TODO
+				// We can also apply constraints here, like
+				// checking against Collision::floor_z
+				if (data->x(idx,2)<0)
+					data->x(idx,2)=0;
+
+			} // end loop inds
+		} // end loop colors
+
+		// TODO check exit condition
+
+	} // end loop GS iters
+
+} // end solve with constraints
+
+void GaussSeidel::init_solve(
+		const Options *options,
+		SolverData *data)
+{
+	BLI_assert(options != nullptr);
+	BLI_assert(data != nullptr);
+	int nx = data->x.rows();
+	BLI_assert(nx>0);
+	BLI_assert(data->x.cols()==3);
+	data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u);
+	BLI_assert(data->b.rows()==nx);
+	BLI_assert(data->b.cols()==data->x.cols());
+
+	if (data->gsdata.last_dx.rows() != nx)
+	{
+		data->gsdata.last_dx.resize(nx,3);
+		data->gsdata.last_dx.setZero();
+	}
+
+	// Do we need to color the default colorings?
+	if( data->gsdata.A_colors.size() == 0 )
+		compute_colors(&data->A,nullptr,data->gsdata.A_colors);
+
+	// TODO: Eventually we'll replace KtK with the full-dof matrix.
+	// For now use z and test collisions against ground plane.
+	bool has_constraints = data->K[2].nonZeros()>0;
+	data->gsdata.KtK = data->K[2].transpose()*data->K[2];
+
+	if (false)//(has_constraints)
+	{
+		(void)(has_constraints);
+		// TODO color A + KtK
+	}
+
+} // end init solve
+
+void GaussSeidel::compute_colors(
+		const RowSparseMatrix<double> *A,
+		const RowSparseMatrix<double> *KtK, // if null, just A
+		std::vector<std::vector<int> > &colors)
+{
+	BLI_assert(A != nullptr);
+	if (KtK != nullptr)
+	{
+		throw std::runtime_error("**GaussSeidel::compute_colors TODO: KtK");
+	}
+
+	colors.clear();
+	int nx = A->rows();
+
+	{ // DEBUGGING
+		colors.resize(nx,std::vector<int>());
+		for (int i=0; i<nx; ++i)
+		{
+			colors[i].emplace_back(i);
+		}
+	}
+
+} // end compute colors
+
+} // namespace admmpd
diff --git a/extern/softbody/src/admmpd_linsolve.h b/extern/softbody/src/admmpd_linsolve.h
new file mode 100644
index 00000000000..b1dd9ae8f4e
--- /dev/null
+++ b/extern/softbody/src/admmpd_linsolve.h
@@ -0,0 +1,36 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_LINSOLVE_H_
+#define ADMMPD_LINSOLVE_H_
+
+#include "admmpd_types.h"
+
+namespace admmpd {
+
+class GaussSeidel {
+public:
+	// Solves (A + KtK) x = (b + Ktl)
+	// x and b passed as separate variables
+	// for debugging/testing purposes.
+	void solve(
+		const Options *options,
+		SolverData *data);
+
+protected:
+	// Allocates data, computes colors
+	void init_solve(
+		const Options *options,
+		SolverData *data);
+
+	// Computes colors of A + KtK
+	void compute_colors(
+		const RowSparseMatrix<double> *A,
+		const RowSparseMatrix<double> *KtK, // if null, just A
+		std::vector<std::vector<int> > &colors);
+
+};
+
+} // namespace admmpd
+
+#endif // ADMMPD_LINSOLVE_H_
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index ef1c1b50ca4..944ebf7826e 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -4,6 +4,7 @@
 #include "admmpd_solver.h"
 #include "admmpd_energy.h"
 #include "admmpd_collision.h"
+#include "admmpd_linsolve.h"
 
 #include <Eigen/Geometry>
 #include <Eigen/Sparse>
@@ -80,6 +81,7 @@ int Solver::solve(
 
 		// Solve Ax=b s.t. Kx=l
 		solve_conjugate_gradients(options,data);
+		//GaussSeidel().solve(options,data);
 
 	} // end solver iters
 
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index 13edb93c3da..46765150e1c 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -18,6 +18,7 @@ struct Options {
     double timestep_s; // TODO: Figure out delta time from blender api
     int max_admm_iters;
     int max_cg_iters;
+    int max_gs_iters;
     double mult_k; // stiffness multiplier for constraints
     double min_res; // min residual for CG solver
     double youngs; // Young's modulus // TODO variable per-tet
@@ -27,6 +28,7 @@ struct Options {
         timestep_s(1.0/24.0),
         max_admm_iters(50),
         max_cg_iters(10),
+        max_gs_iters(30),
         mult_k(1),
         min_res(1e-6),
         youngs(1000000),
@@ -82,6 +84,12 @@ struct SolverData {
         Eigen::MatrixXd p;
         Eigen::MatrixXd Ap; // A * p
     } cgdata;
+    struct GSData { // Temporaries used in Gauss-Seidel
+		RowSparseMatrix<double> KtK; // k * Kt K, different dim than A!
+        Eigen::MatrixXd last_dx; // last GS iter change in x
+		std::vector<std::vector<int> > A_colors; // colors of just A matrix
+        std::vector<std::vector<int> > A_KtK_colors; // colors of just A+KtK
+	} gsdata;
     // Set in append_energies:
 	std::vector<Eigen::Vector2i> indices; // per-energy index into D (row, num rows)
 	std::vector<double> rest_volumes; // per-energy rest volume