diff options
author | over0219 <over0219@umn.edu> | 2020-07-16 02:43:25 +0300 |
---|---|---|
committer | over0219 <over0219@umn.edu> | 2020-07-16 02:43:25 +0300 |
commit | 0087149790a220173776449ce2bc152b747af3fc (patch) | |
tree | cbe0bc0be198df96a5e2bbb47c594b131b690886 /extern/softbody/src/admmpd_collision.cpp | |
parent | 6191b3d2db2d8788e6d8f12cc6e433d247377387 (diff) |
moved collision detection back to inner loopsoftbody-stable-v3
Diffstat (limited to 'extern/softbody/src/admmpd_collision.cpp')
-rw-r--r-- | extern/softbody/src/admmpd_collision.cpp | 253 |
1 files changed, 200 insertions, 53 deletions
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp index a05b27287cc..69e300e598e 100644 --- a/extern/softbody/src/admmpd_collision.cpp +++ b/extern/softbody/src/admmpd_collision.cpp @@ -30,6 +30,7 @@ void Collision::set_obstacles( const unsigned int *faces, int nf) { + (void)(v0); if (nv==0 || nf==0) { // obsdata.mesh = admmpd::EmbeddedMesh(); @@ -60,29 +61,7 @@ void Collision::set_obstacles( } obsdata.tree.init(obsdata.leaves); -/* - if (!obsdata.mesh.generate(V,F,true,2)) - return; - - int nt = obsdata.mesh.lat_tets.rows(); - if ((int)obsdata.tet_leaves.size() != nt) - obsdata.tet_leaves.resize(nt); - for (int i=0; i<nt; ++i) - { - AlignedBox<double,3> &box = obsdata.tet_leaves[i]; - box.setEmpty(); - RowVector4i t = obsdata.mesh.lat_tets.row(i); - box.extend(obsdata.mesh.lat_rest_x.row(t[0]).transpose()); - box.extend(obsdata.mesh.lat_rest_x.row(t[1]).transpose()); - box.extend(obsdata.mesh.lat_rest_x.row(t[2]).transpose()); - box.extend(obsdata.mesh.lat_rest_x.row(t[3]).transpose()); -// box.extend(box.min()-Vector3d::Ones()*settings.collision_eps); -// box.extend(box.max()+Vector3d::Ones()*settings.collision_eps); - } - - obsdata.tet_tree.init(obsdata.tet_leaves); -*/ } // end add obstacle std::pair<bool,VFCollisionPair> @@ -119,13 +98,13 @@ int EmbeddedMeshCollision::detect( return 0; // Do we even need to process collisions? - if (!this->settings.test_floor && - !this->settings.self_collision && - !obsdata.has_obs()) + if (!this->settings.floor_collision && + (!this->settings.obs_collision || !obsdata.has_obs()) && + !this->settings.self_collision) return 0; - // Updates the BVH of deforming mesh - update_bvh(x0,x1); + if (!tet_tree.root()) + throw std::runtime_error("EmbeddedMeshCollision::Detect: No tree"); // We store the results of the collisions in a per-vertex buffer. // This is a workaround so we can create them in threads. @@ -136,7 +115,7 @@ int EmbeddedMeshCollision::detect( // // Thread data for detection // - typedef struct DetectThreadData { + typedef struct { const Collision::Settings *settings; const Collision *collision; const TetMeshData *tetmesh; @@ -162,18 +141,10 @@ int EmbeddedMeshCollision::detect( std::vector<VFCollisionPair> &vi_pairs = td->per_vertex_pairs->at(vi); vi_pairs.clear(); - - int tet_idx = td->embmesh->emb_vtx_to_tet[vi]; - RowVector4i tet = td->embmesh->lat_tets.row(tet_idx); - Vector4d bary = td->embmesh->emb_barys.row(vi); - Vector3d pt_t1 = - 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]); + Vector3d pt_t1 = td->embmesh->get_mapped_vertex(td->x1,vi); // Special case, check if we are below the floor - if (td->settings->test_floor) + if (td->settings->floor_collision) { if (pt_t1[2] < td->settings->floor_z) { @@ -188,14 +159,28 @@ int EmbeddedMeshCollision::detect( } } - std::pair<bool,VFCollisionPair> pt_hit_obs = - td->collision->detect_against_obs(pt_t1,td->obsdata); + // Detect against obstacles + if (td->settings->obs_collision) + { + std::pair<bool,VFCollisionPair> pt_hit_obs = + td->collision->detect_against_obs(pt_t1,td->obsdata); + if (pt_hit_obs.first) + { + pt_hit_obs.second.p_idx = vi; + pt_hit_obs.second.p_is_obs = false; + vi_pairs.emplace_back(pt_hit_obs.second); + } + } - if (pt_hit_obs.first) + // Detect against self + if (td->settings->self_collision) { - pt_hit_obs.second.p_idx = vi; - pt_hit_obs.second.p_is_obs = false; - vi_pairs.emplace_back(pt_hit_obs.second); + std::pair<bool,VFCollisionPair> pt_hit_self = + td->collision->detect_against_self(vi, pt_t1, td->x1); + if (pt_hit_self.first) + { + vi_pairs.emplace_back(pt_hit_self.second); + } } }; // end detect for a single embedded vertex @@ -226,13 +211,138 @@ int EmbeddedMeshCollision::detect( return vf_pairs.size(); } // end detect +void EmbeddedMeshCollision::init_bvh( + const Eigen::MatrixXd *x0, + const Eigen::MatrixXd *x1) +{ + + (void)(x0); + int nt = mesh->lat_tets.rows(); + if (nt==0) + throw std::runtime_error("EmbeddedMeshCollision::init_bvh: No tets"); + + if ((int)tet_boxes.size() != nt) + tet_boxes.resize(nt); + + // Update leaf boxes + for (int i=0; i<nt; ++i) + { + RowVector4i tet = mesh->lat_tets.row(i); + tet_boxes[i].setEmpty(); + tet_boxes[i].extend(x1->row(tet[0]).transpose()); + tet_boxes[i].extend(x1->row(tet[1]).transpose()); + tet_boxes[i].extend(x1->row(tet[2]).transpose()); + tet_boxes[i].extend(x1->row(tet[3]).transpose()); + } + + tet_tree.init(tet_boxes); +} void EmbeddedMeshCollision::update_bvh( const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1) { - (void)(x0); - (void)(x1); + if (!tet_tree.root()) + { + init_bvh(x0,x1); + return; + } + + int nt = mesh->lat_tets.rows(); + if ((int)tet_boxes.size() != nt) + tet_boxes.resize(nt); + + // Update leaf boxes + for (int i=0; i<nt; ++i) + { + RowVector4i tet = mesh->lat_tets.row(i); + tet_boxes[i].setEmpty(); + tet_boxes[i].extend(x1->row(tet[0]).transpose()); + tet_boxes[i].extend(x1->row(tet[1]).transpose()); + tet_boxes[i].extend(x1->row(tet[2]).transpose()); + tet_boxes[i].extend(x1->row(tet[3]).transpose()); + } + + tet_tree.update(tet_boxes); + +} // end update bvh + +void EmbeddedMeshCollision::update_constraints( + const Eigen::MatrixXd *x0, + const Eigen::MatrixXd *x1) +{ + + (void)(x0); (void)(x1); +// BLI_assert(x != NULL); +// BLI_assert(x->cols() == 3); + +// int np = vf_pairs.size(); +// if (np==0) +// return; + + //int nx = x->rows(); +// d->reserve((int)d->size() + np); +// trips->reserve((int)trips->size() + np*3*4); + +// for (int i=0; i<np; ++i) +// { +// const Vector2i &pair_idx = vf_pairs[i]; +// } +} + +// Self collisions +std::pair<bool,VFCollisionPair> +EmbeddedMeshCollision::detect_against_self( + int pt_idx, + const Eigen::Vector3d &pt, + const Eigen::MatrixXd *x) const +{ + std::pair<bool,VFCollisionPair> ret = + std::make_pair(false, VFCollisionPair()); + + int self_tet_idx = mesh->emb_vtx_to_tet[pt_idx]; + RowVector4i self_tet = mesh->lat_tets.row(self_tet_idx); + std::vector<int> skip_tet_inds = {self_tet[0],self_tet[1],self_tet[2],self_tet[3]}; + PointInTetMeshTraverse<double> pt_in_tet(pt,x,&mesh->lat_tets,skip_tet_inds); + bool in_mesh = tet_tree.traverse(pt_in_tet); + if (!in_mesh) + return ret; + + // Transform point to rest shape + int tet_idx = pt_in_tet.output.prim; + RowVector4i tet = mesh->lat_tets.row(tet_idx); + Vector4d barys = geom::point_tet_barys(pt, + x->row(tet[0]), x->row(tet[1]), + x->row(tet[2]), x->row(tet[3])); + Vector3d rest_pt = + barys[0]*mesh->lat_rest_x.row(tet[0])+ + barys[1]*mesh->lat_rest_x.row(tet[1])+ + barys[2]*mesh->lat_rest_x.row(tet[2])+ + barys[3]*mesh->lat_rest_x.row(tet[3]); + + // Find triangle surface projection that doesn't + // include the penetration vertex + std::vector<int> skip_tri_inds = {pt_idx}; + NearestTriangleTraverse<double> nearest_tri(rest_pt, + &mesh->emb_rest_x,&mesh->emb_faces,skip_tri_inds); + mesh->emb_rest_tree.traverse(nearest_tri); + + ret.first = true; + ret.second.p_idx = pt_idx; + ret.second.p_is_obs = false; + ret.second.q_idx = nearest_tri.output.prim; + ret.second.q_is_obs = false; + ret.second.q_pt = nearest_tri.output.pt_on_tri; + + // Compute barycoords of projection + RowVector3i f = mesh->emb_faces.row(nearest_tri.output.prim); + Vector3d v3[3] = { + mesh->emb_rest_x.row(f[0]), + mesh->emb_rest_x.row(f[1]), + mesh->emb_rest_x.row(f[2])}; + ret.second.q_bary = geom::point_triangle_barys<double>( + nearest_tri.output.pt_on_tri, v3[0], v3[1], v3[2]); + return ret; } @@ -341,11 +451,6 @@ void EmbeddedMeshCollision::linearize( q_tris[0], q_tris[1], q_tris[2]); } - // Is the constraint active? - bool active = (p_pt-pair.q_pt).dot(q_n) <= 0.0; - if (!active) - continue; - // Get the four deforming verts that embed // the surface vertices, and add constraints on those. RowVector4d bary = mesh->emb_barys.row(emb_p_idx); @@ -359,9 +464,51 @@ void EmbeddedMeshCollision::linearize( trips->emplace_back(c_idx, tet[j]*3+1, bary[j]*q_n[1]); trips->emplace_back(c_idx, tet[j]*3+2, bary[j]*q_n[2]); } - continue; - } + } // end q is obs + else + { + int c_idx = d->size(); + d->emplace_back(0); + + // Compute the normal in the deformed space + RowVector3i q_face = mesh->emb_faces.row(pair.q_idx); + Vector3d q_v0 = mesh->get_mapped_vertex(x,q_face[0]); + Vector3d q_v1 = mesh->get_mapped_vertex(x,q_face[1]); + Vector3d q_v2 = mesh->get_mapped_vertex(x,q_face[2]); + Vector3d q_n = (q_v1-q_v0).cross(q_v2-q_v0); + q_n.normalize(); + + // The penetrating vertex: + { + int tet_idx = mesh->emb_vtx_to_tet[emb_p_idx]; + RowVector4d bary = mesh->emb_barys.row(emb_p_idx); + RowVector4i tet = mesh->lat_tets.row(tet_idx); + for (int j=0; j<4; ++j) + { + trips->emplace_back(c_idx, tet[j]*3+0, bary[j]*q_n[0]); + trips->emplace_back(c_idx, tet[j]*3+1, bary[j]*q_n[1]); + trips->emplace_back(c_idx, tet[j]*3+2, bary[j]*q_n[2]); + } + } + + // The intersected face: + for (int j=0; j<3; ++j) + { + int v_idx = q_face[j]; + RowVector4d bary = mesh->emb_barys.row(v_idx); + int tet_idx = mesh->emb_vtx_to_tet[v_idx]; + RowVector4i tet = mesh->lat_tets.row(tet_idx); + for (int k=0; k<4; ++k) + { + trips->emplace_back(c_idx, tet[k]*3+0, -pair.q_bary[j]*bary[k]*q_n[0]); + trips->emplace_back(c_idx, tet[k]*3+1, -pair.q_bary[j]*bary[k]*q_n[1]); + trips->emplace_back(c_idx, tet[k]*3+2, -pair.q_bary[j]*bary[k]*q_n[2]); + } + } + + } // end q is obs + } // end loop pairs } // end jacobian |