ope forgot some files

7 files changed, 504 insertions, 12 deletions

diff --git a/extern/softbody/src/admmpd_bvh_traverse.cpp b/extern/softbody/src/admmpd_bvh_traverse.cpp
new file mode 100644
index 00000000000..0695aeef2b5
--- /dev/null
+++ b/extern/softbody/src/admmpd_bvh_traverse.cpp
@@ -0,0 +1,300 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_BVH_H_
+#define ADMMPD_BVH_H_ 1
+
+#include "admmpd_bvh_traverse.h"
+#include "admmpd_math.h"
+#include "BLI_assert.h"
+#include "BLI_math_geom.h"
+
+namespace admmpd {
+using namespace Eigen;
+
+template <typename T>
+void PointInTetMeshTraverse<T>::traverse(
+	const AABB &left_aabb, bool &go_left,
+	const AABB &right_aabb, bool &go_right,
+	bool &go_left_first )
+{
+	if (left_aabb.contains(point))
+		go_left = true;
+	if (right_aabb.contains(point))
+		go_right = true;
+	(void)(go_left_first); // doesn't matter for point-in-tet
+}
+
+template <typename T>
+bool PointInTetMeshTraverse<T>::stop_traversing(
+		const AABB &aabb, int prim )
+{
+	BLI_assert(prim_verts->cols()==3);
+	BLI_assert(prim_inds->cols()==4);
+
+	if (!aabb.contains(point))
+		return false;
+
+	RowVector4i t = prim_inds->row(prim);
+	VecType v[4] = {
+		prim_verts->row(t[0]),
+		prim_verts->row(t[1]),
+		prim_verts->row(t[2]),
+		prim_verts->row(t[3])
+	};
+
+	bool hit = point_in_tet(
+		point.template cast<double>(),
+		v[0].template cast<double>(),
+		v[1].template cast<double>(),
+		v[2].template cast<double>(),
+		v[3].template cast<double>());
+
+	if (hit)
+		output.prim = prim;
+
+	return hit; // stop traversing if hit
+}
+
+template <typename T>
+PointInTriangleMeshTraverse<T>::PointInTriangleMeshTraverse(
+	const VecType &point_,
+	const MatrixXType *prim_verts_,
+	const Eigen::MatrixXi *prim_inds_) :
+	point(point_),
+	dir(0,0,1),
+	prim_verts(prim_verts_),
+	prim_inds(prim_inds_)
+{
+	BLI_assert(prim_verts->rows()>=0);
+	BLI_assert(prim_inds->rows()>=0);
+	BLI_assert(prim_inds->cols()==3);
+	dir.normalize(); // TODO random unit vector
+	for (int i=0; i<3; ++i)
+	{
+		o[i] = (float)point[i];
+		d[i] = (float)dir[i];
+	}
+}
+
+template <typename T>
+void PointInTriangleMeshTraverse<T>::traverse(
+	const AABB &left_aabb, bool &go_left,
+	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);
+} // end point in mesh traverse
+
+template <typename T>
+bool PointInTriangleMeshTraverse<T>::stop_traversing(
+		const AABB &aabb, int prim )
+{
+	// 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;
+	}
+
+	// Get the vertices of the face in float arrays
+	// to interface with Blender kernels.
+	BLI_assert(prim >= 0 && prim < prim_inds->rows());
+	RowVector3i q_f = prim_inds->row(prim);
+	BLI_assert(q_f[0] < prim_verts->rows());
+	BLI_assert(q_f[1] < prim_verts->rows());
+	BLI_assert(q_f[2] < prim_verts->rows());
+	float q0[3], q1[3], q2[3];
+	for (int i=0; i<3; ++i)
+	{
+		q0[i] = (float)prim_verts->operator()(q_f[0],i);
+		q1[i] = (float)prim_verts->operator()(q_f[1],i);
+		q2[i] = (float)prim_verts->operator()(q_f[2],i);
+	}
+
+	// If we didn't have a triangle-triangle intersection
+	// 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);
+
+	if (hit)
+		output.hits.emplace_back(std::make_pair(prim,lambda));
+
+	return false; // multi-hit, so keep traversing
+
+} // end point in mesh stop traversing
+
+template <typename T>
+void NearestTriangleTraverse<T>::traverse(
+	const AABB &left_aabb, bool &go_left,
+	const AABB &right_aabb, bool &go_right,
+	bool &go_left_first)
+{
+	T l_d2 = left_aabb.exteriorDistance(point);
+	go_left = l_d2 < output.dist;
+	T r_d2 = right_aabb.exteriorDistance(point);
+	go_right = r_d2 < output.dist;
+	go_left_first = go_left < go_right;
+}
+
+template <typename T>
+bool NearestTriangleTraverse<T>::stop_traversing(const AABB &aabb, int prim)
+{
+	BLI_assert(prim >= 0);
+	BLI_assert(prim < prim_inds->rows());
+	BLI_assert(prim_inds->cols()==3);
+
+	T b_dist = aabb.exteriorDistance(point);
+	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)
+	{
+		output.prim = prim;
+		output.dist = d2;
+		output.pt_on_tri = pt_on_tri;		
+	}
+
+	return false;
+}
+
+template <typename T>
+TetIntersectsMeshTraverse<T>::TetIntersectsMeshTraverse(
+	const VecType points_[4],
+	const MatrixXType *prim_verts_,
+	const Eigen::MatrixXi *prim_inds_) :
+		prim_verts(prim_verts_),
+		prim_inds(prim_inds_)
+{
+	for (int i=0; i<4; ++i)
+		points[i] = points_[i];
+
+	BLI_assert(prim_verts->cols()==3);
+	BLI_assert(prim_inds->cols()==3);
+
+	for(int i=0; i<3; ++i)
+	{
+		p0[i] = (float)points[0][i];
+		p1[i] = (float)points[1][i];
+		p2[i] = (float)points[2][i];
+		p3[i] = (float)points[3][i];
+	}
+
+	tet_faces.resize(4,std::vector<float*>());
+	tet_faces[0] = {p0,p1,p2};
+	tet_faces[1] = {p0,p2,p3};
+	tet_faces[2] = {p0,p3,p1};
+	tet_faces[3] = {p1,p2,p3};
+
+	tet_aabb.setEmpty();
+	for (int i=0; i<4; ++i)
+		tet_aabb.extend(points[i]);
+
+} // end point in mesh constructor
+
+template <typename T>
+void TetIntersectsMeshTraverse<T>::traverse(
+	const AABB &left_aabb, bool &go_left,
+	const AABB &right_aabb, bool &go_right,
+	bool &go_left_first )
+{
+	go_left = false;
+	go_right = false;
+
+	if (tet_aabb.intersects(left_aabb))
+		go_left = true;
+	if (tet_aabb.intersects(right_aabb))
+		go_right = true;
+
+	go_left_first = true;
+	if (go_right && !go_left)
+		go_left_first = false;
+
+} // end point in mesh traverse
+
+template <typename T>
+bool TetIntersectsMeshTraverse<T>::stop_traversing(
+		const AABB &aabb, int prim )
+{
+	bool tet_hits_aabb = tet_aabb.intersects(aabb);
+	if(!tet_hits_aabb)
+	{
+		return false;
+	}
+
+	// Get the vertices of the face in float arrays
+	// to interface with Blender kernels.
+	BLI_assert(prim >= 0 && prim < prim_inds->rows());
+	RowVector3i q_f = prim_inds->row(prim);
+	float q0[3], q1[3], q2[3];
+	for (int i=0; i<3; ++i)
+	{
+		q0[i] = (float)prim_verts->operator()(q_f[0],i);
+		q1[i] = (float)prim_verts->operator()(q_f[1],i);
+		q2[i] = (float)prim_verts->operator()(q_f[2],i);
+	}
+
+	// If the tet-aabb intersects the triangle-aabb, then test
+	// the four faces of the tet against the triangle.
+	for (int i=0; i<4; ++i)
+	{
+		float r_i1[3] = {0,0,0};
+		float r_i2[3] = {0,0,0};
+		const std::vector<float*> &f = tet_faces[i];
+		bool hit = isect_tri_tri_epsilon_v3(
+			f[0], f[1], f[2], q0, q1, q2, r_i1, r_i2, 1e-8);
+		if (hit)
+		{
+			output.hit_face = prim;
+			return true;
+		}
+	}
+
+	return false; // multi-hit, so keep traversing
+
+} // end point in mesh stop traversing
+
+// Compile template types
+template class admmpd::PointInTetMeshTraverse<double>;
+template class admmpd::PointInTetMeshTraverse<float>;
+template class admmpd::PointInTriangleMeshTraverse<double>;
+template class admmpd::PointInTriangleMeshTraverse<float>;
+template class admmpd::NearestTriangleTraverse<double>;
+template class admmpd::NearestTriangleTraverse<float>;
+template class admmpd::TetIntersectsMeshTraverse<double>;
+template class admmpd::TetIntersectsMeshTraverse<float>;
+
+} // namespace admmpd
+
+#endif // ADMMPD_BVH_H_
+
diff --git a/extern/softbody/src/admmpd_bvh_traverse.h b/extern/softbody/src/admmpd_bvh_traverse.h
new file mode 100644
index 00000000000..774eb6a6b10
--- /dev/null
+++ b/extern/softbody/src/admmpd_bvh_traverse.h
@@ -0,0 +1,191 @@
+// Copyright Matt Overby 2020.
+// Distributed under the MIT License.
+
+#ifndef ADMMPD_BVH_TRAVERSE_H_
+#define ADMMPD_BVH_TRAVERSE_H_ 1
+
+#include <Eigen/Geometry>
+#include <vector>
+
+namespace admmpd {
+
+// Traverse class for traversing the structures.
+template <typename T, int DIM>
+class Traverser
+{
+protected:
+	typedef Eigen::AlignedBox<T,DIM> AABB;
+public:
+	// Set the boolean flags if we should go left, right, or both.
+	// Default for all booleans is true if left unchanged.
+	// Note that if stop_traversing ever returns true, it may not
+	// go left/right, even if you set go_left/go_right.
+	virtual void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first) = 0;
+
+	// Return true to stop traversing.
+	// I.e., returning true is equiv to "hit anything stop checking",
+	// finding a closest object should return false (continue traversing).
+	virtual bool stop_traversing(const AABB &aabb, int prim) = 0;
+};
+
+// Point in tet mesh traversal
+template <typename T>
+class PointInTetMeshTraverse : public Traverser<T,3>
+{
+protected:
+	using typename Traverser<T,3>::AABB;
+	typedef Eigen::Matrix<T,3,1> VecType;
+	typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType;
+
+	VecType point;
+	const MatrixXType *prim_verts;
+	const Eigen::MatrixXi *prim_inds;
+
+public:
+	struct Output {
+		int prim; // -1 if no intersections
+		Output() : prim(-1) {}
+	} output;
+
+	PointInTetMeshTraverse(
+		const VecType &point_,
+		const MatrixXType *prim_verts_,
+		const Eigen::MatrixXi *prim_inds_) :
+		point(point_),
+		prim_verts(prim_verts_),
+		prim_inds(prim_inds_)
+		{}
+
+	void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first);
+
+	bool stop_traversing(const AABB &aabb, int prim);
+};
+
+// Point in triangle mesh traversal.
+// Determined by launching a ray in a random direction from
+// the point and counting the number of (watertight) intersections. If
+// the number of intersections is odd, the point is inside th mesh.
+template <typename T>
+class PointInTriangleMeshTraverse : public Traverser<T,3>
+{
+protected:
+	using typename Traverser<T,3>::AABB;
+	typedef Eigen::Matrix<T,3,1> VecType;
+	typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType;
+
+	VecType point, dir;
+	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
+
+public:
+	struct Output {
+		std::vector< std::pair<int,double> > hits; // [prim,t]
+		int num_hits() const { return hits.size(); }
+	} output;
+
+	PointInTriangleMeshTraverse(
+		const VecType &point_,
+		const MatrixXType *prim_verts_,
+		const Eigen::MatrixXi *prim_inds_);
+
+	void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first);
+
+	// Always returns false (multi-hit, so it doesn't stop)
+	bool stop_traversing(const AABB &aabb, int prim);
+};
+
+// Search for the nearest triangle to a given point
+template <typename T>
+class NearestTriangleTraverse : public Traverser<T,3>
+{
+protected:
+	using typename Traverser<T,3>::AABB;
+	typedef Eigen::Matrix<T,3,1> VecType;
+	typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType;
+
+	VecType point;
+	const MatrixXType *prim_verts; // triangle mesh verts
+	const Eigen::MatrixXi *prim_inds; // triangle mesh inds
+
+public:
+	struct Output {
+		int prim;
+		T dist;
+		VecType pt_on_tri;
+		Output() :
+			prim(-1),
+			dist(std::numeric_limits<T>::max()),
+			pt_on_tri(0,0,0)
+			{}
+	} output;
+
+	NearestTriangleTraverse(
+		const VecType &point_,
+		const MatrixXType *prim_verts_,
+		const Eigen::MatrixXi *prim_inds_) :
+		point(point_),
+		prim_verts(prim_verts_),
+		prim_inds(prim_inds_)
+		{}
+
+	void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first);
+
+	// Always returns false
+	bool stop_traversing(const AABB &aabb, int prim);
+};
+
+// Check if a tet intersects a triangle mesh
+// with tri-tri collision tests
+template <typename T>
+class TetIntersectsMeshTraverse : public Traverser<T,3>
+{
+protected:
+	using typename Traverser<T,3>::AABB;
+	typedef Eigen::Matrix<T,3,1> VecType;
+	typedef Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> MatrixXType;
+
+	const MatrixXType *prim_verts;
+	const Eigen::MatrixXi *prim_inds;
+	VecType points[4]; // verts of the tet with proper winding (0,1,2,3)
+	float p0[3], p1[3], p2[3], p3[3]; // points casted to floats
+	std::vector<std::vector<float*> > tet_faces;
+	AABB tet_aabb;
+
+public:
+	struct Output {
+		int hit_face; // first found
+		int ray_hit_count;
+		Output() : hit_face(-1) {}
+	} output;
+
+	TetIntersectsMeshTraverse(
+		const VecType points_[4],
+		const MatrixXType *prim_verts_,
+		const Eigen::MatrixXi *prim_inds_);
+
+	void traverse(
+		const AABB &left_aabb, bool &go_left,
+		const AABB &right_aabb, bool &go_right,
+		bool &go_left_first);
+
+	// Returns true if intersection
+	bool stop_traversing(const AABB &aabb, int prim);
+};
+
+} // namespace admmpd
+
+#endif // ADMMPD_BVH_TRAVERSE_H_
+
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index be6bd0a21b7..bbda72f2428 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -247,8 +247,6 @@ int EmbeddedMeshCollision::detect(
 	return vf_pairs.size();
 } // end detect
 
