several bugfixes but I think I'm going to need to change the way I handle constrained solve

11 files changed, 446 insertions, 165 deletions

diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index 4d6ff3328b8..3e006f74ae3 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -43,6 +43,8 @@ set(SRC
   src/admmpd_math.cpp
   src/admmpd_solver.h
   src/admmpd_solver.cpp
+  src/admmpd_tetmesh.h
+  src/admmpd_tetmesh.cpp
   src/admmpd_types.h
 )
 
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 98c5653e7f7..be6bd0a21b7 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -10,6 +10,7 @@
 #include "BLI_threads.h"
 
 #include <iostream>
+#include <sstream>
 
 namespace admmpd {
 using namespace Eigen;
@@ -23,7 +24,7 @@ VFCollisionPair::VFCollisionPair() :
 	q_n(0,0,0)
 	{}
 
-void EmbeddedMeshCollision::set_obstacles(
+void Collision::set_obstacles(
 	const float *v0,
 	const float *v1,
 	int nv,
@@ -67,53 +68,32 @@ void EmbeddedMeshCollision::set_obstacles(
 
 } // end add obstacle
 
-typedef struct DetectThreadData {
-	const EmbeddedMeshData *mesh;
-	const EmbeddedMeshCollision::ObstacleData *obsdata;
-	const Eigen::MatrixXd *x0;
-	const Eigen::MatrixXd *x1;
-	double floor_z;
-	std::vector<std::vector<VFCollisionPair> > *pt_vf_pairs; // per thread pairs
-} DetectThreadData;
-
-static void parallel_detect_obstacles(
-	void *__restrict userdata,
-	const int i,
-	const TaskParallelTLS *__restrict tls)
+void Collision::detect_discrete_vf(
+	const Eigen::Vector3d &pt,
+	int pt_idx,
+	bool pt_is_obs,
+	const AABBTree<double,3> *mesh_tree,
+	const Eigen::MatrixXd *mesh_x,
+	const Eigen::MatrixXi *mesh_tris,
+	bool mesh_is_obs,
+	double floor_z,
+	std::vector<VFCollisionPair> *pairs)
 {
-	DetectThreadData *td = (DetectThreadData*)userdata;
-
-	// Comments say "don't use this" but how else am I supposed
-	// to get the thread ID? It appears to return the right thing...
-	int thread_idx = BLI_task_parallel_thread_id(tls);
-	std::vector<VFCollisionPair> &tl_pairs = td->pt_vf_pairs->at(thread_idx);
-
-	int tet_idx = td->mesh->vtx_to_tet[i];
-	RowVector4i tet = td->mesh->tets.row(tet_idx);
-	Vector4d bary = td->mesh->barys.row(i);
-
-	// First, get the surface vertex
-	Vector3d pt = 
-		bary[0] * td->x1->row(tet[0]) +
-		bary[1] * td->x1->row(tet[1]) +
-		bary[2] * td->x1->row(tet[2]) +
-		bary[3] * td->x1->row(tet[3]);
-
 	// Special case, check if we are below the floor
-	if (pt[2] < td->floor_z)
+	if (pt[2] < floor_z)
 	{
-		tl_pairs.emplace_back();
-		VFCollisionPair &pair = tl_pairs.back();
-		pair.p_idx = i;
-		pair.p_is_obs = 0;
+		pairs->emplace_back();
+		VFCollisionPair &pair = pairs->back();
+		pair.p_idx = pt_idx;
+		pair.p_is_obs = pt_is_obs;
 		pair.q_idx = -1;
 		pair.q_is_obs = 1;
-		pair.pt_on_q = Vector3d(pt[0],pt[1],td->floor_z);
+		pair.pt_on_q = Vector3d(pt[0],pt[1],floor_z);
 		pair.q_n = Vector3d(0,0,1);
 	}
 
-	// Any obstacles?
-	if (td->obsdata->F.rows()==0)
+	// Any faces to detect against?
+	if (mesh_tris->rows()==0)
 		return;
 
 	// TODO
@@ -122,8 +102,8 @@ static void parallel_detect_obstacles(
 	// or continuous collision detection.
 
 	PointInTriangleMeshTraverse<double> pt_in_mesh(
-		pt, &td->obsdata->V1, &td->obsdata->F);
-	td->obsdata->tree.traverse(pt_in_mesh);
+		pt, mesh_x, mesh_tris);
+	mesh_tree->traverse(pt_in_mesh);
 	if (pt_in_mesh.output.num_hits() % 2 != 1)
 		return;
 
@@ -138,40 +118,93 @@ static void parallel_detect_obstacles(
 	// for now.
 
 	NearestTriangleTraverse<double> find_nearest_tri(
-		pt, &td->obsdata->V1, &td->obsdata->F);
-	td->obsdata->tree.traverse(find_nearest_tri);
+		pt, mesh_x, mesh_tris);
+	mesh_tree->traverse(find_nearest_tri);
 	if (find_nearest_tri.output.prim < 0) // error
 		return;
 
 	// Create collision pair
-	tl_pairs.emplace_back();
-	VFCollisionPair &pair = tl_pairs.back();
-	pair.p_idx = i;
-	pair.p_is_obs = 0;
+	pairs->emplace_back();
+	VFCollisionPair &pair = pairs->back();
+	pair.p_idx = pt_idx;
+	pair.p_is_obs = pt_is_obs;
 	pair.q_idx = find_nearest_tri.output.prim;
-	pair.q_is_obs = 1;
+	pair.q_is_obs = mesh_is_obs;
 	pair.pt_on_q = find_nearest_tri.output.pt_on_tri;
 
 	// Compute face normal
 	BLI_assert(pair.q_idx >= 0);
-	BLI_assert(pair.q_idx < td->obsdata->F.rows());
-	RowVector3i tri_inds = td->obsdata->F.row(pair.q_idx);
+	BLI_assert(pair.q_idx < mesh_tris->rows());
+	RowVector3i tri_inds = mesh_tris->row(pair.q_idx);
+	BLI_assert(tri_inds[0]>=0 && tri_inds[0]<mesh_x->rows());
+	BLI_assert(tri_inds[1]>=0 && tri_inds[1]<mesh_x->rows());
+	BLI_assert(tri_inds[2]>=0 && tri_inds[2]<mesh_x->rows());
 	Vector3d tri[3] = {
-		td->obsdata->V1.row(tri_inds[0]),
-		td->obsdata->V1.row(tri_inds[1]),
-		td->obsdata->V1.row(tri_inds[2])
+		mesh_x->row(tri_inds[0]),
+		mesh_x->row(tri_inds[1]),
+		mesh_x->row(tri_inds[2])
 	};
 	pair.q_n = ((tri[1]-tri[0]).cross(tri[2]-tri[0])).normalized();
 
-} // end parallel detect against obstacles
+//	std::stringstream ss;
+//	ss << "\nhit:" <<
+//		"\n\t normal: " << pair.q_n.transpose() <<
+//		"\n\t pt: " << pt.transpose() <<
+//		"\n\t pt_on_tri: " << pair.pt_on_q.transpose() <<
+//		std::endl;
+//	printf("%s",ss.str().c_str());
+
+} // end detect_discrete_vf
+
+typedef struct DetectThreadData {
+	const TetMeshData *tetmesh;
+	const EmbeddedMeshData *embmesh;
+	const Collision::ObstacleData *obsdata;
+	const Eigen::MatrixXd *x0;
+	const Eigen::MatrixXd *x1;
+	double floor_z;
+	std::vector<std::vector<VFCollisionPair> > *pt_vf_pairs; // per thread pairs
+} DetectThreadData;
 
 static void parallel_detect(
 	void *__restrict userdata,
 	const int i,
 	const TaskParallelTLS *__restrict tls)
 {
-	parallel_detect_obstacles(userdata,i,tls);
-	//parallel_detect_self(userdata,i,tls);	
+	DetectThreadData *td = (DetectThreadData*)userdata;
+
+	// Comments say "don't use this" but how else am I supposed
+	// to get the thread ID? It appears to return the right thing...
+	int thread_idx = BLI_task_parallel_thread_id(tls);
+	std::vector<VFCollisionPair> &tl_pairs = td->pt_vf_pairs->at(thread_idx);
+
+	if (td->tetmesh != nullptr)
+	{
+
+	} // end detect with tet meshes
+	
+	if (td->embmesh != nullptr)
+	{
+		int tet_idx = td->embmesh->vtx_to_tet[i];
+		RowVector4i tet = td->embmesh->tets.row(tet_idx);
+		Vector4d bary = td->embmesh->barys.row(i);
+		Vector3d pt = 
+			bary[0] * td->x1->row(tet[0]) +
+			bary[1] * td->x1->row(tet[1]) +
+			bary[2] * td->x1->row(tet[2]) +
+			bary[3] * td->x1->row(tet[3]);
+
+		Collision::detect_discrete_vf(
+			pt, i, false,
+			&td->obsdata->tree,
+			&td->obsdata->V1,
+			&td->obsdata->F,
+			true,
+			td->floor_z,
+			&tl_pairs );
+
+	} // end detect with embedded meshes
+
 } // end parallel detect
 
 int EmbeddedMeshCollision::detect(
@@ -187,25 +220,114 @@ int EmbeddedMeshCollision::detect(
 	std::vector<std::vector<VFCollisionPair> > pt_vf_pairs
 		(max_threads, std::vector<VFCollisionPair>());
 
+	DetectThreadData thread_data = {
+		.tetmesh = nullptr,
+		.embmesh = mesh,
+		.obsdata = &obsdata,
+		.x0 = x0,
+		.x1 = x1,
+		.floor_z = floor_z,
+		.pt_vf_pairs = &pt_vf_pairs
+	};
+
+	int nev = mesh->x_rest.rows();
+
+	TaskParallelSettings settings;
+	BLI_parallel_range_settings_defaults(&settings);
+	BLI_task_parallel_range(0, nev, &thread_data, parallel_detect, &settings);
+
+	// Combine thread-local results
+	vf_pairs.clear();
+	for (int i=0; i<max_threads; ++i)
+	{
+		const std::vector<VFCollisionPair> &tl_pairs = pt_vf_pairs[i];
+		vf_pairs.insert(vf_pairs.end(), tl_pairs.begin(), tl_pairs.end());
+	}
 
+	return vf_pairs.size();
+} // end detect
 
+static Vector3d vx0(0,0,0);
 
+void EmbeddedMeshCollision::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)
+{
+	BLI_assert(x != NULL);
+	BLI_assert(x->cols() == 3);
 
+	int np = vf_pairs.size();
+	if (np==0)
+		return;
 
-floor_z = -2;
+	l->reserve((int)l->size() + np);
+	trips_x->reserve((int)trips_x->size() + np*4);
+	trips_y->reserve((int)trips_y->size() + np*4);
+	trips_z->reserve((int)trips_z->size() + np*4);
 
+	for (int i=0; i<np; ++i)
+	{
+		VFCollisionPair &pair = vf_pairs[i];
+		int emb_p_idx = pair.p_idx;
 
+		// TODO: obstacle point intersecting mesh
+		if (pair.p_is_obs)
+		{
+			continue;
+		}
 
+		// Obstacle collision
+		if (pair.q_is_obs)
+		{
+			RowVector4d bary = mesh->barys.row(emb_p_idx);
+			int tet_idx = mesh->vtx_to_tet[emb_p_idx];
+			RowVector4i tet = mesh->tets.row(tet_idx);
 
+			// Basically a pin constraint
+			for (int j=0; j<3; ++j)
+			{
+				int c_idx = l->size();
+				for (int k=0; k<4; ++k)
+				{
+					if (j==0)
+						trips_x->emplace_back(c_idx, tet[k], bary[k]*pair.q_n[0]);
+					if (j==1)
+						trips_y->emplace_back(c_idx, tet[k], bary[k]*pair.q_n[1]);
+					if (j==2)
+						trips_z->emplace_back(c_idx, tet[k], bary[k]*pair.q_n[2]);
+				}
+				l->emplace_back(pair.pt_on_q[j]*pair.q_n[j]);
+			}
 
+			continue;
+		}
 
+		// Self collisions
+	
+	} // end loop pairs
 
+} // end jacobian
 
+/*
+int TetMeshCollision::detect(
+	const Eigen::MatrixXd *x0,
+	const Eigen::MatrixXd *x1)
+{
+	if (mesh==NULL)
+		return 0;
 
+	update_bvh(x0,x1);
 
+	int max_threads = std::max(1,BLI_system_thread_count());
+	std::vector<std::vector<VFCollisionPair> > pt_vf_pairs
+		(max_threads, std::vector<VFCollisionPair>());
 
 	DetectThreadData thread_data = {
-		.mesh = mesh,
+		.tetmesh = mesh,
+		.embmesh = nullptr,
 		.obsdata = &obsdata,
 		.x0 = x0,
 		.x1 = x1,
@@ -213,10 +335,10 @@ floor_z = -2;
 		.pt_vf_pairs = &pt_vf_pairs
 	};
 
-	int nev = mesh->x_rest.rows();
+	int nv = x1->rows();
 	TaskParallelSettings settings;
 	BLI_parallel_range_settings_defaults(&settings);
-	BLI_task_parallel_range(0, nev, &thread_data, parallel_detect, &settings);
+	BLI_task_parallel_range(0, nv, &thread_data, parallel_detect, &settings);
 
 	// Combine thread-local results
 	vf_pairs.clear();
@@ -227,9 +349,9 @@ floor_z = -2;
 	}
 
 	return vf_pairs.size();
-} // end detect
+}
 
-void EmbeddedMeshCollision::jacobian(
+void TetMeshCollision::jacobian(
 	const Eigen::MatrixXd *x,
 	std::vector<Eigen::Triplet<double> > *trips_x,
 	std::vector<Eigen::Triplet<double> > *trips_y,
@@ -244,38 +366,40 @@ void EmbeddedMeshCollision::jacobian(
 		return;
 
 	l->reserve((int)l->size() + np);
-	trips_x->reserve((int)trips_x->size() + np*3);
-	trips_y->reserve((int)trips_y->size() + np*3);
-	trips_z->reserve((int)trips_z->size() + np*3);
+	trips_x->reserve((int)trips_x->size() + np*4);
+	trips_y->reserve((int)trips_y->size() + np*4);
+	trips_z->reserve((int)trips_z->size() + np*4);
 
 	for (int i=0; i<np; ++i)
 	{
 		VFCollisionPair &pair = vf_pairs[i];
 
-		int emb_p_idx = pair.p_idx;
+		// TODO: obstacle point intersecting mesh
 		if (pair.p_is_obs)
-		{ // TODO: obstacle point intersecting mesh
+		{
 			continue;
 		}
 
 		// Obstacle collision
 		if (pair.q_is_obs)
 		{
-			RowVector4d bary = mesh->barys.row(emb_p_idx);
-			int tet_idx = mesh->vtx_to_tet[emb_p_idx];
-			RowVector4i tet = mesh->tets.row(tet_idx);
 			int c_idx = l->size();
-			for( int j=0; j<4; ++j ){
-				trips_x->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[0]);
-				trips_y->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[1]);
-				trips_z->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[2]);
-			}
+			bool has_qnx = std::abs(pair.q_n[0])>0;
+			bool has_qny = std::abs(pair.q_n[1])>0;
+			bool has_qnz = std::abs(pair.q_n[2])>0;
+			if (has_qnx)
+				trips_x->emplace_back(c_idx, pair.p_idx, pair.q_n[0]);
+			if (has_qny)
+				trips_y->emplace_back(c_idx, pair.p_idx, pair.q_n[1]);
+			if (has_qnz)
+				trips_z->emplace_back(c_idx, pair.p_idx, pair.q_n[2]);
 			l->emplace_back( pair.q_n.dot(pair.pt_on_q));
 			continue;
 		}
-
+	
 	} // end loop pairs
 
-} // end jacobian
+} // end jacobian of tet mesh intersect
+*/
 
 } // namespace admmpd
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index f49a314db47..f9c6363b89f 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -21,13 +21,16 @@ struct VFCollisionPair {
     VFCollisionPair();
 };
 
-// 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 Collision {
 public:
+    // Obstacle data created in set_obstacles
+    struct ObstacleData {
+        Eigen::MatrixXd V0, V1;
+        Eigen::MatrixXi F;
+        std::vector<Eigen::AlignedBox<double,3> > aabbs;
+        AABBTree<double,3> tree;
+    } obsdata;
+
     virtual ~Collision() {}
 
     // Performs collision detection.
@@ -37,12 +40,14 @@ public:
         const Eigen::MatrixXd *x1) = 0;
 
     // Set the soup of obstacles for this time step.
