working on collision

5 files changed, 121 insertions, 73 deletions

diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 54bca95729e..d190bacae43 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -7,6 +7,13 @@
 namespace admmpd {
 using namespace Eigen;
 
+EmbeddedMeshCollision::VFCollisionPair::VFCollisionPair() :
+    p(-1), // point
+    p_is_obs(0), // 0 or 1
+    q(-1), // face
+    q_is_obs(0) // 0 or 1
+	{}
+
 void EmbeddedMeshCollision::set_obstacles(
 	const float *v0,
 	const float *v1,
@@ -14,42 +21,64 @@ void EmbeddedMeshCollision::set_obstacles(
 	const int *faces,
 	int nf)
 {
-	if (obs_V0.rows() != nv)
-		obs_V0.resize(nv,3);
+	if (obsdata.V0.rows() != nv)
+		obsdata.V0.resize(nv,3);
 
-	if (obs_V1.rows() != nv)
-		obs_V1.resize(nv,3);
+	if (obsdata.V1.rows() != nv)
+		obsdata.V1.resize(nv,3);
 
 	for (int i=0; i<nv; ++i)
 	{
 		for (int j=0; j<3; ++j)
 		{
-			obs_V0(i,j) = v0[i*3+j];
-			obs_V1(i,j) = v1[i*3+j];
+			obsdata.V0(i,j) = v0[i*3+j];
+			obsdata.V1(i,j) = v1[i*3+j];
 		}
 	}
 
-	if (obs_F.rows() != nf)
+	if (obsdata.F.rows() != nf)
 	{
-		obs_F.resize(nf,3);
-		obs_aabbs.resize(nf);
+		obsdata.F.resize(nf,3);
+		obsdata.aabbs.resize(nf);
 	}
 
 	for (int i=0; i<nf; ++i)
 	{
-		obs_aabbs[i].setEmpty();
+		obsdata.aabbs[i].setEmpty();
 		for (int j=0; j<3; ++j)
 		{
 			int fj = faces[i*3+j];
-			obs_F(i,j) = fj;
-			obs_aabbs[i].extend(obs_V0.row(fj).transpose());
-			obs_aabbs[i].extend(obs_V1.row(fj).transpose());
+			obsdata.F(i,j) = fj;
+			obsdata.aabbs[i].extend(obsdata.V0.row(fj).transpose());
+			obsdata.aabbs[i].extend(obsdata.V1.row(fj).transpose());
 		}
 	}
 
-	obs_tree.init(obs_aabbs);
+	obsdata.tree.init(obsdata.aabbs);
 
 } // end add obstacle
+
+typedef struct DetectThreadData {
+	const EmbeddedMeshData *mesh;
+	const EmbeddedMeshCollision::ObstacleData *obsdata;
+	const Eigen::MatrixXd *x0;
+	const Eigen::MatrixXd *x1;
+} DetectThreadData;
+
+int EmbeddedMeshCollision::detect(
+	const Eigen::MatrixXd *x0,
+	const Eigen::MatrixXd *x1)
+{
+	if (mesh==NULL)
+		return 0;
+
+	update_bvh(x0,x1);
+
+
+
+
+	return 0;
+}
 	/*
 void EmbeddedMeshCollision::detect(const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1)
 {
@@ -79,7 +108,7 @@ void EmbeddedMeshCollision::detect(const Eigen::MatrixXd *x0, const Eigen::Matri
 
 		// Check if we are inside the mesh.
 		// If so, find the nearest face in the rest pose.
-		PointInTriangleMeshTraverse<double> pt_in_mesh(pt, &obs_V1, &obs_F);
+		PointInTriangleMeshTraverse<double> pt_in_mesh(pt, &V1, &F);
 		obs_tree.traverse(pt_in_mesh);
 		if (pt_in_mesh.output.num_hits() % 2 == 1)
 		{
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index 2996249158e..4f2cb973b5c 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -16,10 +16,11 @@ namespace admmpd {
 // for-all vertices loops.
 class Collision {
 public:
-//    virtual void detect(
-//        int meshnum,
-//        const Eigen::MatrixXd *x,
-//        const Eigen::MatrixXi *faces) = 0;
+    // Returns the number of active constraints
+    virtual int detect(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1) = 0;
+
 //    virtual void jacobian(
 //        const Eigen::MatrixXd *x,
 //    	std::vector<Eigen::Triplet<double> > *trips_x,
@@ -30,27 +31,29 @@ public:
 
 // Collision detection against multiple meshes
 class EmbeddedMeshCollision : public Collision {
-protected:
-    // We progressively build a list of vertices and faces with each
-    // add_obstacle call, reindexing as needed. Then we build a tree
-    // with all of them. Alternatively we could just build separate trees and combine them.
-    Eigen::MatrixXd obs_V0, obs_V1;
-    Eigen::MatrixXi obs_F;
-    std::vector<Eigen::AlignedBox<double,3> > obs_aabbs;
-    AABBTree<double,3> obs_tree;
-
-    Eigen::MatrixXd emb_V0, emb_V1; // copy of embedded vertices
-    const Eigen::MatrixXd *emb_barys; // barys of the embedded vtx
-    const Eigen::VectorXi *vtx_to_tet; // vertex to tet embedding
-    const Eigen::MatrixXi *tets; // tets that embed faces
-
-    struct CollisionPair {
+public:
+    EmbeddedMeshCollision() :
+        mesh(NULL),
+        floor_z(-std::numeric_limits<double>::max())
+        {}
+
+    struct VFCollisionPair {
         int p; // point
+        int p_is_obs; // 0 or 1
         int q; // face
-        Eigen::Vector3d barys; // barycoords of collision
+        int q_is_obs; // 0 or 1
+        VFCollisionPair();
+        Eigen::Vector3d barys;
     };
 
-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;
+
     // I don't really like having to switch up interface style, but we'll
     // do so here to avoid copies that would happen in admmpd_api.
     void set_obstacles(
@@ -60,24 +63,27 @@ public:
         const int *faces,
         int nf);
 
-    // Updates the tetmesh BVH for self collisions
-    // TODO
-    void update_bvh(
-        const EmbeddedMeshData *mesh,
-        const Eigen::MatrixXd *x0,
-        const Eigen::MatrixXd *x1)
-        { (void)(mesh); (void)(x0); (void)(x1); }
+    // Ptr is stored after this call. Should be
+    // a smart ptr I guess...
+    void set_mesh(const EmbeddedMeshData *mesh_)
+    {
+        mesh=mesh_;
+    }
+
+    // A floor is so common that it makes sense to hard
+    // code floor collision instead of using a floor mesh.
+    void set_floor(double z)
+    {
+        floor_z = z;
+    };
 
-    // Given a list of deformable vertices (the lattice)
-    // perform collision detection of the surface mesh against
-    // obstacles and possibly self.
-    void detect(
-        const EmbeddedMeshData *mesh,
+    // Performs collision detection and stores pairs
+    int detect(
         const Eigen::MatrixXd *x0,
-        const Eigen::MatrixXd *x1){
-            
-        }
+        const Eigen::MatrixXd *x1);
 
+    // Linearizes the collision pairs about x
+    // for the constraint Kx=l
     void jacobian(
         const Eigen::MatrixXd *x,
     	std::vector<Eigen::Triplet<double> > *trips_x,
@@ -87,22 +93,24 @@ public:
     {
         
     }
+
+protected:
+    // A ptr to the embedded mesh data
+    const EmbeddedMeshData *mesh;
+    double floor_z;
+
+    // Pairs are compute on detect
+    std::vector<VFCollisionPair> vf_pairs;
+
+    // Updates the tetmesh BVH for self collisions.
+    // Called by detect()
+    // TODO
+    void update_bvh(
+        const Eigen::MatrixXd *x0,
+        const Eigen::MatrixXd *x1)
+    { (void)(x0); (void)(x1); }
 };
-/*
-class FloorCollider : public Collider {
-public:
-    virtual void detect(
-        int meshnum,
-        const Eigen::MatrixXd *x,
-        const Eigen::MatrixXi *faces);
-    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);
-};
-*/
+
 } // namespace admmpd
 
 #endif // ADMMPD_COLLISION_H_
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index 17bd8c7d008..5f81c9c565c 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -49,7 +49,8 @@ bool Solver::init(
 
 int Solver::solve(
 	const Options *options,
-	SolverData *data)
+	SolverData *data,
+	Collision *collision)
 {
 	BLI_assert(data != NULL);
 	BLI_assert(options != NULL);
@@ -71,7 +72,7 @@ int Solver::solve(
 		solve_local_step(options,data);
 
 		// Perform collision detection and linearization
-		update_constraints(options,data);
+		update_constraints(options,data,collision);
 
 		// Solve Ax=b s.t. Kx=l
 		data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u);
@@ -152,18 +153,24 @@ void Solver::solve_local_step(
 
 void Solver::update_constraints(
 	const Options *options,
-	SolverData *data)
+	SolverData *data,
+	Collision *collision)
 {
 	BLI_assert(data != NULL);
 	BLI_assert(options != NULL);
 
+	if (collision==NULL)
+		return;
+
+	collision->detect(&data->x_start, &data->x);
+
 	std::vector<double> l_coeffs;
 	std::vector<Eigen::Triplet<double> > trips_x;
     std::vector<Eigen::Triplet<double> > trips_y;
     std::vector<Eigen::Triplet<double> > trips_z;
 
 	// TODO collision detection
-//	FloorCollider().jacobian(
+//	collision->jacobian(
 //		&data->x,
 //		&trips_x,
 //		&trips_y,
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index d26da293bc8..23d79931202 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -5,6 +5,7 @@
 #define ADMMPD_SOLVER_H_
 
 #include "admmpd_types.h"
+#include "admmpd_collision.h"
 
 namespace admmpd {
 
@@ -21,15 +22,18 @@ public:
 
     // Solve a single time step.
     // Returns number of iterations.
+    // Collision ptr can be null.
     int solve(
         const Options *options,
-        SolverData *data);
+        SolverData *data,
+        Collision *collision);
 
 protected:
 
     void update_constraints(
         const Options *options,
-        SolverData *data);
+        SolverData *data,
+        Collision *collision);
 
     void init_solve(
         const Options *options,
diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h
index 925072b282f..941a92bdd94 100644
--- a/extern/softbody/src/admmpd_types.h
+++ b/extern/softbody/src/admmpd_types.h
@@ -12,6 +12,7 @@
 // TODO template type for float/double
 
 namespace admmpd {
+template <typename T> using RowSparseMatrix = Eigen::SparseMatrix<T,Eigen::RowMajor>;
 
 struct Options {
     double timestep_s; // TODO: Figure out delta time from blender api
@@ -63,7 +64,6 @@ struct SolverData {
     Eigen::MatrixXd M_xbar; // M*(x + dt v)
     Eigen::MatrixXd Dx; // D * x
     Eigen::MatrixXd b; // M xbar + DtW2(z-u)
-    template <typename T> using RowSparseMatrix = Eigen::SparseMatrix<T,Eigen::RowMajor>;
     RowSparseMatrix<double> D; // reduction matrix
     RowSparseMatrix<double> DtW2; // D'W^2
     RowSparseMatrix<double> A; // M + D'W^2D