still pretty buggy. The solver should be working, but lattice is too lousy to see. The interface between blender and solver needs work too, doesn't seem to reset as often as I think it should be?

10 files changed, 312 insertions, 89 deletions

diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index 08a36882f28..8f6ec97d7dd 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -35,6 +35,8 @@ set(SRC
   src/admmpd_lattice.cpp
   src/admmpd_solver.h
   src/admmpd_solver.cpp
+  src/admmpd_collision.h
+  src/admmpd_collision.cpp
 )
 
 set(LIB
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
new file mode 100644
index 00000000000..2bce56fa9f0
--- /dev/null
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -0,0 +1,33 @@
+
+
+#include "admmpd_collision.h"
+
+namespace admmpd {
+
+void FloorCollider::detect(const Eigen::MatrixXd *x)
+{
+	(void)(x);
+	// Can just do detection in jacobian I guess.
+}
+
+void FloorCollider::jacobian(
+	const Eigen::MatrixXd *x,
+	std::vector<Eigen::Triplet<double> > *trips_x,
+	std::vector<Eigen::Triplet<double> > *trips_y,
+	std::vector<Eigen::Triplet<double> > *trips_z,
+	std::vector<double> *l)
+{
+	int nx = x->rows();
+	for (int i=0; i<nx; ++i)
+	{
+		Eigen::Vector3d p = x->row(i);
+		if (p[2]>0)
+			continue;
+	
+		int c_idx = l->size();
+		trips_z->emplace_back(c_idx,i,1.0);
+		l->emplace_back(0);
+	}
+} // end floor collider Jacobian
+
+} // namespace admmpd
\ No newline at end of file
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
new file mode 100644
index 00000000000..274be91be6d
--- /dev/null
+++ b/extern/softbody/src/admmpd_collision.h
@@ -0,0 +1,42 @@
+
+
+
+#ifndef _ADMMPD_COLLISION_H
+#define _ADMMPD_COLLISION_H
+
+#include <Eigen/Sparse>
+#include <vector>
+
+namespace admmpd {
+
+// I'll update this class/structure another day.
+// For now let's get something in place to do floor collisions.
+// Probably will work better to use uber-collision class for
+// all self and obstacle collisions, reducing the amount of
+// for-all vertices loops.
+class Collider {
+public:
+    virtual void detect(
+        const Eigen::MatrixXd *x) = 0;
+    virtual void jacobian(
+        const Eigen::MatrixXd *x,
+    	std::vector<Eigen::Triplet<double> > *trips_x,
+        std::vector<Eigen::Triplet<double> > *trips_y,
+    	std::vector<Eigen::Triplet<double> > *trips_z,
+		std::vector<double> *l) = 0;
+};
+
+class FloorCollider : public Collider {
+public:
+    void detect(const Eigen::MatrixXd *x);
+    void jacobian(
+        const Eigen::MatrixXd *x,
+    	std::vector<Eigen::Triplet<double> > *trips_x,
+        std::vector<Eigen::Triplet<double> > *trips_y,
+    	std::vector<Eigen::Triplet<double> > *trips_z,
+		std::vector<double> *l);
+};
+
+} // namespace admmpd
+
+#endif //_ADMMPD_COLLISION_H
diff --git a/extern/softbody/src/admmpd_energy.cpp b/extern/softbody/src/admmpd_energy.cpp
index 978fd565aa4..ea7c68b6dca 100644
--- a/extern/softbody/src/admmpd_energy.cpp
+++ b/extern/softbody/src/admmpd_energy.cpp
@@ -8,7 +8,7 @@ using namespace Eigen;
 
 Lame::Lame() : m_model(0)
 {
-	set_from_youngs_poisson(10000000,0.299);
+	set_from_youngs_poisson(10000000,0.399);
 }
 
 void Lame::set_from_youngs_poisson(double youngs, double poisson)
diff --git a/extern/softbody/src/admmpd_lattice.cpp b/extern/softbody/src/admmpd_lattice.cpp
index 4323f8fce33..be7a5bc63d2 100644
--- a/extern/softbody/src/admmpd_lattice.cpp
+++ b/extern/softbody/src/admmpd_lattice.cpp
@@ -3,6 +3,7 @@
 
 #include "admmpd_lattice.h"
 #include "admmpd_math.h"
+#include <iostream>
 
 //#include "vdb.h"
 
diff --git a/extern/softbody/src/admmpd_math.cpp b/extern/softbody/src/admmpd_math.cpp
index 98b1fcc2520..7c76dfb8b3a 100644
--- a/extern/softbody/src/admmpd_math.cpp
+++ b/extern/softbody/src/admmpd_math.cpp
@@ -6,26 +6,26 @@ namespace admmpd {
 namespace barycoords {
 
     Eigen::Matrix<double,4,1> point_tet(
-        const Eigen::Matrix<double,3,1> &p,
-        const Eigen::Matrix<double,3,1> &a,
-        const Eigen::Matrix<double,3,1> &b,
-        const Eigen::Matrix<double,3,1> &c,
-        const Eigen::Matrix<double,3,1> &d)
+        const Eigen::Vector3d &p,
+        const Eigen::Vector3d &a,
+        const Eigen::Vector3d &b,
+        const Eigen::Vector3d &c,
+        const Eigen::Vector3d &d)
     {
 		auto scalar_triple_product = [](
-			const Eigen::Matrix<double,3,1> &u,
-			const Eigen::Matrix<double,3,1> &v,
-			const Eigen::Matrix<double,3,1> &w )
+			const Eigen::Vector3d &u,
+			const Eigen::Vector3d &v,
+			const Eigen::Vector3d &w )
 		{
 			return u.dot(v.cross(w));
 		};
-		Eigen::Matrix<double,3,1> ap = p - a;
-		Eigen::Matrix<double,3,1> bp = p - b;
-		Eigen::Matrix<double,3,1> ab = b - a;
-		Eigen::Matrix<double,3,1> ac = c - a;
-		Eigen::Matrix<double,3,1> ad = d - a;
-		Eigen::Matrix<double,3,1> bc = c - b;
-		Eigen::Matrix<double,3,1> bd = d - b;
+		Eigen::Vector3d ap = p - a;
+		Eigen::Vector3d bp = p - b;
+		Eigen::Vector3d ab = b - a;
+		Eigen::Vector3d ac = c - a;
+		Eigen::Vector3d ad = d - a;
+		Eigen::Vector3d bc = c - b;
+		Eigen::Vector3d bd = d - b;
 		double va6 = scalar_triple_product(bp, bd, bc);
 		double vb6 = scalar_triple_product(ap, ac, ad);
 		double vc6 = scalar_triple_product(ap, ad, ab);
@@ -37,11 +37,11 @@ namespace barycoords {
 	// Checks that it's on the "correct" side of the normal
 	// for each face of the tet. Assumes winding points inward.
 	bool point_in_tet(
-		const Eigen::Matrix<double,3,1> &p,
-		const Eigen::Matrix<double,3,1> &a,
-		const Eigen::Matrix<double,3,1> &b,
-		const Eigen::Matrix<double,3,1> &c,
-		const Eigen::Matrix<double,3,1> &d)
+		const Eigen::Vector3d &p,
+		const Eigen::Vector3d &a,
+		const Eigen::Vector3d &b,
+		const Eigen::Vector3d &c,
+		const Eigen::Vector3d &d)
 	{
 		using namespace Eigen;
 		auto check_face = [](
diff --git a/extern/softbody/src/admmpd_math.h b/extern/softbody/src/admmpd_math.h
index d777df5ce2e..25ff2d3114f 100644
--- a/extern/softbody/src/admmpd_math.h
+++ b/extern/softbody/src/admmpd_math.h
@@ -9,19 +9,19 @@
 namespace admmpd {
 namespace barycoords {
 
-Eigen::Matrix<double,4,1> point_tet(
-    const Eigen::Matrix<double,3,1> &p,
-    const Eigen::Matrix<double,3,1> &a,
-    const Eigen::Matrix<double,3,1> &b,
-    const Eigen::Matrix<double,3,1> &c,
-    const Eigen::Matrix<double,3,1> &d);
+Eigen::Vector4d point_tet(
+    const Eigen::Vector3d &p,
+    const Eigen::Vector3d &a,
+    const Eigen::Vector3d &b,
+    const Eigen::Vector3d &c,
+    const Eigen::Vector3d &d);
 
 bool point_in_tet(
-    const Eigen::Matrix<double,3,1> &p,
-    const Eigen::Matrix<double,3,1> &a,
-    const Eigen::Matrix<double,3,1> &b,
-    const Eigen::Matrix<double,3,1> &c,
-    const Eigen::Matrix<double,3,1> &d);
+    const Eigen::Vector3d &p,
+    const Eigen::Vector3d &a,
+    const Eigen::Vector3d &b,
+    const Eigen::Vector3d &c,
+    const Eigen::Vector3d &d);
 
 } // namespace barycoords
 
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index eb2222ef3bb..052008121b9 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -4,11 +4,13 @@
 #include "admmpd_solver.h"
 #include "admmpd_lattice.h"
 #include "admmpd_energy.h"
+#include "admmpd_collision.h"
 
 #include <Eigen/Geometry>
 #include <Eigen/Sparse>
 
 #include <stdio.h>
+#include <iostream>
 
 namespace admmpd {
 using namespace Eigen;
@@ -29,34 +31,6 @@ bool Solver::init(
 	return true;
 } // end init
 
-void Solver::init_solve(
-	const ADMMPD_Options *options,
-	ADMMPD_Data *data)
-{
-	double dt = std::max(0.0, options->timestep_s);
-	int nx = data->x.rows();
-	if (data->M_xbar.rows() != nx)
-		data->M_xbar.resize(nx,3);
-
-	// velocity and position
-	data->x_start = data->x;
-	for( int i=0; i<nx; ++i )
-	{
-		data->v.row(i) += options->grav;
-		data->M_xbar.row(i) =
-			data->m[i] * data->x.row(i) +
-			dt*data->m[i]*data->v.row(i);
-	}
-
-	// ADMM variables
-	data->Dx.noalias() = data->D * data->x;
-	data->z = data->Dx;
-	data->z_prev = data->z;
-	data->u.setZero();
-	data->u_prev.setZero();
-
-} // end init solve
-
 int Solver::solve(
 	const ADMMPD_Options *options,
 	ADMMPD_Data *data)
@@ -67,17 +41,14 @@ int Solver::solve(
 	init_solve(options,data);
 
 	int ne = data->rest_volumes.size();
-	Lame lame;
+	Lame lame; // TODO lame params as input
 
 	// Begin solver loop
 	int iters = 0;
-	int max_iters = options->max_iters < 0 ? 10000 : options->max_iters;
-	for (; iters < max_iters; ++iters)
+	for (; iters < options->max_admm_iters; ++iters)
 	{
 		// Local step
 		data->Dx.noalias() = data->D * data->x;
-		data->z_prev.noalias() = data->z;
-		data->u_prev.noalias() = data->u;
 		for(int i=0; i<ne; ++i)
 		{
 			EnergyTerm().update(
@@ -92,18 +63,162 @@ int Solver::solve(
 		}
 
 		// Global step
+		update_constraints(options,data);
 		data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u);
-		data->x.noalias() = data->ldltA.solve(data->b);
+		solve_conjugate_gradients(options,data);
 
 	} // end solver iters
 
-	double dt = std::max(0.0, options->timestep_s);
+	double dt = options->timestep_s;
 	if (dt > 0.0)
 		data->v.noalias() = (data->x-data->x_start)*(1.0/dt);
 
 	return iters;
 } // end solve
 
+void Solver::init_solve(
+	const ADMMPD_Options *options,
+	ADMMPD_Data *data)
+{
+	int nx = data->x.rows();
+	if (data->M_xbar.rows() != nx)
+		data->M_xbar.resize(nx,3);
+
+	// velocity and position
+	double dt = std::max(0.0, options->timestep_s);
+	data->x_start = data->x;
+	for( int i=0; i<nx; ++i )
+	{
+		data->v.row(i) += options->grav;
+		data->M_xbar.row(i) =
+			data->m[i] * data->x.row(i) +
+			dt*data->m[i]*data->v.row(i);
+	}
+
+	// ADMM variables
+	data->Dx.noalias() = data->D * data->x;
+	data->z = data->Dx;
+	data->u.setZero();
+
+} // end init solve
+
+void Solver::update_constraints(
+	const ADMMPD_Options *options,
+	ADMMPD_Data *data)
+{
+
+	std::vector<double> l_coeffs;
+	std::vector<Eigen::Triplet<double> > trips_x;
+    std::vector<Eigen::Triplet<double> > trips_y;
+    std::vector<Eigen::Triplet<double> > trips_z;
+
+	// TODO collision detection
+	FloorCollider().jacobian(
+		&data->x,
+		&trips_x,
+		&trips_y,
+		&trips_z,
+		&l_coeffs);
+
+	// Check number of constraints.
+	// If no constraints, clear Jacobian.
+	int nx = data->x.rows();
+	int nc = l_coeffs.size();
+	if (nc==0)
+	{
+		data->l.setZero();
+		for (int i=0; i<3; ++i)
+			data->K[i].setZero();
+
+		return;
+	}
+
+	// Otherwise update the data.
+	data->l = Map<VectorXd>(l_coeffs.data(),nc);
+	data->K[0].resize(nc,nx);
+	data->K[0].setFromTriplets(trips_x.begin(),trips_x.end());
+	data->K[1].resize(nc,nx);
+	data->K[1].setFromTriplets(trips_y.begin(),trips_y.end());
+	data->K[2].resize(nc,nx);
+	data->K[2].setFromTriplets(trips_z.begin(),trips_z.end());
+
+} // end update constraints
+
+void Solver::solve_conjugate_gradients(
+	const ADMMPD_Options *options,
+	ADMMPD_Data *data)
+{
+	// If we don't have any constraints,
+	// we don't need to perform CG
+	int nnz = std::max(std::max(
+		data->K[0].nonZeros(),
+		data->K[1].nonZeros()),
+		data->K[2].nonZeros());
+	if (nnz==0)
+	{
+		data->x.noalias() = data->ldltA.solve(data->b);
+		return;
+	}
+
+	// Inner product of matrices interpreted
+	// if they were instead vectorized
+	auto mat_inner = [](
+		const MatrixXd &A,
+		const MatrixXd &B )
+	{
+		double dot = 0.0;
+		int nr = std::min(A.rows(), B.rows());
+		for( int i=0; i<nr; ++i )
+			for(int j=0; j<3; ++j)
+				dot += A(i,j)*B(i,j);
+
+		return dot;
+	};
+
+	double eps = options->min_res;
+	MatrixXd b = data->b;
+	int nv = b.rows();
+	RowSparseMatrix<double> A[3];
+	MatrixXd r(b.rows(),3);
+	MatrixXd z(nv,3);
+	MatrixXd p(nv,3);
+	MatrixXd Ap(nv,3);
+
+	for (int i=0; i<3; ++i)
+	{
+		RowSparseMatrix<double> Kt = data->K[i].transpose();
+		A[i] = data->A + data->spring_k*RowSparseMatrix<double>(Kt*data->K[i]);
+		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);
+	p = z;
+
+	for (int iter=0; iter<options->max_cg_iters; ++iter)
+	{
+		for( int i=0; i<3; ++i )
+			Ap.col(i) = A[i]*p.col(i);
+
+		double p_dot_Ap = mat_inner(p,Ap);
+		if( p_dot_Ap==0.0 )
+			break;
+
+		double zk_dot_rk = mat_inner(z,r);
+		if( zk_dot_rk==0.0 )
+			break;
+
+		double alpha = zk_dot_rk / p_dot_Ap;
+		data->x += alpha * p;
+		r -= alpha * Ap;
+		if( r.lpNorm<Infinity>() < eps )
+			break;
+
+		z = data->ldltA.solve(r);
+		double beta = mat_inner(z,r) / zk_dot_rk;
+		p = z + beta*p;
+	}
+} // end solve conjugate gradients
+
 void Solver::compute_matrices(
 	const ADMMPD_Options *options,
 	ADMMPD_Data *data)
@@ -124,33 +239,45 @@ void Solver::compute_matrices(
 	{ // TODO get from BodyPoint
 		data->m.resize(nx);
 		data->m.setOnes();
+		data->m *= 0.01;
 	}
 
 	// Add per-element energies to data
 	std::vector< Triplet<double> > trips;
 	append_energies(options,data,trips);
 	int nw = trips.back().row()+1;
+	double dt2 = std::max(0.0, options->timestep_s * options->timestep_s);
 
 	// Global matrix
 	data->D.resize(nw,nx);
 	data->D.setFromTriplets(trips.begin(), trips.end());
 	data->Dt = data->D.transpose();
-	VectorXd w_diag = Map<VectorXd>(data->weights.data(), data->weights.size());
-	data->DtW2 = data->Dt * w_diag.asDiagonal() * w_diag.asDiagonal();
+	VectorXd wd = Map<VectorXd>(data->weights.data(), data->weights.size());
+	RowSparseMatrix<double> W2(nw,nw);
+	VectorXi W_nnz = VectorXi::Ones(nw);
+	W2.reserve(W_nnz);
+	for(int i=0; i<nw; ++i)
+		W2.coeffRef(i,i) = data->weights[i]*data->weights[i];
+
+	data->DtW2 = dt2 * data->Dt * W2;
 	data->A = data->DtW2 * data->D;
-	data->A.diagonal() += data->m;
+	for (int i=0; i<nx; ++i)
+		data->A.coeffRef(i,i) += data->m[i];
+
 	data->ldltA.compute(data->A);
 	data->b.resize(nx,3);
 	data->b.setZero();
 
+	data->spring_k = options->mult_k*data->A.diagonal().maxCoeff();
+	data->l = VectorXd::Zero(1);
+	for (int i=0; i<3; ++i)
+		data->K[i].resize(1,nx);
+
+	// ADMM variables
 	data->z.resize(nw,3);
 	data->z.setZero();
-	data->z_prev.resize(nw,3);
-	data->z_prev.setZero();
 	data->u.resize(nw,3);
 	data->u.setZero();
-	data->u_prev.resize(nw,3);
-	data->u_prev.setZero();
 
 } // end compute matrices
 
@@ -171,12 +298,11 @@ void Solver::append_energies(
 	int energy_index = 0;
 	for (int i=0; i<nt; ++i)
 	{
+		RowVector4i ele = data->tets.row(i);
+
 		data->rest_volumes.emplace_back();
 		data->weights.emplace_back();
-		int energy_dim = 0;
-
-		RowVector4i ele = data->tets.row(i);
-		energy_dim = EnergyTerm().init_tet(
+		int energy_dim = EnergyTerm().init_tet(
 			energy_index,
 			lame,
 			ele,
@@ -197,4 +323,4 @@ void Solver::append_energies(
 	}
 } // end append energies
 
-} // namespace admmpd
+} // namespace admmpd
\ No newline at end of file
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index 10dc103719c..ec836d3c466 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -13,25 +13,31 @@ namespace admmpd {
 
 struct ADMMPD_Options {
     double timestep_s;
-    int max_iters;
+    int max_admm_iters;
+    int max_cg_iters;
+    double mult_k; // stiffness multiplier for constraints
+    double min_res; // min residual for CG solver
     Eigen::Vector3d grav;
     ADMMPD_Options() :
-        timestep_s(1.0/100.0), // TODO: Figure out delta time from blender api!
-        max_iters(20),
+        timestep_s(1.0/100.0), // TODO: Figure out delta time from blender api
+        max_admm_iters(20),
+        max_cg_iters(10),
+        mult_k(3.0),
+        min_res(1e-4),
         grav(0,0,-9.8)
         {}
 };
 
 struct ADMMPD_Data {
     // Input:
-    Eigen::MatrixXi tets; // elements lattice, t x 4
-    Eigen::MatrixXd x; // vertices of lattice, n x 3
+    Eigen::MatrixXi tets; // elements t x 4
+    Eigen::MatrixXd x; // vertices, n x 3
+    Eigen::MatrixXd v; // velocity, n x 3 TODO: from cache
     // Set in compute_matrices: 
     Eigen::MatrixXd x_start; // x at beginning of timestep, n x 3
-    Eigen::MatrixXd v; // velocity of lattice mesh, n x 3
-    Eigen::VectorXd m; // masses of lattice verts, n x 1
-    Eigen::MatrixXd z, z_prev; // ADMM z variable
-    Eigen::MatrixXd u, u_prev; // ADMM u aug lag with W inv
+    Eigen::VectorXd m; // masses, n x 1 TODO: from BodyPoint
+    Eigen::MatrixXd z; // ADMM z variable
+    Eigen::MatrixXd u; // ADMM u aug lag with W inv
     Eigen::MatrixXd M_xbar; // M*(x + dt v)
     Eigen::MatrixXd Dx; // D * x
     Eigen::MatrixXd b; // M xbar + DtW2(z-u)
@@ -40,7 +46,10 @@ struct ADMMPD_Data {
     RowSparseMatrix<double> Dt; // transpose reduction matrix
     RowSparseMatrix<double> DtW2; // D'W^2
     RowSparseMatrix<double> A; // M + D'W^2D
+    RowSparseMatrix<double> K[3]; // constraint Jacobian
+    Eigen::VectorXd l; // constraint rhs (Kx=l)
 	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > ldltA;
+    double spring_k;
     // 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
@@ -67,10 +76,20 @@ public:
 
 protected:
 
+    void update_constraints(
+        const ADMMPD_Options *options,
+        ADMMPD_Data *data);
+
     void init_solve(
         const ADMMPD_Options *options,
         ADMMPD_Data *data);
 
+    // Global step with CG:
+    // 1/2||Ax-b||^2 + k/2||Kx-l||^2
+	void solve_conjugate_gradients(
+        const ADMMPD_Options *options,
+        ADMMPD_Data *data);
+
     void compute_lattice(
         const ADMMPD_Options *options,
         ADMMPD_Data *data);
diff --git a/intern/softbody/admmpd_api.cpp b/intern/softbody/admmpd_api.cpp
index 7f5a238214e..5963c4f71c8 100644
--- a/intern/softbody/admmpd_api.cpp
+++ b/intern/softbody/admmpd_api.cpp
@@ -128,7 +128,7 @@ void admmpd_to_bodypoint(
     bpi.pos[0] = xi[0];
     bpi.pos[1] = xi[1];
     bpi.pos[2] = xi[2];
-    bpi.vec[0] = vi[0];
+    bpi.vec[0] = vi[0]; // vec is velocity?
     bpi.vec[1] = vi[1];
     bpi.vec[2] = vi[2];
   }