+    // I don't really like having to switch up interface style, but we'll
+    // do so here to avoid copies that would happen in admmpd_api.
     virtual void set_obstacles(
         const float *v0,
         const float *v1,
         int nv,
         const unsigned int *faces,
-        int nf) = 0;
+        int nf);
 
     // Special case for floor since it's common.
     virtual void set_floor(double z) = 0;
@@ -56,6 +61,21 @@ public:
         std::vector<Eigen::Triplet<double> > *trips_y,
     	std::vector<Eigen::Triplet<double> > *trips_z,
 		std::vector<double> *l) = 0;
+
+    // Given a point and a surface mesh,
+    // perform discrete collision and create
+    // a vertex-face collision pair if colliding.
+    // Also adds collision pairs if below floor.
+    static void detect_discrete_vf(
+        const Eigen::Vector3d &pt,
+        int pt_idx,
+        bool pt_is_obs,
+        const AABBTree<double,3> *mesh_tree,
+        const Eigen::MatrixXd *mesh_x,
+        const Eigen::MatrixXi *mesh_tris,
+        bool mesh_is_obs,
+        double floor_z,
+        std::vector<VFCollisionPair> *pairs);
 };
 
 // Collision detection against multiple meshes
@@ -66,23 +86,6 @@ public:
         floor_z(-std::numeric_limits<double>::max())
         {}
 
