From b650bdc5ecfca7357cbd4cc1f0f5b2279db91a9c Mon Sep 17 00:00:00 2001
From: over0219 <over0219@umn.edu>
Date: Thu, 9 Jul 2020 10:06:30 -0500
Subject: using CG solver as default for now

---
 extern/softbody/src/admmpd_collision.cpp |  8 +-------
 extern/softbody/src/admmpd_collision.h   |  2 +-
 extern/softbody/src/admmpd_linsolve.cpp  | 33 ++++++++++++++++++++++++++------
 extern/softbody/src/admmpd_solver.cpp    |  4 ++--
 extern/softbody/src/admmpd_types.h       |  4 ++--
 5 files changed, 33 insertions(+), 18 deletions(-)

(limited to 'extern/softbody/src')

diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 3ff7f949215..9508b74a740 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -320,6 +320,7 @@ void EmbeddedMeshCollision::linearize(
 	if (np==0)
 		return;
 
+	//int nx = x->rows();
 	d->reserve((int)d->size() + np);
 	trips->reserve((int)trips->size() + np*3*4);
 
@@ -328,13 +329,6 @@ void EmbeddedMeshCollision::linearize(
 		VFCollisionPair &pair = vf_pairs[i];
 		int emb_p_idx = pair.p_idx;
 
-
-    //p_idx(-1), // point
-    //p_is_obs(0), // 0 or 1
-    //q_idx(-1), // face
-    //q_is_obs(0), // 0 or 1
-//	pt_on_q(0,0,0)
-
 		// Compute normal of triangle at x
 		Vector3d q_n(0,0,0);
 		RowVector3i q_inds(-1,-1,-1);
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index 22d7dd58f8c..baf92c28754 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -34,7 +34,7 @@ public:
         double floor_z;
         bool test_floor;
         Settings() :
-            floor_z(0),
+            floor_z(-0.5),
 //            floor_z(-std::numeric_limits<double>::max()),
             test_floor(true)
             {}
diff --git a/extern/softbody/src/admmpd_linsolve.cpp b/extern/softbody/src/admmpd_linsolve.cpp
index 05f7d5da5f4..1ad3fe9a78d 100644
--- a/extern/softbody/src/admmpd_linsolve.cpp
+++ b/extern/softbody/src/admmpd_linsolve.cpp
@@ -199,12 +199,12 @@ void GaussSeidel::solve(
 			InnerIter rit(td->data->gsdata.A3_plus_CtC, idx*3+j);
 			for (; rit; ++rit)
 			{
-				int r = rit.row();
-				int c = rit.col();
 				double v = rit.value();
 				if (v==0.0)
 					continue;
 
+				int r = rit.row();
+				int c = rit.col();
 				if (r==c) // Diagonal
 				{
 					inv_aii[j] = 1.0/v;
@@ -218,17 +218,29 @@ void GaussSeidel::solve(
 		} // end loop segment
 
 		// Update x
-		Vector3d bi = td->data->b.row(idx).transpose()
-			+ td->data->gsdata.Ctd.segment<3>(idx*3);
+		Vector3d bi = td->data->gsdata.b3_plus_Ctd.segment<3>(idx*3);
+
+		//Vector3d bi = td->data->b.row(idx);
 		Vector3d xi = td->data->x.row(idx);
 		Vector3d xi_new = (bi-LUx);
-
 		for (int j=0; j<3; ++j)
 			xi_new[j] *= inv_aii[j];
+
+		if (xi_new.norm()>1000 || xi_new.norm()<-1000)
+		{
+			std::cout << "idx: " << idx << std::endl;
+			std::cout << "xi: " << xi_new.transpose() << std::endl;
+			std::cout << "bi+ctd: " << bi.transpose() << std::endl;
+			std::cout << "bi: " << td->data->b.row(idx) << std::endl;
+			std::cout << "LUx: " << LUx.transpose() << std::endl;
+			std::cout << "aii: " << inv_aii.transpose() << std::endl;
+			throw std::runtime_error("Gauss Seidel exploded");
+		}
+
 		td->data->x.row(idx) = xi*(1.0-omega) + xi_new*omega;
 
 		// Check fast-query constraints
-		double floor_z = td->collision->get_floor();
+//		double floor_z = td->collision->get_floor();
 //		if (td->data->x(idx,2) < floor_z)
 //			td->data->x(idx,2) = floor_z;
 
@@ -249,6 +261,7 @@ void GaussSeidel::solve(
 		{
 			thread_data.color = color;
 			int n_inds = data->gsdata.A3_plus_CtC_colors[color].size();
+			thrd_settings.use_threading = false;
 			BLI_task_parallel_range(0, n_inds, &thread_data, parallel_gs_sweep, &thrd_settings);
 		} // end loop colors
 
@@ -341,6 +354,14 @@ void GaussSeidel::compute_colors(
 		vertex_constraints_graph.size()==0 ||
 		(int)vertex_constraints_graph.size()==n_nodes);
 
+	{
+		colors.clear();
+		colors.resize(n_nodes, std::vector<int>());
+		for (int i=0; i<n_nodes; ++i)
+			colors[i].emplace_back(i);
+		return;
+	}
+	
 	// Graph color settings
 	int init_palette_size = 6;
 
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index 7a586106d21..eeb3163ccd9 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -79,8 +79,8 @@ int Solver::solve(
 		update_constraints(options,data,collision);
 
 		// Solve Ax=b s.t. Cx=d
-		//ConjugateGradients().solve(options,data,collision);
-		GaussSeidel().solve(options,data,collision);
+		ConjugateGradients().solve(options,data,collision);
+		//GaussSeidel().solve(options,data,collision);
 
 	} // end solver iters
 
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index eaeb1d0c93a..350cc926696 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -30,9 +30,9 @@ struct Options {
         timestep_s(1.0/24.0),
         max_admm_iters(50),
         max_cg_iters(10),
-        max_gs_iters(30),
+        max_gs_iters(100),
         gs_omega(1),
-        mult_k(3),
+        mult_k(1),
         min_res(1e-8),
         youngs(10000000),
         poisson(0.399),
-- 
cgit v1.2.3