From 0087149790a220173776449ce2bc152b747af3fc Mon Sep 17 00:00:00 2001 From: over0219 Date: Wed, 15 Jul 2020 18:43:25 -0500 Subject: moved collision detection back to inner loop --- extern/softbody/src/admmpd_bvh.cpp | 20 +-- extern/softbody/src/admmpd_bvh.h | 6 +- extern/softbody/src/admmpd_bvh_traverse.cpp | 19 +++ extern/softbody/src/admmpd_bvh_traverse.h | 16 +- extern/softbody/src/admmpd_collision.cpp | 253 ++++++++++++++++++++++------ extern/softbody/src/admmpd_collision.h | 105 +++++------- extern/softbody/src/admmpd_embeddedmesh.cpp | 22 ++- extern/softbody/src/admmpd_linsolve.cpp | 15 +- extern/softbody/src/admmpd_solver.cpp | 66 ++++---- extern/softbody/src/admmpd_solver.h | 2 +- extern/softbody/src/admmpd_types.h | 4 +- 11 files changed, 351 insertions(+), 177 deletions(-) diff --git a/extern/softbody/src/admmpd_bvh.cpp b/extern/softbody/src/admmpd_bvh.cpp index 6ea7a676d27..afa16874062 100644 --- a/extern/softbody/src/admmpd_bvh.cpp +++ b/extern/softbody/src/admmpd_bvh.cpp @@ -12,35 +12,35 @@ namespace admmpd { template void AABBTree::clear() { - root = std::make_shared(); + m_root = std::make_shared(); } template void AABBTree::init(const std::vector &leaves) { - root = std::make_shared(); + m_root = std::make_shared(); int np = leaves.size(); if (np==0) return; std::vector queue(np); std::iota(queue.begin(), queue.end(), 0); - create_children(root.get(), queue, leaves); + create_children(m_root.get(), queue, leaves); } template void AABBTree::update(const std::vector &leaves) { - if (!root || (int)leaves.size()==0) + if (!m_root || (int)leaves.size()==0) return; - update_children(root.get(), leaves); + update_children(m_root.get(), leaves); } template bool AABBTree::traverse(Traverser &traverser) const { - if (!root) + if (!m_root) return false; - return traverse_children(root.get(), traverser); + return traverse_children(m_root.get(), traverser); } // If we are traversing with function pointers, we'll just @@ -71,12 +71,12 @@ bool AABBTree::traverse( std::function t, std::function s) const { - if (!root) + if (!m_root) return false; TraverserFromFunctionPtrs traverser; traverser.t = t; traverser.s = s; - return traverse_children(root.get(), traverser); + return traverse_children(m_root.get(), traverser); } template @@ -264,7 +264,7 @@ typename Octree::Node* Octree::create_children( const std::vector &boxes) { BLI_assert((int)queue.size()>0); - BLI_assert((int)prim_boxes.size()>0); + BLI_assert((int)boxes.size()>0); BLI_assert(F != nullptr); BLI_assert(V != nullptr); BLI_assert(F->cols()==3); diff --git a/extern/softbody/src/admmpd_bvh.h b/extern/softbody/src/admmpd_bvh.h index 20b810ac7be..88ff5393dc4 100644 --- a/extern/softbody/src/admmpd_bvh.h +++ b/extern/softbody/src/admmpd_bvh.h @@ -58,9 +58,13 @@ public: } }; + // Return ptr to the root node + // Becomes invalidated after init() + std::shared_ptr root() { return m_root; } + protected: - std::shared_ptr root; + std::shared_ptr m_root; void create_children( Node *node, diff --git a/extern/softbody/src/admmpd_bvh_traverse.cpp b/extern/softbody/src/admmpd_bvh_traverse.cpp index 9ec4c281368..fc0a8370ae8 100644 --- a/extern/softbody/src/admmpd_bvh_traverse.cpp +++ b/extern/softbody/src/admmpd_bvh_traverse.cpp @@ -45,6 +45,7 @@ void RayClosestHit::traverse( (eps > 0 ? left_aabb.exteriorDistance(o) < eps : false); go_right = geom::ray_aabb(o,d,right_aabb,t_min,output.t_max) || (eps > 0 ? right_aabb.exteriorDistance(o) < eps : false); + (void)(go_left_first); } template @@ -109,6 +110,15 @@ bool PointInTetMeshTraverse::stop_traversing( return false; RowVector4i t = prim_inds->row(prim); + int n_skip = skip_inds.size(); + for (int i=0; irow(t[0]), prim_verts->row(t[1]), @@ -229,6 +239,15 @@ bool NearestTriangleTraverse::stop_traversing(const AABB &aabb, int prim) return false; RowVector3i tri = prim_inds->row(prim); + + int n_skip = skip_inds.size(); + for (int i=0; irow(tri[0]), prim_verts->row(tri[1]), diff --git a/extern/softbody/src/admmpd_bvh_traverse.h b/extern/softbody/src/admmpd_bvh_traverse.h index dce83d07a20..c6cc93c0037 100644 --- a/extern/softbody/src/admmpd_bvh_traverse.h +++ b/extern/softbody/src/admmpd_bvh_traverse.h @@ -6,7 +6,7 @@ #include #include -#include +#include "BLI_math_geom.h" namespace admmpd { @@ -90,6 +90,7 @@ protected: VecType point; const MatrixXType *prim_verts; const Eigen::MatrixXi *prim_inds; + std::vector skip_inds; public: struct Output { @@ -100,10 +101,12 @@ public: PointInTetMeshTraverse( const VecType &point_, const MatrixXType *prim_verts_, - const Eigen::MatrixXi *prim_inds_) : + const Eigen::MatrixXi *prim_inds_, + const std::vector &skip_inds_=std::vector()) : point(point_), prim_verts(prim_verts_), - prim_inds(prim_inds_) + prim_inds(prim_inds_), + skip_inds(skip_inds_) {} void traverse( @@ -165,6 +168,7 @@ protected: VecType point; const MatrixXType *prim_verts; // triangle mesh verts const Eigen::MatrixXi *prim_inds; // triangle mesh inds + std::vector skip_inds; public: struct Output { @@ -181,10 +185,12 @@ public: NearestTriangleTraverse( const VecType &point_, const MatrixXType *prim_verts_, - const Eigen::MatrixXi *prim_inds_) : + const Eigen::MatrixXi *prim_inds_, + const std::vector &skip_inds_=std::vector()) : point(point_), prim_verts(prim_verts_), - prim_inds(prim_inds_) + prim_inds(prim_inds_), + skip_inds(skip_inds_) {} void traverse( 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 &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 @@ -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 &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 pt_hit_obs = - td->collision->detect_against_obs(pt_t1,td->obsdata); + // Detect against obstacles + if (td->settings->obs_collision) + { + std::pair 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 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; ilat_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; ilat_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 +EmbeddedMeshCollision::detect_against_self( + int pt_idx, + const Eigen::Vector3d &pt, + const Eigen::MatrixXd *x) const +{ + std::pair 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 skip_tet_inds = {self_tet[0],self_tet[1],self_tet[2],self_tet[3]}; + PointInTetMeshTraverse 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 skip_tri_inds = {pt_idx}; + NearestTriangleTraverse 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( + 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 diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h index 3382d5aa9a5..4d917ccfc72 100644 --- a/extern/softbody/src/admmpd_collision.h +++ b/extern/softbody/src/admmpd_collision.h @@ -27,12 +27,14 @@ public: struct Settings { double collision_eps; double floor_z; - bool test_floor; + bool floor_collision; + bool obs_collision; bool self_collision; Settings() : collision_eps(1e-10), floor_z(-std::numeric_limits::max()), - test_floor(true), + floor_collision(true), + obs_collision(true), self_collision(false) {} } settings; @@ -47,6 +49,22 @@ public: virtual ~Collision() {} + // Resets the BVH + virtual void init_bvh( + const Eigen::MatrixXd *x0, + const Eigen::MatrixXd *x1) = 0; + + // Updates the BVH without sorting + virtual void update_bvh( + const Eigen::MatrixXd *x0, + const Eigen::MatrixXd *x1) = 0; + + // Updates the active set of constraints, + // but does not detect new ones. + virtual void update_constraints( + const Eigen::MatrixXd *x0, + const Eigen::MatrixXd *x1) = 0; + // Performs collision detection. // Returns the number of active constraints. virtual int detect( @@ -80,22 +98,16 @@ public: // Given a point and a mesh, perform // discrete collision detection. - std::pair + virtual std::pair detect_against_obs( const Eigen::Vector3d &pt, const ObstacleData *obs) const; - // Given a point and a mesh, perform - // discrete collision detection. -// std::pair -// detect_point_against_mesh( -// int pt_idx, -// bool pt_is_obs, -// const Eigen::Vector3d &pt, -// bool mesh_is_obs, -// const EmbeddedMesh *emb_mesh, -// const Eigen::MatrixXd *mesh_tets_x, -// const AABBTree *mesh_tets_tree) const; + virtual std::pair + detect_against_self( + int pt_idx, + const Eigen::Vector3d &pt, + const Eigen::MatrixXd *x) const = 0; }; // Collision detection against multiple meshes @@ -118,68 +130,37 @@ public: std::vector > *trips, std::vector *d); -protected: - // A ptr to the embedded mesh data - const EmbeddedMesh *mesh; - - // Pairs are compute on detect - std::vector vf_pairs; // index into per_vertex_pairs - std::vector > per_vertex_pairs; + void init_bvh( + const Eigen::MatrixXd *x0, + const Eigen::MatrixXd *x1); // Updates the tetmesh BVH for self collisions. - // Called by detect() - // TODO void update_bvh( const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1); -}; - -/* -class TetMeshCollision : public Collision { -public: - TetMeshCollision(const TetMeshData *mesh_) : - mesh(mesh_), - floor_z(-std::numeric_limits::max()) - {} - // Performs collision detection. - // Returns the number of active constraints. - int detect( + // Updates the active set of constraints, + // but does not detect new ones. + void update_constraints( const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1); - // Special case for floor since it's common. - void set_floor(double z) - { - floor_z = z; - } - - // Linearize the constraints and return Jacobian tensor. - // Constraints are linearized about x for constraint - // K x = l - void jacobian( - const Eigen::MatrixXd *x, - std::vector > *trips_x, - std::vector > *trips_y, - std::vector > *trips_z, - std::vector *l) = 0; - protected: - const TetMeshData *mesh; - double floor_z; + // A ptr to the embedded mesh data + const EmbeddedMesh *mesh; + std::vector > tet_boxes; + AABBTree tet_tree; // Pairs are compute on detect - std::vector vf_pairs; + std::vector vf_pairs; // index into per_vertex_pairs + std::vector > per_vertex_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); } + std::pair + detect_against_self( + int pt_idx, + const Eigen::Vector3d &pt, + const Eigen::MatrixXd *x) const; }; -*/ } // namespace admmpd diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp index 7e677b42455..e939114faeb 100644 --- a/extern/softbody/src/admmpd_embeddedmesh.cpp +++ b/extern/softbody/src/admmpd_embeddedmesh.cpp @@ -175,11 +175,10 @@ bool EmbeddedMesh::generate( face_boxes[i].extend(V.row(F(i,2)).transpose()); } - AABBTree face_tree; - face_tree.init(face_boxes); + emb_rest_tree.init(face_boxes); Octree::Node *root = octree.root().get(); - gather_octree_tets(root,&V,&F,&face_tree,data.verts,data.tets); + gather_octree_tets(root,&V,&F,&emb_rest_tree,data.verts,data.tets); merge_close_vertices(&data); int nv = data.verts.size(); @@ -198,6 +197,11 @@ bool EmbeddedMesh::generate( } } + // Now compute the baryweighting for embedded vertices + bool embed_success = compute_embedding(); + + if (!emb_rest_tree.root()) + throw std::runtime_error("EmbeddedMesh::generate Error: Failed to create tree"); if (lat_rest_x.rows()==0) throw std::runtime_error("EmbeddedMesh::generate Error: Failed to create verts"); if (lat_tets.rows()==0) @@ -206,13 +210,12 @@ bool EmbeddedMesh::generate( throw std::runtime_error("EmbeddedMesh::generate Error: Did not set faces"); if (emb_rest_x.rows()==0) throw std::runtime_error("EmbeddedMesh::generate Error: Did not set verts"); - - // Now compute the baryweighting for embedded vertices - bool embed_success = compute_embedding(); + if (!embed_success) + throw std::runtime_error("EmbeddedMesh::generate Error: Failed embedding"); // Export the mesh for funsies - std::ofstream of("v_lattice.txt"); of << lat_rest_x; of.close(); - std::ofstream of2("t_lattice.txt"); of2 << lat_tets; of2.close(); +// std::ofstream of("v_lattice.txt"); of << lat_rest_x; of.close(); +// std::ofstream of2("t_lattice.txt"); of2 << lat_tets; of2.close(); return embed_success; @@ -270,8 +273,9 @@ typedef struct FindTetThreadData { static void parallel_point_in_tet( void *__restrict userdata, const int i, - const TaskParallelTLS *__restrict UNUSED(tls)) + const TaskParallelTLS *__restrict tls) { + (void)(tls); FindTetThreadData *td = (FindTetThreadData*)userdata; Vector3d pt = td->emb_mesh->emb_rest_x.row(i); PointInTetMeshTraverse traverser( diff --git a/extern/softbody/src/admmpd_linsolve.cpp b/extern/softbody/src/admmpd_linsolve.cpp index b91d3041913..a4ff75ab77e 100644 --- a/extern/softbody/src/admmpd_linsolve.cpp +++ b/extern/softbody/src/admmpd_linsolve.cpp @@ -140,8 +140,9 @@ void ConjugateGradients::solve_Ax_b( auto parallel_lin_solve = []( void *__restrict userdata, const int i, - const TaskParallelTLS *__restrict UNUSED(tls))->void + const TaskParallelTLS *__restrict tls)->void { + (void)(tls); LinSolveThreadData *td = (LinSolveThreadData*)userdata; int nx = td->ls_x->rows()/3; VectorXd b(nx); @@ -189,8 +190,9 @@ void GaussSeidel::solve( // Inner iteration of Gauss-Seidel auto parallel_gs_sweep = [](void *__restrict userdata, const int i_, - const TaskParallelTLS *__restrict UNUSED(tls)) -> void + const TaskParallelTLS *__restrict tls) -> void { + (void)(tls); GaussSeidelThreadData *td = (GaussSeidelThreadData*)userdata; int idx = td->colors->at(td->color)[i_]; double omega = td->options->gs_omega; @@ -421,8 +423,9 @@ void GaussSeidel::compute_colors( // Graph color initialization // auto init_graph = [](void *__restrict userdata, const int i, - const TaskParallelTLS *__restrict UNUSED(tls)) -> void + const TaskParallelTLS *__restrict tls) -> void { + (void)(tls); GraphColorThreadData *td = (GraphColorThreadData*)userdata; for( int j=0; jinit_palette_size; ++j ) // init colors td->palette->at(i).insert(j); @@ -440,8 +443,9 @@ void GaussSeidel::compute_colors( // Generate a random color auto generate_color = [](void *__restrict userdata, const int i, - const TaskParallelTLS *__restrict UNUSED(tls)) -> void + const TaskParallelTLS *__restrict tls) -> void { + (void)(tls); GraphColorThreadData *td = (GraphColorThreadData*)userdata; int idx = td->node_queue->at(i); if (td->palette->at(idx).size()<2) // Feed the hungry @@ -453,8 +457,9 @@ void GaussSeidel::compute_colors( // Detect conflicts auto detect_conflicts = [](void *__restrict userdata, const int i, - const TaskParallelTLS *__restrict UNUSED(tls)) -> void + const TaskParallelTLS *__restrict tls) -> void { + (void)(tls); GraphColorThreadData *td = (GraphColorThreadData*)userdata; int idx = td->node_queue->at(i); int curr_c = td->node_colors->at(idx); diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp index e322970f8b5..9441ee6f78e 100644 --- a/extern/softbody/src/admmpd_solver.cpp +++ b/extern/softbody/src/admmpd_solver.cpp @@ -19,11 +19,6 @@ namespace admmpd { using namespace Eigen; -struct ThreadData { - const Options *options; - SolverData *data; -}; - bool Solver::init( const Eigen::MatrixXd &V, const Eigen::MatrixXi &T, @@ -70,9 +65,6 @@ int Solver::solve( // the variables are sized correctly. init_solve(options,data,collision,pin); - // Perform collision detection and linearization - linearize_collision_constraints(options,data,collision); - // Begin solver loop int iters = 0; for (; iters < options->max_admm_iters; ++iters) @@ -80,6 +72,9 @@ int Solver::solve( // Update ADMM z/u solve_local_step(options,data); + // Collision detection and linearization + update_collisions(options,data,collision); + // Solve Ax=b s.t. Cx=d ConjugateGradients().solve(options,data,collision); //GaussSeidel().solve(options,data,collision); @@ -149,6 +144,11 @@ void Solver::init_solve( } } + if (collision) + { + collision->init_bvh(&data->x_start, &data->x); + } + // ADMM variables data->Dx.noalias() = data->D * data->x; data->z = data->Dx; @@ -156,41 +156,49 @@ void Solver::init_solve( } // end init solve -static void parallel_zu_update( - void *__restrict userdata, - const int i, - const TaskParallelTLS *__restrict UNUSED(tls)) -{ - ThreadData *td = (ThreadData*)userdata; - Lame lame; - lame.set_from_youngs_poisson(td->options->youngs,td->options->poisson); - EnergyTerm().update( - td->data->indices[i][0], - lame, - td->data->rest_volumes[i], - td->data->weights[i], - &td->data->x, - &td->data->Dx, - &td->data->z, - &td->data->u ); -} // end parallel zu update - void Solver::solve_local_step( const Options *options, SolverData *data) { BLI_assert(data != NULL); BLI_assert(options != NULL); + + struct LocalStepThreadData { + const Options *options; + SolverData *data; + }; + + auto parallel_zu_update = []( + void *__restrict userdata, + const int i, + const TaskParallelTLS *__restrict tls)->void + { + (void)(tls); + LocalStepThreadData *td = (LocalStepThreadData*)userdata; + Lame lame; + lame.set_from_youngs_poisson(td->options->youngs,td->options->poisson); + EnergyTerm().update( + td->data->indices[i][0], + lame, + td->data->rest_volumes[i], + td->data->weights[i], + &td->data->x, + &td->data->Dx, + &td->data->z, + &td->data->u ); + }; // end parallel zu update + data->Dx.noalias() = data->D * data->x; int ne = data->indices.size(); BLI_assert(ne > 0); - ThreadData thread_data = {.options=options, .data = data}; + LocalStepThreadData thread_data = {.options=options, .data = data}; TaskParallelSettings settings; BLI_parallel_range_settings_defaults(&settings); BLI_task_parallel_range(0, ne, &thread_data, parallel_zu_update, &settings); + } // end local step -void Solver::linearize_collision_constraints( +void Solver::update_collisions( const Options *options, SolverData *data, Collision *collision) diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h index 584f54be43c..64433dbd263 100644 --- a/extern/softbody/src/admmpd_solver.h +++ b/extern/softbody/src/admmpd_solver.h @@ -34,7 +34,7 @@ public: protected: - void linearize_collision_constraints( + void update_collisions( const Options *options, SolverData *data, Collision *collision); diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h index c2ae0e67c88..3133773a478 100644 --- a/extern/softbody/src/admmpd_types.h +++ b/extern/softbody/src/admmpd_types.h @@ -28,7 +28,7 @@ struct Options { double poisson; // Poisson ratio // TODO variable per-tet Eigen::Vector3d grav; Options() : - timestep_s(1.0/100.0), + timestep_s(1.0/24.0), max_admm_iters(30), max_cg_iters(10), max_gs_iters(100), @@ -37,7 +37,7 @@ struct Options { mult_pk(0.01), min_res(1e-8), youngs(1000000), - poisson(0.399), + poisson(0.299), grav(0,0,-9.8) {} }; -- cgit v1.2.3