-    // Obstacle data created in set_obstacles
-    struct ObstacleData {
-        Eigen::MatrixXd V0, V1;
-        Eigen::MatrixXi F;
-        std::vector<Eigen::AlignedBox<double,3> > aabbs;
-        AABBTree<double,3> tree;
-    } obsdata;
-
-    // I don't really like having to switch up interface style, but we'll
-    // do so here to avoid copies that would happen in admmpd_api.
-    void set_obstacles(
-        const float *v0,
-        const float *v1,
-        int nv,
-        const unsigned int *faces,
-        int nf);
-
     // A floor is so common that it makes sense to hard
     // code floor collision instead of using a floor mesh.
     void set_floor(double z)
@@ -121,6 +124,53 @@ protected:
     { (void)(x0); (void)(x1); }
 };
 
+/*
+class TetMeshCollision : public Collision {
+public:
+    TetMeshCollision(const TetMeshData *mesh_) :
+        mesh(mesh_),
+        floor_z(-std::numeric_limits<double>::max())
+        {}
+
+    // Performs collision detection.
+    // Returns the number of active constraints.
+    int detect(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1);
+
+    // Special case for floor since it's common.
+    void set_floor(double z)
+    {
+        floor_z = z;
+    }
+
+    // Linearize the constraints and return Jacobian tensor.
+    // Constraints are linearized about x for constraint
+    // K x = l
+    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;
+
+protected:
+    const TetMeshData *mesh;
+    double floor_z;
+
+    // Pairs are compute on detect
+    std::vector<VFCollisionPair> vf_pairs;
+
+    // Updates the tetmesh BVH for self collisions.
+    // Called by detect()
+    // TODO
+    void update_bvh(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1)
+    { (void)(x0); (void)(x1); }
+};
+*/
+
 } // namespace admmpd
 
 #endif // ADMMPD_COLLISION_H_
diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp
index 78993ff73da..3476c34c720 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.cpp
+++ b/extern/softbody/src/admmpd_embeddedmesh.cpp
@@ -255,6 +255,58 @@ bool EmbeddedMesh::generate(
 
 } // end gen lattice
 
+void EmbeddedMesh::compute_masses(
+	EmbeddedMeshData *emb_mesh, // where embedding is stored
+	const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3
+	const Eigen::MatrixXd *x_tets, // lattice vertices, n x 3
+	Eigen::VectorXd *masses_tets, // masses of the lattice verts
+	double density_kgm3)
+{
+	BLI_assert(emb_mesh != NULL);
+	BLI_assert(x_embed != NULL);
+	BLI_assert(x_tets != NULL);
+	BLI_assert(x_tets->rows() > 0);
+	BLI_assert(x_tets->cols() == 3);
+	BLI_assert(masses_tets != NULL);
+	BLI_assert(density_kgm3 > 0);
+
+	// TODO
+	// map the area of the surface to the tet vertices
+
+	// 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
+	int nx = x_tets->rows();
+	masses_tets->resize(nx);
+	masses_tets->setZero();
+	int n_tets = emb_mesh->tets.rows();
+	for (int t=0; t<n_tets; ++t)
+	{
+		RowVector4i tet = emb_mesh->tets.row(t);
+		RowVector3d tet_v0 = x_tets->row(tet[0]);
+		Matrix3d edges;
+		edges.col(0) = x_tets->row(tet[1]) - tet_v0;
+		edges.col(1) = x_tets->row(tet[2]) - tet_v0;
+		edges.col(2) = x_tets->row(tet[3]) - tet_v0;
+		double vol = std::abs((edges).determinant()/6.f);
+		double tet_mass = density_kgm3 * vol;
+		masses_tets->operator[](tet[0]) += tet_mass / 4.f;
+		masses_tets->operator[](tet[1]) += tet_mass / 4.f;
+		masses_tets->operator[](tet[2]) += tet_mass / 4.f;
+		masses_tets->operator[](tet[3]) += tet_mass / 4.f;
+	}
+
+	// Verify masses
+	for (int i=0; i<nx; ++i)
+	{
+		if (masses_tets->operator[](i) <= 0.0)
+		{
+			printf("**EmbeddedMesh::compute_masses Error: unreferenced vertex\n");
+			masses_tets->operator[](i)=1;
+		}
+	}
+} // end compute masses
+
 typedef struct FindTetThreadData {
 	AABBTree<double,3> *tree;
 	EmbeddedMeshData *emb_mesh; // thread sets vtx_to_tet and barys
diff --git a/extern/softbody/src/admmpd_embeddedmesh.h b/extern/softbody/src/admmpd_embeddedmesh.h
index a1abe02f904..8b0cdc5bb04 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.h
+++ b/extern/softbody/src/admmpd_embeddedmesh.h
@@ -1,8 +1,8 @@
 // Copyright Matt Overby 2020.
 // Distributed under the MIT License.
 
-#ifndef ADMMPD_LATTICE_H_
-#define ADMMPD_LATTICE_H_
+#ifndef ADMMPD_EMBEDDEDMESH_H_
+#define ADMMPD_EMBEDDEDMESH_H_
 
 #include "admmpd_types.h"
 
@@ -23,6 +23,15 @@ public:
         const Eigen::MatrixXd *x_data,
         int idx);
 
