diff options
author | over0219 <over0219@umn.edu> | 2020-06-23 04:19:25 +0300 |
---|---|---|
committer | over0219 <over0219@umn.edu> | 2020-06-23 04:19:25 +0300 |
commit | 0d317dddc9c3a9277fc559f18f28d958cff72a30 (patch) | |
tree | b012733a69f86c7548f22b7dffd09c4ad773fefb | |
parent | 88ab1c138dcb370829ae0362ad6ee8a462d8a6ca (diff) |
working on collision
-rw-r--r-- | extern/softbody/src/admmpd_collision.cpp | 59 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_collision.h | 110 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_solver.cpp | 15 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_solver.h | 8 | ||||
-rw-r--r-- | extern/softbody/src/admmpd_types.h | 2 | ||||
-rw-r--r-- | intern/softbody/admmpd_api.cpp | 2 |
6 files changed, 122 insertions, 74 deletions
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp index 54bca95729e..d190bacae43 100644 --- a/extern/softbody/src/admmpd_collision.cpp +++ b/extern/softbody/src/admmpd_collision.cpp @@ -7,6 +7,13 @@ namespace admmpd { using namespace Eigen; +EmbeddedMeshCollision::VFCollisionPair::VFCollisionPair() : + p(-1), // point + p_is_obs(0), // 0 or 1 + q(-1), // face + q_is_obs(0) // 0 or 1 + {} + void EmbeddedMeshCollision::set_obstacles( const float *v0, const float *v1, @@ -14,42 +21,64 @@ void EmbeddedMeshCollision::set_obstacles( const int *faces, int nf) { - if (obs_V0.rows() != nv) - obs_V0.resize(nv,3); + if (obsdata.V0.rows() != nv) + obsdata.V0.resize(nv,3); - if (obs_V1.rows() != nv) - obs_V1.resize(nv,3); + if (obsdata.V1.rows() != nv) + obsdata.V1.resize(nv,3); for (int i=0; i<nv; ++i) { for (int j=0; j<3; ++j) { - obs_V0(i,j) = v0[i*3+j]; - obs_V1(i,j) = v1[i*3+j]; + obsdata.V0(i,j) = v0[i*3+j]; + obsdata.V1(i,j) = v1[i*3+j]; } } - if (obs_F.rows() != nf) + if (obsdata.F.rows() != nf) { - obs_F.resize(nf,3); - obs_aabbs.resize(nf); + obsdata.F.resize(nf,3); + obsdata.aabbs.resize(nf); } for (int i=0; i<nf; ++i) { - obs_aabbs[i].setEmpty(); + obsdata.aabbs[i].setEmpty(); for (int j=0; j<3; ++j) { int fj = faces[i*3+j]; - obs_F(i,j) = fj; - obs_aabbs[i].extend(obs_V0.row(fj).transpose()); - obs_aabbs[i].extend(obs_V1.row(fj).transpose()); + obsdata.F(i,j) = fj; + obsdata.aabbs[i].extend(obsdata.V0.row(fj).transpose()); + obsdata.aabbs[i].extend(obsdata.V1.row(fj).transpose()); } } - obs_tree.init(obs_aabbs); + obsdata.tree.init(obsdata.aabbs); } // end add obstacle + +typedef struct DetectThreadData { + const EmbeddedMeshData *mesh; + const EmbeddedMeshCollision::ObstacleData *obsdata; + const Eigen::MatrixXd *x0; + const Eigen::MatrixXd *x1; +} DetectThreadData; + +int EmbeddedMeshCollision::detect( + const Eigen::MatrixXd *x0, + const Eigen::MatrixXd *x1) +{ + if (mesh==NULL) + return 0; + + update_bvh(x0,x1); + + + + + return 0; +} /* void EmbeddedMeshCollision::detect(const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1) { @@ -79,7 +108,7 @@ void EmbeddedMeshCollision::detect(const Eigen::MatrixXd *x0, const Eigen::Matri // Check if we are inside the mesh. // If so, find the nearest face in the rest pose. - PointInTriangleMeshTraverse<double> pt_in_mesh(pt, &obs_V1, &obs_F); + PointInTriangleMeshTraverse<double> pt_in_mesh(pt, &V1, &F); obs_tree.traverse(pt_in_mesh); if (pt_in_mesh.output.num_hits() % 2 == 1) { diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h index 2996249158e..4f2cb973b5c 100644 --- a/extern/softbody/src/admmpd_collision.h +++ b/extern/softbody/src/admmpd_collision.h @@ -16,10 +16,11 @@ namespace admmpd { // for-all vertices loops. class Collision { public: -// virtual void detect( -// int meshnum, -// const Eigen::MatrixXd *x, -// const Eigen::MatrixXi *faces) = 0; + // Returns the number of active constraints + virtual int detect( + const Eigen::MatrixXd *x0, + const Eigen::MatrixXd *x1) = 0; + // virtual void jacobian( // const Eigen::MatrixXd *x, // std::vector<Eigen::Triplet<double> > *trips_x, @@ -30,27 +31,29 @@ public: // Collision detection against multiple meshes class EmbeddedMeshCollision : public Collision { -protected: - // We progressively build a list of vertices and faces with each - // add_obstacle call, reindexing as needed. Then we build a tree - // with all of them. Alternatively we could just build separate trees and combine them. - Eigen::MatrixXd obs_V0, obs_V1; - Eigen::MatrixXi obs_F; - std::vector<Eigen::AlignedBox<double,3> > obs_aabbs; - AABBTree<double,3> obs_tree; - - Eigen::MatrixXd emb_V0, emb_V1; // copy of embedded vertices - const Eigen::MatrixXd *emb_barys; // barys of the embedded vtx - const Eigen::VectorXi *vtx_to_tet; // vertex to tet embedding - const Eigen::MatrixXi *tets; // tets that embed faces - - struct CollisionPair { +public: + EmbeddedMeshCollision() : + mesh(NULL), + floor_z(-std::numeric_limits<double>::max()) + {} + + struct VFCollisionPair { int p; // point + int p_is_obs; // 0 or 1 int q; // face - Eigen::Vector3d barys; // barycoords of collision + int q_is_obs; // 0 or 1 + VFCollisionPair(); + Eigen::Vector3d barys; }; -public: + // Obstacle data created in set_obstacles + struct ObstacleData { + Eigen::MatrixXd V0, V1; + Eigen::MatrixXi F; + std::vector<Eigen::AlignedBox<double,3> > aabbs; + AABBTree<double,3> 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( @@ -60,24 +63,27 @@ public: const int *faces, int nf); - // Updates the tetmesh BVH for self collisions - // TODO - void update_bvh( - const EmbeddedMeshData *mesh, - const Eigen::MatrixXd *x0, - const Eigen::MatrixXd *x1) - { (void)(mesh); (void)(x0); (void)(x1); } + // Ptr is stored after this call. Should be + // a smart ptr I guess... + void set_mesh(const EmbeddedMeshData *mesh_) + { + mesh=mesh_; + } + + // 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; + }; - // Given a list of deformable vertices (the lattice) - // perform collision detection of the surface mesh against - // obstacles and possibly self. - void detect( - const EmbeddedMeshData *mesh, + // Performs collision detection and stores pairs + int detect( const Eigen::MatrixXd *x0, - const Eigen::MatrixXd *x1){ - - } + const Eigen::MatrixXd *x1); + // Linearizes the collision pairs about x + // for the constraint Kx=l void jacobian( const Eigen::MatrixXd *x, std::vector<Eigen::Triplet<double> > *trips_x, @@ -87,22 +93,24 @@ 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; + + // 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); } }; -/* -class FloorCollider : public Collider { -public: - virtual void detect( - int meshnum, - const Eigen::MatrixXd *x, - const Eigen::MatrixXi *faces); - void jacobian( - const Eigen::MatrixXd *x, - std::vector<Eigen::Triplet<double> > *trips_x, - std::vector<Eigen::Triplet<double> > *trips_y, - std::vector<Eigen::Triplet<double> > *trips_z, - std::vector<double> *l); -}; -*/ + } // namespace admmpd #endif // ADMMPD_COLLISION_H_ diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp index 17bd8c7d008..5f81c9c565c 100644 --- a/extern/softbody/src/admmpd_solver.cpp +++ b/extern/softbody/src/admmpd_solver.cpp @@ -49,7 +49,8 @@ bool Solver::init( int Solver::solve( const Options *options, - SolverData *data) + SolverData *data, + Collision *collision) { BLI_assert(data != NULL); BLI_assert(options != NULL); @@ -71,7 +72,7 @@ int Solver::solve( solve_local_step(options,data); // Perform collision detection and linearization - update_constraints(options,data); + update_constraints(options,data,collision); // Solve Ax=b s.t. Kx=l data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u); @@ -152,18 +153,24 @@ void Solver::solve_local_step( void Solver::update_constraints( const Options *options, - SolverData *data) + SolverData *data, + Collision *collision) { BLI_assert(data != NULL); BLI_assert(options != NULL); + if (collision==NULL) + return; + + collision->detect(&data->x_start, &data->x); + std::vector<double> l_coeffs; std::vector<Eigen::Triplet<double> > trips_x; std::vector<Eigen::Triplet<double> > trips_y; std::vector<Eigen::Triplet<double> > trips_z; // TODO collision detection -// FloorCollider().jacobian( +// collision->jacobian( // &data->x, // &trips_x, // &trips_y, diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h index d26da293bc8..23d79931202 100644 --- a/extern/softbody/src/admmpd_solver.h +++ b/extern/softbody/src/admmpd_solver.h @@ -5,6 +5,7 @@ #define ADMMPD_SOLVER_H_ #include "admmpd_types.h" +#include "admmpd_collision.h" namespace admmpd { @@ -21,15 +22,18 @@ public: // Solve a single time step. // Returns number of iterations. + // Collision ptr can be null. int solve( const Options *options, - SolverData *data); + SolverData *data, + Collision *collision); protected: void update_constraints( const Options *options, - SolverData *data); + SolverData *data, + Collision *collision); void init_solve( const Options *options, diff --git a/extern/softbody/src/admmpd_types.h b/extern/softbody/src/admmpd_types.h index 925072b282f..941a92bdd94 100644 --- a/extern/softbody/src/admmpd_types.h +++ b/extern/softbody/src/admmpd_types.h @@ -12,6 +12,7 @@ // TODO template type for float/double namespace admmpd { +template <typename T> using RowSparseMatrix = Eigen::SparseMatrix<T,Eigen::RowMajor>; struct Options { double timestep_s; // TODO: Figure out delta time from blender api @@ -63,7 +64,6 @@ struct SolverData { Eigen::MatrixXd M_xbar; // M*(x + dt v) Eigen::MatrixXd Dx; // D * x Eigen::MatrixXd b; // M xbar + DtW2(z-u) - template <typename T> using RowSparseMatrix = Eigen::SparseMatrix<T,Eigen::RowMajor>; RowSparseMatrix<double> D; // reduction matrix RowSparseMatrix<double> DtW2; // D'W^2 RowSparseMatrix<double> A; // M + D'W^2D diff --git a/intern/softbody/admmpd_api.cpp b/intern/softbody/admmpd_api.cpp index 203ec9bf9c6..4fa549211d3 100644 --- a/intern/softbody/admmpd_api.cpp +++ b/intern/softbody/admmpd_api.cpp @@ -276,7 +276,7 @@ void admmpd_solve(ADMMPDInterfaceData *iface) try { - admmpd::Solver().solve(iface->idata->options,iface->idata->data); + admmpd::Solver().solve(iface->idata->options,iface->idata->data,NULL); } catch(const std::exception &e) { |