improved lattice gen

7 files changed, 246 insertions, 142 deletions

diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index b8f036feebb..cadd9ef51ee 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -19,35 +19,35 @@
 # ***** END GPL LICENSE BLOCK *****
 
 set(INC
-  src
+	src
 )
 
 set(INC_SYS
-  ${EIGEN3_INCLUDE_DIRS}
-  ../../source/blender/blenlib
-  ../../source/blender/makesdna # BLI_math_geom requires DNA
+	${EIGEN3_INCLUDE_DIRS}
+	../../source/blender/blenlib
+	../../source/blender/makesdna # BLI_math_geom requires DNA
 )
 
 set(SRC
-  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_linsolve.h
-  src/admmpd_linsolve.cpp
-  src/admmpd_math.h
-  src/admmpd_math.cpp
-  src/admmpd_solver.h
-  src/admmpd_solver.cpp
-  src/admmpd_tetmesh.h
-  src/admmpd_tetmesh.cpp
-  src/admmpd_types.h
+	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_linsolve.h
+	src/admmpd_linsolve.cpp
+	src/admmpd_math.h
+	src/admmpd_math.cpp
+	src/admmpd_solver.h
+	src/admmpd_solver.cpp
+	src/admmpd_tetmesh.h
+	src/admmpd_tetmesh.cpp
+	src/admmpd_types.h
 )
 
 set(LIB
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index b7e835a9185..289af9fc68b 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -78,18 +78,6 @@ void Collision::detect_discrete_vf(
 	double floor_z,
 	std::vector<VFCollisionPair> *pairs)
 {
-	// Special case, check if we are below the floor
-	if (pt[2] < floor_z)
-	{
-		pairs->emplace_back();
-		VFCollisionPair &pair = pairs->back();
-		pair.p_idx = pt_idx;
-		pair.p_is_obs = pt_is_obs;
-		pair.q_idx = -1;
-		pair.q_is_obs = 1;
-		pair.pt_on_q = Vector3d(pt[0],pt[1],floor_z);
-	}
-
 	// Any faces to detect against?
 	if (mesh_tris->rows()==0)
 		return;
@@ -191,6 +179,19 @@ static void parallel_detect(
 			bary[2] * td->x1->row(tet[2]) +
 			bary[3] * td->x1->row(tet[3]);
 
+
+		// Special case, check if we are below the floor
+		if (pt[2] < td->floor_z)
+		{
+			tl_pairs.emplace_back();
+			VFCollisionPair &pair = tl_pairs.back();
+			pair.p_idx = i;
+			pair.p_is_obs = false;
+			pair.q_idx = -1;
+			pair.q_is_obs = 1;
+			pair.pt_on_q = Vector3d(pt[0],pt[1],td->floor_z);
+		}
+
 		Collision::detect_discrete_vf(
 			pt, i, false,
 			&td->obsdata->tree,
@@ -200,6 +201,15 @@ static void parallel_detect(
 			td->floor_z,
 			&tl_pairs );
 
+	//	Collision::detect_discrete_vf(
+	//		pt, i, false,
+	//		&td->obsdata->tree,
+	//		&td->obsdata->V1,
+	//		&td->obsdata->F,
+	//		true,
+	//		td->floor_z,
+	//		&tl_pairs );
+
 	} // end detect with embedded meshes
 
 } // end parallel detect
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index 2ab7d343c05..95623510969 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -12,11 +12,9 @@ namespace admmpd {
 struct VFCollisionPair {
     int p_idx; // point
     int p_is_obs; // 0 or 1
-    int q_idx; // face
+    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
-//    int floor; // 0 or 1, special case
-//    Eigen::Vector3d barys;
     Eigen::Vector3d q_n; // face normal
     VFCollisionPair();
 };
diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp
index 22ac59fef0c..0c2cdcac938 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.cpp
+++ b/extern/softbody/src/admmpd_embeddedmesh.cpp
@@ -6,6 +6,7 @@
 #include "admmpd_bvh.h"
 #include "admmpd_bvh_traverse.h"
 #include <iostream>
+#include <fstream>
 #include <unordered_map>
 #include <set>
 #include "BLI_task.h" // threading
@@ -63,6 +64,153 @@ static void parallel_keep_tet(
 
 } // end parallel test if keep tet
 
+// Gen lattice with subdivision
+struct LatticeData {
+	const Eigen::MatrixXd *V;
+	std::vector<Vector3d> verts;
+	std::vector<Vector4i> tets;
+};
+
+static inline void merge_close_vertices(LatticeData *data, double eps=1e-12)
+{
+	int nv = data->verts.size();
+	std::vector<Vector3d> new_v(nv); // new verts
+	std::vector<int> idx(nv,0); // index mapping
+	std::vector<int> visited(nv,0);
+	int count = 0;
+	for (int i=0; i<nv; ++i)
+	{
+		if(!visited[i])
+		{
+			visited[i] = 1;
+			new_v[count] = data->verts[i];
+			idx[i] = count;
+			Vector3d vi = data->verts[i];
+			for (int j = i+1; j<nv; ++j)
+			{
+				if((data->verts[j]-vi).norm() < eps)
+				{
+					visited[j] = 1;
+					idx[j] = count;
+				}
+			}
+			count++;
+		}
+	}
+	new_v.resize(count);
+	data->verts = new_v;
+	int nt = data->tets.size();
+	for (int i=0; i<nt; ++i)
+	{
+		for (int j=0; j<4; ++j)
+		{
+			data->tets[i][j] = idx[data->tets[i][j]];
+		}
+	}
+}
+
+static inline void subdivide_cube(
+	LatticeData *data,
+	const std::vector<Vector3d> &cv,
+	const std::vector<int> &pts_in_box,
+	int level)
+{
+	BLI_assert((int)cv.size()==8);
+	auto add_tets_from_box = [&]()
+	{
+		Vector3d max = cv[5];
+		Vector3d min = cv[3];
+		std::vector<Vector3d> v = {
+			// Top plane, clockwise looking down
+			max,
+			Vector3d(min[0], max[1], max[2]),
+			Vector3d(min[0], max[1], min[2]),
+			Vector3d(max[0], max[1], min[2]),
+			// Bottom plane, clockwise looking down
+			Vector3d(max[0], min[1], max[2]),
+			Vector3d(min[0], min[1], max[2]),
+			min,
+			Vector3d(max[0], min[1], min[2])
+		};
+		// Add vertices and get indices of the box
+		std::vector<int> b;
+		for(int i=0; i<8; ++i)
+		{
+			b.emplace_back(data->verts.size());
+			data->verts.emplace_back(v[i]);
+		}
+		// From the box, create five new tets
+		std::vector<Vector4i> new_tets = {
+			Vector4i( b[0], b[5], b[7], b[4] ),
+			Vector4i( b[5], b[7], b[2], b[0] ),
+			Vector4i( b[5], b[0], b[2], b[1] ),
+			Vector4i( b[7], b[2], b[0], b[3] ),
+			Vector4i( b[5], b[2], b[7], b[6] )
+		};
+		for(int i=0; i<5; ++i)
+			data->tets.emplace_back(new_tets[i]);
+	};
+
+	// Add this cube because we're at bottom
+	if (level==0)
+	{
+		add_tets_from_box();
+		return;
+	}
+
+	// Only subdivide if we contain vertices
+	// Otherwise just return.
+	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)
+	{
+		int idx = pts_in_box[i];
+		if (aabb.contains(data->V->row(idx).transpose()))
+			new_pts_in_box.emplace_back(idx);
+	}
+	if (new_pts_in_box.size()==0)
+	{
+		add_tets_from_box();
+		return;
+	}
+
+	// cv are the cube vertices, listed clockwise
+	// with the bottom plane first, then top plane
+	Vector3d vfront = 0.25*(cv[0]+cv[1]+cv[5]+cv[4]); // front (+z)
+	Vector3d vback = 0.25*(cv[3]+cv[2]+cv[6]+cv[7]); // back (-z)
+	Vector3d vleft = 0.25*(cv[0]+cv[3]+cv[7]+cv[4]); // left (-x)
+	Vector3d vright = 0.25*(cv[1]+cv[2]+cv[6]+cv[5]); // right (+x)
+	Vector3d vtop = 0.25*(cv[4]+cv[5]+cv[6]+cv[7]); // top (+y)
+	Vector3d vbot = 0.25*(cv[0]+cv[1]+cv[2]+cv[3]); // bot (-y)
+	Vector3d vcent = 0.125*(cv[0]+cv[1]+cv[2]+cv[3]+cv[4]+cv[5]+cv[6]+cv[7]); // center
+	Vector3d v01 = 0.5*(cv[0]+cv[1]);
+	Vector3d v03 = 0.5*(cv[0]+cv[3]);
+	Vector3d v04 = 0.5*(cv[0]+cv[4]);
+	Vector3d v12 = 0.5*(cv[1]+cv[2]);
+	Vector3d v15 = 0.5*(cv[1]+cv[5]);
+	Vector3d v23 = 0.5*(cv[2]+cv[3]);
+	Vector3d v26 = 0.5*(cv[2]+cv[6]);
+	Vector3d v37 = 0.5*(cv[3]+cv[7]);
+	Vector3d v45 = 0.5*(cv[4]+cv[5]);
+	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);
+}
+
+
+
 bool EmbeddedMesh::generate(
 	const Eigen::MatrixXd &V, // embedded verts
 	const Eigen::MatrixXi &F, // embedded faces
@@ -70,99 +218,38 @@ bool EmbeddedMesh::generate(
 	Eigen::MatrixXd *x_tets, // lattice vertices, n x 3
 	bool trim_lattice)
 {
-	// How big the grid cells are as a fraction
-	// of the total mesh.
-	static const double GRID_FRAC = 0.15;
-
-	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)
-		aabb.extend(V.row(i).transpose());
-	
-	aabb.extend(aabb.min()-Vector3d::Ones()*1e-6);
-	aabb.extend(aabb.max()+Vector3d::Ones()*1e-6);
-	Vector3d mesh_boxmin = aabb.min();
-	Vector3d sizes = aabb.sizes();
-	double grid_dx = sizes.maxCoeff() * GRID_FRAC;
-
-	// Generate vertices and tets
-	std::vector<Vector3d> verts;
-	std::vector<Vector4i> tets;
+	int nev = V.rows();
+	std::vector<int> pts_in_box;
+	for (int i=0; i<nev; ++i)
 	{
-		std::unordered_map<std::string, int> vertex_map; // [i,j,k]->index
-
-		auto grid_hash = [&](const Vector3d &x)
-		{
-			Vector3i ll = Vector3i( // nearest gridcell
-				std::round((x[0]-mesh_boxmin[0])/grid_dx),
-				std::round((x[1]-mesh_boxmin[1])/grid_dx),
-				std::round((x[2]-mesh_boxmin[2])/grid_dx));
-			return std::to_string(ll[0])+' '+std::to_string(ll[1])+' '+std::to_string(ll[2]);
-		};
-
-		auto add_box = [&](int i_, int j_, int k_)
-		{
-			Vector3d min = mesh_boxmin + Vector3d(i_*grid_dx, j_*grid_dx, k_*grid_dx);
-			Vector3d max = mesh_boxmin + Vector3d((i_+1)*grid_dx, (j_+1)*grid_dx, (k_+1)*grid_dx);
-			std::vector<Vector3d> v = {
-				// Top plane, clockwise looking down
-				max,
-				Vector3d(min[0], max[1], max[2]),
-				Vector3d(min[0], max[1], min[2]),
-				Vector3d(max[0], max[1], min[2]),
-				// Bottom plan, clockwise looking down
-				Vector3d(max[0], min[1], max[2]),
-				Vector3d(min[0], min[1], max[2]),
-				min,
-				Vector3d(max[0], min[1], min[2])
-			};
-			// Add vertices and get indices of the box
-			std::vector<int> b;
-			for(int i=0; i<8; ++i)
-			{
-				std::string hash = grid_hash(v[i]);
-				if( vertex_map.count(hash)==0 )
-				{
-					vertex_map[hash] = verts.size();
-					verts.emplace_back(v[i]);
-				}
-				b.emplace_back(vertex_map[hash]);
-			}
-			// From the box, create five new tets
-			std::vector<Vector4i> new_tets = {
-				Vector4i( b[0], b[5], b[7], b[4] ),
-				Vector4i( b[5], b[7], b[2], b[0] ),
-				Vector4i( b[5], b[0], b[2], b[1] ),
-				Vector4i( b[7], b[2], b[0], b[3] ),
-				Vector4i( b[5], b[2], b[7], b[6] )
-			};
-			for(int i=0; i<5; ++i)
-				tets.emplace_back(new_tets[i]);
-		};
+		aabb.extend(V.row(i).transpose());
+		pts_in_box.emplace_back(i);
+	}
 
-		int ni = std::ceil(sizes[0]/grid_dx);
-		int nj = std::ceil(sizes[1]/grid_dx);
-		int nk = std::ceil(sizes[2]/grid_dx);
-		for(int i=0; i<ni; ++i)
-		{
-			for(int j=0; j<nj; ++j)
-			{
-				for(int k=0; k<nk; ++k)
-				{
-					add_box(i,j,k);
-				}
-			}
-		}
+	// Create initial box
+	aabb.extend(aabb.min()-Vector3d::Ones()*1e-4);
+	aabb.extend(aabb.max()+Vector3d::Ones()*1e-4);
+	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])
+	};
 
