threading

5 files changed, 184 insertions, 56 deletions

diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index 8f6ec97d7dd..05caac30633 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -24,6 +24,7 @@ set(INC
 
 set(INC_SYS
   ${EIGEN3_INCLUDE_DIRS}
+  ../../source/blender/blenlib
 )
 
 set(SRC
diff --git a/extern/softbody/src/admmpd_energy.cpp b/extern/softbody/src/admmpd_energy.cpp
index cbe60cb1511..36f0bfa1478 100644
--- a/extern/softbody/src/admmpd_energy.cpp
+++ b/extern/softbody/src/admmpd_energy.cpp
@@ -9,7 +9,7 @@ using namespace Eigen;
 
 Lame::Lame() : m_model(0)
 {
-	set_from_youngs_poisson(100000,0.299);
+	set_from_youngs_poisson(10000000,0.399);
 }
 
 void Lame::set_from_youngs_poisson(double youngs, double poisson)
@@ -55,6 +55,7 @@ void EnergyTerm::update(
 	Eigen::MatrixXd *z,
 	Eigen::MatrixXd *u)
 {
+	(void)(x);
 	Matrix3d Dix = Dx->block<3,3>(index,0);
 	Matrix3d ui = u->block<3,3>(index,0);
 	Matrix3d zi = Dix + ui;
@@ -92,9 +93,11 @@ int EnergyTerm::init_tet(
 	Matrix<double,3,3> edges_inv = edges.inverse();
 	volume = edges.determinant() / 6.0f;
 	if( volume < 0 )
-		throw std::runtime_error("**Solver::energy_init: Inverted initial tet");
+	{
+		printf("**EnergyTerm::init_tet: Inverted initial tet");
+		return 0;
+	}
 	double k = lame.m_bulk_mod;
-std::cout << "IDX: " << index << " bulk mod: " << k << std::endl;
 	weight = std::sqrt(k*volume);
 	Matrix<double,4,3> S = Matrix<double,4,3>::Zero();
 	S(0,0) = -1; S(0,1) = -1; S(0,2) = -1;
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index c5831077b0b..ad64e4ed277 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -12,10 +12,17 @@
 #include <stdio.h>
 #include <iostream>
 
+#include "BLI_task.h" // threading
+
 namespace admmpd {
 using namespace Eigen;
 template <typename T> using RowSparseMatrix = SparseMatrix<T,RowMajor>;
 
+typedef struct ThreadData {
+	const Options *options;
+	Data *data;
+} ThreadData;
+
 bool Solver::init(
     const Eigen::MatrixXd &V,
 	const Eigen::MatrixXi &T,
@@ -41,30 +48,14 @@ int Solver::solve(
 	// the variables are sized correctly.
 	init_solve(options,data);
 
-	int ne = data->rest_volumes.size();
-	Lame lame; // TODO lame params as input
-
 	// Begin solver loop
 	int iters = 0;
 	for (; iters < options->max_admm_iters; ++iters)
 	{
-		// Local step
-		data->Dx.noalias() = data->D * data->x;
-		for(int i=0; i<ne; ++i)
-		{
-			EnergyTerm().update(
-				data->indices[i][0],
-				lame,
-				data->rest_volumes[i],
-				data->weights[i],
-				&data->x,
-				&data->Dx,
-				&data->z,
-				&data->u );
-		}
 
-		// Global step
+		solve_local_step(options,data);
 		update_constraints(options,data);
+
 		data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u);
 		solve_conjugate_gradients(options,data);
 
@@ -103,6 +94,35 @@ void Solver::init_solve(
 
 } // end init solve
 
+static void parallel_zu_update(
+	void *__restrict userdata,
+	const int i,
+	const TaskParallelTLS *__restrict UNUSED(tls))
+{
+	Lame lame; // TODO lame params as input
+	ThreadData *td = (ThreadData*)userdata;
+	EnergyTerm().update(
+		td->data->indices[i][0],
+		lame,
+		td->data->rest_volumes[i],
+		td->data->weights[i],
+		&td->data->x,
+		&td->data->Dx,
+		&td->data->z,
+		&td->data->u );
+} // end parallel zu update
+
+void Solver::solve_local_step(
+	const Options *options,
+	Data *data)
+{
+	int ne = data->rest_volumes.size();
+  	ThreadData thread_data = {.options=options, .data = data};
+	TaskParallelSettings settings;
+	BLI_parallel_range_settings_defaults(&settings);
+	BLI_task_parallel_range(0, ne, &thread_data, parallel_zu_update, &settings);
+} // end local step
+
 void Solver::update_constraints(
 	const Options *options,
 	Data *data)
@@ -145,19 +165,45 @@ void Solver::update_constraints(
 
 } // end update constraints
 
+typedef struct LinSolveThreadData {
+	Data *data;
+	MatrixXd *x;
+	MatrixXd *b;
+} LinSolveThreadData;
+
+static void parallel_lin_solve(
+	void *__restrict userdata,
+	const int i,
+	const TaskParallelTLS *__restrict UNUSED(tls))
+{
+	LinSolveThreadData *td = (LinSolveThreadData*)userdata;
+	td->x->col(i) = td->data->ldltA.solve(td->b->col(i));
+} // end parallel lin solve
+
 void Solver::solve_conjugate_gradients(
 	const Options *options,
 	Data *data)
 {
+	// Solve Ax = b in parallel
+	auto solve_Ax_b = [](
+		Data *data_,
+		MatrixXd *x_,
+		MatrixXd *b_)
+	{
+		LinSolveThreadData thread_data = {.data=data_, .x=x_, .b=b_};
+		TaskParallelSettings settings;
+		BLI_parallel_range_settings_defaults(&settings);
+		BLI_task_parallel_range(0, 3, &thread_data, parallel_lin_solve, &settings);
+	};
+
 	// If we don't have any constraints,
 	// we don't need to perform CG
-	int nnz = std::max(std::max(
+	if (std::max(std::max(
 		data->K[0].nonZeros(),
 		data->K[1].nonZeros()),
-		data->K[2].nonZeros());
-	if (nnz==0)
+		data->K[2].nonZeros())==0)
 	{
-		data->x.noalias() = data->ldltA.solve(data->b);
+		solve_Ax_b(data,&data->x,&data->b);
 		return;
 	}
 
@@ -165,7 +211,7 @@ void Solver::solve_conjugate_gradients(
 	// if they were instead vectorized
 	auto mat_inner = [](
 		const MatrixXd &A,
-		const MatrixXd &B )
+		const MatrixXd &B)
 	{
 		double dot = 0.0;
 		int nr = std::min(A.rows(), B.rows());
@@ -192,7 +238,7 @@ void Solver::solve_conjugate_gradients(
 		b.col(i) += data->spring_k*Kt*data->l;
 		r.col(i) = b.col(i) - A[i]*data->x.col(i);
 	}
-	z = data->ldltA.solve(r);
+	solve_Ax_b(data,&z,&r);
 	p = z;
 
 	for (int iter=0; iter<options->max_cg_iters; ++iter)
@@ -213,8 +259,7 @@ void Solver::solve_conjugate_gradients(
 		r -= alpha * Ap;
 		if( r.lpNorm<Infinity>() < eps )
 			break;
-
-		z = data->ldltA.solve(r);
+		solve_Ax_b(data,&z,&r);
 		double beta = mat_inner(z,r) / zk_dot_rk;
 		p = z + beta*p;
 	}
@@ -237,10 +282,7 @@ void Solver::compute_matrices(
 		data->v.setZero();
 	}
 	if (data->m.rows() != nx)
-	{ // TODO get from input
-		data->m.resize(nx);
-		data->m.setOnes();
-	}
+		compute_masses(options,data);
 
 	// Add per-element energies to data
 	std::vector< Triplet<double> > trips;
@@ -290,6 +332,33 @@ void Solver::compute_matrices(
 
 } // end compute matrices
 
+void Solver::compute_masses(
+	const Options *options,
+	Data *data)
+{
+	// Source: https://github.com/mattoverby/mclscene/blob/master/include/MCL/TetMesh.hpp
+	// Computes volume-weighted masses for each vertex
+	// density_kgm3 is the unit-volume density (e.g. soft rubber: 1100)
+	double density_kgm3 = 1100;
+	data->m.resize(data->x.rows());
+	data->m.setZero();
+	int n_tets = data->tets.rows();
+	for (int t=0; t<n_tets; ++t)
+	{
+		RowVector4i tet = data->tets.row(t);
+		Matrix3d edges;
+		edges.col(0) = data->x.row(tet[1]) - data->x.row(tet[0]);
+		edges.col(1) = data->x.row(tet[2]) - data->x.row(tet[0]);
+		edges.col(2) = data->x.row(tet[3]) - data->x.row(tet[0]);
+		double v = std::abs((edges).determinant()/6.f);
+		double tet_mass = density_kgm3 * v;
+		data->m[ tet[0] ] += tet_mass / 4.f;
+		data->m[ tet[1] ] += tet_mass / 4.f;
+		data->m[ tet[2] ] += tet_mass / 4.f;
+		data->m[ tet[3] ] += tet_mass / 4.f;
+	}
+}
+
 void Solver::append_energies(
 	const Options *options,
 	Data *data,
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index 6b397bee406..970cbfb6368 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -84,6 +84,10 @@ protected:
         const Options *options,
         Data *data);
 
+	void solve_local_step(
+        const Options *options,
+        Data *data);
+
     // Global step with CG:
     // 1/2||Ax-b||^2 + k/2||Kx-l||^2
 	void solve_conjugate_gradients(
@@ -98,6 +102,10 @@ protected:
         const Options *options,
         Data *data);
 
+    void compute_masses(
+        const Options *options,
+        Data *data);
+
 	void append_energies(
 		const Options *options,
 		Data *data,
diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index a14cb41a4af..63c01e6ef3f 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -3545,6 +3545,46 @@ static void sbStoreLastFrame(struct Depsgraph *depsgraph, Object *object, float
   object_orig->soft->last_frame = framenr;
 }
 
+static void admm_resize_softbody(Object *ob, int totpoint)
+{
+  if (totpoint<=0)
+    return;
+
+  SoftBody *sb = ob->soft;
+  if (sb->totpoint && sb->bpoint !=NULL)
+    MEM_freeN(sb->bpoint);
+  printf("bp ptr: %p\n",sb);
+  printf("bodypt: %d\n",sb->totpoint);
+
+  sb->totpoint = totpoint;
+  sb->totspring = 0;
+//  sb->bpoint = MEM_mallocN(totpoint * sizeof(BodyPoint), "bodypoint");
+}
+
+static void admm_copy_to_softbody(Object *ob)
+{
+  SoftBody *sb = ob->soft;
+  ADMMPDInterfaceData *admmpd = sb->admmpd;
+  if (admmpd == NULL)
+    return;
+
+  if (sb->totpoint != admmpd->out_totverts)
+  {
+    printf("**admm_copy_to_softbody error: DOF missmatch");
+    return;
+  }
+
+  for (int i=0; i<admmpd->out_totverts; ++i)
+  {
+    BodyPoint *pt = &sb->bpoint[i];
+    for (int j=0; j<3; ++j)
+    {
+      pt->pos[j] = admmpd->out_verts[i*3+j];
+      pt->vec[j] = admmpd->out_vel[i*3+j];
+    }
+  }
+}
+
 /* simulates one step. framenr is in frames */
 void sbObjectStep_admmpd(struct Depsgraph *depsgraph,
                   Scene *scene,
@@ -3558,22 +3598,24 @@ void sbObjectStep_admmpd(struct Depsgraph *depsgraph,
 
   Mesh *me = ob->data;
   SoftBody *sb = ob->soft;
-  PointCache *cache = sb->shared->pointcache;
-  int framenr = (int)cfra;
-  int framedelta = framenr - cache->simframe;
-
-  PTCacheID pid;
-  BKE_ptcache_id_from_softbody(&pid, ob, sb);
-  float timescale;
-  int startframe, endframe; // start and end frame of the cache
-  BKE_ptcache_id_time(&pid, scene, framenr, &startframe, &endframe, &timescale);
-  framenr = framenr < endframe ? framenr : endframe; // min(framenr,endframe)
 
-  if (framenr < startframe)
-  {
-    BKE_ptcache_invalidate(cache);
-    return;
-  }
+//  PointCache *cache = sb->shared->pointcache;
+  int framenr = (int)cfra;
+//  int framedelta = framenr - cache->simframe;
+  int startframe = 1;
+
+//  PTCacheID pid;
+//  BKE_ptcache_id_from_softbody(&pid, ob, sb);
+//  float timescale;
+//  int startframe, endframe; // start and end frame of the cache
+//  BKE_ptcache_id_time(&pid, scene, framenr, &startframe, &endframe, &timescale);
+//  framenr = framenr < endframe ? framenr : endframe; // min(framenr,endframe)
+
+//  if (framenr < startframe)
+//  {
+//    BKE_ptcache_invalidate(cache);
+//    return;
+//  }
 
   // Reset simulation
   bool reset_sim =
@@ -3583,7 +3625,7 @@ void sbObjectStep_admmpd(struct Depsgraph *depsgraph,
 
   if (reset_sim)
   {
-      BKE_ptcache_invalidate(cache);
+//      BKE_ptcache_invalidate(cache);
 
       if (sb->admmpd == NULL)
         sb->admmpd = MEM_callocN(sizeof(ADMMPDInterfaceData), "SoftBody_ADMMPD");
@@ -3622,15 +3664,19 @@ void sbObjectStep_admmpd(struct Depsgraph *depsgraph,
       // Initalize solver
       admmpd_init(sb->admmpd);
 
+      // Set up softbody to store defo verts
+      int num_defo_verts = sb->admmpd->out_totverts;
+      admm_resize_softbody(ob,num_defo_verts);
+
   } // end reset ADMMPD data
 
   // Cache vertices at initializer
   if (framenr == startframe)
   {
-    BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
-    BKE_ptcache_validate(cache, framenr);
-    cache->flag &= ~PTCACHE_REDO_NEEDED;
-    sbStoreLastFrame(depsgraph, ob, framenr);
+//    BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
+//    BKE_ptcache_validate(cache, framenr);
+//    cache->flag &= ~PTCACHE_REDO_NEEDED;
+//    sbStoreLastFrame(depsgraph, ob, framenr);
     return;
   }
 
@@ -3642,10 +3688,11 @@ void sbObjectStep_admmpd(struct Depsgraph *depsgraph,
 
   admmpd_solve(sb->admmpd);
   admmpd_map_vertices(sb->admmpd,vertexCos,numVerts);
+//  admm_copy_to_softbody(ob);
 
 //  BKE_ptcache_validate(cache, framenr);
 //  BKE_ptcache_write(&pid, framenr);
-  //sbStoreLastFrame(depsgraph, ob, framenr);
+//  sbStoreLastFrame(depsgraph, ob, framenr);
 
 } // end step object with ADMMPD
 
@@ -3657,8 +3704,8 @@ void sbObjectStep(struct Depsgraph *depsgraph,
                   float (*vertexCos)[3],
                   int numVerts)
 {
- sbObjectStep_admmpd(depsgraph,scene,ob,cfra,vertexCos,numVerts);
- return;
+sbObjectStep_admmpd(depsgraph,scene,ob,cfra,vertexCos,numVerts);
+return;
 
   SoftBody *sb = ob->soft;
   PointCache *cache;