changed up interface for lattice a bit

11 files changed, 403 insertions, 200 deletions

diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index 09fdc947dde..4d6ff3328b8 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -29,20 +29,21 @@ set(INC_SYS
 )
 
 set(SRC
-  src/admmpd_math.h
-  src/admmpd_math.cpp
-  src/admmpd_energy.h
-  src/admmpd_energy.cpp
-  src/admmpd_lattice.h
-  src/admmpd_lattice.cpp
-  src/admmpd_solver.h
-  src/admmpd_solver.cpp
-  src/admmpd_collision.h
-  src/admmpd_collision.cpp
   src/admmpd_bvh.h
   src/admmpd_bvh.cpp
   src/admmpd_bvh_traverse.h
   src/admmpd_bvh_traverse.cpp
+  src/admmpd_collision.h
+  src/admmpd_collision.cpp
+  src/admmpd_embeddedmesh.h
+  src/admmpd_embeddedmesh.cpp
+  src/admmpd_energy.h
+  src/admmpd_energy.cpp
+  src/admmpd_math.h
+  src/admmpd_math.cpp
+  src/admmpd_solver.h
+  src/admmpd_solver.cpp
+  src/admmpd_types.h
 )
 
 set(LIB
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 7a53c91a150..02a197c4138 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -2,9 +2,101 @@
 // Distributed under the MIT License.
 
 #include "admmpd_collision.h"
+#include "BLI_assert.h"
 
 namespace admmpd {
+using namespace Eigen;
 
+void EmbeddedMeshCollision::set_obstacles(
+	const float *v0,
+	const float *v1,
+	int nv,
+	const int *faces,
+	int nf)
+{
+	if (obs_V0.rows() != nv)
+		obs_V0.resize(nv,3);
+
+	if (obs_V1.rows() != nv)
+		obs_V1.resize(nv,3);
+
+	for (int i=0; i<nv; ++i)
+	{
+		for (int j=0; j<3; ++j)
+		{
+			obs_V0(i,j) = v0[i*3+j];
+			obs_V1(i,j) = v1[i*3+j];
+		}
+	}
+
+	if (obs_F.rows() != nf)
+	{
+		obs_F.resize(nf,3);
+		obs_aabbs.resize(nf);
+	}
+
+	for (int i=0; i<nf; ++i)
+	{
+		obs_aabbs[i].setEmpty();
+		for (int j=0; j<3; ++j)
+		{
+			int fj = faces[i*3+j];
+			obs_F(i,j) = fj;
+			obs_aabbs[i].extend(obs_V0.row(fj).transpose());
+			obs_aabbs[i].extend(obs_V1.row(fj).transpose());
+		}
+	}
+
+	obs_tree.init(obs_aabbs);
+
+} // end add obstacle
+
+void EmbeddedMeshCollision::detect(const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1)
+{
+	/*
+	// First, update the positions of the embedded vertex
+	// and perform collision detection against obstacles
+	int n_ev = emb_V0.rows();
+	std::vector<int> colliding(n_ev,0);
+	for (int i=0; i<n_ev; ++i)
+	{
+		int t = vtx_to_tet->operator[](i);
+		BLI_assert(t >= 0);
+		BLI_assert(t < tets->rows());
+		RowVector4i tet = tets->row(t);
+		Vector4d bary = emb_barys->row(i);
+//		emb_V0.row(i) =
+//			bary[0] * x0->row(tet[0]) +
+//			bary[1] * x0->row(tet[1]) +
+//			bary[2] * x0->row(tet[2]) +
+//			bary[3] * x0->row(tet[3]);
+		Vector3d pt = 
+			bary[0] * x1->row(tet[0]) +
+			bary[1] * x1->row(tet[1]) +
+			bary[2] * x1->row(tet[2]) +
+			bary[3] * x1->row(tet[3]);
+//		emb_V1.row(i) =
+
+		// Check if we are inside the mesh.
+		// If so, find the nearest face in the rest pose.
+		PointInTriangleMeshTraverse<double> pt_in_mesh(pt, &obs_V1, &obs_F);
+		obs_tree.traverse(pt_in_mesh);
+		if (pt_in_mesh.output.num_hits() % 2 == 1)
+		{
+			// Find 
+//			hit = true;
+		}
+
+//		colliding[i] = hit;
+	}
+
+	// Only bother with self collision if it
+	// is not colliding with an obstacle.
+	// This is only useful for discrete tests.
+*/
+} // end emb collision detect
+
+/*
 void FloorCollider::detect(const Eigen::MatrixXd *x)
 {
 	(void)(x);
@@ -32,5 +124,5 @@ void FloorCollider::jacobian(
 		l->emplace_back(floor_z);
 	}
 } // end floor collider Jacobian
-
+*/
 } // namespace admmpd
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index c3fd9cb8c4f..5126d1a6958 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -5,7 +5,9 @@
 #define ADMMPD_COLLISION_H_
 
 #include <Eigen/Sparse>
+#include <Eigen/Geometry>
 #include <vector>
+#include "admmpd_bvh.h"
 
 namespace admmpd {
 
@@ -14,21 +16,80 @@ namespace admmpd {
 // 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 {
+class Collision {
 public:
-    virtual void detect(
-        const Eigen::MatrixXd *x) = 0;
-    virtual void jacobian(
+//    virtual void detect(
+//        int meshnum,
+//        const Eigen::MatrixXd *x,
+//        const Eigen::MatrixXi *faces) = 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;
+};
+
+// Collision detection against multiple meshes
+class EmbeddedMeshCollision : public Collision {
+protected:
+    // We progressively build a list of vertices and faces with each
+    // add_obstacle call, reindexing as needed. Then we build a tree
+    // with all of them. Alternatively we could just build separate trees and combine them.
+    Eigen::MatrixXd obs_V0, obs_V1;
+    Eigen::MatrixXi obs_F;
+    std::vector<Eigen::AlignedBox<double,3> > obs_aabbs;
+    AABBTree<double,3> obs_tree;
+
+    Eigen::MatrixXd emb_V0, emb_V1; // copy of embedded vertices
+    const Eigen::MatrixXd *emb_barys; // barys of the embedded vtx
+    const Eigen::VectorXi *vtx_to_tet; // vertex to tet embedding
+    const Eigen::MatrixXi *tets; // tets that embed faces
+
+    struct CollisionPair {
+        int p; // point
+        int q; // face
+        Eigen::Vector3d barys; // barycoords of collision
+    };
+
+public:
+    // 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 int *faces,
+        int nf);
+
+    // Unlike set_obstacles, the pointers 
+    void set_embedded(
+        const Eigen::MatrixXd *emb_barys,
+        const Eigen::VectorXi *vtx_to_tet,
+        const Eigen::MatrixXi *tets){}
+
+    // Given a list of deformable vertices (the lattice)
+    // perform collision detection of the surface mesh against
+    // obstacles and possibly self.
+    void detect(const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1);
+
+    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;
+		std::vector<double> *l)
+    {
+        
+    }
 };
-
+/*
 class FloorCollider : public Collider {
 public:
-    void detect(const Eigen::MatrixXd *x);
+    virtual void detect(
+        int meshnum,
+        const Eigen::MatrixXd *x,
+        const Eigen::MatrixXi *faces);
     void jacobian(
         const Eigen::MatrixXd *x,
     	std::vector<Eigen::Triplet<double> > *trips_x,
@@ -36,7 +97,7 @@ public:
     	std::vector<Eigen::Triplet<double> > *trips_z,
 		std::vector<double> *l);
 };
-
+*/
 } // namespace admmpd
 
 #endif // ADMMPD_COLLISION_H_
diff --git a/extern/softbody/src/admmpd_lattice.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp
index d97ab9c7bc8..0fd012c34f3 100644
--- a/extern/softbody/src/admmpd_lattice.cpp
+++ b/extern/softbody/src/admmpd_embeddedmesh.cpp
@@ -1,7 +1,7 @@
 // Copyright Matt Overby 2020.
 // Distributed under the MIT License.
 
-#include "admmpd_lattice.h"
+#include "admmpd_embeddedmesh.h"
 #include "admmpd_math.h"
 #include "admmpd_bvh.h"
 #include "admmpd_bvh_traverse.h"
@@ -63,12 +63,20 @@ static void parallel_keep_tet(
 
 } // end parallel test if keep tet
 
-bool Lattice::generate(
-	const Eigen::MatrixXd &V,
-	const Eigen::MatrixXi &F,
-    Eigen::MatrixXd *X, // lattice vertices, n x 3
-    Eigen::MatrixXi *T) // lattice elements, m x 4
+bool EmbeddedMesh::generate(
+	const Eigen::MatrixXd &V, // embedded verts
+	const Eigen::MatrixXi &F, // embedded faces
+	EmbeddedMeshData *emb_mesh, // where embedding is stored
+	Eigen::MatrixXd *x_tets) // lattice vertices, n x 3
 {
+	if (emb_mesh==NULL)
+		return false;
+	if (x_tets==NULL)
+		return false;
+
+	emb_mesh->faces = F;
+	emb_mesh->x_rest = V;
+
 	AlignedBox<double,3> aabb;
 	int nv = V.rows();
 	for(int i=0; i<nv; ++i)
@@ -201,8 +209,6 @@ bool Lattice::generate(
 			else { it = tets.erase(it); }
 		}
 
-		printf("Lattice: removed %d tets\n", tet_idx-(int)tets.size());
-
 	} // end remove unnecessary tets
 
 	// Copy data into matrices and remove unreferenced
@@ -213,14 +219,14 @@ bool Lattice::generate(
 		int ntv0 = verts.size(); // original num verts
 		int ntv1 = refd_verts.size(); // reduced num verts
 		BLI_assert(ntv1 <= ntv0);
-		X->resize(ntv1,3);
+		x_tets->resize(ntv1,3);
 		int vtx_count = 0;
 		for (int i=0; i<ntv0; ++i)
 		{
 			if (refd_verts.count(i)>0)
 			{
 				for(int j=0; j<3; ++j){
-					X->operator()(vtx_count,j) = verts[i][j];
+					x_tets->operator()(vtx_count,j) = verts[i][j];
 				}
 				vtx_old_to_new[i] = vtx_count;
 				vtx_count++;
@@ -229,30 +235,27 @@ bool Lattice::generate(
 
 		// Copy tets to matrix data and update vertices
 		int nt = tets.size();
-		T->resize(nt,4);
+		emb_mesh->tets.resize(nt,4);
 		for(int i=0; i<nt; ++i){
 			for(int j=0; j<4; ++j){
 				int old_idx = tets[i][j];
 				BLI_assert(vtx_old_to_new.count(old_idx)>0);
-				T->operator()(i,j) = vtx_old_to_new[old_idx];
+				emb_mesh->tets(i,j) = vtx_old_to_new[old_idx];
 			}
 		}
 	}
 
 	// Now compute the baryweighting for embedded vertices
-	return compute_vtx_tet_mapping(
-		&V, &vtx_to_tet, &barys,
-		X, T);
+	return compute_embedding(
+		emb_mesh, &V, x_tets);
 
 } // end gen lattice
 
 typedef struct FindTetThreadData {
 	AABBTree<double,3> *tree;
-	const MatrixXd *pts;
-	VectorXi *pts_to_tet;
-	MatrixXd *barys;
-	const MatrixXd *tet_x;
-	const MatrixXi *tets;
+	EmbeddedMeshData *emb_mesh; // thread sets vtx_to_tet and barys
+	const Eigen::MatrixXd *x_embed;
+	const Eigen::MatrixXd *x_tets;
 } FindTetThreadData;
 
 static void parallel_point_in_tet(
@@ -261,43 +264,42 @@ static void parallel_point_in_tet(
 	const TaskParallelTLS *__restrict UNUSED(tls))
 {
 	FindTetThreadData *td = (FindTetThreadData*)userdata;
-	Vector3d pt = td->pts->row(i);
-	PointInTetMeshTraverse<double> traverser(pt, td->tet_x, td->tets);
+	Vector3d pt = td->x_embed->row(i);
+	PointInTetMeshTraverse<double> traverser(pt, td->x_tets, &td->emb_mesh->tets);
 	bool success = td->tree->traverse(traverser);
 	int tet_idx = traverser.output.prim;
 	if (success && tet_idx >= 0)
 	{
-		RowVector4i tet = td->tets->row(tet_idx);
+		RowVector4i tet = td->emb_mesh->tets.row(tet_idx);
 		Vector3d t[4] = {
-			td->tet_x->row(tet[0]),
-			td->tet_x->row(tet[1]),
-			td->tet_x->row(tet[2]),
-			td->tet_x->row(tet[3])
+			td->x_tets->row(tet[0]),
+			td->x_tets->row(tet[1]),
+			td->x_tets->row(tet[2]),
+			td->x_tets->row(tet[3])
 		};
-		td->pts_to_tet->operator[](i) = tet_idx;
+		td->emb_mesh->vtx_to_tet[i] = tet_idx;
 		Vector4d b = barycoords::point_tet(pt,t[0],t[1],t[2],t[3]);
-		td->barys->row(i) = b;
+		td->emb_mesh->barys.row(i) = b;
 	}
 } // end parallel lin solve
 
-bool Lattice::compute_vtx_tet_mapping(
-	const Eigen::MatrixXd *vtx_, // embedded vertices, p x 3
-	Eigen::VectorXi *vtx_to_tet_, // what tet vtx is embedded in, p x 1
-	Eigen::MatrixXd *barys_, // barycoords of the embedding, p x 4
-	const Eigen::MatrixXd *x_, // lattice vertices, n x 3
-	const Eigen::MatrixXi *tets_) // lattice elements, m x 4
+bool EmbeddedMesh::compute_embedding(
+	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
 {
-	if (!vtx_ || !vtx_to_tet_ || !barys_ || !x_ || !tets_)
-		return false;
+	BLI_assert(emb_mesh!=NULL);
+	BLI_assert(x_embed!=NULL);
+	BLI_assert(x_tets!=NULL);
 
-	int nv = vtx_->rows();
+	int nv = x_embed->rows();
 	if (nv==0)
 		return false;
 
-	barys_->resize(nv,4);
-	barys_->setOnes();
-	vtx_to_tet_->resize(nv);
-	int nt = tets_->rows();
+	emb_mesh->barys.resize(nv,4);
+	emb_mesh->barys.setOnes();
+	emb_mesh->vtx_to_tet.resize(nv);
+	int nt = emb_mesh->tets.rows();
 
 	// BVH tree for finding point-in-tet and computing
 	// barycoords for each embedded vertex
@@ -307,11 +309,10 @@ bool Lattice::compute_vtx_tet_mapping(
 	for (int i=0; i<nt; ++i)
 	{
 		tet_aabbs[i].setEmpty();
-		RowVector4i tet = tets_->row(i);
+		RowVector4i tet = emb_mesh->tets.row(i);
 		for (int j=0; j<4; ++j)
-		{
-			tet_aabbs[i].extend(x_->row(tet[j]).transpose());
-		}
+			tet_aabbs[i].extend(x_tets->row(tet[j]).transpose());
+
 		tet_aabbs[i].extend(tet_aabbs[i].min()-veta);
 		tet_aabbs[i].extend(tet_aabbs[i].max()+veta);
 	}
@@ -321,11 +322,9 @@ bool Lattice::compute_vtx_tet_mapping(
 
 	FindTetThreadData thread_data = {
 		.tree = &tree,
-		.pts = vtx_,
-		.pts_to_tet = vtx_to_tet_,
-		.barys = barys_,
-		.tet_x = x_,
-		.tets = tets_
+		.emb_mesh = emb_mesh,
+		.x_embed = x_embed,
+		.x_tets = x_tets
 	};
 	TaskParallelSettings settings;
 	BLI_parallel_range_settings_defaults(&settings);
@@ -335,7 +334,7 @@ bool Lattice::compute_vtx_tet_mapping(
 	const double eps = 1e-8;
 	for (int i=0; i<nv; ++i)
 	{
-		RowVector4d b = barys_->row(i);
+		RowVector4d b = emb_mesh->barys.row(i);
 		if (b.minCoeff() < -eps)
 		{
 			printf("**Lattice::generate Error: negative barycoords\n");
@@ -357,19 +356,19 @@ bool Lattice::compute_vtx_tet_mapping(
 
 } // end compute vtx to tet mapping
 
-Eigen::Vector3d Lattice::get_mapped_vertex(
-	int idx,
-	const Eigen::MatrixXd *x_or_v,
-	const Eigen::MatrixXi *tets )
+Eigen::Vector3d EmbeddedMesh::get_mapped_vertex(
+	const EmbeddedMeshData *emb_mesh,
+	const Eigen::MatrixXd *x_data,
+	int idx)
 {
-    int t_idx = vtx_to_tet[idx];
-    RowVector4i tet = tets->row(t_idx);
-    RowVector4d b = barys.row(idx);
+    int t_idx = emb_mesh->vtx_to_tet[idx];
+    RowVector4i tet = emb_mesh->tets.row(t_idx);
+    RowVector4d b = emb_mesh->barys.row(idx);
     return Vector3d(
-		x_or_v->row(tet[0]) * b[0] +
-		x_or_v->row(tet[1]) * b[1] +
-		x_or_v->row(tet[2]) * b[2] +
-		x_or_v->row(tet[3]) * b[3]);
+		x_data->row(tet[0]) * b[0] +
+		x_data->row(tet[1]) * b[1] +
+		x_data->row(tet[2]) * b[2] +
+		x_data->row(tet[3]) * b[3]);
 }
 
 } // namespace admmpd
diff --git a/extern/softbody/src/admmpd_embeddedmesh.h b/extern/softbody/src/admmpd_embeddedmesh.h
new file mode 100644
index 00000000000..a1abe02f904
--- /dev/null
+++ b/extern/softbody/src/admmpd_embeddedmesh.h
@@ -0,0 +1,39 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_LATTICE_H_
+#define ADMMPD_LATTICE_H_
+
+#include "admmpd_types.h"
+
+namespace admmpd {
+
+class EmbeddedMesh {
+public:
+    // Returns true on success
+    bool generate(
+        const Eigen::MatrixXd &V, // embedded verts
+        const Eigen::MatrixXi &F, // embedded faces
+        EmbeddedMeshData *emb_mesh, // where embedding is stored
+        Eigen::MatrixXd *x_tets); // lattice vertices, n x 3
+
+    // Returns the vtx mapped from x/v and tets
+    Eigen::Vector3d get_mapped_vertex(
+        const EmbeddedMeshData *emb_mesh,
+        const Eigen::MatrixXd *x_data,
+        int idx);
+
+protected:
+
+    // Returns true on success
+    // Computes the embedding data, like barycoords
+    bool compute_embedding(
+        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
+
+}; // class Lattice
+
+} // namespace admmpd
+
+#endif // ADMMPD_LATTICE_H_
diff --git a/extern/softbody/src/admmpd_lattice.h b/extern/softbody/src/admmpd_lattice.h
deleted file mode 100644
index 41454a1650c..00000000000
--- a/extern/softbody/src/admmpd_lattice.h
+++ /dev/null
@@ -1,44 +0,0 @@
-// Copyright Matt Overby 2020.
-// Distributed under the MIT License.
-
-#ifndef ADMMPD_LATTICE_H_
-#define ADMMPD_LATTICE_H_
-
-#include <Eigen/Dense>
-#include <vector>
-
-namespace admmpd {
-
-class Lattice {
-public:
-    Eigen::VectorXi vtx_to_tet; // what tet vtx is embedded in, p x 1
-    Eigen::MatrixXd barys; // barycoords of the embedding, p x 4
-
-    // Returns true on success
-    bool generate(
-        const Eigen::MatrixXd &V, // embedded verts
-        const Eigen::MatrixXi &F, // embedded faces
-        Eigen::MatrixXd *x, // lattice vertices, n x 3
-        Eigen::MatrixXi *tets); // lattice elements, m x 4
-
-    // Returns the vtx mapped from x/v and tets
-    Eigen::Vector3d get_mapped_vertex(
-        int idx,
-        const Eigen::MatrixXd *x_or_v,
-        const Eigen::MatrixXi *tets );
-
-protected:
-
-    // Returns true on success
-    bool compute_vtx_tet_mapping(
-        const Eigen::MatrixXd *vtx_, // embedded vertices, p x 3
-        Eigen::VectorXi *vtx_to_tet_, // what tet vtx is embedded in, p x 1
-        Eigen::MatrixXd *barys_, // barycoords of the embedding, p x 4
-        const Eigen::MatrixXd *x_, // lattice vertices, n x 3
-        const Eigen::MatrixXi *tets_); // lattice elements, m x 4
-
-}; // class Lattice
-
-} // namespace admmpd
-
-#endif // ADMMPD_LATTICE_H_
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index 03680425427..da766d43cc1 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -2,7 +2,6 @@
 // Distributed under the MIT License.
 
 #include "admmpd_solver.h"
-#include "admmpd_lattice.h"
 #include "admmpd_energy.h"
 #include "admmpd_collision.h"
 
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index e61ddfeb82e..229411ae340 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -4,75 +4,12 @@
 #ifndef ADMMPD_SOLVER_H_
 #define ADMMPD_SOLVER_H_
 
-#include <Eigen/Geometry>
-#include <Eigen/Sparse>
-#include <Eigen/SparseCholesky>
-#include <vector>
+#include "admmpd_types.h"
 
 namespace admmpd {
 
-struct Options {
-    double timestep_s; // TODO: Figure out delta time from blender api
-    int max_admm_iters;
-    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(20),
-        max_cg_iters(10),
-        mult_k(1.0),
-        min_res(1e-4),
-        density_kgm3(1100),
-        youngs(100000000),
-        poisson(0.399),
-        grav(0,0,-9.8)
-        {}
-};
-
-// TODO template type for float/double
-struct Data {
-    // Set from input
-    Eigen::MatrixXi tets; // elements t x 4
-    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::VectorXd m; // masses, n x 1
-    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)
-    template <typename T> using RowSparseMatrix = Eigen::SparseMatrix<T,Eigen::RowMajor>;
-    RowSparseMatrix<double> D; // 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)
-    double spring_k; // constraint stiffness
-	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > ldltA;
-    struct CGData { // Temporaries used in conjugate gradients
-        RowSparseMatrix<double> A[3]; // (M + D'W^2D) + k * Kt K
-        Eigen::MatrixXd b; // M xbar + DtW2(z-u) + Kt l
-        Eigen::MatrixXd r; // residual
-        Eigen::MatrixXd z;
-        Eigen::MatrixXd p;
-        Eigen::MatrixXd Ap; // A * p
-    } cgdata;
-    // 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
-	std::vector<double> weights; // per-energy weights
-};
-
 class Solver {
 public:
-
     // Initialies solver data. If a per-vertex
     // variable is resized it is initialized to zero.
     // Returns true on success
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
new file mode 100644
index 00000000000..48f3121f721
--- /dev/null
+++ b/extern/softbody/src/admmpd_types.h
@@ -0,0 +1,84 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_TYPES_H_
+#define ADMMPD_TYPES_H_
+
+#include <Eigen/Geometry>
+#include <Eigen/Sparse>
+#include <Eigen/SparseCholesky>
+#include <vector>
+
+// TODO template type for float/double
+
+namespace admmpd {
+
+struct Options {
+    double timestep_s; // TODO: Figure out delta time from blender api
+    int max_admm_iters;
+    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(20),
+        max_cg_iters(10),
+        mult_k(1.0),
+        min_res(1e-4),
+        density_kgm3(1100),
+        youngs(100000000),
+        poisson(0.399),
+        grav(0,0,-9.8)
+        {}
+};
+
+struct Data {
+    // Set from input
+    Eigen::MatrixXi tets; // elements t x 4
+    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::VectorXd m; // masses, n x 1
+    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)
+    template <typename T> using RowSparseMatrix = Eigen::SparseMatrix<T,Eigen::RowMajor>;
+    RowSparseMatrix<double> D; // 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)
+    double spring_k; // constraint stiffness
+	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > ldltA;
+    struct CGData { // Temporaries used in conjugate gradients
+        RowSparseMatrix<double> A[3]; // (M + D'W^2D) + k * Kt K
+        Eigen::MatrixXd b; // M xbar + DtW2(z-u) + Kt l
+        Eigen::MatrixXd r; // residual
+        Eigen::MatrixXd z;
+        Eigen::MatrixXd p;
+        Eigen::MatrixXd Ap; // A * p
+    } cgdata;
+    // 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
+	std::vector<double> weights; // per-energy weights
+};
+
+struct EmbeddedMeshData { // i.e. the lattice
+    Eigen::MatrixXd x_rest; // embedded verts at rest
+    Eigen::MatrixXi faces; // embedded faces
+    Eigen::MatrixXi tets; // lattice elements, m x 4
+    Eigen::VectorXi vtx_to_tet; // what tet vtx is embedded in, p x 1
+    Eigen::MatrixXd barys; // barycoords of the embedding, p x 4
+};
+
+} // namespace admmpd
+
+#endif // ADMMPD_TYPES_H_
diff --git a/intern/softbody/admmpd_api.cpp b/intern/softbody/admmpd_api.cpp
index cdcede4ca72..56971ed4262 100644
--- a/intern/softbody/admmpd_api.cpp
+++ b/intern/softbody/admmpd_api.cpp
@@ -22,8 +22,9 @@
  */
 
 #include "admmpd_api.h"
+#include "admmpd_types.h"
 #include "admmpd_solver.h"
-#include "admmpd_lattice.h"
+#include "admmpd_embeddedmesh.h"
 #include "tetgen_api.h"
 #include "DNA_mesh_types.h" // Mesh
 #include "DNA_meshdata_types.h" // MVert
@@ -38,7 +39,7 @@
 struct ADMMPDInternalData {
   admmpd::Options *options;
   admmpd::Data *data;
-  admmpd::Lattice *lattice;
+  admmpd::EmbeddedMeshData *embmesh;
   int in_totverts; // number of input verts
 };
 
@@ -55,8 +56,8 @@ void admmpd_dealloc(ADMMPDInterfaceData *iface)
         delete iface->data->options;
     if(iface->data->data)
         delete iface->data->data;
-    if(iface->data->lattice)
-        delete iface->data->lattice;
+    if(iface->data->embmesh)
+        delete iface->data->embmesh;
     delete iface->data;
   }
 
@@ -110,6 +111,7 @@ static int admmpd_init_with_lattice(
   ADMMPDInterfaceData *iface, float *in_verts, unsigned int *in_faces,
   Eigen::MatrixXd *V, Eigen::MatrixXi *T)
 {
+
   int nv = iface->mesh_totverts;
   Eigen::MatrixXd in_V(nv,3);
   for (int i=0; i<nv; ++i)
@@ -131,12 +133,14 @@ static int admmpd_init_with_lattice(
   }
 
   iface->totverts = 0;
-  bool success = iface->data->lattice->generate(in_V,in_F,V,T);
+  bool success = admmpd::EmbeddedMesh().generate(in_V,in_F,iface->data->embmesh,V);
   if (success)
   {
     iface->totverts = V->rows();
+    *T = iface->data->embmesh->tets;
     return 1;
   }
+
   return 0;
 }
 
@@ -158,7 +162,7 @@ int admmpd_init(ADMMPDInterfaceData *iface, float *in_verts, unsigned int *in_fa
   admmpd::Options *options = iface->data->options;
   iface->data->data = new admmpd::Data();
   admmpd::Data *data = iface->data->data;
-  iface->data->lattice = new admmpd::Lattice();
+  iface->data->embmesh = new admmpd::EmbeddedMeshData();
 
   // Generate tets and vertices
   Eigen::MatrixXd V;
@@ -218,6 +222,7 @@ void admmpd_copy_from_bodypoint(ADMMPDInterfaceData *iface, const BodyPoint *pts
 
 void admmpd_copy_to_bodypoint_and_object(ADMMPDInterfaceData *iface, BodyPoint *pts, float (*vertexCos)[3])
 {
+
   if (iface == NULL)
     return;
 
@@ -249,8 +254,9 @@ void admmpd_copy_to_bodypoint_and_object(ADMMPDInterfaceData *iface, BodyPoint *
   {
       for (int i=0; i<iface->mesh_totverts; ++i)
       {
-        Eigen::Vector3d xi = iface->data->lattice->get_mapped_vertex(
-          i, &iface->data->data->x, &iface->data->data->tets);
+        
+        Eigen::Vector3d xi = admmpd::EmbeddedMesh().get_mapped_vertex(
+          iface->data->embmesh, &iface->data->data->x, i);
         vertexCos[i][0] = xi[0];
         vertexCos[i][1] = xi[1];
         vertexCos[i][2] = xi[2];
diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index d97b76ba1e3..12f44013bea 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -3639,6 +3639,34 @@ static void init_admmpd_interface(Object *ob, float (*vertexCos)[3])
   admmpd_copy_to_bodypoint_and_object(sb->admmpd,sb->bpoint,NULL);
 }
 
+static void admmpd_update_collider(
+  Depsgraph *depsgraph,
+  Collection *collection,
+  Object *vertexowner)
+{
+  unsigned int numobjects;
+  Object **objects = BKE_collision_objects_create(
+      depsgraph,
+      vertexowner,
+      collection,
+      &numobjects,
+      eModifierType_Collision);
+
+  for (int i = 0; i < numobjects; ++i) {
+    Object *ob = objects[i];
+    if (ob->type == OB_MESH) {
+      if (ob->pd && ob->pd->deflect){
+        ccd_Mesh *ccdmesh = ccd_mesh_make(ob);
+        printf("tri num %d\n", ccdmesh->tri_num);
+        ccd_mesh_free(ccdmesh);
+      }
+      //ccd_build_deflector_hash_single(hash, ob);
+    }
+  }
+
+  BKE_collision_objects_free(objects);
+}
+
 /* simulates one step. framenr is in frames */
 void sbObjectStep_admmpd(
                   struct Depsgraph *depsgraph,
@@ -3767,7 +3795,7 @@ void sbObjectStep_admmpd(
   // Update ADMMPD interface variables from cache
   // and perform a solve.
   admmpd_copy_from_bodypoint(sb->admmpd,sb->bpoint);
-
+  admmpd_update_collider(depsgraph, sb->collision_group, ob);
   admmpd_solve(sb->admmpd);
   admmpd_copy_to_bodypoint_and_object(sb->admmpd,sb->bpoint,vertexCos);
   for (int i=0; i<me->totvert; ++i)
@@ -3782,6 +3810,7 @@ void sbObjectStep_admmpd(
 
 } // end step object with ADMMPD
 
+
 /* simulates one step. framenr is in frames */
 void sbObjectStep(struct Depsgraph *depsgraph,
                   Scene *scene,