-	} // end create boxes and vertices
+	LatticeData data;
+	data.V = &V;
+	subdivide_cube(&data,b0,pts_in_box,3);
+	merge_close_vertices(&data);
 
 	// We only want to keep tets that are either
 	// a) intersecting the surface mesh
@@ -179,7 +266,7 @@ bool EmbeddedMesh::generate(
 				face_aabb[i].extend(V.row(f[j]).transpose());
 		}
 
-		int nt0 = tets.size();
+		int nt0 = data.tets.size();
 		std::vector<int> keep_tet(nt0,1);
 
 		AABBTree<double,3> mesh_tree;
@@ -188,8 +275,8 @@ bool EmbeddedMesh::generate(
 			.tree = &mesh_tree,
 			.pts = &V,
 			.faces = &F,
-			.tet_x = &verts,
-			.tets = &tets,
+			.tet_x = &data.verts,
+			.tets = &data.tets,
 			.keep_tet = &keep_tet
 		};
 		if (trim_lattice)
@@ -202,7 +289,8 @@ bool EmbeddedMesh::generate(
 		// Loop over tets and remove as needed.
 		// Mark referenced vertices to compute a mapping.
 		int tet_idx = 0;
-		for (std::vector<Vector4i>::iterator it = tets.begin(); it != tets.end(); ++tet_idx)
+		for (std::vector<Vector4i>::iterator it = data.tets.begin();
+			it != data.tets.end(); ++tet_idx)
 		{
 			bool keep = keep_tet[tet_idx];
 			if (keep)
@@ -214,7 +302,7 @@ bool EmbeddedMesh::generate(
 				refd_verts.emplace(t[3]);
 				++it;
 			}
-			else { it = tets.erase(it); }
+			else { it = data.tets.erase(it); }
 		}
 
 	} // end remove unnecessary tets
