moved collision detection back to inner loopsoftbody-stable-v3

11 files changed, 351 insertions, 177 deletions

diff --git a/extern/softbody/src/admmpd_bvh.cpp b/extern/softbody/src/admmpd_bvh.cpp
index 6ea7a676d27..afa16874062 100644
--- a/extern/softbody/src/admmpd_bvh.cpp
+++ b/extern/softbody/src/admmpd_bvh.cpp
@@ -12,35 +12,35 @@ namespace admmpd {
 template <typename T, int DIM>
 void AABBTree<T,DIM>::clear()
 {
-    root = std::make_shared<Node>();
+    m_root = std::make_shared<Node>();
 }
 
 template <typename T, int DIM>
 void AABBTree<T,DIM>::init(const std::vector<AABB> &leaves)
 {
-    root = std::make_shared<Node>();
+    m_root = std::make_shared<Node>();
     int np = leaves.size();
     if (np==0)
         return;
     std::vector<int> queue(np);
     std::iota(queue.begin(), queue.end(), 0);
-    create_children(root.get(), queue, leaves);
+    create_children(m_root.get(), queue, leaves);
 }
 
 template <typename T, int DIM>
 void AABBTree<T,DIM>::update(const std::vector<AABB> &leaves)
 {
-    if (!root || (int)leaves.size()==0)
+    if (!m_root || (int)leaves.size()==0)
         return;
-    update_children(root.get(), leaves);
+    update_children(m_root.get(), leaves);
 }
 
 template <typename T, int DIM>
 bool AABBTree<T,DIM>::traverse(Traverser<T,DIM> &traverser) const
 {
-    if (!root)
+    if (!m_root)
         return false;
-    return traverse_children(root.get(), traverser);
+    return traverse_children(m_root.get(), traverser);
 }
 
 // If we are traversing with function pointers, we'll just
@@ -71,12 +71,12 @@ bool AABBTree<T,DIM>::traverse(
     std::function<void(const AABB&, bool&, const AABB&, bool&, bool&)> t,
     std::function<bool(const AABB&, int)> s) const
 {
-    if (!root)
+    if (!m_root)
         return false;
     TraverserFromFunctionPtrs<T,DIM> traverser;
     traverser.t = t;
     traverser.s = s;
-    return traverse_children(root.get(), traverser);
+    return traverse_children(m_root.get(), traverser);
 }
 
 template <typename T, int DIM>
@@ -264,7 +264,7 @@ typename Octree<T,DIM>::Node* Octree<T,DIM>::create_children(
     const std::vector<AABB> &boxes)
 {
     BLI_assert((int)queue.size()>0);
-    BLI_assert((int)prim_boxes.size()>0);
+    BLI_assert((int)boxes.size()>0);
     BLI_assert(F != nullptr);
     BLI_assert(V != nullptr);
     BLI_assert(F->cols()==3);
diff --git a/extern/softbody/src/admmpd_bvh.h b/extern/softbody/src/admmpd_bvh.h
index 20b810ac7be..88ff5393dc4 100644
--- a/extern/softbody/src/admmpd_bvh.h
+++ b/extern/softbody/src/admmpd_bvh.h
@@ -58,9 +58,13 @@ public:
 		}
 	};
 
+	// Return ptr to the root node
+	// Becomes invalidated after init()
+	std::shared_ptr<Node> root() { return m_root; }
+
 protected:
 
