working on nearest-face traversal

5 files changed, 190 insertions, 35 deletions

diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index d190bacae43..78675ea3a0b 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -2,12 +2,15 @@
 // Distributed under the MIT License.
 
 #include "admmpd_collision.h"
+#include "admmpd_bvh_traverse.h"
 #include "BLI_assert.h"
+#include "BLI_task.h"
+#include "BLI_threads.h"
 
 namespace admmpd {
 using namespace Eigen;
 
-EmbeddedMeshCollision::VFCollisionPair::VFCollisionPair() :
+VFCollisionPair::VFCollisionPair() :
     p(-1), // point
     p_is_obs(0), // 0 or 1
     q(-1), // face
@@ -18,7 +21,7 @@ void EmbeddedMeshCollision::set_obstacles(
 	const float *v0,
 	const float *v1,
 	int nv,
-	const int *faces,
+	const unsigned int *faces,
 	int nf)
 {
 	if (obsdata.V0.rows() != nv)
@@ -63,8 +66,47 @@ typedef struct DetectThreadData {
 	const EmbeddedMeshCollision::ObstacleData *obsdata;
 	const Eigen::MatrixXd *x0;
 	const Eigen::MatrixXd *x1;
+	std::vector<std::vector<VFCollisionPair> > *pt_vf_pairs; // per thread pairs
 } DetectThreadData;
 
+static void parallel_detect(
+	void *__restrict userdata,
+	const int i,
+	const TaskParallelTLS *__restrict tls)
+{
+	// Comments say "don't use this" but how else am I supposed
+	// to get the thread ID?
+	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]) +
+		bary[1] * td->x1->row(tet[1]) +
+		bary[2] * td->x1->row(tet[2]) +
+		bary[3] * td->x1->row(tet[3]);
+
+	// TODO
+	// This won't work for overlapping obstacles.
+	// We would instead need something like a signed distance field
+	// or continuous collision detection.
+
+	PointInTriangleMeshTraverse<double> pt_in_mesh(
+		pt, &td->obsdata->V1, &td->obsdata->F);
+	td->obsdata->tree.traverse(pt_in_mesh);
+	if (pt_in_mesh.output.num_hits() % 2 != 1)
+		return;
+
+	// If we are inside an obstacle, we
+	// have to project to the nearest surface
+
+} // end parallel lin solve
+
 int EmbeddedMeshCollision::detect(
 	const Eigen::MatrixXd *x0,
 	const Eigen::MatrixXd *x1)
