added goal positions

8 files changed, 263 insertions, 33 deletions

diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index cadd9ef51ee..7f631400ce0 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -43,6 +43,8 @@ set(SRC
 	src/admmpd_linsolve.cpp
 	src/admmpd_math.h
 	src/admmpd_math.cpp
+	src/admmpd_pin.h
+	src/admmpd_pin.cpp
 	src/admmpd_solver.h
 	src/admmpd_solver.cpp
 	src/admmpd_tetmesh.h
diff --git a/extern/softbody/src/admmpd_bvh.h b/extern/softbody/src/admmpd_bvh.h
index dca84e6803f..19f47545435 100644
--- a/extern/softbody/src/admmpd_bvh.h
+++ b/extern/softbody/src/admmpd_bvh.h
@@ -49,6 +49,8 @@ protected:
 		AABB aabb;
 		Node *left, *right;
 		std::vector<int> prims;
+		VecType normal;
+		T angle;
 		bool is_leaf() const { return prims.size()>0; }
 		Node() : left(nullptr), right(nullptr) {}
 		~Node()
diff --git a/extern/softbody/src/admmpd_linsolve.cpp b/extern/softbody/src/admmpd_linsolve.cpp
index 1ad3fe9a78d..7d5a29eb964 100644
--- a/extern/softbody/src/admmpd_linsolve.cpp
+++ b/extern/softbody/src/admmpd_linsolve.cpp
@@ -54,10 +54,12 @@ void ConjugateGradients::solve(
 	BLI_assert(data->C.cols() == nx*3);
 	BLI_assert(data->d.rows() > 0);
 	BLI_assert(data->C.rows() == data->d.rows());
+	BLI_assert(data->PtP.cols() == nx*3);
+	BLI_assert(data->PtP.rows() == nx*3);
 
 	// If we don't have any constraints, we don't need to perform CG
 	data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u);
-	if (data->C.nonZeros()==0)
+	if (data->C.nonZeros()==0 && data->PtP.nonZeros()==0)
 	{
 		data->x = data->ldltA.solve(data->b);
 		return;
@@ -71,28 +73,30 @@ void ConjugateGradients::solve(
 		gsdata->z.resize(nx*3);
 		gsdata->p.resize(nx*3);
 		gsdata->A3p.resize(nx*3);
-		gsdata->b3_plus_Ctd.resize(nx*3);
+		gsdata->b3_Ctd_Ptx.resize(nx*3);
 		make_n3(data->A, gsdata->A3);
 	}
 
-	gsdata->Ctd = data->spring_k * data->C.transpose()*data->d;
 	gsdata->CtC = data->spring_k * data->C.transpose()*data->C;
-	gsdata->A3_plus_CtC = gsdata->A3 + gsdata->CtC;
+	gsdata->Ctd = data->spring_k * data->C.transpose()*data->d;
+	gsdata->A3_CtC_PtP = gsdata->A3 + gsdata->CtC + data->PtP;
 	VectorXd x3(nx*3);
 	for (int i=0; i<nx; ++i)
 	{
-		Vector3d bi = data->b.row(i);
-		gsdata->b3_plus_Ctd.segment<3>(i*3) = bi+gsdata->Ctd.segment<3>(i*3);
+		gsdata->b3_Ctd_Ptx.segment<3>(i*3) =
+			data->b.row(i).transpose() + 
+			gsdata->Ctd.segment<3>(i*3) +
+			data->Ptq.segment<3>(i*3);
 		x3.segment<3>(i*3) = data->x.row(i);
 	}
 
-	gsdata->r.noalias() = gsdata->b3_plus_Ctd - gsdata->A3_plus_CtC*x3;
+	gsdata->r.noalias() = gsdata->b3_Ctd_Ptx - gsdata->A3_CtC_PtP*x3;
 	solve_Ax_b(data,&gsdata->z,&gsdata->r);
 	gsdata->p = gsdata->z;
 
 	for (int iter=0; iter<options->max_cg_iters; ++iter)
 	{
-		gsdata->A3p.noalias() = gsdata->A3_plus_CtC*gsdata->p;
+		gsdata->A3p.noalias() = gsdata->A3_CtC_PtP*gsdata->p;
 		double p_dot_Ap = gsdata->p.dot(gsdata->A3p);
 		if (p_dot_Ap==0.0)
 			break;
@@ -196,7 +200,7 @@ void GaussSeidel::solve(
 		Vector3d inv_aii(0,0,0);
 		for (int j=0; j<3; ++j)
 		{
-			InnerIter rit(td->data->gsdata.A3_plus_CtC, idx*3+j);
+			InnerIter rit(td->data->gsdata.A3_CtC_PtP, idx*3+j);
 			for (; rit; ++rit)
 			{
 				double v = rit.value();
@@ -218,7 +222,7 @@ void GaussSeidel::solve(
 		} // end loop segment
 
 		// Update x
-		Vector3d bi = td->data->gsdata.b3_plus_Ctd.segment<3>(idx*3);
+		Vector3d bi = td->data->gsdata.b3_Ctd_Ptx.segment<3>(idx*3);
 
 		//Vector3d bi = td->data->b.row(idx);
 		Vector3d xi = td->data->x.row(idx);
@@ -277,6 +281,15 @@ void GaussSeidel::init_solve(
 		SolverData *data,
 		Collision *collision)
 {
+
+	// TODO:
+	//
+	// When it comes to improving run time of Gauss-Seidel after
+	// the instability issues have been addressed, reducing the
+	// matrix-vector mults that occur here should be a priority.
+	// Many of them are unnecessary and can be done
+	// within the Gauss-Seidel sweeps!
+
 	BLI_assert(options != nullptr);
 	BLI_assert(data != nullptr);
 	BLI_assert(collision != nullptr);
@@ -308,13 +321,13 @@ void GaussSeidel::init_solve(
 	{
 		data->gsdata.CtC = data->spring_k * data->C.transpose()*data->C;
 		data->gsdata.Ctd.noalias() = data->spring_k * data->C.transpose()*data->d;
-		data->gsdata.A3_plus_CtC = data->gsdata.A3 + data->gsdata.CtC;
-		data->gsdata.b3_plus_Ctd.resize(nx*3);
+		data->gsdata.A3_CtC_PtP = data->gsdata.A3 + data->gsdata.CtC;
+		data->gsdata.b3_Ctd_Ptx.resize(nx*3);
 		for (int i=0; i<nx; ++i)
 		{
-			data->gsdata.b3_plus_Ctd[i*3+0] = data->b(i,0)+data->gsdata.Ctd[i*3+0];
-			data->gsdata.b3_plus_Ctd[i*3+1] = data->b(i,1)+data->gsdata.Ctd[i*3+1];
-			data->gsdata.b3_plus_Ctd[i*3+2] = data->b(i,2)+data->gsdata.Ctd[i*3+2];
+			data->gsdata.b3_Ctd_Ptx[i*3+0] = data->b(i,0)+data->gsdata.Ctd[i*3+0];
+			data->gsdata.b3_Ctd_Ptx[i*3+1] = data->b(i,1)+data->gsdata.Ctd[i*3+1];
+			data->gsdata.b3_Ctd_Ptx[i*3+2] = data->b(i,2)+data->gsdata.Ctd[i*3+2];
 		}
 		std::vector<std::set<int> > c_graph;
 		collision->graph(c_graph);
@@ -328,13 +341,13 @@ void GaussSeidel::init_solve(
 		if (data->gsdata.Ctd.rows() != nx*3)
 			data->gsdata.Ctd.resize(nx*3);
 		data->gsdata.Ctd.setZero();
-		data->gsdata.A3_plus_CtC = data->gsdata.A3;
-		data->gsdata.b3_plus_Ctd.resize(nx*3);
+		data->gsdata.A3_CtC_PtP = data->gsdata.A3;
+		data->gsdata.b3_Ctd_Ptx.resize(nx*3);
 		for (int i=0; i<nx; ++i)
 		{
-			data->gsdata.b3_plus_Ctd[i*3+0] = data->b(i,0);
-			data->gsdata.b3_plus_Ctd[i*3+1] = data->b(i,1);
-			data->gsdata.b3_plus_Ctd[i*3+2] = data->b(i,2);
+			data->gsdata.b3_Ctd_Ptx[i*3+0] = data->b(i,0);
+			data->gsdata.b3_Ctd_Ptx[i*3+1] = data->b(i,1);
+			data->gsdata.b3_Ctd_Ptx[i*3+2] = data->b(i,2);
 		}
 		data->gsdata.A3_plus_CtC_colors = data->gsdata.A_colors;
 	}
diff --git a/extern/softbody/src/admmpd_pin.cpp b/extern/softbody/src/admmpd_pin.cpp
new file mode 100644
index 00000000000..ff983a08e87
--- /dev/null
+++ b/extern/softbody/src/admmpd_pin.cpp
@@ -0,0 +1,74 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_pin.h"
+#include "BLI_assert.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+void EmbeddedMeshPin::clear()
+{
+	pin_k.clear();
+	pin_pos.clear();
+}
+
+void EmbeddedMeshPin::set_pin(
+		int idx,
+		const Eigen::Vector3d &qi,
+		const Eigen::Vector3d &ki_)
+{
+	if (!mesh)
+		return;
+
+	if (idx<0 || idx>=mesh->emb_rest_x.rows())
+		return;
+
+	// Clamp
+	Vector3d ki = ki_;
+	for (int i=0; i<3; ++i)
+		ki[i] = std::max(0.0, ki[i]);
+
+	pin_k[idx] = ki;
+	pin_pos[idx] = qi;
+}
+
+void EmbeddedMeshPin::linearize(
+		const Eigen::MatrixXd *x, // not used yet
+		std::vector<Eigen::Triplet<double> > *trips,
+		std::vector<double> *q)
+{
+
+	(void)(x);
+	int np = pin_k.size();
+	trips->reserve((int)trips->size() + np*3*4);
+	q->reserve((int)q->size() + np*3);
+
+	std::unordered_map<int,Eigen::Vector3d>::const_iterator it_k = pin_k.begin();
+	for (; it_k != pin_k.end(); ++it_k)
+	{
+		int emb_idx = it_k->first;
+		const Vector3d &qi = pin_pos[emb_idx];
+		const Vector3d &ki = it_k->second;
+
+		int tet_idx = mesh->emb_vtx_to_tet[emb_idx];
+		RowVector4d bary = mesh->emb_barys.row(emb_idx);
+		RowVector4i tet = mesh->tets.row(tet_idx);
+
+		for (int i=0; i<3; ++i)
+		{
+			int p_idx = q->size();
+			q->emplace_back(qi[i]*ki[i]);
+			for (int j=0; j<4; ++j)
+				trips->emplace_back(p_idx, tet[j]*3+i, bary[j]*ki[i]);
+		}
+	}
+
+} // end linearize
+
+//bool EmbeddedMeshPin::has_pin(int idx) const
+//{
+//	return pin_k.count(idx);
+//}
+
+} // namespace admmpd
diff --git a/extern/softbody/src/admmpd_pin.h b/extern/softbody/src/admmpd_pin.h
new file mode 100644
index 00000000000..8080d9d2c85
--- /dev/null
+++ b/extern/softbody/src/admmpd_pin.h
@@ -0,0 +1,88 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_PIN_H_
+#define ADMMPD_PIN_H_
+
+#include "admmpd_bvh.h"
+#include "admmpd_types.h"
+#include <unordered_map>
+#include <vector>
+
+namespace admmpd {
+
+class Pin {
+public:
+
+    virtual ~Pin() {}
+
+    // Clears all pins
+    virtual void clear() = 0;
+
+    // Set the pin location (q) and per-axis stiffness (k)
+    // Stiffness should be 0 to 1. It can go larger, but
+    // the resulting matrix will be poorly conditioned.
+    virtual void set_pin(
+        int idx,
+        const Eigen::Vector3d &q,
+        const Eigen::Vector3d &k) = 0;
+
+    // Returns true if the vert is pinned
+//    virtual bool has_pin(int idx) const = 0;
+
+    // Creates linearization for constraint:
+    // Px=q with stiffnesses baked in
+    virtual void linearize(
+        const Eigen::MatrixXd *x, // not used yet
+    	std::vector<Eigen::Triplet<double> > *trips,
+		std::vector<double> *q) = 0;
+
+    // Returns per-axis pin stiffness
+//    virtual Eigen::Vector3d get_pin_k(int idx) const = 0;
+
+    // Returns pin location, or zero vector if not set
+ //   virtual Eigen::Vector3d get_pin_pos(int idx) const = 0;
+};
+
+class EmbeddedMeshPin : public Pin {
+public:
+    EmbeddedMeshPin(const EmbeddedMeshData *mesh_) : mesh(mesh_){}
+
+    // Clears all pins
+    void clear();
+
+    // Set the pin location of the embedded vertex
+    void set_pin(
+        int idx,
+        const Eigen::Vector3d &p,
+        const Eigen::Vector3d &k);
+
+    // Returns true if the deforming vertex is pinned
+//    bool has_pin(int idx) const;
+
+    void linearize(
+        const Eigen::MatrixXd *x, // not used yet
+    	std::vector<Eigen::Triplet<double> > *trips,
+		std::vector<double> *q);
+
+    // Returns per-axis pin stiffness of the deforming vertex (not embedded)
+    // or zero if not pinned
+    // Baryweights are included.
+//    Eigen::Vector3d get_pin_k(int idx) const;
+
+    // Returns pin location of the deforming vertex (not embedded)
+    // or zero vector if not set
+//    Eigen::Vector3d get_pin_pos(int idx) const;
+
+protected:
+    // A ptr to the embedded mesh data
+    const EmbeddedMeshData *mesh;
+
+    // Pins for embedded vertices:
+    std::unordered_map<int,Eigen::Vector3d> pin_k;
+    std::unordered_map<int,Eigen::Vector3d> pin_pos;
+};
+
+} // namespace admmpd
+
+#endif // ADMMPD_PIN_H_
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index eeb3163ccd9..e1408b4d53e 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -37,6 +37,8 @@ bool Solver::init(
 	BLI_assert(V.cols() == 3);
 	BLI_assert(T.rows() > 0);
 	BLI_assert(T.cols() == 4);
+	int nx = V.rows();
+	BLI_assert(m.rows() == nx);
 
 	data->x = V;
 	data->v.resize(V.rows(),3);
@@ -54,7 +56,8 @@ bool Solver::init(
 int Solver::solve(
 	const Options *options,
 	SolverData *data,
-	Collision *collision)
+	Collision *collision,
+	Pin *pin)
 {
 	BLI_assert(data != NULL);
 	BLI_assert(options != NULL);
@@ -66,7 +69,7 @@ int Solver::solve(
 	// Init the solve which computes
 	// quantaties like M_xbar and makes sure
 	// the variables are sized correctly.
-	init_solve(options,data);
+	init_solve(options,data,collision,pin);
 
 	// Begin solver loop
 	int iters = 0;
@@ -94,17 +97,24 @@ int Solver::solve(
 
 void Solver::init_solve(
 	const Options *options,
-	SolverData *data)
+	SolverData *data,
+	Collision *collision,
+    Pin *pin)
 {
 	BLI_assert(data != NULL);
 	BLI_assert(options != NULL);
 	int nx = data->x.rows();
 	BLI_assert(nx > 0);
+	BLI_assert(data->pin_sqrt_k.rows()==nx);
+	(void)(collision);
 
 	if (data->M_xbar.rows() != nx)
 		data->M_xbar.resize(nx,3);
 
-	// velocity and position
+	// Initialize:
+	// - update velocity with explicit forces
+	// - update pin constraint matrix (goal positions)
+	// - set x init guess
 	double dt = std::max(0.0, options->timestep_s);
 	data->x_start = data->x;
 	for (int i=0; i<nx; ++i)
@@ -112,7 +122,33 @@ void Solver::init_solve(
 		data->v.row(i) += dt*options->grav;
 		RowVector3d xbar_i = data->x.row(i) + dt*data->v.row(i);
 		data->M_xbar.row(i) = data->m[i]*xbar_i;
-		data->x.row(i) = xbar_i; // initial geuss
+		data->x.row(i) = xbar_i; // initial guess
+	}
+
+	// Create pin constraint matrix
+	if (pin)
+	{
+		std::vector<Triplet<double> > trips;
+		std::vector<double> q_coeffs;
+		pin->linearize(&data->x, &trips, &q_coeffs);
+		if (q_coeffs.size()==0) // no springs
+		{
+			data->PtP.resize(nx*3,nx*3);
+			data->PtP.setZero();
+			data->Ptq.resize(nx*3);
+			data->Ptq.setZero();
+		}
+		else
+		{
+			// Scale stiffness by A diagonal max
+			double pin_k_scale = std::sqrt(data->A_diag_max);
+			int np = q_coeffs.size();
+			RowSparseMatrix<double> P(np, nx*3);
+			P.setFromTriplets(trips.begin(), trips.end());
+			data->PtP = pin_k_scale * P.transpose()*P;
+			VectorXd q = Map<VectorXd>(q_coeffs.data(), q_coeffs.size());
+			data->Ptq = pin_k_scale * P.transpose()*q;
+		}
 	}
 
 	// ADMM variables
@@ -205,6 +241,7 @@ bool Solver::compute_matrices(
 	int nx = data->x.rows();
 	BLI_assert(nx > 0);
 	BLI_assert(data->x.cols() == 3);
+	BLI_assert(data->pin_sqrt_k.rows() == nx);
 
 	// Allocate per-vertex data
 	data->x_start = data->x;
@@ -253,12 +290,17 @@ bool Solver::compute_matrices(
 	data->ldltA.compute(data->A);
 	data->b.resize(nx,3);
 	data->b.setZero();
+	data->A_diag_max = data->A.diagonal().maxCoeff();
 
 	// Constraint data
-	data->spring_k = options->mult_k*data->A.diagonal().maxCoeff();
+	data->spring_k = options->mult_k*data->A_diag_max;
 	data->C.resize(1,nx*3);
 	data->d = VectorXd::Zero(1);
 
+	data->PtP.resize(nx*3,nx*3);
+	data->pin_sqrt_k.resize(nx);
+	data->pin_sqrt_k.setZero();
+
 	// ADMM dual/lagrange
 	data->z.resize(n_row_D,3);
 	data->z.setZero();
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index dc94bedd5a4..0344122f0b5 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -6,6 +6,7 @@
 
 #include "admmpd_types.h"
 #include "admmpd_collision.h"
+#include "admmpd_pin.h"
 
 namespace admmpd {
 
@@ -24,10 +25,12 @@ public:
     // Solve a single time step.
     // Returns number of iterations.
     // Collision ptr can be null.
+    // Pin ptr can be null
     int solve(
         const Options *options,
         SolverData *data,
-        Collision *collision);
+        Collision *collision,
+        Pin *pin);
 
 protected:
 
@@ -38,7 +41,9 @@ protected:
 
     void init_solve(
         const Options *options,
-        SolverData *data);
+        SolverData *data,
+        Collision *collision,
+        Pin *pin);
 
 	void solve_local_step(
         const Options *options,
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index 350cc926696..d951687f11c 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -66,7 +66,7 @@ struct SolverData {
     Eigen::MatrixXd x; // vertices, n x 3
     Eigen::MatrixXd v; // velocity, n x 3
     // Set in compute_matrices: 
-    Eigen::MatrixXd x_start; // x at beginning of timestep, n x 3
+    Eigen::MatrixXd x_start; // x at t=0 (and goal if k>0), n x 3
     Eigen::VectorXd m; // masses, n x 1
     Eigen::MatrixXd z; // ADMM z variable
     Eigen::MatrixXd u; // ADMM u aug lag with W inv
@@ -76,16 +76,20 @@ struct SolverData {
     RowSparseMatrix<double> D; // reduction matrix
     RowSparseMatrix<double> DtW2; // D'W^2
     RowSparseMatrix<double> A; // M + D'W^2D
-    RowSparseMatrix<double> C; // linearized constraints
+    double A_diag_max; // Max coeff of diag of A
+    RowSparseMatrix<double> C; // linearized constraints (cols = n x 3)
     Eigen::VectorXd d; // constraints rhs
     double spring_k; // constraint stiffness
+    RowSparseMatrix<double> PtP; // pin_k Pt P
+    Eigen::VectorXd Ptq; // pin_k Pt q
+    Eigen::VectorXd pin_sqrt_k; // per-vertex pin (goal) sqrt stiffness
 	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > ldltA;
     struct GlobalStepData { // Temporaries used in global step
         RowSparseMatrix<double> A3; // (M + D'W^2D) n3 x n3
-        RowSparseMatrix<double> CtC; // k * Ct C
-        RowSparseMatrix<double> A3_plus_CtC;
+        RowSparseMatrix<double> CtC; // col_k * Ct C
+        RowSparseMatrix<double> A3_CtC_PtP;
         Eigen::VectorXd Ctd; // k * Ct d
-        Eigen::VectorXd b3_plus_Ctd; // M xbar + DtW2(z-u) + k Kt l
+        Eigen::VectorXd b3_Ctd_Ptx; // M xbar + DtW2(z-u) + col_k Ct d + pin_k Pt x_start
         // Used by Conjugate-Gradients:
         Eigen::VectorXd r; // residual
         Eigen::VectorXd z; // auxilary