@@ -224,7 +312,7 @@ bool EmbeddedMesh::generate(
 		// Computing a mapping of vertices from old to new
 		// Delete any unreferenced vertices
 		std::unordered_map<int,int> vtx_old_to_new;
-		int ntv0 = verts.size(); // original num verts
+		int ntv0 = data.verts.size(); // original num verts
 		int ntv1 = refd_verts.size(); // reduced num verts
 		BLI_assert(ntv1 <= ntv0);
 		x_tets->resize(ntv1,3);
@@ -234,7 +322,7 @@ bool EmbeddedMesh::generate(
 			if (refd_verts.count(i)>0)
 			{
 				for(int j=0; j<3; ++j){
-					x_tets->operator()(vtx_count,j) = verts[i][j];
+					x_tets->operator()(vtx_count,j) = data.verts[i][j];
 				}
 				vtx_old_to_new[i] = vtx_count;
 				vtx_count++;
@@ -242,11 +330,11 @@ bool EmbeddedMesh::generate(
 		}
 
 		// Copy tets to matrix data and update vertices
-		int nt = tets.size();
+		int nt = data.tets.size();
 		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];
+				int old_idx = data.tets[i][j];
 				BLI_assert(vtx_old_to_new.count(old_idx)>0);
 				emb_mesh->tets(i,j) = vtx_old_to_new[old_idx];
 			}
@@ -259,6 +347,9 @@ bool EmbeddedMesh::generate(
 
 } // end gen lattice
 
+
+
+
 void EmbeddedMesh::compute_masses(
 	EmbeddedMeshData *emb_mesh, // where embedding is stored
 	const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3
@@ -412,6 +503,11 @@ bool EmbeddedMesh::compute_embedding(
 		}
 	}
 
+
+// Export the mesh for funsies
+std::ofstream of("v_lattice.txt"); of << *x_tets; of.close();
+std::ofstream of2("t_lattice.txt"); of2 << emb_mesh->tets; of2.close();
+
 	return true;
 
 } // end compute vtx to tet mapping
