collision working a little better now

10 files changed, 474 insertions, 356 deletions

diff --git a/extern/softbody/src/admmpd_bvh_traverse.cpp b/extern/softbody/src/admmpd_bvh_traverse.cpp
index 7def5340085..8fd4b1118f6 100644
--- a/extern/softbody/src/admmpd_bvh_traverse.cpp
+++ b/extern/softbody/src/admmpd_bvh_traverse.cpp
@@ -139,6 +139,8 @@ PointInTriangleMeshTraverse<T>::PointInTriangleMeshTraverse(
 	prim_verts(prim_verts_),
 	prim_inds(prim_inds_)
 {
+	//dir = VecType::Random();
+
 	BLI_assert(prim_verts->rows()>=0);
 	BLI_assert(prim_inds->rows()>=0);
 	BLI_assert(prim_inds->cols()==3);
@@ -148,6 +150,7 @@ PointInTriangleMeshTraverse<T>::PointInTriangleMeshTraverse(
 		o[i] = (float)point[i];
 		d[i] = (float)dir[i];
 	}
+	isect_ray_tri_watertight_v3_precalc(&isect_precalc, d);
 }
 
 template <typename T>
@@ -156,31 +159,23 @@ void PointInTriangleMeshTraverse<T>::traverse(
 	const AABB &right_aabb, bool &go_right,
 	bool &go_left_first )
 {
-	float tmin = 0;
-	float tmax = std::numeric_limits<float>::max();
-	float l_bb_min[3] = { (float)left_aabb.min()[0], (float)left_aabb.min()[1], (float)left_aabb.min()[2] };
-	float l_bb_max[3] = { (float)left_aabb.max()[0], (float)left_aabb.max()[1], (float)left_aabb.max()[2] };
-	go_left = isect_ray_aabb_v3_simple(o,d,l_bb_min,l_bb_max,&tmin,&tmax);
-	tmin = 0;
-	tmax = std::numeric_limits<float>::max();
-	float r_bb_min[3] = { (float)right_aabb.min()[0], (float)right_aabb.min()[1], (float)right_aabb.min()[2] };
-	float r_bb_max[3] = { (float)right_aabb.max()[0], (float)right_aabb.max()[1], (float)right_aabb.max()[2] };
-	go_right = isect_ray_aabb_v3_simple(o,d,r_bb_min,r_bb_max,&tmin,&tmax);
+	const T t_min = 0;
+	const T t_max = std::numeric_limits<T>::max();
+	go_left = geom::ray_aabb<T>(point,dir,left_aabb,t_min,t_max);
+	go_right = geom::ray_aabb<T>(point,dir,right_aabb,t_min,t_max);
+	go_left_first = go_left;
 } // end point in mesh traverse
 
 template <typename T>
 bool PointInTriangleMeshTraverse<T>::stop_traversing(
 		const AABB &aabb, int prim )
 {
+	const T t_min = 0;
+	T t_max = std::numeric_limits<T>::max();
+
 	// Check if the tet box doesn't intersect the triangle box
-	{
-		float tmin = 0;
-		float tmax = std::numeric_limits<float>::max();
-		float bb_min[3] = { (float)aabb.min()[0], (float)aabb.min()[1], (float)aabb.min()[2] };
-		float bb_max[3] = { (float)aabb.max()[0], (float)aabb.max()[1], (float)aabb.max()[2] };
-		if (!isect_ray_aabb_v3_simple(o,d,bb_min,bb_max,&tmin,&tmax))
-			return false;
-	}
+	if (!geom::ray_aabb<T>(point,dir,aabb,t_min,t_max))
+		return false;
 
 	// Get the vertices of the face in float arrays
 	// to interface with Blender kernels.
@@ -201,8 +196,7 @@ bool PointInTriangleMeshTraverse<T>::stop_traversing(
 	// then record if it was a ray-hit.
 	float lambda = 0;
 	float uv[2] = {0,0};
-	bool hit = isect_ray_tri_watertight_v3_simple(
-		o, d, q0, q1, q2, &lambda, uv);
+	bool hit = isect_ray_tri_watertight_v3(o, &isect_precalc, q0, q1, q2, &lambda, uv);
 
 	if (hit)
 		output.hits.emplace_back(std::make_pair(prim,lambda));
@@ -235,26 +229,18 @@ bool NearestTriangleTraverse<T>::stop_traversing(const AABB &aabb, int prim)
 	if (b_dist > output.dist)
 		return false;
 
-	float p[3] = { (float)point[0], (float)point[1], (float)point[2] };
-	float r[3] = { p[0], p[1], p[2] };
 	RowVector3i tri = prim_inds->row(prim);
-	float t1[3], t2[3], t3[3];
-	for (int i=0; i<3; ++i)
-	{
-		t1[i] = (float)prim_verts->operator()(tri[0],i);
-		t2[i] = (float)prim_verts->operator()(tri[1],i);
-		t3[i] = (float)prim_verts->operator()(tri[2],i);
-	}
-
-	// I was hoping there would be kernels that are a bit faster
-	// to get point-triangle distance, but I guess this does what I need.
-	closest_on_tri_to_point_v3(r, p, t1, t2, t3);
-	VecType pt_on_tri((T)r[0], (T)r[1], (T)r[2]);
-	double d2 = (point-pt_on_tri).norm();
-	if (d2 < output.dist)
+	VecType v[3] = {
+		prim_verts->row(tri[0]),
+		prim_verts->row(tri[1]),
+		prim_verts->row(tri[2])
+	};
+	VecType pt_on_tri = geom::point_on_triangle<T>(point,v[0],v[1],v[2]);
+	double dist = (point-pt_on_tri).norm();
+	if (dist < output.dist)
 	{
 		output.prim = prim;
-		output.dist = d2;
+		output.dist = dist;
 		output.pt_on_tri = pt_on_tri;		
 	}
 
diff --git a/extern/softbody/src/admmpd_bvh_traverse.h b/extern/softbody/src/admmpd_bvh_traverse.h
index 29eebd0163d..ecbb85f3277 100644
--- a/extern/softbody/src/admmpd_bvh_traverse.h
+++ b/extern/softbody/src/admmpd_bvh_traverse.h
@@ -6,6 +6,7 @@
 
 #include <Eigen/Geometry>
 #include <vector>
+#include <BLI_math_geom.h>
 
 namespace admmpd {
 
@@ -129,10 +130,11 @@ protected:
 	const MatrixXType *prim_verts; // triangle mesh verts
 	const Eigen::MatrixXi *prim_inds; // triangle mesh inds
 	float o[3], d[3]; // pt and dir casted to float for Blender kernels
+	struct IsectRayPrecalc isect_precalc;
 
 public:
 	struct Output {
-		std::vector< std::pair<int,double> > hits; // [prim,t]
+		std::vector< std::pair<int,T> > hits; // [prim,t]
 		int num_hits() const { return hits.size(); }
 	} output;
 
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 892a599264d..5e14bc3a756 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -20,7 +20,7 @@ VFCollisionPair::VFCollisionPair() :
     p_is_obs(0), // 0 or 1
     q_idx(-1), // face
     q_is_obs(0), // 0 or 1
-	pt_on_q(0,0,0)
+	q_bary(0,0,0)
 	{}
 
 void Collision::set_obstacles(
@@ -30,125 +30,86 @@ void Collision::set_obstacles(
 	const unsigned int *faces,
 	int nf)
 {
-	if (obsdata.V0.rows() != nv)
-		obsdata.V0.resize(nv,3);
-
-	if (obsdata.V1.rows() != nv)
-		obsdata.V1.resize(nv,3);
+	if (nv==0 || nf==0)
+	{
+//		obsdata.mesh = admmpd::EmbeddedMesh();
+		return;
+	}
 
+	if (obsdata.V.rows() != nv)
+		obsdata.V.resize(nv,3);
 	for (int i=0; i<nv; ++i)
 	{
 		for (int j=0; j<3; ++j)
-		{
-			obsdata.V0(i,j) = v0[i*3+j];
-			obsdata.V1(i,j) = v1[i*3+j];
-		}
+			obsdata.V(i,j) = v1[i*3+j];
 	}
 
+	if ((int)obsdata.leaves.size() != nf)
+		obsdata.leaves.resize(nf);
 	if (obsdata.F.rows() != nf)
-	{
 		obsdata.F.resize(nf,3);
-		obsdata.aabbs.resize(nf);
-	}
-
-	Vector3d v_eta = Vector3d::Ones()*settings.collision_eps;
 	for (int i=0; i<nf; ++i)
 	{
-		obsdata.aabbs[i].setEmpty();
+		obsdata.leaves[i].setEmpty();
 		for (int j=0; j<3; ++j)
 		{
-			int fj = faces[i*3+j];
-			obsdata.F(i,j) = fj;
-			obsdata.aabbs[i].extend(obsdata.V0.row(fj).transpose());
-			obsdata.aabbs[i].extend(obsdata.V1.row(fj).transpose());
+			obsdata.F(i,j) = faces[i*3+j];
+			Vector3d vi = obsdata.V.row(obsdata.F(i,j)).transpose();
+			obsdata.leaves[i].extend(vi);
 		}
-		obsdata.aabbs[i].extend(obsdata.aabbs[i].min()-v_eta);
-		obsdata.aabbs[i].extend(obsdata.aabbs[i].max()+v_eta);
 	}
 
-	obsdata.tree.init(obsdata.aabbs);
+	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>
-Collision::detect_point_against_mesh(
-        int pt_idx,
-        bool pt_is_obs,
-        const Eigen::Vector3d &pt_t0,
-        const Eigen::Vector3d &pt_t1,
-        bool mesh_is_obs,
-        const Eigen::MatrixXd *mesh_x,
-        const Eigen::MatrixXi *mesh_tris,
-        const AABBTree<double,3> *mesh_tree) const
+Collision::detect_against_obs(
+        const Eigen::Vector3d &pt,
+        const ObstacleData *obs) const
 {
 	std::pair<bool,VFCollisionPair> ret = 
 		std::make_pair(false, VFCollisionPair());
 
-	// Any faces to detect against?
-	if (mesh_tris->rows()==0)
-		return ret;	
+	if (!obs->has_obs())
+		return ret;
 
-/*
-	VFCollisionPair &pair = ret.second;
-	pair.p_idx = pt_idx;
-	pair.p_is_obs = false;
-	pair.q_idx = 0;
-	pair.q_is_obs = 1;
-	double max_z = mesh_x->col(2).maxCoeff();
-	pair.pt_on_q = Vector3d(pt_t1[0],pt_t1[1],max_z);
-*/
-	return ret;
-/*
-	// Use a ray that is pos at t0 to t1
-	// with t_max being the displacment. If not moving,
-	// set the "eps" variable used by traversal which
-	// does point-triangle distance, and considers it a hit
-	// if the distance is less than the eps.
-	Vector3d dx = (pt_t1-pt_t0);
-	double t_max = dx.norm();
-	double eps = -1; // used as point-triangle distance query
-	//if (t_max < settings.collision_eps)
-	if (false)
-	{
-		dx = Vector3d(0,0,1);
-		t_max = settings.collision_eps;
-		eps = settings.collision_eps;
-	}
-	dx.normalize();
-
-	// Traverse the BVH
-	RayClosestHit<double> ray_hit_mesh(
-		pt_t0, dx,
-		mesh_x, mesh_tris,
-		eps, 0, t_max);
-	mesh_tree->traverse(ray_hit_mesh);
-
-//	if (pt_idx==0)
-//	{
-//		std::cout << "\n\nV0 (z): " << pt_t0[2] << std::endl;
-//		std::cout << "dir: " << dx.transpose() << std::endl;
-//		std::cout << ray_hit_mesh.output.prim << std::endl;
-//		if(ray_hit_mesh.output.prim >= 0)
-//			throw std::runtime_error("YAY HIT");
-//	}
-
-	if (ray_hit_mesh.output.prim < 0)
+	PointInTriangleMeshTraverse<double> pt_in_mesh(pt,&obs->V,&obs->F);
+	obs->tree.traverse(pt_in_mesh);
+	if (pt_in_mesh.output.num_hits()%2==0)
 		return ret;
 
+	NearestTriangleTraverse<double> nearest_tri(pt,&obs->V,&obs->F);
+	obs->tree.traverse(nearest_tri);
+
 	ret.first = true;
-	VFCollisionPair &pair = ret.second;
-	pair.p_idx = pt_idx;
-	pair.p_is_obs = pt_is_obs;
-	pair.q_idx = ray_hit_mesh.output.prim;
-	pair.q_is_obs = mesh_is_obs;
-	RowVector3i tris = mesh_tris->row(pair.q_idx);
-	pair.pt_on_q =
-		mesh_x->row(tris[0])*ray_hit_mesh.output.barys[0] +
-		mesh_x->row(tris[1])*ray_hit_mesh.output.barys[1] +
-		mesh_x->row(tris[2])*ray_hit_mesh.output.barys[2];
+	ret.second.q_idx = nearest_tri.output.prim;
+	ret.second.q_is_obs = true;
+	ret.second.q_pt = nearest_tri.output.pt_on_tri;
 	return ret;
-*/
-} // end detect_point_against_mesh
+}
 
 int EmbeddedMeshCollision::detect(
 	const Eigen::MatrixXd *x0,
@@ -160,34 +121,29 @@ int EmbeddedMeshCollision::detect(
 	// Do we even need to process collisions?
 	if (!this->settings.test_floor &&
 		!this->settings.self_collision &&
-		this->obsdata.F.rows()==0)
+		!obsdata.has_obs())
 		return 0;
 
-	// Updates the BVH of self-intersection mesh
+	// Updates the BVH of deforming mesh
 	update_bvh(x0,x1);
 
-	// We store the results of the collisions in a
-	// per-vertex buffer.
+	// We store the results of the collisions in a per-vertex buffer.
+	// This is a workaround so we can create them in threads.
 	int nev = mesh->emb_rest_x.rows();
 	if ((int)per_vertex_pairs.size() != nev)
 		per_vertex_pairs.resize(nev, std::vector<VFCollisionPair>());
 
-	// Set the floor z to inf if bool detect says to
-	double floor_z = this->settings.floor_z;
-	if (!this->settings.test_floor)
-		floor_z = -std::numeric_limits<double>::max();
-
 	//
 	// Thread data for detection
 	//
 	typedef struct DetectThreadData {
+		const Collision::Settings *settings;
 		const Collision *collision;
 		const TetMeshData *tetmesh;
 		const EmbeddedMesh *embmesh;
 		const Collision::ObstacleData *obsdata;
 		const Eigen::MatrixXd *x0;
 		const Eigen::MatrixXd *x1;
-		double floor_z;
 		std::vector<std::vector<VFCollisionPair> > *per_vertex_pairs;
 	} DetectThreadData;
 
@@ -210,11 +166,6 @@ int EmbeddedMeshCollision::detect(
 		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_t0 = 
-			bary[0] * td->x0->row(tet[0]) +
-			bary[1] * td->x0->row(tet[1]) +
-			bary[2] * td->x0->row(tet[2]) +
-			bary[3] * td->x0->row(tet[3]);
 		Vector3d pt_t1 = 
 			bary[0] * td->x1->row(tet[0]) +
 			bary[1] * td->x1->row(tet[1]) +
@@ -222,34 +173,41 @@ int EmbeddedMeshCollision::detect(
 			bary[3] * td->x1->row(tet[3]);
 
 		// Special case, check if we are below the floor
-		if (pt_t1[2] < td->floor_z)
+		if (td->settings->test_floor)
 		{
-			vi_pairs.emplace_back();
-			VFCollisionPair &pair = vi_pairs.back();
-			pair.p_idx = vi;
-			pair.p_is_obs = false;
-			pair.q_idx = -1;
-			pair.q_is_obs = 1;
-			pair.pt_on_q = Vector3d(pt_t1[0],pt_t1[1],td->floor_z);
+			if (pt_t1[2] < td->settings->floor_z)
+			{
+				vi_pairs.emplace_back();
+				VFCollisionPair &pair = vi_pairs.back();
+				pair.p_idx = vi;
+				pair.p_is_obs = false;
+				pair.q_idx = -1;
+				pair.q_is_obs = 1;
+				pair.q_bary.setZero();
+				pair.q_pt = Vector3d(pt_t1[0],pt_t1[1],td->settings->floor_z);
+			}
 		}
 
 		std::pair<bool,VFCollisionPair> pt_hit_obs =
-			td->collision->detect_point_against_mesh(vi, false, pt_t0, pt_t1,
-			true, &td->obsdata->V1, &td->obsdata->F, &td->obsdata->tree);
+			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);
+		}
 
 	}; // end detect for a single embedded vertex
 
 	DetectThreadData thread_data = {
+		.settings = &settings,
 		.collision = this,
 		.tetmesh = nullptr,
 		.embmesh = mesh,
 		.obsdata = &obsdata,
 		.x0 = x0,
 		.x1 = x1,
-		.floor_z = floor_z,
 		.per_vertex_pairs = &per_vertex_pairs
 	};
 
@@ -354,12 +312,12 @@ void EmbeddedMeshCollision::linearize(
 		VFCollisionPair &pair = per_vertex_pairs[pair_idx[0]][pair_idx[1]];
 		int emb_p_idx = pair.p_idx;
 
-		// Compute normal of triangle at x
+		//
+		// If we collided with an obstacle
+		//
 		if (pair.q_is_obs)
 		{
 			Vector3d q_n(0,0,0);
-			RowVector3i q_inds(-1,-1,-1);
-			std::vector<Vector3d> q_tris;
 
 			// Special case, floor
 			if (pair.q_idx == -1)
@@ -368,21 +326,23 @@ void EmbeddedMeshCollision::linearize(
 			}
 			else
 			{
-				q_inds = obsdata.F.row(pair.q_idx);
-				q_tris = {
-					obsdata.V1.row(q_inds[0]),
-					obsdata.V1.row(q_inds[1]),
-					obsdata.V1.row(q_inds[2])
+				RowVector3i q_inds = obsdata.F.row(pair.q_idx);
+				Vector3d q_tris[3] = {
+					obsdata.V.row(q_inds[0]),
+					obsdata.V.row(q_inds[1]),
+					obsdata.V.row(q_inds[2])
 				};
 				q_n = ((q_tris[1]-q_tris[0]).cross(q_tris[2]-q_tris[0]));
 				q_n.normalize();
 			}
 
+			// Get the four deforming verts that embed
+			// the surface vertices, and add constraints on those.
 			RowVector4d bary = mesh->emb_barys.row(emb_p_idx);
 			int tet_idx = mesh->emb_vtx_to_tet[emb_p_idx];
 			RowVector4i tet = mesh->lat_tets.row(tet_idx);
 			int c_idx = d->size();
-			d->emplace_back(q_n.dot(pair.pt_on_q));
+			d->emplace_back(q_n.dot(pair.q_pt));
 			for (int j=0; j<4; ++j)
 			{
 				trips->emplace_back(c_idx, tet[j]*3+0, bary[j]*q_n[0]);
@@ -390,19 +350,77 @@ void EmbeddedMeshCollision::linearize(
 				trips->emplace_back(c_idx, tet[j]*3+2, bary[j]*q_n[2]);
 			}
 			continue;
-
-		}
-
-		// TODO: obstacle point intersecting mesh
-		if (pair.p_is_obs)
-		{
-			continue;
 		}
 
-		// Self collisions
-	
 	} // end loop pairs
 
 } // end jacobian
 
 } // namespace admmpd
+
+/*
+std::pair<bool,VFCollisionPair>
+Collision::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
+{
+	std::pair<bool,VFCollisionPair> ret = 
+		std::make_pair(false, VFCollisionPair());
+
+	if (mesh_tets_x->rows()==0)
+		return ret;
+
+	// Point in tet?
+	PointInTetMeshTraverse<double> pt_in_tet(pt,mesh_tets_x,&emb_mesh->lat_tets);
+	bool in_mesh = mesh_tets_tree->traverse(pt_in_tet);
+	if (!in_mesh)
+		return ret;
+
+	// Transform to rest shape
+	int tet_idx = pt_in_tet.output.prim;
+	RowVector4i tet = emb_mesh->lat_tets.row(tet_idx);
+	Vector4d barys = geom::point_tet_barys(pt,
+		mesh_tets_x->row(tet[0]), mesh_tets_x->row(tet[1]),
+		mesh_tets_x->row(tet[2]), mesh_tets_x->row(tet[3]));
+	Vector3d rest_pt =
+		barys[0]*emb_mesh->lat_rest_x.row(tet[0])+
+		barys[1]*emb_mesh->lat_rest_x.row(tet[1])+
+		barys[2]*emb_mesh->lat_rest_x.row(tet[2])+
+		barys[3]*emb_mesh->lat_rest_x.row(tet[3]);
+
+	// We are inside the lattice. Find nearest
+	// face and see if we are on the inside of the mesh.
+	NearestTriangleTraverse<double> nearest_tri(rest_pt,
+		&emb_mesh->emb_rest_x,&emb_mesh->emb_faces);
+	emb_mesh->emb_rest_tree.traverse(nearest_tri);
+
+	if (nearest_tri.output.prim < 0)
+		throw std::runtime_error("detect_point_against_mesh failed to project out");
+
+	RowVector3i f = emb_mesh->emb_faces.row(nearest_tri.output.prim);
+	Vector3d fv[3] = {
+		emb_mesh->emb_rest_x.row(f[0]),
+		emb_mesh->emb_rest_x.row(f[1]),
+		emb_mesh->emb_rest_x.row(f[2])
+	};
+	Vector3d n = (fv[1]-fv[0]).cross(fv[2]-fv[0]);
+	n.normalize();
+	if ((rest_pt-fv[0]).dot(n)>0)
+		return ret; // outside of surface
+
+	ret.first = true;
+	ret.second.p_idx = pt_idx;
+	ret.second.p_is_obs = pt_is_obs;
+	ret.second.q_idx = 
+	ret.second.q_is_obs = mesh_is_obs;
+	ret.second.q_bary = geom::point_triangle_barys<double>(
+		nearest_tri.output.pt_on_tri, fv[0], fv[1], fv[2]);
+
+	return ret;
+} // end detect_point_against_mesh
+*/
\ No newline at end of file
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index 3e3ffa37778..3382d5aa9a5 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -16,20 +16,13 @@ struct VFCollisionPair {
     int p_is_obs; // 0 or 1
     int q_idx; // face, or -1 if floor
     int q_is_obs; // 0 or 1
-    Eigen::Vector3d pt_on_q; // point of collision on q
-    Eigen::Vector3d q_n; // face normal
+    Eigen::Vector3d q_pt; // pt of collision (if q obs)
+    Eigen::Vector3d q_bary; // barys of collision (if q !obs)
     VFCollisionPair();
 };
 
 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;
 
     struct Settings {
         double collision_eps;
@@ -38,13 +31,20 @@ public:
         bool self_collision;
         Settings() :
             collision_eps(1e-10),
-            floor_z(-1.5),
-//            floor_z(-std::numeric_limits<double>::max()),
+            floor_z(-std::numeric_limits<double>::max()),
             test_floor(true),
             self_collision(false)
             {}
     } settings;
 
+    struct ObstacleData {
+        bool has_obs() const { return F.rows()>0; }
+        Eigen::MatrixXd V;
+        Eigen::MatrixXi F;
+        AABBTree<double,3> tree;
+        std::vector<Eigen::AlignedBox<double,3> > leaves;
+    } obsdata;
+
     virtual ~Collision() {}
 
     // Performs collision detection.
@@ -81,15 +81,21 @@ public:
     // 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_t0,
-        const Eigen::Vector3d &pt_t1,
-        bool mesh_is_obs,
-        const Eigen::MatrixXd *mesh_x,
-        const Eigen::MatrixXi *mesh_tris,
-        const AABBTree<double,3> *mesh_tree) const;
+    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;
 };
 
 // Collision detection against multiple meshes
diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp
index 28cc1522719..b16d92b640a 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.cpp
+++ b/extern/softbody/src/admmpd_embeddedmesh.cpp
@@ -5,11 +5,13 @@
 #include "admmpd_geom.h"
 #include "admmpd_bvh.h"
 #include "admmpd_bvh_traverse.h"
+
 #include <iostream>
 #include <fstream>
 #include <unordered_map>
 #include <set>
 #include <numeric>
+
 #include "BLI_task.h" // threading
 #include "BLI_assert.h"
 
@@ -67,7 +69,9 @@ static void parallel_keep_tet(
 
 // Gen lattice with subdivision
 struct LatticeData {
+	//SDF<double> *emb_sdf;
 	const Eigen::MatrixXd *V;
+	const Eigen::MatrixXi *F;
 	std::vector<Vector3d> verts;
 	std::vector<Vector4i> tets;
 };
@@ -113,7 +117,7 @@ static inline void merge_close_vertices(LatticeData *data, double eps=1e-12)
 static inline void subdivide_cube(
 	LatticeData *data,
 	const std::vector<Vector3d> &cv,
-	const std::vector<int> &pts_in_box,
+	const std::vector<int> &faces,
 	int level)
 {
 	BLI_assert((int)cv.size()==8);
@@ -159,21 +163,24 @@ static inline void subdivide_cube(
 		return;
 	}
 
-	// Only subdivide if we contain vertices
-	// Otherwise just return.
+	// Only subdivide if the box contains the surface.
 	AlignedBox<double,3> aabb;
 	aabb.extend(cv[3]); aabb.extend(cv[5]);
 	aabb.extend(aabb.min()-Vector3d::Ones()*1e-8);
 	aabb.extend(aabb.max()+Vector3d::Ones()*1e-8);
-	std::vector<int> new_pts_in_box;
-	int nv = pts_in_box.size();
-	for (int i=0; i<nv; ++i)
+	std::vector<int> new_faces;
+	int nf = faces.size();
+	for (int i=0; i<nf; ++i)
 	{
-		int idx = pts_in_box[i];
-		if (aabb.contains(data->V->row(idx).transpose()))
-			new_pts_in_box.emplace_back(idx);
+		int f_idx = faces[i];
+		RowVector3i f = data->F->row(f_idx);
+		Vector3d v0 = data->V->row(f[0]);
+		Vector3d v1 = data->V->row(f[1]);
+		Vector3d v2 = data->V->row(f[2]);
+		if (geom::aabb_triangle_intersect(aabb.min(),aabb.max(),v0,v1,v2))
+			new_faces.emplace_back(f_idx);
 	}
-	if (new_pts_in_box.size()==0)
+	if (new_faces.size()==0)
 	{
 		add_tets_from_box();
 		return;
@@ -201,17 +208,16 @@ static inline void subdivide_cube(
 	Vector3d v56 = 0.5*(cv[5]+cv[6]);
 	Vector3d v67 = 0.5*(cv[6]+cv[7]);
 	Vector3d v47 = 0.5*(cv[4]+cv[7]);
-	subdivide_cube(data, { cv[0], v01, vbot, v03, v04, vfront, vcent, vleft }, new_pts_in_box, level-1);
-	subdivide_cube(data, { v01, cv[1], v12, vbot, vfront, v15, vright, vcent }, new_pts_in_box, level-1);
-	subdivide_cube(data, { vbot, v12, cv[2], v23, vcent, vright, v26, vback }, new_pts_in_box, level-1);
-	subdivide_cube(data, { v03, vbot, v23, cv[3], vleft, vcent, vback, v37 }, new_pts_in_box, level-1);
-	subdivide_cube(data, { v04, vfront, vcent, vleft, cv[4], v45, vtop, v47 }, new_pts_in_box, level-1);
-	subdivide_cube(data, { vfront, v15, vright, vcent, v45, cv[5], v56, vtop }, new_pts_in_box, level-1);
-	subdivide_cube(data, { vcent, vright, v26, vback, vtop, v56, cv[6], v67 }, new_pts_in_box, level-1);
-	subdivide_cube(data, { vleft, vcent, vback, v37, v47, vtop, v67, cv[7] }, new_pts_in_box, level-1);
-}
-
+	subdivide_cube(data, { cv[0], v01, vbot, v03, v04, vfront, vcent, vleft }, new_faces, level-1);
+	subdivide_cube(data, { v01, cv[1], v12, vbot, vfront, v15, vright, vcent }, new_faces, level-1);
+	subdivide_cube(data, { vbot, v12, cv[2], v23, vcent, vright, v26, vback }, new_faces, level-1);
+	subdivide_cube(data, { v03, vbot, v23, cv[3], vleft, vcent, vback, v37 }, new_faces, level-1);
+	subdivide_cube(data, { v04, vfront, vcent, vleft, cv[4], v45, vtop, v47 }, new_faces, level-1);
+	subdivide_cube(data, { vfront, v15, vright, vcent, v45, cv[5], v56, vtop }, new_faces, level-1);
+	subdivide_cube(data, { vcent, vright, v26, vback, vtop, v56, cv[6], v67 }, new_faces, level-1);
+	subdivide_cube(data, { vleft, vcent, vback, v37, v47, vtop, v67, cv[7] }, new_faces, level-1);
 
+}
 
 bool EmbeddedMesh::generate(
 	const Eigen::MatrixXd &V, // embedded verts
@@ -221,99 +227,14 @@ bool EmbeddedMesh::generate(
 {
 	emb_faces = F;
 	emb_rest_x = V;
-	emb_sdf.generate(&V,&F);
-	AlignedBox<double,3> aabb = emb_sdf.box();
-
-	// Recursive subdivide of cells
-	LatticeData data;
-	data.V = &V;
-	int nev = V.rows();
-	std::vector<int> pts_in_box(nev);
-	std::iota(pts_in_box.begin(), pts_in_box.end(), 0);
-	Vector3d min = aabb.min();
-	Vector3d max = aabb.max();
-	std::vector<Vector3d> b0 = {
-		Vector3d(min[0], min[1], max[2]),
-		Vector3d(max[0], min[1], max[2]),
-		Vector3d(max[0], min[1], min[2]),
-		min,
-		Vector3d(min[0], max[1], max[2]),
-		max,
-		Vector3d(max[0], max[1], min[2]),
-		Vector3d(min[0], max[1], min[2])
-	};
-	subdivide_cube(&data,b0,pts_in_box,subdiv_levels);
-	merge_close_vertices(&data);
-
-	// Loop over tets and remove as needed.
-	// Mark referenced vertices to compute a mapping.
-	int nt0 = data.tets.size();
-	std::vector<int> keep_tet(nt0,1);
-	std::set<int> refd_verts;
-	{
-		int tet_idx = 0;
-		for (std::vector<Vector4i>::iterator it = data.tets.begin();
-			it != data.tets.end(); ++tet_idx)
-		{
-			bool keep = keep_tet[tet_idx];
-			if (keep)
-			{
-				const Vector4i &t = *it;
-				refd_verts.emplace(t[0]);
-				refd_verts.emplace(t[1]);
-				refd_verts.emplace(t[2]);
-				refd_verts.emplace(t[3]);
-				++it;
-			}
-			else { it = data.tets.erase(it); }
-		}
-	}
 
-	// Copy data into matrices and remove unreferenced
-	{
-		// Computing a mapping of vertices from old to new
-		// Delete any unreferenced vertices
-		std::unordered_map<int,int> vtx_old_to_new;
-		int ntv0 = data.verts.size(); // original num verts
-		int ntv1 = refd_verts.size(); // reduced num verts
-		BLI_assert(ntv1 <= ntv0);
-		lat_rest_x.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){
-					lat_rest_x(vtx_count,j) = data.verts[i][j];
-				}
-				vtx_old_to_new[i] = vtx_count;
-				vtx_count++;
-			}
-		}
+	if (F.rows()==0 || V.rows()==0)
+		throw std::runtime_error("EmbeddedMesh::generate Error: Missing data");
 
-		// Copy tets to matrix data and update vertices
-		int nt = data.tets.size();
-		lat_tets.resize(nt,4);
-		for(int i=0; i<nt; ++i){
-			for(int j=0; j<4; ++j){
-				int old_idx = data.tets[i][j];
-				BLI_assert(vtx_old_to_new.count(old_idx)>0);
-				lat_tets(i,j) = vtx_old_to_new[old_idx];
-			}
-		}
-	}
-
-	return compute_embedding();
-
-/*
 	AlignedBox<double,3> aabb;
 	int nev = V.rows();
-	std::vector<int> pts_in_box;
 	for (int i=0; i<nev; ++i)
-	{
 		aabb.extend(V.row(i).transpose());
-		pts_in_box.emplace_back(i);
-	}
 
 	// Create initial box
 	aabb.extend(aabb.min()-Vector3d::Ones()*1e-4);
@@ -331,9 +252,13 @@ bool EmbeddedMesh::generate(
 		Vector3d(min[0], max[1], min[2])
 	};
 
+	std::vector<int> faces_in_box(F.rows());
+	std::iota(faces_in_box.begin(), faces_in_box.end(), 0);
+
 	LatticeData data;
 	data.V = &V;
-	subdivide_cube(&data,b0,pts_in_box,subdiv_levels);
+	data.F = &F;
+	subdivide_cube(&data,b0,faces_in_box,subdiv_levels);
 	merge_close_vertices(&data);
 
 	// We only want to keep tets that are either
@@ -353,11 +278,9 @@ bool EmbeddedMesh::generate(
 
 		int nt0 = data.tets.size();
 		std::vector<int> keep_tet(nt0,1);
-
-		AABBTree<double,3> mesh_tree;
-		mesh_tree.init(face_aabb);
+		emb_rest_tree.init(face_aabb);
 		KeepTetThreadData thread_data = {
-			.tree = &mesh_tree,
+			.tree = &emb_rest_tree,
 			.pts = &V,
 			.faces = &F,
 			.tet_x = &data.verts,
@@ -427,8 +350,14 @@ bool EmbeddedMesh::generate(
 	}
 
 	// Now compute the baryweighting for embedded vertices
-	return compute_embedding();
-*/
+	bool embed_success = compute_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();
+
+	return embed_success;
+
 } // end gen lattice
 
 void EmbeddedMesh::compute_masses(
@@ -571,18 +500,12 @@ bool EmbeddedMesh::compute_embedding()
 		}
 	}
 
-
-// Export the mesh for funsies
-//std::ofstream of("v_lattice.txt"); of << emb_mesh->rest_x; of.close();
-//std::ofstream of2("t_lattice.txt"); of2 << emb_mesh->tets; of2.close();
-
 	return true;
 
 } // end compute vtx to tet mapping
 
 Eigen::Vector3d EmbeddedMesh::get_mapped_vertex(
-	const Eigen::MatrixXd *x_data,
-	int idx)
+	const Eigen::MatrixXd *x_data, int idx) const
 {
     int t_idx = emb_vtx_to_tet[idx];
     RowVector4i tet = lat_tets.row(t_idx);
diff --git a/extern/softbody/src/admmpd_embeddedmesh.h b/extern/softbody/src/admmpd_embeddedmesh.h
index 550cca22809..0ee8cacf43e 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.h
+++ b/extern/softbody/src/admmpd_embeddedmesh.h
@@ -5,7 +5,8 @@
 #define ADMMPD_EMBEDDEDMESH_H_
 
 #include "admmpd_types.h"
-#include "admmpd_sdf.h"
+#include "admmpd_bvh.h"
+//#include "admmpd_sdf.h"
 
 namespace admmpd {
 
@@ -15,7 +16,7 @@ public:
     Eigen::MatrixXi emb_faces; // embedded faces
     Eigen::VectorXi emb_vtx_to_tet; // what tet vtx is embedded in, p x 1
     Eigen::MatrixXd emb_barys; // barycoords of the embedding, p x 4
-    SDF<double> emb_sdf; // embedded SDF at rest
+    admmpd::AABBTree<double,3> emb_rest_tree; // tree of embedded verts at rest
     Eigen::MatrixXi lat_tets; // lattice elements, m x 4
     Eigen::MatrixXd lat_rest_x; // lattice verts at rest
 
@@ -27,9 +28,11 @@ public:
         int subdiv_levels=3); // number of subdivs (resolution)
 
     // Returns the vtx mapped from x/v and tets
+    // Idx is the embedded vertex, and x_data is the
+    // data it should be mapped to.
     Eigen::Vector3d get_mapped_vertex(
         const Eigen::MatrixXd *x_data,
-        int idx);
+        int idx) const;
 
     // Given an embedding, compute masses
     // for the lattice vertices
diff --git a/extern/softbody/src/admmpd_geom.cpp b/extern/softbody/src/admmpd_geom.cpp
index a70eeb3fa10..c03c5718383 100644
--- a/extern/softbody/src/admmpd_geom.cpp
+++ b/extern/softbody/src/admmpd_geom.cpp
@@ -151,7 +151,7 @@ Eigen::Matrix<T,3,1> geom::point_on_triangle(
 
 // From Real-Time Collision Detection by Christer Ericson
 template<typename T>
-bool aabb_plane_intersect(
+bool geom::aabb_plane_intersect(
 		const Eigen::Matrix<T,3,1> &bmin,
 		const Eigen::Matrix<T,3,1> &bmax,
 		const Eigen::Matrix<T,3,1> &p, // pt on plane
@@ -172,14 +172,23 @@ bool aabb_plane_intersect(
 
 // From Real-Time Collision Detection by Christer Ericson
 template<typename T>
-bool aabb_triangle_intersect(
+bool geom::aabb_triangle_intersect(
     const Eigen::Matrix<T,3,1> &bmin,
     const Eigen::Matrix<T,3,1> &bmax,
     const Eigen::Matrix<T,3,1> &a,
     const Eigen::Matrix<T,3,1> &b,
     const Eigen::Matrix<T,3,1> &c)
 {
-throw std::runtime_error("admmpd::geom::aabb_triangle_intersect: verify correctness first");
+	Eigen::AlignedBox<T,3> box;
+	box.extend(bmin);
+	box.extend(bmax);
+	if (box.contains(a))
+		return true;
+	if (box.contains(b))
+		return true;
+	if (box.contains(c))
+		return true;
+
 	typedef Eigen::Matrix<T,3,1> VecType;
 	auto Max = [](T x, T y, T z){ return std::max(std::max(x,y),z); };
 	auto Min = [](T x, T y, T z){ return std::min(std::min(x,y),z); };
@@ -203,7 +212,7 @@ throw std::runtime_error("admmpd::geom::aabb_triangle_intersect: verify correctn
 		const VecType &u = box_normals[i];
 		for (int j=0; j<3; ++j)
 		{
-			const VecType &f = edge_vectors[i];
+			const VecType &f = edge_vectors[j];
 			aij = u.cross(f);
 			// Axis is separating axis
 			p0 = v0.dot(aij);
@@ -352,23 +361,23 @@ template Eigen::Matrix<float,3,1>
 	admmpd::geom::point_on_triangle<float>(
 	const Eigen::Vector3f&, const Eigen::Vector3f&,
 	const Eigen::Vector3f&, const Eigen::Vector3f&);
-template bool aabb_plane_intersect<double>(
+template bool geom::aabb_plane_intersect<double>(
 	const Eigen::Matrix<double,3,1>&,
 	const Eigen::Matrix<double,3,1>&,
 	const Eigen::Matrix<double,3,1>&,
 	const Eigen::Matrix<double,3,1>&);
-template bool aabb_plane_intersect<float>(
+template bool geom::aabb_plane_intersect<float>(
 	const Eigen::Matrix<float,3,1>&,
 	const Eigen::Matrix<float,3,1>&,
 	const Eigen::Matrix<float,3,1>&,
 	const Eigen::Matrix<float,3,1>&);
-template bool aabb_triangle_intersect<double>(
+template bool geom::aabb_triangle_intersect<double>(
     const Eigen::Matrix<double,3,1>&,
     const Eigen::Matrix<double,3,1>&,
     const Eigen::Matrix<double,3,1>&,
     const Eigen::Matrix<double,3,1>&,
     const Eigen::Matrix<double,3,1>&);
-template bool aabb_triangle_intersect<float>(
+template bool geom::aabb_triangle_intersect<float>(
     const Eigen::Matrix<float,3,1>&,
     const Eigen::Matrix<float,3,1>&,
     const Eigen::Matrix<float,3,1>&,
diff --git a/extern/softbody/src/admmpd_sdf.cpp b/extern/softbody/src/admmpd_sdf.cpp
index b844de1ba47..16bcc545903 100644
--- a/extern/softbody/src/admmpd_sdf.cpp
+++ b/extern/softbody/src/admmpd_sdf.cpp
@@ -23,9 +23,12 @@ bool SDF<T>::generate(const MatrixXT *V_, const Eigen::MatrixXi *F_, T dx_frac)
     sdf_dx = -1;
 	sdf.clear();
     aabb.setEmpty();
+    V_map.clear();
+    F_map.clear();
 
     int nv = V.rows();
-    if (nv == 0)
+    int nf = F.rows();
+    if (nv==0 || nf==0)
     {
         return false;
     }
@@ -39,38 +42,51 @@ bool SDF<T>::generate(const MatrixXT *V_, const Eigen::MatrixXi *F_, T dx_frac)
 			vertList.back()[j]=(float)V(i,j);
     }
 
-    // Consider k=n^(1/3) rule, with grid
-    // length = k and n = number of primitives.
-    // Grid cells should be larger than the largest primitive.
-
-	VecType sizes = aabb.sizes();
-	T grid_dx = sizes.maxCoeff()*dx_frac;
-    sdf_dx = grid_dx * 0.5;
-    sdfgen::Vec3f min_box = sdfgen::Vec3f(aabb.min()[0], aabb.min()[1], aabb.min()[2]);
-    sdfgen::Vec3f max_box = sdfgen::Vec3f(aabb.max()[0], aabb.max()[1], aabb.max()[2]);
-    sdfgen::Vec3ui sdf_sizes = sdfgen::Vec3ui((max_box-min_box)/sdf_dx)+sdfgen::Vec3ui(1,1,1);
-
+    T max_edge_len = 0;
     std::vector<sdfgen::Vec3ui> faceList;
-	int nf = F.rows();
 	for (int i=0; i<nf; ++i)
 	{
 		faceList.emplace_back();
 		for(int j=0; j<3; ++j)
+        {
 			faceList.back()[j]=(unsigned int)F(i,j);
+            T edge_len = (V.row(F(i,j))-V.row(F(i,(j+1)%3))).norm();
+            if (edge_len > max_edge_len)
+                max_edge_len = edge_len;
+        }
 	}
 
+    aabb.extend(aabb.min()-VecType::Ones()*1e-4);
+    aabb.extend(aabb.max()+VecType::Ones()*1e-4);
+
+    VecType sizes = aabb.sizes();
+    T grid_dx = sizes.maxCoeff()*dx_frac;
+    if (dx_frac<0)
+    {
+        // k=n^(1/3) rule, with num grid cells=k
+        // and n=number of primitives.
+        T k = std::pow(T(F.rows()), T(1.0/3.0));
+        grid_dx = sizes.maxCoeff()/k;
+//        grid_dx = max_edge_len;
+    }
+
+    sdf_dx = grid_dx * 0.5;
+    sdfgen::Vec3f min_box = sdfgen::Vec3f(aabb.min()[0], aabb.min()[1], aabb.min()[2]);
+    sdfgen::Vec3f max_box = sdfgen::Vec3f(aabb.max()[0], aabb.max()[1], aabb.max()[2]);
+    sdfgen::Vec3ui sdf_sizes = sdfgen::Vec3ui((max_box-min_box)/sdf_dx)+sdfgen::Vec3ui(1,1,1);
+
     sdfgen::make_level_set3(
         faceList, vertList, min_box, sdf_dx,
         sdf_sizes[0], sdf_sizes[1], sdf_sizes[2],
         sdf);
 
-    update_mapping(V_,F_);
+    compute_mapping(V_,F_);
 
     return true;
 } // end generate SDF
 
 template<typename T>
-void SDF<T>::update_mapping(
+void SDF<T>::compute_mapping(
         const MatrixXT *V_,
         const Eigen::MatrixXi *F_)
 {
@@ -141,11 +157,129 @@ template<typename T>
 T SDF<T>::sample(const VecType &x) const
 {
     if (sdf_dx < 0)
-        return 0;
+        return 1;
+
+    for (int i=0; i<3; ++i)
+    {
+        if (x[i]<aabb.min()[i])
+            return 1;
+        if (x[i]>aabb.max()[i])
+            return 1;
+    }
+
+    Vector3i ind = index(x);
+    T val = sdf(ind[0],ind[1],ind[2]);
+
+    // If our SDF is very coarse, we want to check if the cell actually contains a face,
+    // since the sampling will be poor. If it does, return 0 (on surface).
+    if (val > 0 && val < sdf_dx)
+    {
+        int idx = ind[0]+sdf.ni*(ind[1]+sdf.nj*ind[2]);
+        std::unordered_map<int,std::set<int> >::const_iterator it = F_map.find(idx);
+        if (it != F_map.end())
+        {
+            if (it->second.size()>0)
+                return 0;
+        }
+    }
+    return val;
+}
+
+template<typename T>
+T SDF<T>::sample(const VecType &bmin, const VecType &bmax) const
+{
+    Vector3i bmin_cell = index(bmin);
+    Vector3i bmax_cell = index(bmax);
+    T min_val = std::numeric_limits<T>::max();
+    Vector3i cell(0,0,0);
+    for (cell[0]=bmin_cell[0]; cell[0]<=bmax_cell[0]; ++cell[0])
+    {
+		for (cell[1]=bmin_cell[1]; cell[1]<=bmax_cell[1]; ++cell[1])
+        {
+            for (cell[2]=bmin_cell[2]; cell[2]<=bmax_cell[2]; ++cell[2])
+            {
+                T val = sdf(cell[0],cell[1],cell[2]);
+                if (val > 0 && val < sdf_dx)
+                {
+                    int idx = cell[0]+sdf.ni*(cell[1]+sdf.nj*cell[2]);
+                    std::unordered_map<int,std::set<int> >::const_iterator it = F_map.find(idx);
+                    if (it != F_map.end())
+                    {
+                        if (it->second.size()>0)
+                            val = 0;
+                    }
+                }
+                min_val = std::min(min_val, val);
+            }
+        }
+    }
+    return min_val;
+}
+
+template<typename T>
+bool SDF<T>::project_out(
+    const VecType &pt,
+    const MatrixXT *V,
+    const Eigen::MatrixXi *F,
+    int &face_idx,
+    VecType &proj_on_face) const
+{
+
+    auto is_surface = [&](const Vector3i &ind)->bool
+    {
+        std::vector<int> f_list;
+        faces(ind,f_list);
+        int nf = f_list.size();
+        if (nf>0)
+        {
+            // Project onto nearest face
+            T min_dist = std::numeric_limits<T>::max();
+            for (int i=0; i<nf; ++i)
+            {
+                RowVector3i f = F->row(f_list[i]);
+                VecType pt_on_face = geom::point_on_triangle<T>(
+                    pt, V->row(f[0]), V->row(f[1]), V->row(f[2]));
+                T dist = (pt_on_face-pt).norm();
+                if (dist < min_dist)
+                {
+                    min_dist = dist;
+                    face_idx = f_list[i];
+                    proj_on_face = pt_on_face;
+                }
+            }
+            return true;
+        }
+        return false;
+    };
 
-    Vector3i x_inds = index(x);
-    return sdf(x_inds[0],x_inds[1],x_inds[2]);
+    std::function<bool(const Vector3i&)> walk;
+    walk = [&](const Vector3i &ind)->bool
+    {
+        // Are we done walking?
+        if (is_surface(ind))
+            return true;
+
+        // Get the sdf in each direction
+        Matrix<T,6,1> dirs;
+        dirs[0] = ind[0]<sdf.ni-2 ? sdf(ind[0]+1,ind[1],ind[2]) : 1;
+        dirs[1] = ind[0]>0 ? sdf(ind[0]-1,ind[1],ind[2]) : 1;
+        dirs[2] = ind[1]<sdf.nj-2 ? sdf(ind[0],ind[1]+1,ind[2]) : 1;
+        dirs[3] = ind[1]>0 ? sdf(ind[0],ind[1]-1,ind[2]) : 1;
+        dirs[4] = ind[2]<sdf.nk-2 ? sdf(ind[0],ind[1],ind[2]+1) : 1;
+        dirs[5] = ind[2]>0 ? sdf(ind[0],ind[1],ind[2]-1) : 1;
+        int max_dir = -1;
+        dirs.maxCoeff(&max_dir);
+        if (max_dir==0) return walk(ind+Vector3i(1,0,0));
+        if (max_dir==1) return walk(ind-Vector3i(1,0,0));
+        if (max_dir==2) return walk(ind+Vector3i(0,1,0));
+        if (max_dir==3) return walk(ind-Vector3i(0,1,0));
+        if (max_dir==4) return walk(ind+Vector3i(0,0,1));
+        if (max_dir==5) return walk(ind-Vector3i(0,0,1));
+        return false;
+    };
 
+    Vector3i idx = index(pt);
+    return walk(idx);
 }
 
 template<typename T>
@@ -168,6 +302,24 @@ void SDF<T>::faces(const Eigen::Vector3i &ind, std::vector<int> &f) const
     f.insert(f.end(), it->second.begin(), it->second.end());
 }
 
+template<typename T>
+void SDF<T>::faces(const VecType &bmin, const VecType &bmax, std::vector<int> &f) const
+{
+    Vector3i bmin_cell = index(bmin);
+    Vector3i bmax_cell = index(bmax);
+    Vector3i cell(0,0,0);
+	for (cell[0]=bmin_cell[0]; cell[0]<=bmax_cell[0]; ++cell[0])
+    {
+		for (cell[1]=bmin_cell[1]; cell[1]<=bmax_cell[1]; ++cell[1])
+        {
+            for (cell[2]=bmin_cell[2]; cell[2]<=bmax_cell[2]; ++cell[2])
+            {
+                faces(cell,f);
+            }
+        }
+    }
+}
+
 template <typename T>
 void SDF<T>::get_cells(
         const VecType &bmin, const VecType &bmax,
@@ -180,7 +332,7 @@ void SDF<T>::get_cells(
     {
 		for (cell[1]=bmin_cell[1]; cell[1]<=bmax_cell[1]; ++cell[1])
         {
-            for (cell[1]=bmin_cell[1]; cell[1]<=bmax_cell[1]; ++cell[1])
+            for (cell[2]=bmin_cell[2]; cell[2]<=bmax_cell[2]; ++cell[2])
             {
                 int idx = cell[0]+sdf.ni*(cell[1]+sdf.nj*cell[2]);
                 cells_inds.emplace_back(idx);
diff --git a/extern/softbody/src/admmpd_sdf.h b/extern/softbody/src/admmpd_sdf.h
index 4369b415367..fce7689da88 100644
--- a/extern/softbody/src/admmpd_sdf.h
+++ b/extern/softbody/src/admmpd_sdf.h
@@ -29,6 +29,10 @@ protected:
         const VecType &bmin, const VecType &bmax,
 	    std::vector<int> &cells_inds) const;
 
+    void compute_mapping(
+        const MatrixXT *V,
+        const Eigen::MatrixXi *F);
+
 public:
     SDF();
 
@@ -39,26 +43,41 @@ public:
     bool generate(
         const MatrixXT *V,
         const Eigen::MatrixXi *F,
-        T dx_frac=0.1); // (0-1), fraction of AABB to set dx
-
-    // Recomputes mapping of verts and faces to grid
-    void update_mapping(
-        const MatrixXT *V,
-        const Eigen::MatrixXi *F);
+        T dx_frac=-1); // (0-1), fraction of AABB to set dx, -1=auto
 
     Eigen::AlignedBox<T,3> box() const { return aabb; }
+    T dx() const { return sdf_dx; }
 
     // Samples the SDF at position x in space
+    // by mapping it to a cell.
+    // < 0 = cell inside surface
+    // 0 = cell contains surface
+    // > 0 = cell outside surface
     T sample(const VecType &x) const;
+    // Sample an area, returns min value
+    T sample(const VecType &bmin, const VecType &bmax) const;
+
+    // Given a point on the interior, move along the
+    // signed distance field until the surface is reached.
+    // Then, project on the surface face.
+    // The input mesh (V,F) should be the same as generate(V,F).
+    // Returns true if success.
+    bool project_out(
+        const VecType &pt,
+        const MatrixXT *V,
+        const Eigen::MatrixXi *F,
+        int &face_idx,
+        VecType &proj_on_face) const;
 
     // Returns index into SDF from 3D pt (clamped to AABB bounds)
     Eigen::Vector3i index(const VecType &x) const;
 
-    // Returns list of verts in the cell at ind
+    // Returns list of verts in the cell at ind or box
     void vertices(const Eigen::Vector3i &ind, std::vector<int> &v) const;
 
-    // Returns list of faces in the cell at ind
+    // Returns list of faces in the cell at ind or box
     void faces(const Eigen::Vector3i &ind, std::vector<int> &f) const;
+    void faces(const VecType &bmin, const VecType &bmax, std::vector<int> &f) const;
 
 }; // class sdf
 
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index 878e615d826..7e08b328396 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -29,12 +29,12 @@ struct Options {
     Eigen::Vector3d grav;
     Options() :
         timestep_s(1.0/24.0),
-        max_admm_iters(50),
+        max_admm_iters(30),
         max_cg_iters(10),
         max_gs_iters(100),
         gs_omega(1),
-        mult_ck(1),
-        mult_pk(0.001),
+        mult_ck(3),
+        mult_pk(0.01),
         min_res(1e-8),
         youngs(1000000),
         poisson(0.399),