added goal positions

11 files changed, 383 insertions, 45 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
diff --git a/intern/softbody/admmpd_api.cpp b/intern/softbody/admmpd_api.cpp
index 2002682fd85..9a31f99df3f 100644
--- a/intern/softbody/admmpd_api.cpp
+++ b/intern/softbody/admmpd_api.cpp
@@ -27,6 +27,7 @@
 #include "admmpd_tetmesh.h"
 #include "admmpd_embeddedmesh.h"
 #include "admmpd_collision.h"
+#include "admmpd_pin.h"
 
 #include "tetgen_api.h"
 #include "DNA_mesh_types.h" // Mesh
@@ -44,6 +45,7 @@ struct ADMMPDInternalData {
   admmpd::TetMeshData *tetmesh; // init_mode=0
   admmpd::EmbeddedMeshData *embmesh; // init_mode=1
   admmpd::Collision *collision;
+  admmpd::Pin *pin;
   int in_totverts; // number of input verts
 };
 
@@ -172,11 +174,11 @@ static int admmpd_init_with_lattice(
   return 0;
 }
 
-int admmpd_init(ADMMPDInterfaceData *iface, float *in_verts, unsigned int *in_faces)
+int admmpd_init(ADMMPDInterfaceData *iface, ADMMPDInitData *in_mesh)
 {
   if (iface==NULL)
     return 0;
-  if (in_verts==NULL || in_faces==NULL)
+  if (in_mesh->verts==NULL || in_mesh->faces==NULL)
     return 0;
   if (iface->mesh_totverts<=0 || iface->mesh_totfaces<=0)
     return 0;
@@ -193,6 +195,7 @@ int admmpd_init(ADMMPDInterfaceData *iface, float *in_verts, unsigned int *in_fa
   iface->idata->tetmesh = new admmpd::TetMeshData();
   iface->idata->embmesh = new admmpd::EmbeddedMeshData();
   iface->idata->collision = NULL;
+  iface->idata->pin = NULL;
 
   // Generate tets and vertices
   Eigen::MatrixXd V; // defo verts
@@ -203,19 +206,20 @@ int admmpd_init(ADMMPDInterfaceData *iface, float *in_verts, unsigned int *in_fa
   {
     default:
     case 0: {
-      gen_success = admmpd_init_with_tetgen(iface,in_verts,in_faces,&V,&T,&m);
+      gen_success = admmpd_init_with_tetgen(iface,in_mesh->verts,in_mesh->faces,&V,&T,&m);
       //iface->idata->collision = new admmpd::TetMeshCollision();
       } break;
     case 1: {
-      gen_success = admmpd_init_with_lattice(iface,in_verts,in_faces,&V,&T,&m);
+      gen_success = admmpd_init_with_lattice(iface,in_mesh->verts,in_mesh->faces,&V,&T,&m);
       iface->idata->collision = new admmpd::EmbeddedMeshCollision(iface->idata->embmesh);
+      iface->idata->pin = new admmpd::EmbeddedMeshPin(iface->idata->embmesh);
     } break;
   }
   if (!gen_success || iface->totverts==0)
   {
     printf("**ADMMPD Failed to generate tets\n");
     return 0;
-  } 
+  }
 
   // Initialize
   bool init_success = false;
@@ -272,6 +276,30 @@ void admmpd_update_obstacles(
       in_verts_0, in_verts_1, nv, in_faces, nf);
 }
 
+void admmpd_update_goals(
+    ADMMPDInterfaceData *iface,
+    float *goal_k, // goal stiffness, nv
+    float *goal_pos, // goal position, nv x 3
+    int nv)
+{
+    if (iface==NULL || goal_k==NULL || goal_pos==NULL)
+      return;
+    if (iface->idata==NULL)
+      return;
+    if (iface->idata->pin==NULL)
+      return;
+
+    for (int i=0; i<nv; ++i)
+    {
+      if (goal_k[i] <= 0.f)
+        continue;
+
+      Eigen::Vector3d ki = Eigen::Vector3d::Ones() * goal_k[i];
+      Eigen::Vector3d qi(goal_pos[i*3+0], goal_pos[i*3+1], goal_pos[i*3+2]);
+      iface->idata->pin->set_pin(i,qi,ki);
+    }
+}
+
 void admmpd_copy_to_bodypoint_and_object(ADMMPDInterfaceData *iface, BodyPoint *pts, float (*vertexCos)[3])
 {
 
@@ -328,7 +356,11 @@ void admmpd_solve(ADMMPDInterfaceData *iface)
 
   try
   {
-    admmpd::Solver().solve(iface->idata->options,iface->idata->data,iface->idata->collision);
+    admmpd::Solver().solve(
+        iface->idata->options,
+        iface->idata->data,
+        iface->idata->collision,
+        iface->idata->pin);
   }
   catch(const std::exception &e)
   {
diff --git a/intern/softbody/admmpd_api.h b/intern/softbody/admmpd_api.h
index f29a3bbdc7d..5a67fb72f5a 100644
--- a/intern/softbody/admmpd_api.h
+++ b/intern/softbody/admmpd_api.h
@@ -41,6 +41,11 @@ typedef struct ADMMPDInterfaceData {
     struct ADMMPDInternalData *idata;
 } ADMMPDInterfaceData;
 
+typedef struct ADMMPDInitData {
+    float *verts; // n x 3
+    unsigned int *faces; // m x 3
+} ADMMPDInitData;
+
 // SoftBody bodypoint (contains pos,vec)
 typedef struct BodyPoint BodyPoint;
 
@@ -48,7 +53,7 @@ typedef struct BodyPoint BodyPoint;
 void admmpd_dealloc(ADMMPDInterfaceData*);
 
 // Initializes solver and allocates internal data
-int admmpd_init(ADMMPDInterfaceData*, float *in_verts, unsigned int *in_faces);
+int admmpd_init(ADMMPDInterfaceData*, ADMMPDInitData*);
 
 // Copies BodyPoint data (from SoftBody)
 // to internal vertex position and velocity
@@ -63,6 +68,13 @@ void admmpd_update_obstacles(
     unsigned int *in_faces,
     int nf);
 
+// Updates goal positions
+void admmpd_update_goals(
+    ADMMPDInterfaceData*,
+    float *goal_k, // goal stiffness, nv
+    float *goal_pos, // goal position, nv x 3
+    int nv);
+
 // Copies internal vertex position and velocity data
 // to BodyPoints (from SoftBody) AND surface mesh vertices.
 // If pts or vertexCos is null, its skipped
diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index c4a86869642..eefe1bc2a0e 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -3546,6 +3546,7 @@ static void init_admmpd_interface(Object *ob, float (*vertexCos)[3])
     return;
 
   Mesh *me = ob->data;
+  MEdge *medge = me->medge;
   SoftBody *sb = ob->soft;
 
   if (sb->admmpd != NULL)
@@ -3560,8 +3561,11 @@ static void init_admmpd_interface(Object *ob, float (*vertexCos)[3])
   int totfaces = poly_to_tri_count(me->totpoly, me->totloop);
   unsigned int *in_faces = (unsigned int*)MEM_callocN(sizeof(unsigned int)*totfaces*3, __func__);
   float *in_verts = (float*)MEM_callocN(sizeof(float)*me->totvert*3, __func__);
+//  float *in_goals = (float*)MEM_callocN(sizeof(float)*me->totvert, __func__);
   sb->admmpd->mesh_totverts = me->totvert;
   sb->admmpd->mesh_totfaces = totfaces;
+  float default_goal = 0.7f;
+  int defgroup_index = me->dvert ? (sb->vertgroup - 1) : -1;
 
   // Initialize input vertices
   for (int i=0; i<me->totvert; ++i)
@@ -3574,7 +3578,17 @@ static void init_admmpd_interface(Object *ob, float (*vertexCos)[3])
     mul_m4_v3(ob->obmat, vi);
     for (int j=0; j<3; ++j)
       in_verts[i*3+j] = vi[j];
-  }
+
+//    float goal = 0.f;
+//    if (ob->softflag & OB_SB_GOAL) {
+//      goal = default_goal;
+//    }
+//    if ((ob->softflag & OB_SB_GOAL) && (defgroup_index != -1)) {
+//      goal *= BKE_defvert_find_weight(&me->dvert[i], defgroup_index);
+//    }
+//    in_goals[i] = goal;
+
+  } // end loop input surface verts
 
   // Initialize input faces
   MLoopTri *looptri, *lt;
@@ -3591,8 +3605,14 @@ static void init_admmpd_interface(Object *ob, float (*vertexCos)[3])
 
   // Initalize solver and free tmp data
   sb->admmpd->init_mode = 1; // 0=tetmesh, 1=lattice
-  admmpd_init(sb->admmpd, in_verts, in_faces);
+
+  ADMMPDInitData in_data;
+  in_data.verts = in_verts;
+  in_data.faces = in_faces;
+//  in_data.goals = in_goals;
+  admmpd_init(sb->admmpd, &in_data);
   MEM_freeN(in_verts);
+//  MEM_freeN(in_goals);
   MEM_freeN(in_faces);
 
   // Set up softbody to store defo verts
@@ -3694,6 +3714,45 @@ static void admmpd_update_collider(
   BKE_collision_objects_free(objects);
 }
 
+static void admmpd_update_goal_positions(Object *ob, float (*vertexCos)[3])
+{
+  if(ob->type != OB_MESH)
+    return;
+
+  if(!(ob->softflag & OB_SB_GOAL))
+    return;
+
+  Mesh *me = ob->data;
+//  MEdge *medge = me->medge;
+  SoftBody *sb = ob->soft;
+  if (!sb->admmpd)
+    return;
+
+  int nv = sb->admmpd->mesh_totverts;
+  float *goal_k = MEM_callocN(sizeof(float)*nv, "goal_k");
+  float *goal_pos = MEM_callocN(sizeof(float)*3*nv, "goal_pos");
+  int defgroup_index = me->dvert ? (sb->vertgroup - 1) : -1;
+
+  for (int i=0; i<nv; i++) {
+    goal_k[i] = 0.7; // softbody default
+    if ((ob->softflag & OB_SB_GOAL) && (defgroup_index != -1)) {
+      goal_k[i] *= BKE_defvert_find_weight(&me->dvert[i], defgroup_index);
+    }
+
+    float vi[3];
+    vi[0] = vertexCos[i][0];
+    vi[1] = vertexCos[i][1];
+    vi[2] = vertexCos[i][2];
+    mul_m4_v3(ob->obmat, vi);
+    for (int j=0; j<3; ++j)
+      goal_pos[i*3+j] = vi[j];
+  }
+
+  admmpd_update_goals(sb->admmpd, goal_k, goal_pos, nv);
+  MEM_freeN(goal_k);
+  MEM_freeN(goal_pos);
+}
+
 /* simulates one step. framenr is in frames */
 void sbObjectStep_admmpd(
                   struct Depsgraph *depsgraph,
@@ -3721,7 +3780,7 @@ void sbObjectStep_admmpd(
 
   // check for changes in mesh, should only happen in case the mesh
   // structure changes during an animation.
-  // Hopefully we're short-circuiting the boolean here.
+  // Should be short-circuiting the boolean here.
   if (sb->admmpd != NULL &&
       sb->admmpd->mesh_totverts != numVerts) {
     BKE_ptcache_invalidate(cache);
@@ -3819,11 +3878,16 @@ void sbObjectStep_admmpd(
     BKE_ptcache_write(&pid, startframe);
   }
 
-  // Update ADMMPD interface variables from cache
-  // and perform a solve.
+  // Update ADMMPD interface variables from cache and perform a solve.
+  // This copies from BodyPoint to ADMM-PD: 
+  // - vertex position
+  // - vertex velocity
   admmpd_copy_from_bodypoint(sb->admmpd,sb->bpoint);
   admmpd_update_collider(depsgraph, sb->collision_group, ob);
+  admmpd_update_goal_positions(ob, vertexCos);
   admmpd_solve(sb->admmpd);
+
+  // Copies ADMM-PD data to both bodypoint and vertexCos.
   admmpd_copy_to_bodypoint_and_object(sb->admmpd,sb->bpoint,vertexCos);
   for (int i=0; i<me->totvert; ++i)
   { // TODO move this to admmpd API