+    // Given an embedding, compute masses
+    // for the lattice vertices
+    void compute_masses(
+        EmbeddedMeshData *emb_mesh, // where embedding is stored
+        const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3
+        const Eigen::MatrixXd *x_tets, // lattice vertices, n x 3
+        Eigen::VectorXd *masses_tets, // masses of the lattice verts
+        double density_kgm3 = 1100);
+
 protected:
 
     // Returns true on success
@@ -36,4 +45,4 @@ protected:
 
 } // namespace admmpd
 
-#endif // ADMMPD_LATTICE_H_
+#endif // ADMMPD_EMBEDDEDMESH_H_
diff --git a/extern/softbody/src/admmpd_math.h b/extern/softbody/src/admmpd_math.h
index 939d2d3b230..ff216929f86 100644
--- a/extern/softbody/src/admmpd_math.h
+++ b/extern/softbody/src/admmpd_math.h
@@ -5,6 +5,7 @@
 #define ADMMPD_MATH_H_
 
 #include <Eigen/Geometry>
+#include <Eigen/Sparse>
 
 namespace admmpd {
 namespace barycoords {
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index f3f9b19ee0f..f4956e8d476 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -27,6 +27,7 @@ typedef struct ThreadData {
 bool Solver::init(
     const Eigen::MatrixXd &V,
 	const Eigen::MatrixXi &T,
+	const Eigen::VectorXd &m,
     const Options *options,
     SolverData *data)
 {
@@ -41,6 +42,7 @@ bool Solver::init(
 	data->v.resize(V.rows(),3);
 	data->v.setZero();
 	data->tets = T;
+	data->m = m;
 	if (!compute_matrices(options,data))
 		return false;
 
@@ -105,7 +107,7 @@ void Solver::init_solve(
 	data->x_start = data->x;
 	for (int i=0; i<nx; ++i)
 	{
-		data->v.row(i) += options->grav;
+		data->v.row(i) += dt*options->grav;
 		data->M_xbar.row(i) =
 			data->m[i] * data->x.row(i) +
 			dt*data->m[i]*data->v.row(i);
@@ -143,7 +145,8 @@ void Solver::solve_local_step(
 {
 	BLI_assert(data != NULL);
 	BLI_assert(options != NULL);
-	int ne = data->rest_volumes.size();
+	data->Dx.noalias() = data->D * data->x;
+	int ne = data->indices.size();
 	BLI_assert(ne > 0);
   	ThreadData thread_data = {.options=options, .data = data};
 	TaskParallelSettings settings;
@@ -191,7 +194,9 @@ void Solver::update_constraints(
 	}
 
 	// Otherwise update the data.
-	data->l = Map<VectorXd>(l_coeffs.data(),nc);
+	data->l.resize(nc);
+	for (int i=0; i<nc; ++i)
+		data->l[i] = l_coeffs[i];
 	data->K[0].resize(nc,nx);
 	data->K[0].setFromTriplets(trips_x.begin(),trips_x.end());
 	data->K[1].resize(nc,nx);
@@ -342,8 +347,6 @@ bool Solver::compute_matrices(
 		data->v.resize(nx,3);
 		data->v.setZero();
 	}
-	if (data->m.rows() != nx)
-		compute_masses(options,data);
 
 	// Add per-element energies to data
 	std::vector<Triplet<double> > trips;
@@ -397,46 +400,6 @@ bool Solver::compute_matrices(
 
 } // end compute matrices
 
-void Solver::compute_masses(
-	const Options *options,
-	SolverData *data)
-{
-	BLI_assert(data != NULL);
-	BLI_assert(options != NULL);
-
-	// 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
-	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);
-		RowVector3d tet0 = data->x.row(tet[0]);
-		Matrix3d edges;
-		edges.col(0) = data->x.row(tet[1]) - tet0;
-		edges.col(1) = data->x.row(tet[2]) - tet0;
-		edges.col(2) = data->x.row(tet[3]) - tet0;
-		double v = std::abs((edges).determinant()/6.f);
-		double tet_mass = options->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;
-	}
-	// Verify masses
-	int nx = data->m.rows();
-	for (int i=0; i<nx; ++i)
-	{
-		if (data->m[i] <= 0.0)
-		{
-			printf("**Solver::compute_masses Error: unreferenced vertex\n");
-			data->m[i]=1;
-		}
-	}
-} // end compute masses
-
 void Solver::append_energies(
 	const Options *options,
 	SolverData *data,
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index 1744b0f80c9..3c16b733b82 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -17,6 +17,7 @@ public:
     bool init(
         const Eigen::MatrixXd &V, // vertices
         const Eigen::MatrixXi &T, // tets
+        const Eigen::VectorXd &m, // per-vert masses
         const Options *options,
         SolverData *data);
 
@@ -53,13 +54,6 @@ protected:
         const Options *options,
         SolverData *data);
 
-    // TODO we need to compute masses
-    // based on the embedded mesh, otherwise
-    // we get super ugly dynamics.
-    void compute_masses(
-        const Options *options,
-        SolverData *data);
-
 	void append_energies(
 		const Options *options,
 		SolverData *data,
diff --git a/extern/softbody/src/admmpd_tetmesh.cpp b/extern/softbody/src/admmpd_tetmesh.cpp
new file mode 100644
index 00000000000..43157c416ae
--- /dev/null
+++ b/extern/softbody/src/admmpd_tetmesh.cpp
@@ -0,0 +1,58 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#include "admmpd_tetmesh.h"
+#include "admmpd_math.h"
+#include <unordered_map>
+#include <set>
+#include "BLI_assert.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+void TetMesh::compute_masses(
+    TetMeshData *mesh,
+    const Eigen::MatrixXd *x,
+    Eigen::VectorXd *masses,
+	double density_kgm3)
+{
+	BLI_assert(mesh != NULL);
+	BLI_assert(x != NULL);
+	BLI_assert(masses != NULL);
+	BLI_assert(density_kgm3 > 0);
+
+	// 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
+	int nx = x->rows();
+	masses->resize(nx);
+	masses->setZero();
+	int n_tets = mesh->tets.rows();
+	for (int t=0; t<n_tets; ++t)
+	{
+		RowVector4i tet = mesh->tets.row(t);
+		RowVector3d tet_v0 = x->row(tet[0]);
+		Matrix3d edges;
+		edges.col(0) = x->row(tet[1]) - tet_v0;
+		edges.col(1) = x->row(tet[2]) - tet_v0;
+		edges.col(2) = x->row(tet[3]) - tet_v0;
+		double vol = std::abs((edges).determinant()/6.f);
+		double tet_mass = density_kgm3 * vol;
+		masses->operator[](tet[0]) += tet_mass / 4.f;
+		masses->operator[](tet[1]) += tet_mass / 4.f;
+		masses->operator[](tet[2]) += tet_mass / 4.f;
+		masses->operator[](tet[3]) += tet_mass / 4.f;
+	}
+
+	// Verify masses
+	for (int i=0; i<nx; ++i)
+	{
+		if (masses->operator[](i) <= 0.0)
+		{
+			printf("**TetMesh::compute_masses Error: unreferenced vertex\n");
+			masses->operator[](i)=1;
+		}
+	}
+} // end compute masses
+
+} // namespace admmpd
diff --git a/extern/softbody/src/admmpd_tetmesh.h b/extern/softbody/src/admmpd_tetmesh.h
new file mode 100644
index 00000000000..7d1e4304cd2
--- /dev/null
+++ b/extern/softbody/src/admmpd_tetmesh.h
@@ -0,0 +1,25 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_TETMESH_H_
+#define ADMMPD_TETMESH_H_
+
+#include "admmpd_types.h"
+
+namespace admmpd {
+
+class TetMesh {
+public:
+   // Given an embedding, compute masses
+   // for the lattice vertices
+   static void compute_masses(
+        TetMeshData *mesh,
+        const Eigen::MatrixXd *x,
+        Eigen::VectorXd *masses,
+        double density_kgm3 = 1100);
+
+}; // class Lattice
+
+} // namespace admmpd
+
+#endif // ADMMPD_LATTICE_H_
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index de1fb122a30..42277180da7 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -20,28 +20,31 @@ struct Options {
     int max_cg_iters;
     double mult_k; // stiffness multiplier for constraints
     double min_res; // min residual for CG solver
-    double density_kgm3; // unit-volume density
     double youngs; // Young's modulus // TODO variable per-tet
     double poisson; // Poisson ratio // TODO variable per-tet
     Eigen::Vector3d grav;
     Options() :
-        timestep_s(1.0/100.0),
-        max_admm_iters(40),
-        max_cg_iters(20),
+        timestep_s(1.0/24.0),
+        max_admm_iters(100),
+        max_cg_iters(10),
         mult_k(1),
         min_res(1e-6),
-        density_kgm3(1100),
-        youngs(100000000),
-        poisson(0.399),
+        youngs(10000000),
+        poisson(0.299),
         grav(0,0,-9.8)
         {}
 };
 
-struct TetMesh {
+// I think eventually I'll make the mesh
+// a virtual class with mapping functions.
+// That might clean up the API/interface a bit.
+// Will work out what we need for collisions and such first.
+
+struct TetMeshData {
     Eigen::MatrixXd x_rest; // verts at rest
     Eigen::MatrixXi faces; // surface elements, m x 3
     Eigen::MatrixXi tets; // internal elements, m x 4
-}; // type 0
+};
 
 struct EmbeddedMeshData { // i.e. the lattice
     Eigen::MatrixXd x_rest; // embedded verts at rest