-static Vector3d vx0(0,0,0);
-
 void EmbeddedMeshCollision::jacobian(
 	const Eigen::MatrixXd *x,
 	std::vector<Eigen::Triplet<double> > *trips_x,
@@ -286,7 +284,8 @@ void EmbeddedMeshCollision::jacobian(
 			int tet_idx = mesh->vtx_to_tet[emb_p_idx];
 			RowVector4i tet = mesh->tets.row(tet_idx);
 
-			// Basically a pin constraint
+			// Okay this is pretty ugly. I'm going to have to think about
+			// whether or not this is reasonable.
 			for (int j=0; j<3; ++j)
 			{
 				int c_idx = l->size();
diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp
index 3476c34c720..68cd38b27d5 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.cpp
+++ b/extern/softbody/src/admmpd_embeddedmesh.cpp
@@ -71,7 +71,7 @@ bool EmbeddedMesh::generate(
 {
 	// How big the grid cells are as a fraction
 	// of the total mesh.
-	static const double GRID_FRAC = 0.2;
+	static const double GRID_FRAC = 0.15;
 
 	if (emb_mesh==NULL)
 		return false;
diff --git a/extern/softbody/src/admmpd_embeddedmesh.h b/extern/softbody/src/admmpd_embeddedmesh.h
index 8b0cdc5bb04..85c64c7b0a5 100644
--- a/extern/softbody/src/admmpd_embeddedmesh.h
+++ b/extern/softbody/src/admmpd_embeddedmesh.h
@@ -30,7 +30,7 @@ public:
         const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3
         const Eigen::MatrixXd *x_tets, // lattice vertices, n x 3
         Eigen::VectorXd *masses_tets, // masses of the lattice verts
-        double density_kgm3 = 1100);
+        double density_kgm3 = 2100);
 
 protected:
 
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index 7e0c177ea18..ef1c1b50ca4 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -46,6 +46,8 @@ bool Solver::init(
 	if (!compute_matrices(options,data))
 		return false;
 
+	printf("Solver::init:\n\tNum tets: %d\n\tNum verts: %d\n",T.rows(),V.rows());
+
 	return true;
 } // end init
 
@@ -304,21 +306,21 @@ void Solver::solve_conjugate_gradients(
 
 	for (int iter=0; iter<options->max_cg_iters; ++iter)
 	{
-		for( int i=0; i<3; ++i )
+		for (int i=0; i<3; ++i)
 			cgdata->Ap.col(i).noalias() = cgdata->A[i]*cgdata->p.col(i);
 
 		double p_dot_Ap = mat_inner(cgdata->p,cgdata->Ap);
-		if( p_dot_Ap==0.0 )
+		if (p_dot_Ap==0.0)
 			break;
 
 		double zk_dot_rk = mat_inner(cgdata->z,cgdata->r);
-		if( zk_dot_rk==0.0 )
+		if (zk_dot_rk==0.0)
 			break;
 
 		double alpha = zk_dot_rk / p_dot_Ap;
 		data->x.noalias() += alpha * cgdata->p;
 		cgdata->r.noalias() -= alpha * cgdata->Ap;
-		if( cgdata->r.lpNorm<Infinity>() < eps )
+		if (cgdata->r.lpNorm<Infinity>() < eps)
 			break;
 
 		solve_Ax_b(data,&cgdata->z,&cgdata->r);
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index 42277180da7..13edb93c3da 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -25,11 +25,11 @@ struct Options {
     Eigen::Vector3d grav;
     Options() :
         timestep_s(1.0/24.0),
-        max_admm_iters(100),
+        max_admm_iters(50),
         max_cg_iters(10),
         mult_k(1),
         min_res(1e-6),
-        youngs(10000000),
+        youngs(1000000),
         poisson(0.299),
         grav(0,0,-9.8)
         {}
@@ -76,7 +76,7 @@ struct SolverData {
 	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > ldltA;
     struct CGData { // Temporaries used in conjugate gradients
         RowSparseMatrix<double> A[3]; // (M + D'W^2D) + k * Kt K
-        Eigen::MatrixXd b; // M xbar + DtW2(z-u) + Kt l
+        Eigen::MatrixXd b; // M xbar + DtW2(z-u) + k Kt l
         Eigen::MatrixXd r; // residual
         Eigen::MatrixXd z;
         Eigen::MatrixXd p;