@@ -74,10 +116,32 @@ int EmbeddedMeshCollision::detect(
 
 	update_bvh(x0,x1);
 
+	int max_threads = std::max(1,BLI_system_thread_count());
+	std::vector<std::vector<VFCollisionPair> > pt_vf_pairs
+		(max_threads, std::vector<VFCollisionPair>());
+
+	DetectThreadData thread_data = {
+		.mesh = mesh,
+		.obsdata = &obsdata,
+		.x0 = x0,
+		.x1 = x1,
+		.pt_vf_pairs = &pt_vf_pairs
+	};
+
+	int nv = x1->rows();
+	TaskParallelSettings settings;
+	BLI_parallel_range_settings_defaults(&settings);
+	BLI_task_parallel_range(0, nv, &thread_data, parallel_detect, &settings);
+
+	// Combine thread-local results
+	vf_pairs.clear();
+	for (int i=0; i<max_threads; ++i)
+	{
+		const std::vector<VFCollisionPair> &tl_pairs = pt_vf_pairs[i];
+		vf_pairs.insert(vf_pairs.end(), tl_pairs.begin(), tl_pairs.end());
+	}
 
-
-
-	return 0;
+	return vf_pairs.size();
 }
 	/*
 void EmbeddedMeshCollision::detect(const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1)
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
index 4f2cb973b5c..34b870a30f3 100644
--- a/extern/softbody/src/admmpd_collision.h
+++ b/extern/softbody/src/admmpd_collision.h
@@ -9,6 +9,15 @@
 
 namespace admmpd {
 
+struct VFCollisionPair {
+    int p; // point
+    int p_is_obs; // 0 or 1
+    int q; // face
+    int q_is_obs; // 0 or 1
+    VFCollisionPair();
+    Eigen::Vector3d barys;
+};
+
 // I'll update this class/structure another day.
 // For now let's get something in place to do floor collisions.
 // Probably will work better to use uber-collision class for
@@ -16,11 +25,20 @@ namespace admmpd {
 // for-all vertices loops.
 class Collision {
 public:
+    virtual ~Collision() {}
+
     // Returns the number of active constraints
     virtual int detect(
         const Eigen::MatrixXd *x0,
         const Eigen::MatrixXd *x1) = 0;
 
+    virtual void set_obstacles(
+        const float *v0,
+        const float *v1,
+        int nv,
+        const unsigned int *faces,
+        int nf) = 0;
+
 //    virtual void jacobian(
 //        const Eigen::MatrixXd *x,
 //    	std::vector<Eigen::Triplet<double> > *trips_x,
@@ -32,20 +50,11 @@ public:
 // Collision detection against multiple meshes
 class EmbeddedMeshCollision : public Collision {
 public:
-    EmbeddedMeshCollision() :
-        mesh(NULL),
+    EmbeddedMeshCollision(const EmbeddedMeshData *mesh_) :
+        mesh(mesh_),
         floor_z(-std::numeric_limits<double>::max())
         {}
 
-    struct VFCollisionPair {
-        int p; // point
-        int p_is_obs; // 0 or 1
-        int q; // face
-        int q_is_obs; // 0 or 1
-        VFCollisionPair();
-        Eigen::Vector3d barys;
-    };
-
     // Obstacle data created in set_obstacles
     struct ObstacleData {
         Eigen::MatrixXd V0, V1;
@@ -60,16 +69,9 @@ public:
         const float *v0,
         const float *v1,
         int nv,
-        const int *faces,
+        const unsigned int *faces,
         int nf);
 
-    // 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)
diff --git a/intern/softbody/admmpd_api.cpp b/intern/softbody/admmpd_api.cpp
index 4fa549211d3..302f066ef5b 100644
--- a/intern/softbody/admmpd_api.cpp
+++ b/intern/softbody/admmpd_api.cpp
@@ -25,6 +25,8 @@
 #include "admmpd_types.h"
 #include "admmpd_solver.h"
 #include "admmpd_embeddedmesh.h"
+#include "admmpd_collision.h"
+
 #include "tetgen_api.h"
 #include "DNA_mesh_types.h" // Mesh
 #include "DNA_meshdata_types.h" // MVert
@@ -40,6 +42,7 @@ struct ADMMPDInternalData {
   admmpd::Options *options;
   admmpd::SolverData *data;
   admmpd::EmbeddedMeshData *embmesh;
+  admmpd::Collision *collision;
   int in_totverts; // number of input verts
 };
 
@@ -52,12 +55,14 @@ void admmpd_dealloc(ADMMPDInterfaceData *iface)
 
   if (iface->idata)
   {
-    if(iface->idata->options)
-        delete iface->idata->options;
-    if(iface->idata->data)
-        delete iface->idata->data;
-    if(iface->idata->embmesh)
-        delete iface->idata->embmesh;
+    if (iface->idata->options)
+      delete iface->idata->options;
+    if (iface->idata->data)
+      delete iface->idata->data;
+    if (iface->idata->embmesh)
+      delete iface->idata->embmesh;
+    if (iface->idata->collision)
+      delete iface->idata->collision;
     delete iface->idata;
   }
 
@@ -163,6 +168,7 @@ int admmpd_init(ADMMPDInterfaceData *iface, float *in_verts, unsigned int *in_fa
   iface->idata->data = new admmpd::SolverData();
   admmpd::SolverData *data = iface->idata->data;
   iface->idata->embmesh = new admmpd::EmbeddedMeshData();
+  iface->idata->collision = NULL;
 
   // Generate tets and vertices
   Eigen::MatrixXd V;
@@ -173,9 +179,11 @@ int admmpd_init(ADMMPDInterfaceData *iface, float *in_verts, unsigned int *in_fa
     default:
     case 0:
       gen_success = admmpd_init_with_tetgen(iface,in_verts,in_faces,&V,&T);
+      //iface->idata->collision = new admmpd::EmbeddedMeshData();
       break;
     case 1:
       gen_success = admmpd_init_with_lattice(iface,in_verts,in_faces,&V,&T);
+      iface->idata->collision = new admmpd::EmbeddedMeshCollision(iface->idata->embmesh);
       break;
   }
   if (!gen_success || iface->totverts==0)
@@ -220,6 +228,25 @@ void admmpd_copy_from_bodypoint(ADMMPDInterfaceData *iface, const BodyPoint *pts
   }
 }
 
+void admmpd_update_obstacles(
+    ADMMPDInterfaceData *iface,
+    float *in_verts_0,
+    float *in_verts_1,
+    int nv,
+    unsigned int *in_faces,
+    int nf)
+{
+    if (iface==NULL || in_verts_0==NULL || in_verts_1==NULL || in_faces==NULL)
+      return;
+    if (iface->idata==NULL)
+      return;
+    if (iface->idata->collision==NULL)
+      return;
+
+    iface->idata->collision->set_obstacles(
+      in_verts_0, in_verts_1, nv, in_faces, nf);
+}
+
 void admmpd_copy_to_bodypoint_and_object(ADMMPDInterfaceData *iface, BodyPoint *pts, float (*vertexCos)[3])
 {
 
@@ -276,7 +303,7 @@ void admmpd_solve(ADMMPDInterfaceData *iface)
 
   try
   {
-    admmpd::Solver().solve(iface->idata->options,iface->idata->data,NULL);
+    admmpd::Solver().solve(iface->idata->options,iface->idata->data,iface->idata->collision);
   }
   catch(const std::exception &e)
   {
diff --git a/intern/softbody/admmpd_api.h b/intern/softbody/admmpd_api.h
index bd4ff9a3a1f..f29a3bbdc7d 100644
--- a/intern/softbody/admmpd_api.h
+++ b/intern/softbody/admmpd_api.h
@@ -54,6 +54,15 @@ int admmpd_init(ADMMPDInterfaceData*, float *in_verts, unsigned int *in_faces);
 // to internal vertex position and velocity
 void admmpd_copy_from_bodypoint(ADMMPDInterfaceData*, const BodyPoint *pts);
 
+// Sets the obstacle data for collisions
+void admmpd_update_obstacles(
+    ADMMPDInterfaceData*,
+    float *in_verts_0,
+    float *in_verts_1,
+    int nv,
+    unsigned int *in_faces,
+    int nf);
+
 // Copies internal vertex position and velocity data
 // to BodyPoints (from SoftBody) AND surface mesh vertices.
 // If pts or vertexCos is null, its skipped
diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index 12f44013bea..e4f04fbfe1f 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -3644,6 +3644,10 @@ static void admmpd_update_collider(
   Collection *collection,
   Object *vertexowner)
 {
+  SoftBody *sb = vertexowner->soft;
+  if (sb->admmpd == NULL)
+    return;
+
   unsigned int numobjects;
   Object **objects = BKE_collision_objects_create(
       depsgraph,
@@ -3652,18 +3656,67 @@ static void admmpd_update_collider(
       &numobjects,
       eModifierType_Collision);
 
+  // How many faces and vertices do we need to allocate?
+  int tot_verts = 0;
+  int tot_faces = 0;
   for (int i = 0; i < numobjects; ++i) {
     Object *ob = objects[i];
     if (ob->type == OB_MESH) {
       if (ob->pd && ob->pd->deflect){
-        ccd_Mesh *ccdmesh = ccd_mesh_make(ob);
-        printf("tri num %d\n", ccdmesh->tri_num);
-        ccd_mesh_free(ccdmesh);
+        CollisionModifierData *cmd = (CollisionModifierData *)
+          BKE_modifiers_findby_type(ob, eModifierType_Collision);
+        if (!cmd)
+          continue;
+        tot_verts += cmd->mvert_num;
+        tot_faces += cmd->tri_num;
       }
-      //ccd_build_deflector_hash_single(hash, ob);
     }
   }
 
+  float *obs_v0 = MEM_callocN(sizeof(float)*3*tot_verts, "obs_v0");
+  float *obs_v1 = MEM_callocN(sizeof(float)*3*tot_verts, "obs_v1");
+  unsigned int *obs_faces = MEM_callocN(sizeof(unsigned int)*3*tot_faces, "obs_faces");
+
+  // Copy over vertices and faces
+  int curr_verts = 0;
+  int curr_faces = 0;
+  for (int i = 0; i < numobjects; ++i) {
+    Object *ob = objects[i];
+    if (ob->type == OB_MESH) {
+      if (ob->pd && ob->pd->deflect){
+        CollisionModifierData *cmd = (CollisionModifierData *)
+          BKE_modifiers_findby_type(ob, eModifierType_Collision);
+        if (!cmd)
+          continue;
+
+        for (int j=0; j<cmd->mvert_num; ++j) {
+          int v_idx = j*3 + curr_verts;
+          for (int k=0; k<3; ++k) { 
+            obs_v0[v_idx+k] = cmd->x[j].co[k];
+            obs_v1[v_idx+k] = cmd->xnew[j].co[k];
+          }
+        }
+
+        for (int j=0; j<cmd->tri_num; ++j) {
+          int f_idx = j*3 + curr_faces;
+          for (int k=0; k<3; ++k) { 
+            obs_faces[f_idx+k] = cmd->tri[j].tri[k] + curr_verts;
+          }
+        }
+
+        curr_verts += cmd->mvert_num;
+        curr_faces += cmd->tri_num;
+      }
+    }
+  }
+
+  // Update via API
+  admmpd_update_obstacles(sb->admmpd, obs_v0, obs_v1, tot_verts, obs_faces, tot_faces);
+
+  // Cleanup
+  MEM_freeN(obs_v0);
+  MEM_freeN(obs_v1);
+  MEM_freeN(obs_faces);
   BKE_collision_objects_free(objects);
 }