From 9e2042ca033a2ff8134abd719e60292ce65e720c Mon Sep 17 00:00:00 2001 From: over0219 Date: Tue, 23 Jun 2020 23:03:53 -0500 Subject: several bugfixes but I think I'm going to need to change the way I handle constrained solve --- extern/softbody/CMakeLists.txt | 2 + extern/softbody/src/admmpd_collision.cpp | 280 ++++++++++++++++++++-------- extern/softbody/src/admmpd_collision.h | 96 +++++++--- extern/softbody/src/admmpd_embeddedmesh.cpp | 52 ++++++ extern/softbody/src/admmpd_embeddedmesh.h | 15 +- extern/softbody/src/admmpd_math.h | 1 + extern/softbody/src/admmpd_solver.cpp | 53 +----- extern/softbody/src/admmpd_solver.h | 8 +- extern/softbody/src/admmpd_tetmesh.cpp | 58 ++++++ extern/softbody/src/admmpd_tetmesh.h | 25 +++ extern/softbody/src/admmpd_types.h | 21 ++- 11 files changed, 446 insertions(+), 165 deletions(-) create mode 100644 extern/softbody/src/admmpd_tetmesh.cpp create mode 100644 extern/softbody/src/admmpd_tetmesh.h (limited to 'extern') diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt index 4d6ff3328b8..3e006f74ae3 100644 --- a/extern/softbody/CMakeLists.txt +++ b/extern/softbody/CMakeLists.txt @@ -43,6 +43,8 @@ set(SRC src/admmpd_math.cpp src/admmpd_solver.h src/admmpd_solver.cpp + src/admmpd_tetmesh.h + src/admmpd_tetmesh.cpp src/admmpd_types.h ) 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 +#include 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 > *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 *mesh_tree, + const Eigen::MatrixXd *mesh_x, + const Eigen::MatrixXi *mesh_tris, + bool mesh_is_obs, + double floor_z, + std::vector *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 &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 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 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]rows()); + BLI_assert(tri_inds[1]>=0 && tri_inds[1]rows()); + BLI_assert(tri_inds[2]>=0 && tri_inds[2]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 > *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 &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 > pt_vf_pairs (max_threads, std::vector()); + 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 &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 > *trips_x, + std::vector > *trips_y, + std::vector > *trips_z, + std::vector *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; ibarys.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 > pt_vf_pairs + (max_threads, std::vector()); 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 > *trips_x, std::vector > *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; ibarys.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 diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h index f49a314db47..f9c6363b89f 100644 --- a/extern/softbody/src/admmpd_collision.h +++ b/extern/softbody/src/admmpd_collision.h @@ -21,13 +21,16 @@ struct VFCollisionPair { VFCollisionPair(); }; -// 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 -// all self and obstacle collisions, reducing the amount of -// for-all vertices loops. class Collision { public: + // Obstacle data created in set_obstacles + struct ObstacleData { + Eigen::MatrixXd V0, V1; + Eigen::MatrixXi F; + std::vector > aabbs; + AABBTree tree; + } obsdata; + virtual ~Collision() {} // Performs collision detection. @@ -37,12 +40,14 @@ public: const Eigen::MatrixXd *x1) = 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. virtual void set_obstacles( const float *v0, const float *v1, int nv, const unsigned int *faces, - int nf) = 0; + int nf); // Special case for floor since it's common. virtual void set_floor(double z) = 0; @@ -56,6 +61,21 @@ public: std::vector > *trips_y, std::vector > *trips_z, std::vector *l) = 0; + + // Given a point and a surface mesh, + // perform discrete collision and create + // a vertex-face collision pair if colliding. + // Also adds collision pairs if below floor. + static void detect_discrete_vf( + const Eigen::Vector3d &pt, + int pt_idx, + bool pt_is_obs, + const AABBTree *mesh_tree, + const Eigen::MatrixXd *mesh_x, + const Eigen::MatrixXi *mesh_tris, + bool mesh_is_obs, + double floor_z, + std::vector *pairs); }; // Collision detection against multiple meshes @@ -66,23 +86,6 @@ public: floor_z(-std::numeric_limits::max()) {} - // Obstacle data created in set_obstacles - struct ObstacleData { - Eigen::MatrixXd V0, V1; - Eigen::MatrixXi F; - std::vector > aabbs; - AABBTree tree; - } obsdata; - - // 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. - void set_obstacles( - const float *v0, - const float *v1, - int nv, - const unsigned int *faces, - int nf); - // 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) @@ -121,6 +124,53 @@ protected: { (void)(x0); (void)(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( + 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; + + // Pairs are compute on detect + std::vector vf_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); } +}; +*/ + } // namespace admmpd #endif // ADMMPD_COLLISION_H_ diff --git a/extern/softbody/src/admmpd_embeddedmesh.cpp b/extern/softbody/src/admmpd_embeddedmesh.cpp index 78993ff73da..3476c34c720 100644 --- a/extern/softbody/src/admmpd_embeddedmesh.cpp +++ b/extern/softbody/src/admmpd_embeddedmesh.cpp @@ -255,6 +255,58 @@ bool EmbeddedMesh::generate( } // end gen lattice +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); + + // TODO + // map the area of the surface to the tet vertices + + // 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(); + masses_tets->resize(nx); + masses_tets->setZero(); + int n_tets = emb_mesh->tets.rows(); + for (int t=0; ttets.row(t); + RowVector3d tet_v0 = x_tets->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; + double vol = std::abs((edges).determinant()/6.f); + double tet_mass = density_kgm3 * vol; + masses_tets->operator[](tet[0]) += tet_mass / 4.f; + masses_tets->operator[](tet[1]) += tet_mass / 4.f; + masses_tets->operator[](tet[2]) += tet_mass / 4.f; + masses_tets->operator[](tet[3]) += tet_mass / 4.f; + } + + // Verify masses + for (int i=0; ioperator[](i) <= 0.0) + { + printf("**EmbeddedMesh::compute_masses Error: unreferenced vertex\n"); + masses_tets->operator[](i)=1; + } + } +} // end compute masses + typedef struct FindTetThreadData { AABBTree *tree; EmbeddedMeshData *emb_mesh; // thread sets vtx_to_tet and barys diff --git a/extern/softbody/src/admmpd_embeddedmesh.h b/extern/softbody/src/admmpd_embeddedmesh.h index a1abe02f904..8b0cdc5bb04 100644 --- a/extern/softbody/src/admmpd_embeddedmesh.h +++ b/extern/softbody/src/admmpd_embeddedmesh.h @@ -1,8 +1,8 @@ // Copyright Matt Overby 2020. // Distributed under the MIT License. -#ifndef ADMMPD_LATTICE_H_ -#define ADMMPD_LATTICE_H_ +#ifndef ADMMPD_EMBEDDEDMESH_H_ +#define ADMMPD_EMBEDDEDMESH_H_ #include "admmpd_types.h" @@ -23,6 +23,15 @@ public: const Eigen::MatrixXd *x_data, int idx); + // Given an embedding, compute masses + // 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 = 1100); + protected: // Returns true on success @@ -36,4 +45,4 @@ protected: } // namespace admmpd -#endif // ADMMPD_LATTICE_H_ +#endif // ADMMPD_EMBEDDEDMESH_H_ diff --git a/extern/softbody/src/admmpd_math.h b/extern/softbody/src/admmpd_math.h index 939d2d3b230..ff216929f86 100644 --- a/extern/softbody/src/admmpd_math.h +++ b/extern/softbody/src/admmpd_math.h @@ -5,6 +5,7 @@ #define ADMMPD_MATH_H_ #include +#include namespace admmpd { namespace barycoords { diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp index f3f9b19ee0f..f4956e8d476 100644 --- a/extern/softbody/src/admmpd_solver.cpp +++ b/extern/softbody/src/admmpd_solver.cpp @@ -27,6 +27,7 @@ typedef struct ThreadData { bool Solver::init( const Eigen::MatrixXd &V, const Eigen::MatrixXi &T, + const Eigen::VectorXd &m, const Options *options, SolverData *data) { @@ -41,6 +42,7 @@ bool Solver::init( data->v.resize(V.rows(),3); data->v.setZero(); data->tets = T; + data->m = m; if (!compute_matrices(options,data)) return false; @@ -105,7 +107,7 @@ void Solver::init_solve( data->x_start = data->x; for (int i=0; iv.row(i) += options->grav; + data->v.row(i) += dt*options->grav; data->M_xbar.row(i) = data->m[i] * data->x.row(i) + dt*data->m[i]*data->v.row(i); @@ -143,7 +145,8 @@ void Solver::solve_local_step( { BLI_assert(data != NULL); BLI_assert(options != NULL); - int ne = data->rest_volumes.size(); + data->Dx.noalias() = data->D * data->x; + int ne = data->indices.size(); BLI_assert(ne > 0); ThreadData thread_data = {.options=options, .data = data}; TaskParallelSettings settings; @@ -191,7 +194,9 @@ void Solver::update_constraints( } // Otherwise update the data. - data->l = Map(l_coeffs.data(),nc); + data->l.resize(nc); + for (int i=0; il[i] = l_coeffs[i]; data->K[0].resize(nc,nx); data->K[0].setFromTriplets(trips_x.begin(),trips_x.end()); data->K[1].resize(nc,nx); @@ -342,8 +347,6 @@ bool Solver::compute_matrices( data->v.resize(nx,3); data->v.setZero(); } - if (data->m.rows() != nx) - compute_masses(options,data); // Add per-element energies to data std::vector > trips; @@ -397,46 +400,6 @@ bool Solver::compute_matrices( } // end compute matrices -void Solver::compute_masses( - const Options *options, - SolverData *data) -{ - BLI_assert(data != NULL); - BLI_assert(options != NULL); - - // 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 - data->m.resize(data->x.rows()); - data->m.setZero(); - int n_tets = data->tets.rows(); - for (int t=0; ttets.row(t); - RowVector3d tet0 = data->x.row(tet[0]); - Matrix3d edges; - edges.col(0) = data->x.row(tet[1]) - tet0; - edges.col(1) = data->x.row(tet[2]) - tet0; - edges.col(2) = data->x.row(tet[3]) - tet0; - double v = std::abs((edges).determinant()/6.f); - double tet_mass = options->density_kgm3 * v; - data->m[tet[0]] += tet_mass / 4.f; - data->m[tet[1]] += tet_mass / 4.f; - data->m[tet[2]] += tet_mass / 4.f; - data->m[tet[3]] += tet_mass / 4.f; - } - // Verify masses - int nx = data->m.rows(); - for (int i=0; im[i] <= 0.0) - { - printf("**Solver::compute_masses Error: unreferenced vertex\n"); - data->m[i]=1; - } - } -} // end compute masses - void Solver::append_energies( const Options *options, SolverData *data, diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h index 1744b0f80c9..3c16b733b82 100644 --- a/extern/softbody/src/admmpd_solver.h +++ b/extern/softbody/src/admmpd_solver.h @@ -17,6 +17,7 @@ public: bool init( const Eigen::MatrixXd &V, // vertices const Eigen::MatrixXi &T, // tets + const Eigen::VectorXd &m, // per-vert masses const Options *options, SolverData *data); @@ -53,13 +54,6 @@ protected: const Options *options, SolverData *data); - // TODO we need to compute masses - // based on the embedded mesh, otherwise - // we get super ugly dynamics. - void compute_masses( - const Options *options, - SolverData *data); - void append_energies( const Options *options, SolverData *data, diff --git a/extern/softbody/src/admmpd_tetmesh.cpp b/extern/softbody/src/admmpd_tetmesh.cpp new file mode 100644 index 00000000000..43157c416ae --- /dev/null +++ b/extern/softbody/src/admmpd_tetmesh.cpp @@ -0,0 +1,58 @@ +// Copyright Matt Overby 2020. +// Distributed under the MIT License. + +#include "admmpd_tetmesh.h" +#include "admmpd_math.h" +#include +#include +#include "BLI_assert.h" + +namespace admmpd { +using namespace Eigen; + +void TetMesh::compute_masses( + TetMeshData *mesh, + const Eigen::MatrixXd *x, + Eigen::VectorXd *masses, + double density_kgm3) +{ + BLI_assert(mesh != NULL); + BLI_assert(x != NULL); + BLI_assert(masses != NULL); + BLI_assert(density_kgm3 > 0); + + // 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->rows(); + masses->resize(nx); + masses->setZero(); + int n_tets = mesh->tets.rows(); + for (int t=0; ttets.row(t); + RowVector3d tet_v0 = x->row(tet[0]); + Matrix3d edges; + edges.col(0) = x->row(tet[1]) - tet_v0; + edges.col(1) = x->row(tet[2]) - tet_v0; + edges.col(2) = x->row(tet[3]) - tet_v0; + double vol = std::abs((edges).determinant()/6.f); + double tet_mass = density_kgm3 * vol; + masses->operator[](tet[0]) += tet_mass / 4.f; + masses->operator[](tet[1]) += tet_mass / 4.f; + masses->operator[](tet[2]) += tet_mass / 4.f; + masses->operator[](tet[3]) += tet_mass / 4.f; + } + + // Verify masses + for (int i=0; ioperator[](i) <= 0.0) + { + printf("**TetMesh::compute_masses Error: unreferenced vertex\n"); + masses->operator[](i)=1; + } + } +} // end compute masses + +} // namespace admmpd diff --git a/extern/softbody/src/admmpd_tetmesh.h b/extern/softbody/src/admmpd_tetmesh.h new file mode 100644 index 00000000000..7d1e4304cd2 --- /dev/null +++ b/extern/softbody/src/admmpd_tetmesh.h @@ -0,0 +1,25 @@ +// Copyright Matt Overby 2020. +// Distributed under the MIT License. + +#ifndef ADMMPD_TETMESH_H_ +#define ADMMPD_TETMESH_H_ + +#include "admmpd_types.h" + +namespace admmpd { + +class TetMesh { +public: + // Given an embedding, compute masses + // for the lattice vertices + static void compute_masses( + TetMeshData *mesh, + const Eigen::MatrixXd *x, + Eigen::VectorXd *masses, + double density_kgm3 = 1100); + +}; // class Lattice + +} // namespace admmpd + +#endif // ADMMPD_LATTICE_H_ diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h index de1fb122a30..42277180da7 100644 --- a/extern/softbody/src/admmpd_types.h +++ b/extern/softbody/src/admmpd_types.h @@ -20,28 +20,31 @@ struct Options { int max_cg_iters; double mult_k; // stiffness multiplier for constraints double min_res; // min residual for CG solver - double density_kgm3; // unit-volume density double youngs; // Young's modulus // TODO variable per-tet double poisson; // Poisson ratio // TODO variable per-tet Eigen::Vector3d grav; Options() : - timestep_s(1.0/100.0), - max_admm_iters(40), - max_cg_iters(20), + timestep_s(1.0/24.0), + max_admm_iters(100), + max_cg_iters(10), mult_k(1), min_res(1e-6), - density_kgm3(1100), - youngs(100000000), - poisson(0.399), + youngs(10000000), + poisson(0.299), grav(0,0,-9.8) {} }; -struct TetMesh { +// I think eventually I'll make the mesh +// a virtual class with mapping functions. +// That might clean up the API/interface a bit. +// Will work out what we need for collisions and such first. + +struct TetMeshData { Eigen::MatrixXd x_rest; // verts at rest Eigen::MatrixXi faces; // surface elements, m x 3 Eigen::MatrixXi tets; // internal elements, m x 4 -}; // type 0 +}; struct EmbeddedMeshData { // i.e. the lattice Eigen::MatrixXd x_rest; // embedded verts at rest -- cgit v1.2.3