-	std::shared_ptr<Node> root;
+	std::shared_ptr<Node> m_root;
 
 	void create_children(
 		Node *node,
diff --git a/extern/softbody/src/admmpd_bvh_traverse.cpp b/extern/softbody/src/admmpd_bvh_traverse.cpp
index 9ec4c281368..fc0a8370ae8 100644
--- a/extern/softbody/src/admmpd_bvh_traverse.cpp
+++ b/extern/softbody/src/admmpd_bvh_traverse.cpp
@@ -45,6 +45,7 @@ void RayClosestHit<T>::traverse(
 		(eps > 0 ? left_aabb.exteriorDistance(o) < eps : false);
 	go_right = geom::ray_aabb<T>(o,d,right_aabb,t_min,output.t_max) ||
 		(eps > 0 ? right_aabb.exteriorDistance(o) < eps : false);
+	(void)(go_left_first);
 }
 
 template <typename T>
@@ -109,6 +110,15 @@ bool PointInTetMeshTraverse<T>::stop_traversing(
 		return false;
 
 	RowVector4i t = prim_inds->row(prim);
+	int n_skip = skip_inds.size();
+	for (int i=0; i<n_skip; ++i)
+	{
+		if (skip_inds[i]==t[0]) return false;
+		if (skip_inds[i]==t[1]) return false;
+		if (skip_inds[i]==t[2]) return false;
+		if (skip_inds[i]==t[3]) return false;
+	}
+
 	VecType v[4] = {
 		prim_verts->row(t[0]),
 		prim_verts->row(t[1]),
@@ -229,6 +239,15 @@ bool NearestTriangleTraverse<T>::stop_traversing(const AABB &aabb, int prim)
 		return false;
 
 	RowVector3i tri = prim_inds->row(prim);
+
+	int n_skip = skip_inds.size();
+	for (int i=0; i<n_skip; ++i)
+	{
+		if (skip_inds[i]==tri[0]) return false;
+		if (skip_inds[i]==tri[1]) return false;
+		if (skip_inds[i]==tri[2]) return false;
+	}
+
 	VecType v[3] = {
 		prim_verts->row(tri[0]),
 		prim_verts->row(tri[1]),
diff --git a/extern/softbody/src/admmpd_bvh_traverse.h b/extern/softbody/src/admmpd_bvh_traverse.h
index dce83d07a20..c6cc93c0037 100644
--- a/extern/softbody/src/admmpd_bvh_traverse.h
+++ b/extern/softbody/src/admmpd_bvh_traverse.h
@@ -6,7 +6,7 @@
 
 #include <Eigen/Geometry>
 #include <vector>
-#include <BLI_math_geom.h>
+#include "BLI_math_geom.h"
 
 namespace admmpd {
 
@@ -90,6 +90,7 @@ protected:
 	VecType point;
 	const MatrixXType *prim_verts;
 	const Eigen::MatrixXi *prim_inds;
+	std::vector<int> skip_inds;
 
 public:
 	struct Output {
@@ -100,10 +101,12 @@ public:
 	PointInTetMeshTraverse(
 		const VecType &point_,
 		const MatrixXType *prim_verts_,
-		const Eigen::MatrixXi *prim_inds_) :
+		const Eigen::MatrixXi *prim_inds_,
+		const std::vector<int> &skip_inds_=std::vector<int>()) :
 		point(point_),
 		prim_verts(prim_verts_),
-		prim_inds(prim_inds_)
+		prim_inds(prim_inds_),
+		skip_inds(skip_inds_)
 		{}
 
 	void traverse(
@@ -165,6 +168,7 @@ protected:
 	VecType point;
 	const MatrixXType *prim_verts; // triangle mesh verts
 	const Eigen::MatrixXi *prim_inds; // triangle mesh inds
+	std::vector<int> skip_inds;
 
 public:
 	struct Output {
@@ -181,10 +185,12 @@ public:
 	NearestTriangleTraverse(
 		const VecType &point_,
 		const MatrixXType *prim_verts_,
-		const Eigen::MatrixXi *prim_inds_) :
+		const Eigen::MatrixXi *prim_inds_,
+		const std::vector<int> &skip_inds_=std::vector<int>()) :
 		point(point_),
 		prim_verts(prim_verts_),
-		prim_inds(prim_inds_)
+		prim_inds(prim_inds_),
+		skip_inds(skip_inds_)
 		{}
 
 	void traverse(
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index a05b27287cc..69e300e598e 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -30,6 +30,7 @@ void Collision::set_obstacles(
 	const unsigned int *faces,
 	int nf)
 {
+	(void)(v0);
 	if (nv==0 || nf==0)
 	{
 //		obsdata.mesh = admmpd::EmbeddedMesh();
@@ -60,29 +61,7 @@ void Collision::set_obstacles(
 	}
 
 	obsdata.tree.init(obsdata.leaves);
-/*
-  	if (!obsdata.mesh.generate(V,F,true,2))
-		return;
-
-	int nt = obsdata.mesh.lat_tets.rows();
-	if ((int)obsdata.tet_leaves.size() != nt)
-		obsdata.tet_leaves.resize(nt);
 
-	for (int i=0; i<nt; ++i)
-	{
-		AlignedBox<double,3> &box = obsdata.tet_leaves[i];
-		box.setEmpty();
-		RowVector4i t = obsdata.mesh.lat_tets.row(i);
-		box.extend(obsdata.mesh.lat_rest_x.row(t[0]).transpose());
-		box.extend(obsdata.mesh.lat_rest_x.row(t[1]).transpose());
-		box.extend(obsdata.mesh.lat_rest_x.row(t[2]).transpose());
-		box.extend(obsdata.mesh.lat_rest_x.row(t[3]).transpose());
-//		box.extend(box.min()-Vector3d::Ones()*settings.collision_eps);
-//		box.extend(box.max()+Vector3d::Ones()*settings.collision_eps);
-	}
-
-	obsdata.tet_tree.init(obsdata.tet_leaves);
-*/
 } // end add obstacle
 
 std::pair<bool,VFCollisionPair>
@@ -119,13 +98,13 @@ int EmbeddedMeshCollision::detect(
 		return 0;
 
 	// Do we even need to process collisions?
-	if (!this->settings.test_floor &&
-		!this->settings.self_collision &&
-		!obsdata.has_obs())
+	if (!this->settings.floor_collision &&
+		(!this->settings.obs_collision || !obsdata.has_obs()) &&
+		!this->settings.self_collision)
 		return 0;
 
-	// Updates the BVH of deforming mesh
-	update_bvh(x0,x1);
+	if (!tet_tree.root())
+		throw std::runtime_error("EmbeddedMeshCollision::Detect: No tree");
 
 	// We store the results of the collisions in a per-vertex buffer.
 	// This is a workaround so we can create them in threads.
@@ -136,7 +115,7 @@ int EmbeddedMeshCollision::detect(
 	//
 	// Thread data for detection
 	//
-	typedef struct DetectThreadData {
+	typedef struct {
 		const Collision::Settings *settings;
 		const Collision *collision;
 		const TetMeshData *tetmesh;
@@ -162,18 +141,10 @@ int EmbeddedMeshCollision::detect(
 
 		std::vector<VFCollisionPair> &vi_pairs = td->per_vertex_pairs->at(vi);
 		vi_pairs.clear();
-
-		int tet_idx = td->embmesh->emb_vtx_to_tet[vi];
-		RowVector4i tet = td->embmesh->lat_tets.row(tet_idx);
-		Vector4d bary = td->embmesh->emb_barys.row(vi);
-		Vector3d pt_t1 = 
-			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]);
+		Vector3d pt_t1 = td->embmesh->get_mapped_vertex(td->x1,vi);
 
 		// Special case, check if we are below the floor
-		if (td->settings->test_floor)
+		if (td->settings->floor_collision)
 		{
 			if (pt_t1[2] < td->settings->floor_z)
 			{
@@ -188,14 +159,28 @@ int EmbeddedMeshCollision::detect(
 			}
 		}
 
-		std::pair<bool,VFCollisionPair> pt_hit_obs =
-			td->collision->detect_against_obs(pt_t1,td->obsdata);
+		// Detect against obstacles
+		if (td->settings->obs_collision)
+		{
+			std::pair<bool,VFCollisionPair> pt_hit_obs =
+				td->collision->detect_against_obs(pt_t1,td->obsdata);
+			if (pt_hit_obs.first)
+			{
+				pt_hit_obs.second.p_idx = vi;
+				pt_hit_obs.second.p_is_obs = false;
+				vi_pairs.emplace_back(pt_hit_obs.second);
+			}
+		}
 
-		if (pt_hit_obs.first)
+		// Detect against self
+		if (td->settings->self_collision)
 		{
-			pt_hit_obs.second.p_idx = vi;
-			pt_hit_obs.second.p_is_obs = false;
-			vi_pairs.emplace_back(pt_hit_obs.second);
+			std::pair<bool,VFCollisionPair> pt_hit_self =
+				td->collision->detect_against_self(vi, pt_t1, td->x1);
+			if (pt_hit_self.first)
+			{
+				vi_pairs.emplace_back(pt_hit_self.second);
+			}
 		}
 
 	}; // end detect for a single embedded vertex
@@ -226,13 +211,138 @@ int EmbeddedMeshCollision::detect(
 	return vf_pairs.size();
 } // end detect
 
+void EmbeddedMeshCollision::init_bvh(
+	const Eigen::MatrixXd *x0,
+	const Eigen::MatrixXd *x1)
+{
+
+	(void)(x0);
+	int nt = mesh->lat_tets.rows();
+	if (nt==0)
+		throw std::runtime_error("EmbeddedMeshCollision::init_bvh: No tets");
+
+	if ((int)tet_boxes.size() != nt)
+		tet_boxes.resize(nt);
+
+	// Update leaf boxes
+	for (int i=0; i<nt; ++i)
+	{
+		RowVector4i tet = mesh->lat_tets.row(i);
+		tet_boxes[i].setEmpty();
+		tet_boxes[i].extend(x1->row(tet[0]).transpose());
+		tet_boxes[i].extend(x1->row(tet[1]).transpose());
+		tet_boxes[i].extend(x1->row(tet[2]).transpose());
+		tet_boxes[i].extend(x1->row(tet[3]).transpose());
+	}
+
+	tet_tree.init(tet_boxes);
+}
 
 void EmbeddedMeshCollision::update_bvh(
 	const Eigen::MatrixXd *x0,
 	const Eigen::MatrixXd *x1)
 {
-	(void)(x0);
-	(void)(x1);
+	if (!tet_tree.root())
+	{
+		init_bvh(x0,x1);
+		return;
+	}
+
+	int nt = mesh->lat_tets.rows();
+	if ((int)tet_boxes.size() != nt)
+		tet_boxes.resize(nt);
+
+	// Update leaf boxes
+	for (int i=0; i<nt; ++i)
+	{
+		RowVector4i tet = mesh->lat_tets.row(i);
+		tet_boxes[i].setEmpty();
+		tet_boxes[i].extend(x1->row(tet[0]).transpose());
+		tet_boxes[i].extend(x1->row(tet[1]).transpose());
+		tet_boxes[i].extend(x1->row(tet[2]).transpose());
+		tet_boxes[i].extend(x1->row(tet[3]).transpose());
+	}
+
+	tet_tree.update(tet_boxes);
+
+} // end update bvh
+
+void EmbeddedMeshCollision::update_constraints(
+    const Eigen::MatrixXd *x0,
+    const Eigen::MatrixXd *x1)
+{
+
+	(void)(x0); (void)(x1);
+//	BLI_assert(x != NULL);
+//	BLI_assert(x->cols() == 3);
+
+//	int np = vf_pairs.size();
+//	if (np==0)
+//		return;
+
+	//int nx = x->rows();
+//	d->reserve((int)d->size() + np);
+//	trips->reserve((int)trips->size() + np*3*4);
+
+//	for (int i=0; i<np; ++i)
+//	{
+//		const Vector2i &pair_idx = vf_pairs[i];
+//	}
+}
+
+// Self collisions
+std::pair<bool,VFCollisionPair>
+EmbeddedMeshCollision::detect_against_self(
+	int pt_idx,
+	const Eigen::Vector3d &pt,
+	const Eigen::MatrixXd *x) const
+{
+	std::pair<bool,VFCollisionPair> ret = 
+		std::make_pair(false, VFCollisionPair());
+
+	int self_tet_idx = mesh->emb_vtx_to_tet[pt_idx];
+	RowVector4i self_tet = mesh->lat_tets.row(self_tet_idx);
+	std::vector<int> skip_tet_inds = {self_tet[0],self_tet[1],self_tet[2],self_tet[3]};
+	PointInTetMeshTraverse<double> pt_in_tet(pt,x,&mesh->lat_tets,skip_tet_inds);
+	bool in_mesh = tet_tree.traverse(pt_in_tet);
+	if (!in_mesh)
+		return ret;
+
+	// Transform point to rest shape
+	int tet_idx = pt_in_tet.output.prim;
+	RowVector4i tet = mesh->lat_tets.row(tet_idx);
+	Vector4d barys = geom::point_tet_barys(pt,
+		x->row(tet[0]), x->row(tet[1]),
+		x->row(tet[2]), x->row(tet[3]));
+	Vector3d rest_pt =
+		barys[0]*mesh->lat_rest_x.row(tet[0])+
+		barys[1]*mesh->lat_rest_x.row(tet[1])+
+		barys[2]*mesh->lat_rest_x.row(tet[2])+
+		barys[3]*mesh->lat_rest_x.row(tet[3]);
+
+	// Find triangle surface projection that doesn't
+	// include the penetration vertex
+	std::vector<int> skip_tri_inds = {pt_idx};
+	NearestTriangleTraverse<double> nearest_tri(rest_pt,
+		&mesh->emb_rest_x,&mesh->emb_faces,skip_tri_inds);
+	mesh->emb_rest_tree.traverse(nearest_tri);
+
+	ret.first = true;
+	ret.second.p_idx = pt_idx;
+	ret.second.p_is_obs = false;
+	ret.second.q_idx = nearest_tri.output.prim;
+	ret.second.q_is_obs = false;
+	ret.second.q_pt = nearest_tri.output.pt_on_tri;
+
+	// Compute barycoords of projection
+	RowVector3i f = mesh->emb_faces.row(nearest_tri.output.prim);
+	Vector3d v3[3] = {
+		mesh->emb_rest_x.row(f[0]),
+		mesh->emb_rest_x.row(f[1]),
+		mesh->emb_rest_x.row(f[2])};
+	ret.second.q_bary = geom::point_triangle_barys<double>(
+		nearest_tri.output.pt_on_tri, v3[0], v3[1], v3[2]);
+	return ret;
 }
 
 
@@ -341,11 +451,6 @@ void EmbeddedMeshCollision::linearize(
 					q_tris[0], q_tris[1], q_tris[2]);
 			}
 
-			// Is the constraint active?
-			bool active = (p_pt-pair.q_pt).dot(q_n) <= 0.0;
-			if (!active)
-				continue;
-
 			// Get the four deforming verts that embed
 			// the surface vertices, and add constraints on those.
 			RowVector4d bary = mesh->emb_barys.row(emb_p_idx);
@@ -359,9 +464,51 @@ void EmbeddedMeshCollision::linearize(
 				trips->emplace_back(c_idx, tet[j]*3+1, bary[j]*q_n[1]);
 				trips->emplace_back(c_idx, tet[j]*3+2, bary[j]*q_n[2]);
 			}
-			continue;
-		}
 
+		} // end q is obs
+		else
+		{
+			int c_idx = d->size();
+			d->emplace_back(0);
+
+			// Compute the normal in the deformed space
+			RowVector3i q_face = mesh->emb_faces.row(pair.q_idx);
+			Vector3d q_v0 = mesh->get_mapped_vertex(x,q_face[0]);
+			Vector3d q_v1 = mesh->get_mapped_vertex(x,q_face[1]);
+			Vector3d q_v2 = mesh->get_mapped_vertex(x,q_face[2]);
+			Vector3d q_n = (q_v1-q_v0).cross(q_v2-q_v0);
+			q_n.normalize();
+
+			// The penetrating vertex:
+			{
+				int tet_idx = mesh->emb_vtx_to_tet[emb_p_idx];
+				RowVector4d bary = mesh->emb_barys.row(emb_p_idx);
+				RowVector4i tet = mesh->lat_tets.row(tet_idx);
+				for (int j=0; j<4; ++j)
+				{
+					trips->emplace_back(c_idx, tet[j]*3+0, bary[j]*q_n[0]);
+					trips->emplace_back(c_idx, tet[j]*3+1, bary[j]*q_n[1]);
+					trips->emplace_back(c_idx, tet[j]*3+2, bary[j]*q_n[2]);
+				}
+			}
+
+			// The intersected face:
+			for (int j=0; j<3; ++j)
+			{
+				int v_idx = q_face[j];
+				RowVector4d bary = mesh->emb_barys.row(v_idx);
+				int tet_idx = mesh->emb_vtx_to_tet[v_idx];
+				RowVector4i tet = mesh->lat_tets.row(tet_idx);
+				for (int k=0; k<4; ++k)
+				{
+					trips->emplace_back(c_idx, tet[k]*3+0, -pair.q_bary[j]*bary[k]*q_n[0]);
+					trips->emplace_back(c_idx, tet[k]*3+1, -pair.q_bary[j]*bary[k]*q_n[1]);
+					trips->emplace_back(c_idx, tet[k]*3+2, -pair.q_bary[j]*bary[k]*q_n[2]);
+				}
+			}
+
+		} // end q is obs
+	
 	} // end loop pairs
 
 } // end jacobian
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index 3382d5aa9a5..4d917ccfc72 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -27,12 +27,14 @@ public:
     struct Settings {
         double collision_eps;
         double floor_z;
-        bool test_floor;
+        bool floor_collision;
+        bool obs_collision;
         bool self_collision;
         Settings() :
             collision_eps(1e-10),
             floor_z(-std::numeric_limits<double>::max()),
-            test_floor(true),
+            floor_collision(true),
+            obs_collision(true),
             self_collision(false)
             {}
     } settings;
@@ -47,6 +49,22 @@ public:
 
     virtual ~Collision() {}
 
+    // Resets the BVH
+    virtual void init_bvh(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1) = 0;
+
+    // Updates the BVH without sorting
+    virtual void update_bvh(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1) = 0;
+
+    // Updates the active set of constraints,
+    // but does not detect new ones.
+    virtual void update_constraints(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1) = 0;
+
     // Performs collision detection.
     // Returns the number of active constraints.
     virtual int detect(
@@ -80,22 +98,16 @@ public:
 
     // Given a point and a mesh, perform
     // discrete collision detection.
-    std::pair<bool,VFCollisionPair>
+    virtual std::pair<bool,VFCollisionPair>
     detect_against_obs(
         const Eigen::Vector3d &pt,
         const ObstacleData *obs) const;
 
-    // Given a point and a mesh, perform
-    // discrete collision detection.
-//    std::pair<bool,VFCollisionPair>
-//    detect_point_against_mesh(
-//        int pt_idx,
-//        bool pt_is_obs,
-//        const Eigen::Vector3d &pt,
-//        bool mesh_is_obs,
-//        const EmbeddedMesh *emb_mesh,
-//        const Eigen::MatrixXd *mesh_tets_x,
-//        const AABBTree<double,3> *mesh_tets_tree) const;
+    virtual std::pair<bool,VFCollisionPair>
+    detect_against_self(
+        int pt_idx,
+        const Eigen::Vector3d &pt,
+        const Eigen::MatrixXd *x) const = 0;
 };
 
 // Collision detection against multiple meshes
@@ -118,68 +130,37 @@ public:
     	std::vector<Eigen::Triplet<double> > *trips,
 		std::vector<double> *d);
 
-protected:
-    // A ptr to the embedded mesh data
-    const EmbeddedMesh *mesh;
-
-    // Pairs are compute on detect
-    std::vector<Eigen::Vector2i> vf_pairs; // index into per_vertex_pairs
-    std::vector<std::vector<VFCollisionPair> > per_vertex_pairs;
+    void init_bvh(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1);
 
     // Updates the tetmesh BVH for self collisions.
-    // Called by detect()
-    // TODO
     void update_bvh(
         const Eigen::MatrixXd *x0,
         const Eigen::MatrixXd *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(
+    // Updates the active set of constraints,
+    // but does not detect new ones.
+    void update_constraints(
         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;
+    // A ptr to the embedded mesh data
+    const EmbeddedMesh *mesh;
+    std::vector<Eigen::AlignedBox<double,3> > tet_boxes;
+    AABBTree<double,3> tet_tree;
 
     // Pairs are compute on detect
-    std::vector<VFCollisionPair> vf_pairs;
+    std::vector<Eigen::Vector2i> vf_pairs; // index into per_vertex_pairs
+    std::vector<std::vector<VFCollisionPair> > per_vertex_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); }
+    std::pair<bool,VFCollisionPair>
+    detect_against_self(
+        int pt_idx,
+        const Eigen::Vector3d &pt,
+        const Eigen::MatrixXd *x) const;
 };
-*/
 
 } // namespace admmpd
 
diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp
index 7e677b42455..e939114faeb 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.cpp
+++ b/extern/softbody/src/admmpd_embeddedmesh.cpp
@@ -175,11 +175,10 @@ bool EmbeddedMesh::generate(
 		face_boxes[i].extend(V.row(F(i,2)).transpose());
 	}
 
-	AABBTree<double,3> face_tree;
-	face_tree.init(face_boxes);
+	emb_rest_tree.init(face_boxes);
 
 	Octree<double,3>::Node *root = octree.root().get();
-	gather_octree_tets(root,&V,&F,&face_tree,data.verts,data.tets);
+	gather_octree_tets(root,&V,&F,&emb_rest_tree,data.verts,data.tets);
 	merge_close_vertices(&data);
 
 	int nv = data.verts.size();
@@ -198,6 +197,11 @@ bool EmbeddedMesh::generate(
 		}
 	}
 
+	// Now compute the baryweighting for embedded vertices
+	bool embed_success = compute_embedding();
+
+	if (!emb_rest_tree.root())
+		throw std::runtime_error("EmbeddedMesh::generate Error: Failed to create tree");
 	if (lat_rest_x.rows()==0)
 		throw std::runtime_error("EmbeddedMesh::generate Error: Failed to create verts");
 	if (lat_tets.rows()==0)
@@ -206,13 +210,12 @@ bool EmbeddedMesh::generate(
 		throw std::runtime_error("EmbeddedMesh::generate Error: Did not set faces");
 	if (emb_rest_x.rows()==0)
 		throw std::runtime_error("EmbeddedMesh::generate Error: Did not set verts");
-
-	// Now compute the baryweighting for embedded vertices
-	bool embed_success = compute_embedding();
+	if (!embed_success)
+		throw std::runtime_error("EmbeddedMesh::generate Error: Failed embedding");
 
 	// Export the mesh for funsies
-	std::ofstream of("v_lattice.txt"); of << lat_rest_x; of.close();
-	std::ofstream of2("t_lattice.txt"); of2 << lat_tets; of2.close();
+//	std::ofstream of("v_lattice.txt"); of << lat_rest_x; of.close();
+//	std::ofstream of2("t_lattice.txt"); of2 << lat_tets; of2.close();
 
 	return embed_success;
 
@@ -270,8 +273,9 @@ typedef struct FindTetThreadData {
 static void parallel_point_in_tet(
 	void *__restrict userdata,
 	const int i,
-	const TaskParallelTLS *__restrict UNUSED(tls))
+	const TaskParallelTLS *__restrict tls)
 {
+	(void)(tls);
 	FindTetThreadData *td = (FindTetThreadData*)userdata;
 	Vector3d pt = td->emb_mesh->emb_rest_x.row(i);
 	PointInTetMeshTraverse<double> traverser(
diff --git a/extern/softbody/src/admmpd_linsolve.cpp b/extern/softbody/src/admmpd_linsolve.cpp
index b91d3041913..a4ff75ab77e 100644
--- a/extern/softbody/src/admmpd_linsolve.cpp
+++ b/extern/softbody/src/admmpd_linsolve.cpp
@@ -140,8 +140,9 @@ void ConjugateGradients::solve_Ax_b(
 	auto parallel_lin_solve = [](
 		void *__restrict userdata,
 		const int i,
-		const TaskParallelTLS *__restrict UNUSED(tls))->void
+		const TaskParallelTLS *__restrict tls)->void
 	{
+		(void)(tls);
 		LinSolveThreadData *td = (LinSolveThreadData*)userdata;
 		int nx = td->ls_x->rows()/3;
 		VectorXd b(nx);
@@ -189,8 +190,9 @@ void GaussSeidel::solve(
 
 	// Inner iteration of Gauss-Seidel
 	auto parallel_gs_sweep = [](void *__restrict userdata, const int i_,
-		const TaskParallelTLS *__restrict UNUSED(tls)) -> void
+		const TaskParallelTLS *__restrict tls) -> void
 	{
+		(void)(tls);
 		GaussSeidelThreadData *td = (GaussSeidelThreadData*)userdata;
 		int idx = td->colors->at(td->color)[i_];
 		double omega = td->options->gs_omega;
@@ -421,8 +423,9 @@ void GaussSeidel::compute_colors(
 	// Graph color initialization
 	//
 	auto init_graph = [](void *__restrict userdata, const int i,
-		const TaskParallelTLS *__restrict UNUSED(tls)) -> void
+		const TaskParallelTLS *__restrict tls) -> void
 	{
+		(void)(tls);
 		GraphColorThreadData *td = (GraphColorThreadData*)userdata;
 		for( int j=0; j<td->init_palette_size; ++j ) // init colors
 			td->palette->at(i).insert(j); 
@@ -440,8 +443,9 @@ void GaussSeidel::compute_colors(
 
 		// Generate a random color
 		auto generate_color = [](void *__restrict userdata, const int i,
-			const TaskParallelTLS *__restrict UNUSED(tls)) -> void
+			const TaskParallelTLS *__restrict tls) -> void
 		{
+			(void)(tls);
 			GraphColorThreadData *td = (GraphColorThreadData*)userdata;
 			int idx = td->node_queue->at(i);
 			if (td->palette->at(idx).size()<2) // Feed the hungry
@@ -453,8 +457,9 @@ void GaussSeidel::compute_colors(
 
 		// Detect conflicts
 		auto detect_conflicts = [](void *__restrict userdata, const int i,
-			const TaskParallelTLS *__restrict UNUSED(tls)) -> void
+			const TaskParallelTLS *__restrict tls) -> void
 		{
+			(void)(tls);
 			GraphColorThreadData *td = (GraphColorThreadData*)userdata;
 			int idx = td->node_queue->at(i);
 			int curr_c = td->node_colors->at(idx);
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index e322970f8b5..9441ee6f78e 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -19,11 +19,6 @@
 namespace admmpd {
 using namespace Eigen;
 
-struct ThreadData {
-	const Options *options;
-	SolverData *data;
-};
-
 bool Solver::init(
     const Eigen::MatrixXd &V,
 	const Eigen::MatrixXi &T,
@@ -70,9 +65,6 @@ int Solver::solve(
 	// the variables are sized correctly.
 	init_solve(options,data,collision,pin);
 
-	// Perform collision detection and linearization
-	linearize_collision_constraints(options,data,collision);
-
 	// Begin solver loop
 	int iters = 0;
 	for (; iters < options->max_admm_iters; ++iters)
@@ -80,6 +72,9 @@ int Solver::solve(
 		// Update ADMM z/u
 		solve_local_step(options,data);
 
+		// Collision detection and linearization
+		update_collisions(options,data,collision);
+
 		// Solve Ax=b s.t. Cx=d
 		ConjugateGradients().solve(options,data,collision);
 		//GaussSeidel().solve(options,data,collision);
@@ -149,6 +144,11 @@ void Solver::init_solve(
 		}
 	}
 
+	if (collision)
+	{
+		collision->init_bvh(&data->x_start, &data->x);
+	}
+
 	// ADMM variables
 	data->Dx.noalias() = data->D * data->x;
 	data->z = data->Dx;
@@ -156,41 +156,49 @@ void Solver::init_solve(
 
 } // end init solve
 
-static void parallel_zu_update(
-	void *__restrict userdata,
-	const int i,
-	const TaskParallelTLS *__restrict UNUSED(tls))
-{
-	ThreadData *td = (ThreadData*)userdata;
-	Lame lame;
-	lame.set_from_youngs_poisson(td->options->youngs,td->options->poisson);
-	EnergyTerm().update(
-		td->data->indices[i][0],
-		lame,
-		td->data->rest_volumes[i],
-		td->data->weights[i],
-		&td->data->x,
-		&td->data->Dx,
-		&td->data->z,
-		&td->data->u );
-} // end parallel zu update
-
 void Solver::solve_local_step(
 	const Options *options,
 	SolverData *data)
 {
 	BLI_assert(data != NULL);
 	BLI_assert(options != NULL);
+
+	struct LocalStepThreadData {
+		const Options *options;
+		SolverData *data;
+	};
+
+	auto parallel_zu_update = [](
+		void *__restrict userdata,
+		const int i,
+		const TaskParallelTLS *__restrict tls)->void
+	{
+		(void)(tls);
+		LocalStepThreadData *td = (LocalStepThreadData*)userdata;
+		Lame lame;
+		lame.set_from_youngs_poisson(td->options->youngs,td->options->poisson);
+		EnergyTerm().update(
+			td->data->indices[i][0],
+			lame,
+			td->data->rest_volumes[i],
+			td->data->weights[i],
+			&td->data->x,
+			&td->data->Dx,
+			&td->data->z,
+			&td->data->u );
+	}; // end parallel zu update
+
 	data->Dx.noalias() = data->D * data->x;
 	int ne = data->indices.size();
 	BLI_assert(ne > 0);
-  	ThreadData thread_data = {.options=options, .data = data};
+  	LocalStepThreadData thread_data = {.options=options, .data = data};
 	TaskParallelSettings settings;
 	BLI_parallel_range_settings_defaults(&settings);
 	BLI_task_parallel_range(0, ne, &thread_data, parallel_zu_update, &settings);
+
 } // end local step
 
-void Solver::linearize_collision_constraints(
+void Solver::update_collisions(
 	const Options *options,
 	SolverData *data,
 	Collision *collision)
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index 584f54be43c..64433dbd263 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -34,7 +34,7 @@ public:
 
 protected:
 
-    void linearize_collision_constraints(
+    void update_collisions(
         const Options *options,
         SolverData *data,
         Collision *collision);
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index c2ae0e67c88..3133773a478 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -28,7 +28,7 @@ struct Options {
     double poisson; // Poisson ratio // TODO variable per-tet
     Eigen::Vector3d grav;
     Options() :
-        timestep_s(1.0/100.0),
+        timestep_s(1.0/24.0),
         max_admm_iters(30),
         max_cg_iters(10),
         max_gs_iters(100),
@@ -37,7 +37,7 @@ struct Options {
         mult_pk(0.01),
         min_res(1e-8),
         youngs(1000000),
-        poisson(0.399),
+        poisson(0.299),
         grav(0,0,-9.8)
         {}
 };