obstacle collisions. REALLY need to improve mass computation

8 files changed, 231 insertions, 123 deletions

diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 78675ea3a0b..98c5653e7f7 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -3,18 +3,24 @@
 
 #include "admmpd_collision.h"
 #include "admmpd_bvh_traverse.h"
+#include "admmpd_math.h"
+
 #include "BLI_assert.h"
 #include "BLI_task.h"
 #include "BLI_threads.h"
 
+#include <iostream>
+
 namespace admmpd {
 using namespace Eigen;
 
 VFCollisionPair::VFCollisionPair() :
-    p(-1), // point
+    p_idx(-1), // point
     p_is_obs(0), // 0 or 1
-    q(-1), // face
-    q_is_obs(0) // 0 or 1
+    q_idx(-1), // face
+    q_is_obs(0), // 0 or 1
+	pt_on_q(0,0,0),
+	q_n(0,0,0)
 	{}
 
 void EmbeddedMeshCollision::set_obstacles(
@@ -66,24 +72,26 @@ typedef struct DetectThreadData {
 	const EmbeddedMeshCollision::ObstacleData *obsdata;
 	const Eigen::MatrixXd *x0;
 	const Eigen::MatrixXd *x1;
+	double floor_z;
 	std::vector<std::vector<VFCollisionPair> > *pt_vf_pairs; // per thread pairs
 } DetectThreadData;
 
-static void parallel_detect(
+static void parallel_detect_obstacles(
 	void *__restrict userdata,
 	const int i,
 	const TaskParallelTLS *__restrict tls)
 {
+	DetectThreadData *td = (DetectThreadData*)userdata;
+
 	// Comments say "don't use this" but how else am I supposed
-	// to get the thread ID?
+	// to get the thread ID? It appears to return the right thing...
 	int thread_idx = BLI_task_parallel_thread_id(tls);
-	DetectThreadData *td = (DetectThreadData*)userdata;
 	std::vector<VFCollisionPair> &tl_pairs = td->pt_vf_pairs->at(thread_idx);
 
 	int tet_idx = td->mesh->vtx_to_tet[i];
 	RowVector4i tet = td->mesh->tets.row(tet_idx);
 	Vector4d bary = td->mesh->barys.row(i);
-	
+
 	// First, get the surface vertex
 	Vector3d pt = 
 		bary[0] * td->x1->row(tet[0]) +
@@ -91,6 +99,23 @@ 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 = 0;
+		pair.q_idx = -1;
+		pair.q_is_obs = 1;
+		pair.pt_on_q = Vector3d(pt[0],pt[1],td->floor_z);
+		pair.q_n = Vector3d(0,0,1);
+	}
+
+	// Any obstacles?
+	if (td->obsdata->F.rows()==0)
+		return;
+
 	// TODO
 	// This won't work for overlapping obstacles.
 	// We would instead need something like a signed distance field
@@ -103,9 +128,51 @@ static void parallel_detect(
 		return;
 
 	// If we are inside an obstacle, we
-	// have to project to the nearest surface
+	// have to project to the nearest surface.
+
+	// TODO
+	// Consider replacing this with BLI codes:
+	// BLI_bvhtree_find_nearest in BLI_kdopbvh.h
+	// But since it doesn't have a point-in-mesh
+	// detection, we may as use our own BVH/traverser
+	// for now.
+
+	NearestTriangleTraverse<double> find_nearest_tri(
+		pt, &td->obsdata->V1, &td->obsdata->F);
+	td->obsdata->tree.traverse(find_nearest_tri);
+	if (find_nearest_tri.output.prim < 0) // error
+		return;
+
+	// Create collision pair
+	tl_pairs.emplace_back();
+	VFCollisionPair &pair = tl_pairs.back();
+	pair.p_idx = i;
+	pair.p_is_obs = 0;
+	pair.q_idx = find_nearest_tri.output.prim;
+	pair.q_is_obs = 1;
+	pair.pt_on_q = find_nearest_tri.output.pt_on_tri;
+
+	// Compute face normal
+	BLI_assert(pair.q_idx >= 0);
+	BLI_assert(pair.q_idx < td->obsdata->F.rows());
+	RowVector3i tri_inds = td->obsdata->F.row(pair.q_idx);
+	Vector3d tri[3] = {
+		td->obsdata->V1.row(tri_inds[0]),
+		td->obsdata->V1.row(tri_inds[1]),
+		td->obsdata->V1.row(tri_inds[2])
+	};
+	pair.q_n = ((tri[1]-tri[0]).cross(tri[2]-tri[0])).normalized();
+
+} // end parallel detect against obstacles
 
-} // end parallel lin solve
+static void parallel_detect(
+	void *__restrict userdata,
+	const int i,
+	const TaskParallelTLS *__restrict tls)
+{
+	parallel_detect_obstacles(userdata,i,tls);
+	//parallel_detect_self(userdata,i,tls);	
+} // end parallel detect
 
 int EmbeddedMeshCollision::detect(
 	const Eigen::MatrixXd *x0,
@@ -120,18 +187,36 @@ int EmbeddedMeshCollision::detect(
 	std::vector<std::vector<VFCollisionPair> > pt_vf_pairs
 		(max_threads, std::vector<VFCollisionPair>());
 
+
+
+
+
+
+floor_z = -2;
+
+
+
+
+
+
+
+
+
+
+
 	DetectThreadData thread_data = {
 		.mesh = mesh,
 		.obsdata = &obsdata,
 		.x0 = x0,
 		.x1 = x1,
+		.floor_z = floor_z,
 		.pt_vf_pairs = &pt_vf_pairs
 	};
 
-	int nv = x1->rows();
+	int nev = mesh->x_rest.rows();
 	TaskParallelSettings settings;
 	BLI_parallel_range_settings_defaults(&settings);
-	BLI_task_parallel_range(0, nv, &thread_data, parallel_detect, &settings);
+	BLI_task_parallel_range(0, nev, &thread_data, parallel_detect, &settings);
 
 	// Combine thread-local results
 	vf_pairs.clear();
@@ -142,80 +227,55 @@ int EmbeddedMeshCollision::detect(
 	}
 
 	return vf_pairs.size();
-}
-	/*
-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, &V1, &F);
-		obs_tree.traverse(pt_in_mesh);
-		if (pt_in_mesh.output.num_hits() % 2 == 1)
-		{
-			// Find 
-//			hit = true;
-		}
-
-//		colliding[i] = hit;
-	}
+} // end detect
 
-	// 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);
-	// Can just do detection in jacobian I guess.
-}
-
-void FloorCollider::jacobian(
+void EmbeddedMeshCollision::jacobian(
 	const Eigen::MatrixXd *x,
 	std::vector<Eigen::Triplet<double> > *trips_x,
 	std::vector<Eigen::Triplet<double> > *trips_y,
 	std::vector<Eigen::Triplet<double> > *trips_z,
 	std::vector<double> *l)
 {
-	const double floor_z = -2.0;
+	BLI_assert(x != NULL);
+	BLI_assert(x->cols() == 3);
+
+	int np = vf_pairs.size();
+	if (np==0)
+		return;
+
+	l->reserve((int)l->size() + np);
+	trips_x->reserve((int)trips_x->size() + np*3);
+	trips_y->reserve((int)trips_y->size() + np*3);
+	trips_z->reserve((int)trips_z->size() + np*3);
 
-	int nx = x->rows();
-	for (int i=0; i<nx; ++i)
+	for (int i=0; i<np; ++i)
 	{
-		Eigen::Vector3d xi = x->row(i);
-		if (xi[2]>floor_z)
+		VFCollisionPair &pair = vf_pairs[i];
+
+		int emb_p_idx = pair.p_idx;
+		if (pair.p_is_obs)
+		{ // TODO: obstacle point intersecting mesh
 			continue;
+		}
+
+		// Obstacle collision
+		if (pair.q_is_obs)
+		{
+			RowVector4d bary = mesh->barys.row(emb_p_idx);
+			int tet_idx = mesh->vtx_to_tet[emb_p_idx];
+			RowVector4i tet = mesh->tets.row(tet_idx);
+			int c_idx = l->size();
+			for( int j=0; j<4; ++j ){
+				trips_x->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[0]);
+				trips_y->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[1]);
+				trips_z->emplace_back(c_idx, tet[j], bary[j]*pair.q_n[2]);
+			}
+			l->emplace_back( pair.q_n.dot(pair.pt_on_q));
+			continue;
+		}
+
+	} // end loop pairs
+
+} // end jacobian
 
-		int c_idx = l->size();
-		trips_z->emplace_back(c_idx,i,1.0);
-		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 34b870a30f3..f49a314db47 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -10,12 +10,15 @@
 namespace admmpd {
 
 struct VFCollisionPair {
-    int p; // point
+    int p_idx; // point
     int p_is_obs; // 0 or 1
-    int q; // face
+    int q_idx; // face
     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();
-    Eigen::Vector3d barys;
 };
 
 // I'll update this class/structure another day.
@@ -27,11 +30,13 @@ class Collision {
 public:
     virtual ~Collision() {}
 
-    // Returns the number of active constraints
+    // Performs collision detection.
+    // Returns the number of active constraints.
     virtual int detect(
         const Eigen::MatrixXd *x0,
         const Eigen::MatrixXd *x1) = 0;
 
+    // Set the soup of obstacles for this time step.
     virtual void set_obstacles(
         const float *v0,
         const float *v1,
@@ -39,12 +44,18 @@ public:
         const unsigned int *faces,
         int nf) = 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;
+    // Special case for floor since it's common.
+    virtual void set_floor(double z) = 0;
+
+    // Linearize the constraints and return Jacobian tensor.
+    // Constraints are linearized about x for constraint
+    // K x = l
+    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
@@ -91,10 +102,7 @@ public:
     	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)
-    {
-        
-    }
+		std::vector<double> *l);
 
 protected:
     // A ptr to the embedded mesh data
diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp
index 0fd012c34f3..78993ff73da 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.cpp
+++ b/extern/softbody/src/admmpd_embeddedmesh.cpp
@@ -69,6 +69,10 @@ bool EmbeddedMesh::generate(
 	EmbeddedMeshData *emb_mesh, // where embedding is stored
 	Eigen::MatrixXd *x_tets) // lattice vertices, n x 3
 {
+	// How big the grid cells are as a fraction
+	// of the total mesh.
+	static const double GRID_FRAC = 0.2;
+
 	if (emb_mesh==NULL)
 		return false;
 	if (x_tets==NULL)
@@ -86,7 +90,7 @@ bool EmbeddedMesh::generate(
 	aabb.extend(aabb.max()+Vector3d::Ones()*1e-6);
 	Vector3d mesh_boxmin = aabb.min();
 	Vector3d sizes = aabb.sizes();
-	double grid_dx = sizes.maxCoeff() * 0.2;
+	double grid_dx = sizes.maxCoeff() * GRID_FRAC;
 
 	// Generate vertices and tets
 	std::vector<Vector3d> verts;
diff --git a/extern/softbody/src/admmpd_math.cpp b/extern/softbody/src/admmpd_math.cpp
index 7d1a4f82844..88451e60ee1 100644
--- a/extern/softbody/src/admmpd_math.cpp
+++ b/extern/softbody/src/admmpd_math.cpp
@@ -4,47 +4,74 @@
 #include "admmpd_math.h"
 
 namespace admmpd {
+using namespace Eigen;
+
 namespace barycoords {
 
-    Eigen::Matrix<double,4,1> point_tet(
-        const Eigen::Vector3d &p,
-        const Eigen::Vector3d &a,
-        const Eigen::Vector3d &b,
-        const Eigen::Vector3d &c,
-        const Eigen::Vector3d &d)
+    Matrix<double,4,1> point_tet(
+        const Vector3d &p,
+        const Vector3d &a,
+        const Vector3d &b,
+        const Vector3d &c,
+        const Vector3d &d)
     {
 		auto scalar_triple_product = [](
-			const Eigen::Vector3d &u,
-			const Eigen::Vector3d &v,
-			const Eigen::Vector3d &w )
+			const Vector3d &u,
+			const Vector3d &v,
+			const Vector3d &w )
 		{
 			return u.dot(v.cross(w));
 		};
-		Eigen::Vector3d ap = p - a;
-		Eigen::Vector3d bp = p - b;
-		Eigen::Vector3d ab = b - a;
-		Eigen::Vector3d ac = c - a;
-		Eigen::Vector3d ad = d - a;
-		Eigen::Vector3d bc = c - b;
-		Eigen::Vector3d bd = d - b;
+		Vector3d ap = p - a;
+		Vector3d bp = p - b;
+		Vector3d ab = b - a;
+		Vector3d ac = c - a;
+		Vector3d ad = d - a;
+		Vector3d bc = c - b;
+		Vector3d bd = d - b;
 		double va6 = scalar_triple_product(bp, bd, bc);
 		double vb6 = scalar_triple_product(ap, ac, ad);
 		double vc6 = scalar_triple_product(ap, ad, ab);
 		double vd6 = scalar_triple_product(ap, ab, ac);
 		double v6 = 1.0 / scalar_triple_product(ab, ac, ad);
-		return Eigen::Matrix<double,4,1>(va6*v6, vb6*v6, vc6*v6, vd6*v6);
+		return Matrix<double,4,1>(va6*v6, vb6*v6, vc6*v6, vd6*v6);
     } // end point tet barycoords
 
+	// From Real-Time Collision Detection by Christer Ericson
+    Vector3d point_triangle(
+        const Vector3d &p,
+        const Vector3d &a,
+        const Vector3d &b,
+        const Vector3d &c)
+    {
+		Vector3d v0 = b - a;
+		Vector3d v1 = c - a;
+		Vector3d v2 = p - a;
+		double d00 = v0.dot(v0);
+		double d01 = v0.dot(v1);
+		double d11 = v1.dot(v1);
+		double d20 = v2.dot(v0);
+		double d21 = v2.dot(v1);
+		double denom = d00 * d11 - d01 * d01;
+		if (std::abs(denom)<=0)
+			return Vector3d::Zero();
+		Vector3d r;
+		r[1] = (d11 * d20 - d01 * d21) / denom;
+		r[2] = (d00 * d21 - d01 * d20) / denom;
+		r[0] = 1.0 - r[1] - r[2];
+		return r;
+	} // end point triangle barycoords
+
 } // namespace barycoords
 
 // Checks that it's on the "correct" side of the normal
 // for each face of the tet. Assumes winding points inward.
 bool point_in_tet(
-	const Eigen::Vector3d &p,
-	const Eigen::Vector3d &a,
-	const Eigen::Vector3d &b,
-	const Eigen::Vector3d &c,
-	const Eigen::Vector3d &d)
+	const Vector3d &p,
+	const Vector3d &a,
+	const Vector3d &b,
+	const Vector3d &c,
+	const Vector3d &d)
 {
 	using namespace Eigen;
 	auto check_face = [](
diff --git a/extern/softbody/src/admmpd_math.h b/extern/softbody/src/admmpd_math.h
index e577d6b9dd7..939d2d3b230 100644
--- a/extern/softbody/src/admmpd_math.h
+++ b/extern/softbody/src/admmpd_math.h
@@ -16,6 +16,12 @@ Eigen::Vector4d point_tet(
     const Eigen::Vector3d &c,
     const Eigen::Vector3d &d);
 
+Eigen::Vector3d point_triangle(
+    const Eigen::Vector3d &p,
+    const Eigen::Vector3d &a,
+    const Eigen::Vector3d &b,
+    const Eigen::Vector3d &c);
+
 } // namespace barycoords
 
 bool point_in_tet(
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index 5f81c9c565c..f3f9b19ee0f 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -170,12 +170,12 @@ void Solver::update_constraints(
     std::vector<Eigen::Triplet<double> > trips_z;
 
 	// TODO collision detection
-//	collision->jacobian(
-//		&data->x,
-//		&trips_x,
-//		&trips_y,
-//		&trips_z,
-//		&l_coeffs);
+	collision->jacobian(
+		&data->x,
+		&trips_x,
+		&trips_y,
+		&trips_z,
+		&l_coeffs);
 
 	// Check number of constraints.
 	// If no constraints, clear Jacobian.
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index 23d79931202..1744b0f80c9 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -53,6 +53,9 @@ protected:
         const Options *options,
         SolverData *data);
 
+    // TODO we need to compute masses
+    // based on the embedded mesh, otherwise
+    // we get super ugly dynamics.
     void compute_masses(
         const Options *options,
         SolverData *data);
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index 941a92bdd94..de1fb122a30 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -26,10 +26,10 @@ struct Options {
     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),
+        max_admm_iters(40),
+        max_cg_iters(20),
+        mult_k(1),
+        min_res(1e-6),
         density_kgm3(1100),
         youngs(100000000),
         poisson(0.399),