@@ -431,4 +527,4 @@ Eigen::Vector3d EmbeddedMesh::get_mapped_vertex(
 		x_data->row(tet[3]) * b[3]);
 }
 
-} // namespace admmpd
+} // namespace admmpd
\ No newline at end of file
diff --git a/extern/softbody/src/admmpd_linsolve.cpp b/extern/softbody/src/admmpd_linsolve.cpp
index 0d40c49ef4d..548e1b36dcf 100644
--- a/extern/softbody/src/admmpd_linsolve.cpp
+++ b/extern/softbody/src/admmpd_linsolve.cpp
@@ -124,11 +124,11 @@ void ConjugateGradients::solve_Ax_b(
 	VectorXd *x_,
 	VectorXd *b_)
 {
-	typedef struct LinSolveThreadData {
+	struct LinSolveThreadData {
 		SolverData *data;
 		VectorXd *ls_x;
 		VectorXd *ls_b;
-	} LinSolveThreadData;
+	};
 
 	auto parallel_lin_solve = [](
 		void *__restrict userdata,
@@ -326,7 +326,7 @@ std::cerr << "p: " << p_idx << ", q: " << q_idx << ", tet (" << k << "): " << t
 
 } // end init solve
 
-typedef struct GraphColorThreadData {
+struct GraphColorThreadData {
 	const RowSparseMatrix<double> *A;
 	int stride;
 	std::vector<std::vector<int> > *adjacency;
@@ -334,7 +334,7 @@ typedef struct GraphColorThreadData {
 	int init_palette_size;
 	std::vector<int> *conflict;
 	std::vector<int> *node_colors;
-} GraphColorThreadData;
+};
 
 // Rehash of graph coloring from
 // https://github.com/mattoverby/mclscene/blob/master/include/MCL/GraphColor.hpp
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index 9379d4f297b..6a4e9bc7e02 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -19,10 +19,10 @@
 namespace admmpd {
 using namespace Eigen;
 
-typedef struct ThreadData {
+struct ThreadData {
 	const Options *options;
 	SolverData *data;
-} ThreadData;
+};
 
 bool Solver::init(
     const Eigen::MatrixXd &V,
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index 3615fd9cd5f..688118c1246 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -29,9 +29,9 @@ struct Options {
         max_admm_iters(50),
         max_cg_iters(10),
         max_gs_iters(30),
-        mult_k(1),
+        mult_k(3),
         min_res(1e-6),
-        youngs(1000000),
+        youngs(10000000),
         poisson(0.399),
         grav(0,0,-9.8)
         {}