Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/softbody/src/admmpd_collision.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/softbody/src/admmpd_collision.cpp')
-rw-r--r--extern/softbody/src/admmpd_collision.cpp280
1 files changed, 202 insertions, 78 deletions
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 98c5653e7f7..be6bd0a21b7 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -10,6 +10,7 @@
#include "BLI_threads.h"
#include <iostream>
+#include <sstream>
namespace admmpd {
using namespace Eigen;
@@ -23,7 +24,7 @@ VFCollisionPair::VFCollisionPair() :
q_n(0,0,0)
{}
-void EmbeddedMeshCollision::set_obstacles(
+void Collision::set_obstacles(
const float *v0,
const float *v1,
int nv,
@@ -67,53 +68,32 @@ void EmbeddedMeshCollision::set_obstacles(
} // end add obstacle
-typedef struct DetectThreadData {
- const EmbeddedMeshData *mesh;
- 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_obstacles(
- void *__restrict userdata,
- const int i,
- const TaskParallelTLS *__restrict tls)
+void Collision::detect_discrete_vf(
+ const Eigen::Vector3d &pt,
+ int pt_idx,
+ bool pt_is_obs,
+ const AABBTree<double,3> *mesh_tree,
+ const Eigen::MatrixXd *mesh_x,
+ const Eigen::MatrixXi *mesh_tris,
+ bool mesh_is_obs,
+ double floor_z,
+ std::vector<VFCollisionPair> *pairs)
{
- DetectThreadData *td = (DetectThreadData*)userdata;
-
- // Comments say "don't use this" but how else am I supposed
- // to get the thread ID? It appears to return the right thing...
- int thread_idx = BLI_task_parallel_thread_id(tls);
- 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]);
-
// Special case, check if we are below the floor
- if (pt[2] < td->floor_z)
+ if (pt[2] < floor_z)
{
- tl_pairs.emplace_back();
- VFCollisionPair &pair = tl_pairs.back();
- pair.p_idx = i;
- pair.p_is_obs = 0;
+ 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],td->floor_z);
+ pair.pt_on_q = Vector3d(pt[0],pt[1],floor_z);
pair.q_n = Vector3d(0,0,1);
}
- // Any obstacles?
- if (td->obsdata->F.rows()==0)
+ // Any faces to detect against?
+ if (mesh_tris->rows()==0)
return;
// TODO
@@ -122,8 +102,8 @@ static void parallel_detect_obstacles(
// or continuous collision detection.
PointInTriangleMeshTraverse<double> pt_in_mesh(
- pt, &td->obsdata->V1, &td->obsdata->F);
- td->obsdata->tree.traverse(pt_in_mesh);
+ pt, mesh_x, mesh_tris);
+ mesh_tree->traverse(pt_in_mesh);
if (pt_in_mesh.output.num_hits() % 2 != 1)
return;
@@ -138,40 +118,93 @@ static void parallel_detect_obstacles(
// for now.
NearestTriangleTraverse<double> find_nearest_tri(
- pt, &td->obsdata->V1, &td->obsdata->F);
- td->obsdata->tree.traverse(find_nearest_tri);
+ pt, mesh_x, mesh_tris);
+ mesh_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;
+ pairs->emplace_back();
+ VFCollisionPair &pair = pairs->back();
+ pair.p_idx = pt_idx;
+ pair.p_is_obs = pt_is_obs;
pair.q_idx = find_nearest_tri.output.prim;
- pair.q_is_obs = 1;
+ pair.q_is_obs = mesh_is_obs;
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);
+ BLI_assert(pair.q_idx < mesh_tris->rows());
+ RowVector3i tri_inds = mesh_tris->row(pair.q_idx);
+ BLI_assert(tri_inds[0]>=0 && tri_inds[0]<mesh_x->rows());
+ BLI_assert(tri_inds[1]>=0 && tri_inds[1]<mesh_x->rows());
+ BLI_assert(tri_inds[2]>=0 && tri_inds[2]<mesh_x->rows());
Vector3d tri[3] = {
- td->obsdata->V1.row(tri_inds[0]),
- td->obsdata->V1.row(tri_inds[1]),
- td->obsdata->V1.row(tri_inds[2])
+ mesh_x->row(tri_inds[0]),
+ mesh_x->row(tri_inds[1]),
+ mesh_x->row(tri_inds[2])
};
pair.q_n = ((tri[1]-tri[0]).cross(tri[2]-tri[0])).normalized();
-} // end parallel detect against obstacles
+// std::stringstream ss;
+// ss << "\nhit:" <<
+// "\n\t normal: " << pair.q_n.transpose() <<
+// "\n\t pt: " << pt.transpose() <<
+// "\n\t pt_on_tri: " << pair.pt_on_q.transpose() <<
+// std::endl;
+// printf("%s",ss.str().c_str());
+
+} // end detect_discrete_vf
+
+typedef struct DetectThreadData {
+ const TetMeshData *tetmesh;
+ const EmbeddedMeshData *embmesh;
+ const Collision::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(
void *__restrict userdata,
const int i,
const TaskParallelTLS *__restrict tls)
{
- parallel_detect_obstacles(userdata,i,tls);
- //parallel_detect_self(userdata,i,tls);
+ DetectThreadData *td = (DetectThreadData*)userdata;
+
+ // Comments say "don't use this" but how else am I supposed
+ // to get the thread ID? It appears to return the right thing...
+ int thread_idx = BLI_task_parallel_thread_id(tls);
+ std::vector<VFCollisionPair> &tl_pairs = td->pt_vf_pairs->at(thread_idx);
+
+ if (td->tetmesh != nullptr)
+ {
+
+ } // end detect with tet meshes
+
+ if (td->embmesh != nullptr)
+ {
+ int tet_idx = td->embmesh->vtx_to_tet[i];
+ RowVector4i tet = td->embmesh->tets.row(tet_idx);
+ Vector4d bary = td->embmesh->barys.row(i);
+ 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]);
+
+ 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
int EmbeddedMeshCollision::detect(
@@ -187,25 +220,114 @@ int EmbeddedMeshCollision::detect(
std::vector<std::vector<VFCollisionPair> > pt_vf_pairs
(max_threads, std::vector<VFCollisionPair>());
+ DetectThreadData thread_data = {
+ .tetmesh = nullptr,
+ .embmesh = mesh,
+ .obsdata = &obsdata,
+ .x0 = x0,
+ .x1 = x1,
+ .floor_z = floor_z,
+ .pt_vf_pairs = &pt_vf_pairs
+ };
+
+ int nev = mesh->x_rest.rows();
+
+ TaskParallelSettings settings;
+ BLI_parallel_range_settings_defaults(&settings);
+ BLI_task_parallel_range(0, nev, &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 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,
+ std::vector<Eigen::Triplet<double> > *trips_y,
+ std::vector<Eigen::Triplet<double> > *trips_z,
+ std::vector<double> *l)
+{
+ BLI_assert(x != NULL);
+ BLI_assert(x->cols() == 3);
+ int np = vf_pairs.size();
+ if (np==0)
+ return;
-floor_z = -2;
+ l->reserve((int)l->size() + np);
+ trips_x->reserve((int)trips_x->size() + np*4);
+ trips_y->reserve((int)trips_y->size() + np*4);
+ trips_z->reserve((int)trips_z->size() + np*4);
+ for (int i=0; i<np; ++i)
+ {
+ VFCollisionPair &pair = vf_pairs[i];
+ int emb_p_idx = pair.p_idx;
+ // TODO: obstacle point intersecting mesh
+ if (pair.p_is_obs)
+ {
+ 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);
+ // Basically a pin constraint
+ for (int j=0; j<3; ++j)
+ {
+ int c_idx = l->size();
+ for (int k=0; k<4; ++k)
+ {
+ if (j==0)
+ trips_x->emplace_back(c_idx, tet[k], bary[k]*pair.q_n[0]);
+ if (j==1)
+ trips_y->emplace_back(c_idx, tet[k], bary[k]*pair.q_n[1]);
+ if (j==2)
+ trips_z->emplace_back(c_idx, tet[k], bary[k]*pair.q_n[2]);
+ }
+ l->emplace_back(pair.pt_on_q[j]*pair.q_n[j]);
+ }
+ continue;
+ }
+ // Self collisions
+
+ } // end loop pairs
+} // end jacobian
+/*
+int TetMeshCollision::detect(
+ const Eigen::MatrixXd *x0,
+ const Eigen::MatrixXd *x1)
+{
+ if (mesh==NULL)
+ return 0;
+ 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,
+ .tetmesh = mesh,
+ .embmesh = nullptr,
.obsdata = &obsdata,
.x0 = x0,
.x1 = x1,
@@ -213,10 +335,10 @@ floor_z = -2;
.pt_vf_pairs = &pt_vf_pairs
};
- int nev = mesh->x_rest.rows();
+ int nv = x1->rows();
TaskParallelSettings settings;
BLI_parallel_range_settings_defaults(&settings);
- BLI_task_parallel_range(0, nev, &thread_data, parallel_detect, &settings);
+ BLI_task_parallel_range(0, nv, &thread_data, parallel_detect, &settings);
// Combine thread-local results
vf_pairs.clear();
@@ -227,9 +349,9 @@ floor_z = -2;
}
return vf_pairs.size();
-} // end detect
+}
-void EmbeddedMeshCollision::jacobian(
+void TetMeshCollision::jacobian(
const Eigen::MatrixXd *x,
std::vector<Eigen::Triplet<double> > *trips_x,
std::vector<Eigen::Triplet<double> > *trips_y,
@@ -244,38 +366,40 @@ void EmbeddedMeshCollision::jacobian(
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);
+ trips_x->reserve((int)trips_x->size() + np*4);
+ trips_y->reserve((int)trips_y->size() + np*4);
+ trips_z->reserve((int)trips_z->size() + np*4);
for (int i=0; i<np; ++i)
{
VFCollisionPair &pair = vf_pairs[i];
- int emb_p_idx = pair.p_idx;
+ // TODO: obstacle point intersecting mesh
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]);
- }
+ bool has_qnx = std::abs(pair.q_n[0])>0;
+ bool has_qny = std::abs(pair.q_n[1])>0;
+ bool has_qnz = std::abs(pair.q_n[2])>0;
+ if (has_qnx)
+ trips_x->emplace_back(c_idx, pair.p_idx, pair.q_n[0]);
+ if (has_qny)
+ trips_y->emplace_back(c_idx, pair.p_idx, pair.q_n[1]);
+ if (has_qnz)
+ trips_z->emplace_back(c_idx, pair.p_idx, pair.q_n[2]);
l->emplace_back( pair.q_n.dot(pair.pt_on_q));
continue;
}
-
+
} // end loop pairs
-} // end jacobian
+} // end jacobian of tet mesh intersect
+*/
} // namespace admmpd
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-31 15:07:45 +0300