diff options
Diffstat (limited to 'extern/softbody/src')
-rw-r--r-- | extern/softbody/src/admmpd_collision.cpp | 76 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_collision.h | 37 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_embeddedmesh.cpp | 79 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_embeddedmesh.h | 7 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_linsolve.cpp | 456 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_linsolve.h | 17 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_solver.cpp | 22 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_types.h | 17 |
8 files changed, 424 insertions, 287 deletions
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp index 289af9fc68b..3ff7f949215 100644 --- a/extern/softbody/src/admmpd_collision.cpp +++ b/extern/softbody/src/admmpd_collision.cpp @@ -75,7 +75,6 @@ void Collision::detect_discrete_vf( const Eigen::MatrixXd *mesh_x, const Eigen::MatrixXi *mesh_tris, bool mesh_is_obs, - double floor_z, std::vector<VFCollisionPair> *pairs) { // Any faces to detect against? @@ -170,9 +169,9 @@ static void parallel_detect( if (td->embmesh != nullptr) { - int tet_idx = td->embmesh->vtx_to_tet[i]; + int tet_idx = td->embmesh->emb_vtx_to_tet[i]; RowVector4i tet = td->embmesh->tets.row(tet_idx); - Vector4d bary = td->embmesh->barys.row(i); + Vector4d bary = td->embmesh->emb_barys.row(i); Vector3d pt = bary[0] * td->x1->row(tet[0]) + bary[1] * td->x1->row(tet[1]) + @@ -198,7 +197,6 @@ static void parallel_detect( &td->obsdata->V1, &td->obsdata->F, true, - td->floor_z, &tl_pairs ); // Collision::detect_discrete_vf( @@ -227,6 +225,10 @@ int EmbeddedMeshCollision::detect( std::vector<std::vector<VFCollisionPair> > pt_vf_pairs (max_threads, std::vector<VFCollisionPair>()); + double floor_z = this->settings.floor_z; + if (!this->settings.test_floor) + floor_z = -std::numeric_limits<double>::max(); + DetectThreadData thread_data = { .tetmesh = nullptr, .embmesh = mesh, @@ -237,11 +239,10 @@ int EmbeddedMeshCollision::detect( .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); + int nev = mesh->emb_rest_x.rows(); + TaskParallelSettings thrd_settings; + BLI_parallel_range_settings_defaults(&thrd_settings); + BLI_task_parallel_range(0, nev, &thread_data, parallel_detect, &thrd_settings); // Combine thread-local results vf_pairs.clear(); @@ -254,6 +255,59 @@ int EmbeddedMeshCollision::detect( return vf_pairs.size(); } // end detect +void EmbeddedMeshCollision::graph( + std::vector<std::set<int> > &g) +{ + int np = vf_pairs.size(); + if (np==0) + return; + + int nv = mesh->rest_x.rows(); + if ((int)g.size() < nv) + g.resize(nv, std::set<int>()); + + for (int i=0; i<np; ++i) + { + VFCollisionPair &pair = vf_pairs[i]; + std::set<int> stencil; + + if (!pair.p_is_obs) + { + int tet_idx = mesh->emb_vtx_to_tet[pair.p_idx]; + RowVector4i tet = mesh->tets.row(tet_idx); + stencil.emplace(tet[0]); + stencil.emplace(tet[1]); + stencil.emplace(tet[2]); + stencil.emplace(tet[3]); + } + if (!pair.q_is_obs) + { + RowVector3i emb_face = mesh->emb_faces.row(pair.q_idx); + for (int j=0; j<3; ++j) + { + int tet_idx = mesh->emb_vtx_to_tet[emb_face[j]]; + RowVector4i tet = mesh->tets.row(tet_idx); + stencil.emplace(tet[0]); + stencil.emplace(tet[1]); + stencil.emplace(tet[2]); + stencil.emplace(tet[3]); + } + } + + for (std::set<int>::iterator it = stencil.begin(); + it != stencil.end(); ++it) + { + for (std::set<int>::iterator it2 = stencil.begin(); + it2 != stencil.end(); ++it2) + { + if (*it == *it2) + continue; + g[*it].emplace(*it2); + } + } + } +} // end graph + void EmbeddedMeshCollision::linearize( const Eigen::MatrixXd *x, std::vector<Eigen::Triplet<double> > *trips, @@ -314,8 +368,8 @@ void EmbeddedMeshCollision::linearize( // 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]; + RowVector4d bary = mesh->emb_barys.row(emb_p_idx); + int tet_idx = mesh->emb_vtx_to_tet[emb_p_idx]; RowVector4i tet = mesh->tets.row(tet_idx); int c_idx = d->size(); d->emplace_back(q_n.dot(pair.pt_on_q)); diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h index 95623510969..22d7dd58f8c 100644 --- a/extern/softbody/src/admmpd_collision.h +++ b/extern/softbody/src/admmpd_collision.h @@ -6,6 +6,7 @@ #include "admmpd_bvh.h" #include "admmpd_types.h" +#include <set> namespace admmpd { @@ -29,6 +30,16 @@ public: AABBTree<double,3> tree; } obsdata; + struct Settings { + double floor_z; + bool test_floor; + Settings() : + floor_z(0), +// floor_z(-std::numeric_limits<double>::max()), + test_floor(true) + {} + } settings; + virtual ~Collision() {} // Performs collision detection. @@ -37,6 +48,11 @@ public: const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1) = 0; + // Appends the per-vertex graph of dependencies + // for constraints (ignores obstacles). + virtual void graph( + std::vector<std::set<int> > &g) = 0; + // Set the soup of obstacles for this time step. // 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. @@ -48,7 +64,8 @@ public: int nf); // Special case for floor since it's common. - virtual void set_floor(double z) = 0; + virtual void set_floor(double z) { settings.floor_z=z; } + virtual double get_floor() const { return settings.floor_z; } // Linearize the constraints and return Jacobian. virtual void linearize( @@ -68,31 +85,22 @@ public: const Eigen::MatrixXd *mesh_x, const Eigen::MatrixXi *mesh_tris, bool mesh_is_obs, - double floor_z, std::vector<VFCollisionPair> *pairs); }; // Collision detection against multiple meshes class EmbeddedMeshCollision : public Collision { public: - EmbeddedMeshCollision(const EmbeddedMeshData *mesh_) : - mesh(mesh_), -// floor_z(-std::numeric_limits<double>::max()) - floor_z(0) - {} - - // 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; - }; + EmbeddedMeshCollision(const EmbeddedMeshData *mesh_) : mesh(mesh_){} // Performs collision detection and stores pairs int detect( const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1); + void graph( + std::vector<std::set<int> > &g); + // Linearizes the collision pairs about x // for the constraint Kx=l void linearize( @@ -103,7 +111,6 @@ 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; diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp index 0c2cdcac938..17e2b28c0e8 100644 --- a/extern/softbody/src/admmpd_embeddedmesh.cpp +++ b/extern/softbody/src/admmpd_embeddedmesh.cpp @@ -215,11 +215,10 @@ bool EmbeddedMesh::generate( const Eigen::MatrixXd &V, // embedded verts const Eigen::MatrixXi &F, // embedded faces EmbeddedMeshData *emb_mesh, // where embedding is stored - Eigen::MatrixXd *x_tets, // lattice vertices, n x 3 bool trim_lattice) { - emb_mesh->faces = F; - emb_mesh->x_rest = V; + emb_mesh->emb_faces = F; + emb_mesh->emb_rest_x = V; AlignedBox<double,3> aabb; int nev = V.rows(); @@ -315,14 +314,14 @@ bool EmbeddedMesh::generate( int ntv0 = data.verts.size(); // original num verts int ntv1 = refd_verts.size(); // reduced num verts BLI_assert(ntv1 <= ntv0); - x_tets->resize(ntv1,3); + emb_mesh->rest_x.resize(ntv1,3); int vtx_count = 0; for (int i=0; i<ntv0; ++i) { if (refd_verts.count(i)>0) { for(int j=0; j<3; ++j){ - x_tets->operator()(vtx_count,j) = data.verts[i][j]; + emb_mesh->rest_x(vtx_count,j) = data.verts[i][j]; } vtx_old_to_new[i] = vtx_count; vtx_count++; @@ -342,8 +341,7 @@ bool EmbeddedMesh::generate( } // Now compute the baryweighting for embedded vertices - return compute_embedding( - emb_mesh, &V, x_tets); + return compute_embedding(emb_mesh); } // end gen lattice @@ -352,16 +350,10 @@ bool EmbeddedMesh::generate( void EmbeddedMesh::compute_masses( EmbeddedMeshData *emb_mesh, // where embedding is stored - const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3 - const Eigen::MatrixXd *x_tets, // lattice vertices, n x 3 Eigen::VectorXd *masses_tets, // masses of the lattice verts double density_kgm3) { BLI_assert(emb_mesh != NULL); - BLI_assert(x_embed != NULL); - BLI_assert(x_tets != NULL); - BLI_assert(x_tets->rows() > 0); - BLI_assert(x_tets->cols() == 3); BLI_assert(masses_tets != NULL); BLI_assert(density_kgm3 > 0); @@ -371,18 +363,18 @@ void EmbeddedMesh::compute_masses( // Source: https://github.com/mattoverby/mclscene/blob/master/include/MCL/TetMesh.hpp // Computes volume-weighted masses for each vertex // density_kgm3 is the unit-volume density - int nx = x_tets->rows(); + int nx = emb_mesh->rest_x.rows(); masses_tets->resize(nx); masses_tets->setZero(); int n_tets = emb_mesh->tets.rows(); for (int t=0; t<n_tets; ++t) { RowVector4i tet = emb_mesh->tets.row(t); - RowVector3d tet_v0 = x_tets->row(tet[0]); + RowVector3d tet_v0 = emb_mesh->rest_x.row(tet[0]); Matrix3d edges; - edges.col(0) = x_tets->row(tet[1]) - tet_v0; - edges.col(1) = x_tets->row(tet[2]) - tet_v0; - edges.col(2) = x_tets->row(tet[3]) - tet_v0; + edges.col(0) = emb_mesh->rest_x.row(tet[1]) - tet_v0; + edges.col(1) = emb_mesh->rest_x.row(tet[2]) - tet_v0; + edges.col(2) = emb_mesh->rest_x.row(tet[3]) - tet_v0; double vol = std::abs((edges).determinant()/6.f); double tet_mass = density_kgm3 * vol; masses_tets->operator[](tet[0]) += tet_mass / 4.f; @@ -405,8 +397,6 @@ void EmbeddedMesh::compute_masses( typedef struct FindTetThreadData { AABBTree<double,3> *tree; EmbeddedMeshData *emb_mesh; // thread sets vtx_to_tet and barys - const Eigen::MatrixXd *x_embed; - const Eigen::MatrixXd *x_tets; } FindTetThreadData; static void parallel_point_in_tet( @@ -415,41 +405,40 @@ static void parallel_point_in_tet( const TaskParallelTLS *__restrict UNUSED(tls)) { FindTetThreadData *td = (FindTetThreadData*)userdata; - Vector3d pt = td->x_embed->row(i); - PointInTetMeshTraverse<double> traverser(pt, td->x_tets, &td->emb_mesh->tets); + Vector3d pt = td->emb_mesh->emb_rest_x.row(i); + PointInTetMeshTraverse<double> traverser(pt, &td->emb_mesh->rest_x, &td->emb_mesh->tets); bool success = td->tree->traverse(traverser); int tet_idx = traverser.output.prim; if (success && tet_idx >= 0) { RowVector4i tet = td->emb_mesh->tets.row(tet_idx); Vector3d t[4] = { - td->x_tets->row(tet[0]), - td->x_tets->row(tet[1]), - td->x_tets->row(tet[2]), - td->x_tets->row(tet[3]) + td->emb_mesh->rest_x.row(tet[0]), + td->emb_mesh->rest_x.row(tet[1]), + td->emb_mesh->rest_x.row(tet[2]), + td->emb_mesh->rest_x.row(tet[3]) }; - td->emb_mesh->vtx_to_tet[i] = tet_idx; + td->emb_mesh->emb_vtx_to_tet[i] = tet_idx; Vector4d b = barycoords::point_tet(pt,t[0],t[1],t[2],t[3]); - td->emb_mesh->barys.row(i) = b; + td->emb_mesh->emb_barys.row(i) = b; } } // end parallel lin solve bool EmbeddedMesh::compute_embedding( - EmbeddedMeshData *emb_mesh, // where embedding is stored - const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3 - const Eigen::MatrixXd *x_tets) // lattice vertices, n x 3 + EmbeddedMeshData *emb_mesh) { BLI_assert(emb_mesh!=NULL); - BLI_assert(x_embed!=NULL); - BLI_assert(x_tets!=NULL); - int nv = x_embed->rows(); + int nv = emb_mesh->emb_rest_x.rows(); if (nv==0) + { + printf("**EmbeddedMesh::compute_embedding: No embedded vertices"); return false; + } - emb_mesh->barys.resize(nv,4); - emb_mesh->barys.setOnes(); - emb_mesh->vtx_to_tet.resize(nv); + emb_mesh->emb_barys.resize(nv,4); + emb_mesh->emb_barys.setOnes(); + emb_mesh->emb_vtx_to_tet.resize(nv); int nt = emb_mesh->tets.rows(); // BVH tree for finding point-in-tet and computing @@ -462,7 +451,7 @@ bool EmbeddedMesh::compute_embedding( tet_aabbs[i].setEmpty(); RowVector4i tet = emb_mesh->tets.row(i); for (int j=0; j<4; ++j) - tet_aabbs[i].extend(x_tets->row(tet[j]).transpose()); + tet_aabbs[i].extend(emb_mesh->rest_x.row(tet[j]).transpose()); tet_aabbs[i].extend(tet_aabbs[i].min()-veta); tet_aabbs[i].extend(tet_aabbs[i].max()+veta); @@ -473,9 +462,7 @@ bool EmbeddedMesh::compute_embedding( FindTetThreadData thread_data = { .tree = &tree, - .emb_mesh = emb_mesh, - .x_embed = x_embed, - .x_tets = x_tets + .emb_mesh = emb_mesh }; TaskParallelSettings settings; BLI_parallel_range_settings_defaults(&settings); @@ -485,7 +472,7 @@ bool EmbeddedMesh::compute_embedding( const double eps = 1e-8; for (int i=0; i<nv; ++i) { - RowVector4d b = emb_mesh->barys.row(i); + RowVector4d b = emb_mesh->emb_barys.row(i); if (b.minCoeff() < -eps) { printf("**Lattice::generate Error: negative barycoords\n"); @@ -505,8 +492,8 @@ bool EmbeddedMesh::compute_embedding( // Export the mesh for funsies -std::ofstream of("v_lattice.txt"); of << *x_tets; of.close(); -std::ofstream of2("t_lattice.txt"); of2 << emb_mesh->tets; of2.close(); +//std::ofstream of("v_lattice.txt"); of << emb_mesh->rest_x; of.close(); +//std::ofstream of2("t_lattice.txt"); of2 << emb_mesh->tets; of2.close(); return true; @@ -517,9 +504,9 @@ Eigen::Vector3d EmbeddedMesh::get_mapped_vertex( const Eigen::MatrixXd *x_data, int idx) { - int t_idx = emb_mesh->vtx_to_tet[idx]; + int t_idx = emb_mesh->emb_vtx_to_tet[idx]; RowVector4i tet = emb_mesh->tets.row(t_idx); - RowVector4d b = emb_mesh->barys.row(idx); + RowVector4d b = emb_mesh->emb_barys.row(idx); return Vector3d( x_data->row(tet[0]) * b[0] + x_data->row(tet[1]) * b[1] + diff --git a/extern/softbody/src/admmpd_embeddedmesh.h b/extern/softbody/src/admmpd_embeddedmesh.h index eb7583b93b4..6d1022b8a0c 100644 --- a/extern/softbody/src/admmpd_embeddedmesh.h +++ b/extern/softbody/src/admmpd_embeddedmesh.h @@ -15,7 +15,6 @@ public: const Eigen::MatrixXd &V, // embedded verts const Eigen::MatrixXi &F, // embedded faces EmbeddedMeshData *emb_mesh, // where embedding is stored - Eigen::MatrixXd *x_tets, // lattice vertices, n x 3 bool trim_lattice = true); // remove elements outside embedded volume // Returns the vtx mapped from x/v and tets @@ -28,8 +27,6 @@ public: // for the lattice vertices void compute_masses( EmbeddedMeshData *emb_mesh, // where embedding is stored - const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3 - const Eigen::MatrixXd *x_tets, // lattice vertices, n x 3 Eigen::VectorXd *masses_tets, // masses of the lattice verts double density_kgm3 = 2100); @@ -38,9 +35,7 @@ protected: // Returns true on success // Computes the embedding data, like barycoords bool compute_embedding( - EmbeddedMeshData *emb_mesh, // where embedding is stored - const Eigen::MatrixXd *x_embed, // embedded vertices, p x 3 - const Eigen::MatrixXd *x_tets); // lattice vertices, n x 3 + EmbeddedMeshData *emb_mesh); }; // class Lattice diff --git a/extern/softbody/src/admmpd_linsolve.cpp b/extern/softbody/src/admmpd_linsolve.cpp index 548e1b36dcf..05f7d5da5f4 100644 --- a/extern/softbody/src/admmpd_linsolve.cpp +++ b/extern/softbody/src/admmpd_linsolve.cpp @@ -40,8 +40,10 @@ static inline void make_n3( void ConjugateGradients::solve( const Options *options, - SolverData *data) + SolverData *data, + Collision *collision) { + (void)(collision); // unused BLI_assert(data != NULL); BLI_assert(options != NULL); int nx = data->x.rows(); @@ -154,83 +156,117 @@ void ConjugateGradients::solve_Ax_b( void GaussSeidel::solve( const Options *options, - SolverData *data) + SolverData *data, + Collision *collision) { - init_solve(options,data); - std::vector<std::vector<int> > *colors; - //if (data->gsdata.KtK.nonZeros()==0) - colors = &data->gsdata.A_colors; - //else... - - double omega = 1.0; // over relaxation - int n_colors = colors->size(); + init_solve(options,data,collision); + MatrixXd dx(data->x.rows(),3); + dx.setZero(); + + struct GaussSeidelThreadData { + int iter; + int color; + std::vector<std::vector<int> > *colors; + const Options *options; + SolverData *data; + Collision *collision; + MatrixXd *dx; + }; - // Outer iteration loop - int iter = 0; - for (; iter < options->max_gs_iters; ++iter) + GaussSeidelThreadData thread_data = { + .iter = 0, + .color = 0, + .colors = &data->gsdata.A3_plus_CtC_colors, + .options = options, + .data = data, + .collision = collision, + .dx = &dx }; + + // Inner iteration of Gauss-Seidel + auto parallel_gs_sweep = [](void *__restrict userdata, const int i_, + const TaskParallelTLS *__restrict UNUSED(tls)) -> void { - for (int color=0; color<n_colors; ++color) + GaussSeidelThreadData *td = (GaussSeidelThreadData*)userdata; + int idx = td->colors->at(td->color)[i_]; + double omega = td->options->gs_omega; + typedef RowSparseMatrix<double>::InnerIterator InnerIter; + + // Loop over expanded A matrix, i.e. segment update + Vector3d LUx(0,0,0); + Vector3d inv_aii(0,0,0); + for (int j=0; j<3; ++j) { - const std::vector<int> &inds = colors->at(color); - int n_inds = inds.size(); - for (int i=0; i<n_inds; ++i) + InnerIter rit(td->data->gsdata.A3_plus_CtC, idx*3+j); + for (; rit; ++rit) { - int idx = inds[i]; - - // Special case pins TODO - // We can skip the usual Gauss-Seidel update - // if (is_pinned[idx]) ... + int r = rit.row(); + int c = rit.col(); + double v = rit.value(); + if (v==0.0) + continue; - RowSparseMatrix<double>::InnerIterator rit(data->A,idx); - Vector3d LUx(0,0,0); - Vector3d inv_aii(0,0,0); - for (; rit; ++rit) + if (r==c) // Diagonal { - int r = rit.row(); - int c = rit.col(); - double v = rit.value(); - if (v==0.0) - continue; - - if (r==c) // Diagonal - { - inv_aii.array() = 1.0/v; - continue; - } - Vector3d xj = data->x.row(c); - LUx += v*xj; + inv_aii[j] = 1.0/v; + continue; } - // Update x - Vector3d bi = data->b.row(idx); - Vector3d xi = data->x.row(idx); - Vector3d xi_new = (bi-LUx); + double xj = td->data->x(c/3,j); + LUx[j] += v*xj; + } - for (int j=0; j<3; ++j) - xi_new[j] *= inv_aii[j]; - data->x.row(idx) = xi*(1.0-omega) + xi_new*omega; + } // end loop segment - // TODO - // We can also apply constraints here, like - // checking against Collision::floor_z - if (data->x(idx,2)<0) - data->x(idx,2)=0; + // Update x + Vector3d bi = td->data->b.row(idx).transpose() + + td->data->gsdata.Ctd.segment<3>(idx*3); + Vector3d xi = td->data->x.row(idx); + Vector3d xi_new = (bi-LUx); - } // end loop inds - } // end loop colors + for (int j=0; j<3; ++j) + xi_new[j] *= inv_aii[j]; + td->data->x.row(idx) = xi*(1.0-omega) + xi_new*omega; + + // Check fast-query constraints + double floor_z = td->collision->get_floor(); +// if (td->data->x(idx,2) < floor_z) +// td->data->x(idx,2) = floor_z; + + // Update deltas + td->dx->row(idx) = td->data->x.row(idx)-xi.transpose(); + + }; - // TODO check exit condition + TaskParallelSettings thrd_settings; + BLI_parallel_range_settings_defaults(&thrd_settings); + // Outer iteration loop + int n_colors = data->gsdata.A3_plus_CtC_colors.size(); + int iter = 0; + for (; iter < options->max_gs_iters; ++iter) + { + for (int color=0; color<n_colors; ++color) + { + thread_data.color = color; + int n_inds = data->gsdata.A3_plus_CtC_colors[color].size(); + BLI_task_parallel_range(0, n_inds, &thread_data, parallel_gs_sweep, &thrd_settings); + } // end loop colors + + double dxn = dx.rowwise().lpNorm<Infinity>().maxCoeff(); + if (dxn < options->min_res) + break; } // end loop GS iters } // end solve with constraints void GaussSeidel::init_solve( const Options *options, - SolverData *data) + SolverData *data, + Collision *collision) { BLI_assert(options != nullptr); BLI_assert(data != nullptr); + BLI_assert(collision != nullptr); int nx = data->x.rows(); BLI_assert(nx>0); BLI_assert(data->x.cols()==3); @@ -240,53 +276,13 @@ void GaussSeidel::init_solve( // Do we need to color the default colorings? if (data->gsdata.A_colors.size() == 0) - compute_colors(&data->A, 1, data->gsdata.A_colors); - - // Verify color groups are correct { - std::cout << "TESTING " << data->tets.rows() << " tets" << std::endl; - std::cout << "num colors: " << data->gsdata.A_colors.size() << std::endl; - int nt = data->tets.rows(); - int nc = data->gsdata.A_colors.size(); - for (int i=0; i<nc; ++i) - { - // Each vertex in the color should not - // be a part of a tet with a vertex in the same color - const std::vector<int> &grp = data->gsdata.A_colors[i]; - int n_grp = grp.size(); - for (int j=0; j<n_grp; ++j) - { - int p_idx = grp[j]; - auto in_tet = [](int idx, const RowVector4i &t) - { - return (t[0]==idx||t[1]==idx||t[2]==idx||t[3]==idx); - }; - - for (int k=0; k<nt; ++k) - { - RowVector4i t = data->tets.row(k); - if (!in_tet(p_idx,t)) - continue; - - for (int l=0; l<n_grp; ++l) - { - int q_idx = grp[l]; - if (p_idx==q_idx) - continue; - - if (in_tet(q_idx,t)) - { -std::cerr << "p: " << p_idx << ", q: " << q_idx << ", tet (" << k << "): " << t << std::endl; -// throw std::runtime_error("GaussSeidel Error: Color contains two verts form same tet!"); - } - } - } - } - } + std::vector<std::set<int> > c_graph; + compute_colors(data->energies_graph, c_graph, data->gsdata.A_colors); } // Create large A if we haven't already. - if (data->gsdata.A3.rows() != nx*3) + if (data->gsdata.A3.nonZeros()==0) make_n3(data->A, data->gsdata.A3); // TODO @@ -307,11 +303,17 @@ std::cerr << "p: " << p_idx << ", q: " << q_idx << ", tet (" << k << "): " << t data->gsdata.b3_plus_Ctd[i*3+1] = data->b(i,1)+data->gsdata.Ctd[i*3+1]; data->gsdata.b3_plus_Ctd[i*3+2] = data->b(i,2)+data->gsdata.Ctd[i*3+2]; } - compute_colors(&data->gsdata.A3_plus_CtC, 3, data->gsdata.A3_plus_CtC_colors); + std::vector<std::set<int> > c_graph; + collision->graph(c_graph); + compute_colors(data->energies_graph, c_graph, data->gsdata.A3_plus_CtC_colors); } else { + if (data->gsdata.CtC.rows() != nx*3) + data->gsdata.CtC.resize(nx*3, nx*3); data->gsdata.CtC.setZero(); + if (data->gsdata.Ctd.rows() != nx*3) + data->gsdata.Ctd.resize(nx*3); data->gsdata.Ctd.setZero(); data->gsdata.A3_plus_CtC = data->gsdata.A3; data->gsdata.b3_plus_Ctd.resize(nx*3); @@ -326,145 +328,161 @@ std::cerr << "p: " << p_idx << ", q: " << q_idx << ", tet (" << k << "): " << t } // end init solve -struct GraphColorThreadData { - const RowSparseMatrix<double> *A; - int stride; - std::vector<std::vector<int> > *adjacency; - std::vector<std::set<int> > *palette; - int init_palette_size; - std::vector<int> *conflict; - std::vector<int> *node_colors; -}; - // Rehash of graph coloring from // https://github.com/mattoverby/mclscene/blob/master/include/MCL/GraphColor.hpp void GaussSeidel::compute_colors( - const RowSparseMatrix<double> *A, - int stride, + const std::vector<std::set<int> > &vertex_energies_graph, + const std::vector<std::set<int> > &vertex_constraints_graph, std::vector<std::vector<int> > &colors) { - BLI_assert(A != nullptr); - BLI_assert(stride>0); + int n_nodes = vertex_energies_graph.size(); + BLI_assert(n_nodes>0); + BLI_assert( + vertex_constraints_graph.size()==0 || + (int)vertex_constraints_graph.size()==n_nodes); // Graph color settings int init_palette_size = 6; // Graph coloring tmp data - int n_nodes = A->rows()/stride; - std::vector<std::vector<int> > adjacency(n_nodes, std::vector<int>()); std::vector<std::set<int> > palette(n_nodes, std::set<int>()); std::vector<int> conflict(n_nodes,1); std::vector<int> node_colors(n_nodes,-1); + std::vector<int> node_queue(n_nodes); + std::iota(node_queue.begin(), node_queue.end(), 0); + std::random_device rd; + std::mt19937 mt(rd()); + std::uniform_int_distribution<int> dist(0, n_nodes); + + struct GraphColorThreadData { + int iter; + int init_palette_size; + const std::vector<std::set<int> > *e_graph; // energies + const std::vector<std::set<int> > *c_graph; // constraints + std::vector<std::set<int> > *palette; + std::vector<int> *conflict; + std::vector<int> *node_colors; + std::vector<int> *node_queue; + std::mt19937 *mt; + std::uniform_int_distribution<int> *dist; + }; + GraphColorThreadData thread_data = { + .iter = 0, + .init_palette_size = init_palette_size, + .e_graph = &vertex_energies_graph, + .c_graph = &vertex_constraints_graph, + .palette = &palette, + .conflict = &conflict, + .node_colors = &node_colors, + .node_queue = &node_queue, + .mt = &mt, + .dist = &dist }; + TaskParallelSettings thrd_settings; + BLI_parallel_range_settings_defaults(&thrd_settings); // // Step 1) // Graph color initialization // - auto parallel_init_graph = []( - void *__restrict userdata, - const int i, + auto init_graph = [](void *__restrict userdata, const int i, const TaskParallelTLS *__restrict UNUSED(tls)) -> void { GraphColorThreadData *td = (GraphColorThreadData*)userdata; - typedef RowSparseMatrix<double>::InnerIterator InnerIter; - // Use lower trianglular portion of the matrix. - // That is, store adjacency inds that are lower than - // the current index. - for (InnerIter it(*(td->A),i*td->stride); it; ++it) - { - if (std::abs(it.value())==0.0) - continue; - if (it.col()/td->stride >= it.row()/td->stride) - break; - int idx = it.col()/td->stride; - td->adjacency->at(i).emplace_back(idx); - } for( int j=0; j<td->init_palette_size; ++j ) // init colors td->palette->at(i).insert(j); }; - GraphColorThreadData thread_data = { - .A = A, .stride = stride, - .adjacency = &adjacency, - .palette = &palette, - .init_palette_size = init_palette_size, - .conflict = &conflict, - .node_colors = &node_colors }; - TaskParallelSettings settings; - BLI_parallel_range_settings_defaults(&settings); - BLI_task_parallel_range(0, n_nodes, &thread_data, parallel_init_graph, &settings); - - std::random_device rd; - std::mt19937 mt(rd()); - std::uniform_int_distribution<int> dist(0, n_nodes); + BLI_task_parallel_range(0, n_nodes, &thread_data, init_graph, &thrd_settings); // // Step 2) // Stochastic Graph coloring // - std::vector<int> node_queue(n_nodes); - std::iota(node_queue.begin(), node_queue.end(), 0); int max_iters = n_nodes; - for (int rand_iter=0; n_nodes>0 && rand_iter < max_iters; ++rand_iter) + for (int rand_iter=0; n_nodes>0 && rand_iter<max_iters; ++rand_iter) { - // Generate a random color and find conflicts - for (int i=0; i<n_nodes; ++i) + thread_data.iter = rand_iter; + + // Generate a random color + auto generate_color = [](void *__restrict userdata, const int i, + const TaskParallelTLS *__restrict UNUSED(tls)) -> void { - int idx = node_queue[i]; - if( palette[idx].size() < 2 ){ // Feed if hungry - palette[idx].insert(init_palette_size+rand_iter); + GraphColorThreadData *td = (GraphColorThreadData*)userdata; + int idx = td->node_queue->at(i); + if (td->palette->at(idx).size()<2) // Feed the hungry + td->palette->at(idx).insert(td->init_palette_size+td->iter); + int c_idx = td->dist->operator()(*td->mt) % td->palette->at(idx).size(); + td->node_colors->at(idx) = *std::next(td->palette->at(idx).begin(), c_idx); + }; + BLI_task_parallel_range(0, n_nodes, &thread_data, generate_color, &thrd_settings); + + // Detect conflicts + auto detect_conflicts = [](void *__restrict userdata, const int i, + const TaskParallelTLS *__restrict UNUSED(tls)) -> void + { + GraphColorThreadData *td = (GraphColorThreadData*)userdata; + int idx = td->node_queue->at(i); + int curr_c = td->node_colors->at(idx); + bool curr_conflict = false; + for (std::set<int>::iterator e_it = td->e_graph->at(idx).begin(); + e_it != td->e_graph->at(idx).end() && !curr_conflict; ++e_it) + { + int adj_idx = *e_it; + if (adj_idx<=idx) + continue; // Hungarian heuristic + int adj_c = td->node_colors->at(adj_idx); + if (curr_c==adj_c) + curr_conflict = true; } - - int c_idx = dist(mt) % (int)palette[idx].size(); - int curr_color = *std::next(palette[idx].begin(), c_idx); - node_colors[idx] = curr_color; - conflict[idx] = 0; - - // Hungarian heuristic: node with largest index keeps color - // if both have the same color. - // Check lower-indexed adjacent nodes, and mark them - // in conflict if they generated the same color. - // We can do this if we process colors in an increasing order. - int n_adj = adjacency[idx].size(); - for (int j=0; j<n_adj; ++j) + if ((int)td->c_graph->size() > idx) { - // Adjacency index buffer only stores lower-indexed nodes. - int adj_idx = adjacency[idx][j]; - int adj_color = node_colors[adj_idx]; - if (adj_idx >= idx) - throw std::runtime_error("GaussSeidel Error: Oops, not lower diag"); - if (adj_color==curr_color) - conflict[adj_idx] = 1; + for (std::set<int>::iterator c_it = td->c_graph->at(idx).begin(); + c_it != td->c_graph->at(idx).end() && !curr_conflict; ++c_it) + { + int adj_idx = *c_it; + if (adj_idx<=idx) + continue; // Hungarian heuristic + int adj_c = td->node_colors->at(adj_idx); + if (curr_c==adj_c) + curr_conflict = true; + } } - } + td->conflict->at(idx) = curr_conflict; + }; + BLI_task_parallel_range(0, n_nodes, &thread_data, detect_conflicts, &thrd_settings); - // Resolve conflicts and trim queue - std::set<int> new_queue; + // Resolve conflicts and update queue + std::vector<int> new_queue; for (int i=0; i<n_nodes; ++i) { int idx = node_queue[i]; - int curr_color = node_colors[idx]; - int n_adj = adjacency[idx].size(); - - // If this node not in conflict, remove its - // color from adjacent palettes. If adjacent - // nodes not in conflict, remove their color - // from this palette. - for (int j=0; j<n_adj; ++j) + if (conflict[idx]) + new_queue.emplace_back(idx); + else { - int adj_idx = adjacency[idx][j]; - int adj_color = node_colors[adj_idx]; - if (!conflict[idx]) - palette[adj_idx].erase(curr_color); - if (!conflict[adj_idx]) - palette[idx].erase(adj_color); + int curr_color = node_colors[idx]; + // Remove color from neighbor palletes + for (std::set<int>::iterator e_it = vertex_energies_graph[idx].begin(); + e_it != vertex_energies_graph[idx].end(); ++e_it) + { + int adj_idx = *e_it; + if (conflict[adj_idx]) // still in the set? + palette[adj_idx].erase(curr_color); + } + if ((int)vertex_constraints_graph.size() > idx) + { + for (std::set<int>::iterator c_it = vertex_constraints_graph[idx].begin(); + c_it != vertex_constraints_graph[idx].end(); ++c_it) + { + int adj_idx = *c_it; + if (conflict[adj_idx]) // still in the set? + palette[adj_idx].erase(curr_color); + } + } } - - if (conflict[idx]) - new_queue.emplace(idx); } - n_nodes = new_queue.size(); - node_queue.assign(new_queue.begin(), new_queue.end()); + node_queue = new_queue; + n_nodes = node_queue.size(); } // end color loop @@ -473,6 +491,7 @@ void GaussSeidel::compute_colors( // Map per-vertex colors // colors.clear(); + colors.resize(14,std::vector<int>()); n_nodes = node_colors.size(); for( int i=0; i<n_nodes; ++i ){ int color = node_colors[i]; @@ -489,4 +508,51 @@ void GaussSeidel::compute_colors( } // end compute colors +void GaussSeidel::verify_colors(SolverData *data) +{ + // TODO check constraints too + // Verify color groups are correct + std::cout << "TESTING " << data->tets.rows() << " tets" << std::endl; + std::cout << "num colors: " << data->gsdata.A_colors.size() << std::endl; + int nt = data->tets.rows(); + int nc = data->gsdata.A_colors.size(); + for (int i=0; i<nc; ++i) + { + // Each vertex in the color should not + // be a part of a tet with a vertex in the same color + const std::vector<int> &grp = data->gsdata.A_colors[i]; + int n_grp = grp.size(); + for (int j=0; j<n_grp; ++j) + { + int p_idx = grp[j]; + auto in_tet = [](int idx, const RowVector4i &t) + { + return (t[0]==idx||t[1]==idx||t[2]==idx||t[3]==idx); + }; + + for (int k=0; k<nt; ++k) + { + RowVector4i t = data->tets.row(k); + if (!in_tet(p_idx,t)) + continue; + + for (int l=0; l<n_grp; ++l) + { + int q_idx = grp[l]; + if (p_idx==q_idx) + continue; + + if (in_tet(q_idx,t)) + { + std::cerr << "p: " << p_idx << ", q: " << q_idx << + ", tet (" << k << "): " << t << + ", color: " << i << + std::endl; + } + } + } + } + } +} // end verify colors + } // namespace admmpd
\ No newline at end of file diff --git a/extern/softbody/src/admmpd_linsolve.h b/extern/softbody/src/admmpd_linsolve.h index 3dc3764c6e9..ee6a82b4048 100644 --- a/extern/softbody/src/admmpd_linsolve.h +++ b/extern/softbody/src/admmpd_linsolve.h @@ -5,6 +5,7 @@ #define ADMMPD_LINSOLVE_H_ #include "admmpd_types.h" +#include "admmpd_collision.h" namespace admmpd { @@ -13,7 +14,8 @@ class ConjugateGradients { public: void solve( const Options *options, - SolverData *data); + SolverData *data, + Collision *collision); protected: // Apply preconditioner @@ -28,18 +30,23 @@ class GaussSeidel { public: void solve( const Options *options, - SolverData *data); + SolverData *data, + Collision *collision); protected: void init_solve( const Options *options, - SolverData *data); + SolverData *data, + Collision *collision); void compute_colors( - const RowSparseMatrix<double> *A, - int stride, + const std::vector<std::set<int> > &vertex_energies_graph, + const std::vector<std::set<int> > &vertex_constraints_graph, std::vector<std::vector<int> > &colors); + // For debugging: + void verify_colors(SolverData *data); + }; } // namespace admmpd diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp index 6a4e9bc7e02..7a586106d21 100644 --- a/extern/softbody/src/admmpd_solver.cpp +++ b/extern/softbody/src/admmpd_solver.cpp @@ -46,7 +46,7 @@ bool Solver::init( if (!compute_matrices(options,data)) return false; - printf("Solver::init:\n\tNum tets: %d\n\tNum verts: %d\n",T.rows(),V.rows()); + printf("Solver::init:\n\tNum tets: %d\n\tNum verts: %d\n",(int)T.rows(),(int)V.rows()); return true; } // end init @@ -79,8 +79,8 @@ int Solver::solve( update_constraints(options,data,collision); // Solve Ax=b s.t. Cx=d - ConjugateGradients().solve(options,data); - //GaussSeidel().solve(options,data); + //ConjugateGradients().solve(options,data,collision); + GaussSeidel().solve(options,data,collision); } // end solver iters @@ -279,6 +279,10 @@ void Solver::append_energies( int nt = data->tets.rows(); BLI_assert(nt > 0); + int nx = data->x.rows(); + if ((int)data->energies_graph.size() != nx) + data->energies_graph.resize(nx,std::set<int>()); + data->indices.reserve(nt); data->rest_volumes.reserve(nt); data->weights.reserve(nt); @@ -312,6 +316,18 @@ void Solver::append_energies( continue; } + int ele_dim = ele.cols(); + for (int j=0; j<ele_dim; ++j) + { + int ej = ele[j]; + for (int k=0; k<ele_dim; ++k) + { + int ek = ele[k]; + if (ej==ek) + continue; + data->energies_graph[ej].emplace(ek); + } + } data->indices.emplace_back(energy_index, energy_dim); energy_index += energy_dim; } diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h index 688118c1246..eaeb1d0c93a 100644 --- a/extern/softbody/src/admmpd_types.h +++ b/extern/softbody/src/admmpd_types.h @@ -8,6 +8,7 @@ #include <Eigen/Sparse> #include <Eigen/SparseCholesky> #include <vector> +#include <set> // TODO template type for float/double @@ -19,8 +20,9 @@ struct Options { int max_admm_iters; int max_cg_iters; int max_gs_iters; + double gs_omega; // Gauss-Seidel relaxation double mult_k; // stiffness multiplier for constraints - double min_res; // min residual for CG solver + double min_res; // exit tolerance for global step double youngs; // Young's modulus // TODO variable per-tet double poisson; // Poisson ratio // TODO variable per-tet Eigen::Vector3d grav; @@ -29,8 +31,9 @@ struct Options { max_admm_iters(50), max_cg_iters(10), max_gs_iters(30), + gs_omega(1), mult_k(3), - min_res(1e-6), + min_res(1e-8), youngs(10000000), poisson(0.399), grav(0,0,-9.8) @@ -49,11 +52,12 @@ struct TetMeshData { }; struct EmbeddedMeshData { // i.e. the lattice - Eigen::MatrixXd x_rest; // embedded verts at rest - Eigen::MatrixXi faces; // embedded faces + Eigen::MatrixXd emb_rest_x; // embedded verts at rest + Eigen::MatrixXi emb_faces; // embedded faces + Eigen::VectorXi emb_vtx_to_tet; // what tet vtx is embedded in, p x 1 + Eigen::MatrixXd emb_barys; // barycoords of the embedding, p x 4 Eigen::MatrixXi tets; // lattice elements, m x 4 - Eigen::VectorXi vtx_to_tet; // what tet vtx is embedded in, p x 1 - Eigen::MatrixXd barys; // barycoords of the embedding, p x 4 + Eigen::MatrixXd rest_x; // lattice verts at rest }; // type 1 struct SolverData { @@ -92,6 +96,7 @@ struct SolverData { std::vector<std::vector<int> > A3_plus_CtC_colors; // colors of A3+KtK } gsdata; // Set in append_energies: + std::vector<std::set<int> > energies_graph; // per-vertex adjacency list (graph) std::vector<Eigen::Vector2i> indices; // per-energy index into D (row, num rows) std::vector<double> rest_volumes; // per-energy rest volume std::vector<double> weights; // per-energy weights |