From 9ba5c8b90aa6245c569f7e66b0cd472beb59b5a5 Mon Sep 17 00:00:00 2001 From: over0219 Date: Tue, 2 Jun 2020 19:53:14 -0500 Subject: first commit. interfaced to very basic admmpd solver, but requires restart to begin sim as caching not working for velocity. lattice is simply a packed 5-tet cube around mesh. --- extern/CMakeLists.txt | 1 + extern/softbody/CMakeLists.txt | 43 +++++++ extern/softbody/README.md | 3 + extern/softbody/src/admmpd_energy.cpp | 112 ++++++++++++++++ extern/softbody/src/admmpd_energy.h | 61 +++++++++ extern/softbody/src/admmpd_lattice.cpp | 229 +++++++++++++++++++++++++++++++++ extern/softbody/src/admmpd_lattice.h | 57 ++++++++ extern/softbody/src/admmpd_math.cpp | 67 ++++++++++ extern/softbody/src/admmpd_math.h | 30 +++++ extern/softbody/src/admmpd_solver.cpp | 200 ++++++++++++++++++++++++++++ extern/softbody/src/admmpd_solver.h | 91 +++++++++++++ 11 files changed, 894 insertions(+) create mode 100644 extern/softbody/CMakeLists.txt create mode 100644 extern/softbody/README.md create mode 100644 extern/softbody/src/admmpd_energy.cpp create mode 100644 extern/softbody/src/admmpd_energy.h create mode 100644 extern/softbody/src/admmpd_lattice.cpp create mode 100644 extern/softbody/src/admmpd_lattice.h create mode 100644 extern/softbody/src/admmpd_math.cpp create mode 100644 extern/softbody/src/admmpd_math.h create mode 100644 extern/softbody/src/admmpd_solver.cpp create mode 100644 extern/softbody/src/admmpd_solver.h (limited to 'extern') diff --git a/extern/CMakeLists.txt b/extern/CMakeLists.txt index 235c2fa931a..7d4023cf52a 100644 --- a/extern/CMakeLists.txt +++ b/extern/CMakeLists.txt @@ -34,6 +34,7 @@ endif() add_subdirectory(rangetree) add_subdirectory(wcwidth) +add_subdirectory(softbody) if(WITH_BULLET) if(NOT WITH_SYSTEM_BULLET) diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt new file mode 100644 index 00000000000..08a36882f28 --- /dev/null +++ b/extern/softbody/CMakeLists.txt @@ -0,0 +1,43 @@ +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# The Original Code is Copyright (C) 2006, Blender Foundation +# All rights reserved. +# ***** END GPL LICENSE BLOCK ***** + +set(INC + src +) + +set(INC_SYS + ${EIGEN3_INCLUDE_DIRS} +) + +set(SRC + src/admmpd_math.h + src/admmpd_math.cpp + src/admmpd_energy.h + src/admmpd_energy.cpp + src/admmpd_lattice.h + src/admmpd_lattice.cpp + src/admmpd_solver.h + src/admmpd_solver.cpp +) + +set(LIB +) + +blender_add_lib(extern_admmpd "${SRC}" "${INC}" "${INC_SYS}" "${LIB}") diff --git a/extern/softbody/README.md b/extern/softbody/README.md new file mode 100644 index 00000000000..78144e7b666 --- /dev/null +++ b/extern/softbody/README.md @@ -0,0 +1,3 @@ +# SoftBody # + + diff --git a/extern/softbody/src/admmpd_energy.cpp b/extern/softbody/src/admmpd_energy.cpp new file mode 100644 index 00000000000..978fd565aa4 --- /dev/null +++ b/extern/softbody/src/admmpd_energy.cpp @@ -0,0 +1,112 @@ + + + +#include "admmpd_energy.h" + +namespace admmpd { +using namespace Eigen; + +Lame::Lame() : m_model(0) +{ + set_from_youngs_poisson(10000000,0.299); +} + +void Lame::set_from_youngs_poisson(double youngs, double poisson) +{ + m_mu = youngs/(2.0*(1.0+poisson)); + m_lambda = youngs*poisson/((1.0+poisson)*(1.0-2.0*poisson)); + m_bulk_mod = m_lambda + (2.0/3.0)*m_mu; +} + +void EnergyTerm::signed_svd( + const Eigen::Matrix& A, + Eigen::Matrix &U, + Eigen::Matrix &S, + Eigen::Matrix &V) +{ + JacobiSVD svd(A, ComputeFullU | ComputeFullV); + S = svd.singularValues(); + U = svd.matrixU(); + V = svd.matrixV(); + Matrix3d J = Matrix3d::Identity(); + J(2,2) = -1.0; + if( U.determinant() < 0.0 ) + { + U = U * J; + S[2] = -S[2]; + } + if( V.determinant() < 0.0 ) + { + Matrix3d Vt = V.transpose(); + Vt = J * Vt; + V = Vt.transpose(); + S[2] = -S[2]; + } +} + +void EnergyTerm::update( + int index, + const Lame &lame, + double rest_volume, + double weight, + const Eigen::MatrixXd *x, + const Eigen::MatrixXd *Dx, + Eigen::MatrixXd *z, + Eigen::MatrixXd *u) +{ + Matrix3d Dix = Dx->block<3,3>(index,0); + Matrix3d ui = u->block<3,3>(index,0); + Matrix3d zi = Dix + ui; + + Matrix3d U, V; + Vector3d S; + signed_svd(zi, U, S, V); + S = Vector3d::Ones(); + + Matrix3d p = U * S.asDiagonal() * V.transpose(); + double k = lame.m_bulk_mod; + double kv = k * rest_volume; + double w2 = weight*weight; + zi = (kv*p + w2*zi) / (w2 + kv); + ui += (Dix - zi); + + u->block<3,3>(index,0) = ui; + z->block<3,3>(index,0) = zi; + +} // end EnergyTerm::update + +int EnergyTerm::init_tet( + int index, + const Lame &lame, + const Eigen::Matrix &prim, + const Eigen::MatrixXd *x, + double &volume, + double &weight, + std::vector< Eigen::Triplet > &triplets ) +{ + Matrix edges; + edges.col(0) = x->row(prim[1]) - x->row(prim[0]); + edges.col(1) = x->row(prim[2]) - x->row(prim[0]); + edges.col(2) = x->row(prim[3]) - x->row(prim[0]); + Matrix edges_inv = edges.inverse(); + volume = edges.determinant() / 6.0f; + if( volume < 0 ) + throw std::runtime_error("**Solver::energy_init: Inverted initial tet"); + double k = lame.m_bulk_mod; + weight = std::sqrt(k*volume); + Matrix S = Matrix::Zero(); + S(0,0) = -1; S(0,1) = -1; S(0,2) = -1; + S(1,0) = 1; S(2,1) = 1; S(3,2) = 1; + Eigen::Matrix D = S * edges_inv; + Eigen::Matrix Dt = D.transpose(); + int rows[3] = { index+0, index+1, index+2 }; + int cols[4] = { prim[0], prim[1], prim[2], prim[3] }; + for( int r=0; r<3; ++r ){ + for( int c=0; c<4; ++c ){ + triplets.emplace_back(rows[r], cols[c], Dt(r,c)); + } + } + return 3; +} + +} // end namespace mcl diff --git a/extern/softbody/src/admmpd_energy.h b/extern/softbody/src/admmpd_energy.h new file mode 100644 index 00000000000..4245c9cff9c --- /dev/null +++ b/extern/softbody/src/admmpd_energy.h @@ -0,0 +1,61 @@ + + + +#ifndef _ADMMPD_ENERGY_H +#define _ADMMPD_ENERGY_H 1 + +#include +#include +#include + +namespace admmpd { + +class Lame { +public: + int m_model; + double m_mu; + double m_lambda; + double m_bulk_mod; + void set_from_youngs_poisson(double youngs, double poisson); + Lame(); +}; + +class EnergyTerm { +public: + + void signed_svd( + const Eigen::Matrix &A, + Eigen::Matrix &U, + Eigen::Matrix &S, + Eigen::Matrix &V); + + // Updates the z and u variables for an element energy. + void update( + int index, + const Lame &lame, + double rest_volume, + double weight, + const Eigen::MatrixXd *x, + const Eigen::MatrixXd *Dx, + Eigen::MatrixXd *z, + Eigen::MatrixXd *u); + + // Initializes tet energy, returns num rows for D + int init_tet( + int index, + const Lame &lame, + const Eigen::RowVector4i &prim, + const Eigen::MatrixXd *x, + double &volume, + double &weight, + std::vector< Eigen::Triplet > &triplets); + +}; + +} // end namespace admmpd + +#endif // _ADMMPD_ENERGY_H + + + + diff --git a/extern/softbody/src/admmpd_lattice.cpp b/extern/softbody/src/admmpd_lattice.cpp new file mode 100644 index 00000000000..4323f8fce33 --- /dev/null +++ b/extern/softbody/src/admmpd_lattice.cpp @@ -0,0 +1,229 @@ + + + +#include "admmpd_lattice.h" +#include "admmpd_math.h" + +//#include "vdb.h" + +namespace admmpd { +using namespace Eigen; + +// We store our deformable data in column major to make +// matrix-vector mults and solves faster, but we want +// to map raw data as row major. +inline void map_to_x3d(const std::vector &x_vec, Eigen::MatrixXd *x) +{ + int nx = x_vec.size(); + if (nx==0) + { + *x = MatrixXd(); + return; + } + + x->resize(nx,3); + for (int i=0; ioperator()(i,0) = x_vec[i][0]; + x->operator()(i,1) = x_vec[i][1]; + x->operator()(i,2) = x_vec[i][2]; + } +} + +inline void map_to_x4i(const std::vector &x_vec, Eigen::MatrixXi *x) +{ + int nx = x_vec.size(); + if (nx==0) + { + *x = MatrixXi(); + return; + } + + x->resize(nx,4); + for (int i=0; ioperator()(i,0) = x_vec[i][0]; + x->operator()(i,1) = x_vec[i][1]; + x->operator()(i,2) = x_vec[i][2]; + x->operator()(i,3) = x_vec[i][3]; + } +} + +bool Lattice::generate( + const Eigen::MatrixXd &V, + Eigen::MatrixXd *x, // lattice vertices, n x 3 + Eigen::MatrixXi *tets) // lattice elements, m x 4 +{ + // All vertices enclosed in 5 tets! + // Will generate actual lattice in the future. + AlignedBox box; + vtx = V; + int nv = vtx.rows(); + for (int i=0; i x_vec; + std::vector t_vec; + create_packed_tets(box.min(),box.max(),x_vec,t_vec); + + // Copy vector data to output + map_to_x3d(x_vec, x); + map_to_x4i(t_vec, tets); + return compute_vtx_tet_mapping(&vtx, &vtx_to_tet, &barys, x, tets); + +} // end gen lattice + +// +// Original source (BSD-2): +// github.com/mattoverby/mclscene/blob/master/include/MCL/EmbeddedMesh.hpp +// +void Lattice::create_packed_tets( + const Eigen::Vector3d &min, + const Eigen::Vector3d &max, + std::vector &verts, + std::vector &tets ) +{ + + // Top plane, clockwise looking down + Vector3d a = max; + Vector3d b( min[0], max[1], max[2] ); + Vector3d c( min[0], max[1], min[2] ); + Vector3d d( max[0], max[1], min[2] ); + + // Bottom plan, clockwise looking down + Vector3d e( max[0], min[1], max[2] ); + Vector3d f( min[0], min[1], max[2] ); + Vector3d g( min[0], min[1], min[2] ); + Vector3d h( max[0], min[1], min[2] ); + + // Add the verts + int nv = verts.size(); + verts.emplace_back( a ); // 0 + verts.emplace_back( b ); // 1 + verts.emplace_back( c ); // 2 + verts.emplace_back( d ); // 3 + verts.emplace_back( e ); // 4 + verts.emplace_back( f ); // 5 + verts.emplace_back( g ); // 6 + verts.emplace_back( h ); // 7 + + // Pack 5 tets into the cube + Vector4i t0( 0, 5, 7, 4 ); + Vector4i t1( 5, 7, 2, 0 ); + Vector4i t2( 5, 0, 2, 1 ); + Vector4i t3( 7, 2, 0, 3 ); + Vector4i t4( 5, 2, 7, 6 ); + Vector4i offset(nv,nv,nv,nv); + + // Add the tets + tets.emplace_back( t0+offset ); + tets.emplace_back( t1+offset ); + tets.emplace_back( t2+offset ); + tets.emplace_back( t3+offset ); + tets.emplace_back( t4+offset ); + +//vdb_point(a[0],a[1],a[2]); +//vdb_point(b[0],b[1],b[2]); +//vdb_point(c[0],c[1],c[2]); +//vdb_point(d[0],d[1],d[2]); +//vdb_point(e[0],e[1],e[2]); +//vdb_point(f[0],f[1],f[2]); +//vdb_point(g[0],g[1],g[2]); +//vdb_point(h[0],h[1],h[2]); +//vdb_line(float x0, float y0, float z0, +// float x1, float y1, float z1); + +} // end create packed tets + +bool Lattice::compute_vtx_tet_mapping( + const Eigen::MatrixXd *vtx_, // embedded vertices, p x 3 + Eigen::VectorXi *vtx_to_tet_, // what tet vtx is embedded in, p x 1 + Eigen::MatrixXd *barys_, // barycoords of the embedding, p x 4 + const Eigen::MatrixXd *x_, // lattice vertices, n x 3 + const Eigen::MatrixXi *tets_) // lattice elements, m x 4 +{ + if (!vtx_ || !vtx_to_tet_ || !barys_ || !x_ || !tets_) + return false; + + int nv = vtx_->rows(); + if (nv==0) + return false; + + // TODO: + // Use AABB Tree to do point-in-tet and compute bary weighting + // For now the dumb approach and loop all. + + barys_->resize(nv,4); + barys_->setZero(); + vtx_to_tet_->resize(nv); + int nt = tets_->rows(); + + for (int i=0; irow(i); + for (int j=0; jrow(j); + Vector3d t[4] = { + x_->row(tet[0]), + x_->row(tet[1]), + x_->row(tet[2]), + x_->row(tet[3]) + }; + if (!barycoords::point_in_tet(v,t[0],t[1],t[2],t[3])) + continue; + + Vector4d b = barycoords::point_tet(v,t[0],t[1],t[2],t[3]); + vtx_to_tet_->operator[](i) = j; + barys_->row(i) = b; + break; + } + } + + return true; + +} // end compute vtx to tet mapping + +Eigen::Vector3d Lattice::get_mapped_vertex( + int idx, + const Eigen::MatrixXd *x_or_v, + const Eigen::MatrixXi *tets ) +{ + int t_idx = vtx_to_tet[idx]; + RowVector4i tet = tets->row(t_idx); + RowVector4d b = barys.row(idx); + return Vector3d( + x_or_v->row(tet[0]) * b[0] + + x_or_v->row(tet[1]) * b[1] + + x_or_v->row(tet[2]) * b[2] + + x_or_v->row(tet[3]) * b[3]); +} +/* +void Lattice::map_to_object( + const Eigen::MatrixXd *x, + const Eigen::MatrixXi *tets, + float (*vertexCos)[3]) +{ + int nv = vtx.rows(); + for (int i=0; irow(t_idx); + RowVector4d b = barys.row(i); + vtx.row(i) = + x->row(tet[0]) * b[0] + + x->row(tet[1]) * b[1] + + x->row(tet[2]) * b[2] + + x->row(tet[3]) * b[3]; + vertexCos[i][0] = vtx(i,0); + vertexCos[i][1] = vtx(i,1); + vertexCos[i][2] = vtx(i,2); + } +} // end map to object +*/ +} // namespace admmpd \ No newline at end of file diff --git a/extern/softbody/src/admmpd_lattice.h b/extern/softbody/src/admmpd_lattice.h new file mode 100644 index 00000000000..b341de636ec --- /dev/null +++ b/extern/softbody/src/admmpd_lattice.h @@ -0,0 +1,57 @@ + + + +#ifndef _ADMMPD_LATTICE_H +#define _ADMMPD_LATTICE_H + +#include +#include + +namespace admmpd { + +class Lattice { +public: + + Eigen::MatrixXd vtx; // rest embedded vertices, p x 3 + Eigen::VectorXi vtx_to_tet; // what tet vtx is embedded in, p x 1 + Eigen::MatrixXd barys; // barycoords of the embedding, p x 4 + + // Returns true on success + bool generate( + const Eigen::MatrixXd &V, + Eigen::MatrixXd *x, // lattice vertices, n x 3 + Eigen::MatrixXi *tets); // lattice elements, m x 4 + + // Given boxmin and boxmax, adds + // 5 tets (and verts) to the vectors + void create_packed_tets( + const Eigen::Vector3d &min, + const Eigen::Vector3d &max, + std::vector &verts, + std::vector &tets ); + + // Returns true on success + // Works on ptrs intead of local vars in case + // I want to use it for something else. + bool compute_vtx_tet_mapping( + const Eigen::MatrixXd *vtx_, // embedded vertices, p x 3 + Eigen::VectorXi *vtx_to_tet_, // what tet vtx is embedded in, p x 1 + Eigen::MatrixXd *barys_, // barycoords of the embedding, p x 4 + const Eigen::MatrixXd *x_, // lattice vertices, n x 3 + const Eigen::MatrixXi *tets_); // lattice elements, m x 4 + + // Returns the vtx mapped from x/v and tets + Eigen::Vector3d get_mapped_vertex( + int idx, + const Eigen::MatrixXd *x_or_v, + const Eigen::MatrixXi *tets ); +// void map_to_object( +// const Eigen::MatrixXd *x, +// const Eigen::MatrixXi *tets, +// float (*vertexCos)[3]); + +}; // class Lattice + +} // namespace admmpd + +#endif // _ADMMPD_LATTICE_H \ No newline at end of file diff --git a/extern/softbody/src/admmpd_math.cpp b/extern/softbody/src/admmpd_math.cpp new file mode 100644 index 00000000000..98b1fcc2520 --- /dev/null +++ b/extern/softbody/src/admmpd_math.cpp @@ -0,0 +1,67 @@ + + +#include "admmpd_math.h" + +namespace admmpd { +namespace barycoords { + + Eigen::Matrix point_tet( + const Eigen::Matrix &p, + const Eigen::Matrix &a, + const Eigen::Matrix &b, + const Eigen::Matrix &c, + const Eigen::Matrix &d) + { + auto scalar_triple_product = []( + const Eigen::Matrix &u, + const Eigen::Matrix &v, + const Eigen::Matrix &w ) + { + return u.dot(v.cross(w)); + }; + Eigen::Matrix ap = p - a; + Eigen::Matrix bp = p - b; + Eigen::Matrix ab = b - a; + Eigen::Matrix ac = c - a; + Eigen::Matrix ad = d - a; + Eigen::Matrix bc = c - b; + Eigen::Matrix bd = d - b; + double va6 = scalar_triple_product(bp, bd, bc); + double vb6 = scalar_triple_product(ap, ac, ad); + double vc6 = scalar_triple_product(ap, ad, ab); + double vd6 = scalar_triple_product(ap, ab, ac); + double v6 = 1.0 / scalar_triple_product(ab, ac, ad); + return Eigen::Matrix(va6*v6, vb6*v6, vc6*v6, vd6*v6); + } // end point tet barycoords + + // Checks that it's on the "correct" side of the normal + // for each face of the tet. Assumes winding points inward. + bool point_in_tet( + const Eigen::Matrix &p, + const Eigen::Matrix &a, + const Eigen::Matrix &b, + const Eigen::Matrix &c, + const Eigen::Matrix &d) + { + using namespace Eigen; + auto check_face = []( + const Vector3d &point, + const Vector3d &p0, + const Vector3d &p1, + const Vector3d &p2, + const Vector3d &p3 ) + { + Vector3d n = (p1-p0).cross(p2-p0); + double dp3 = n.dot(p3-p0); + double dp = n.dot(point-p0); + return (dp3*dp >= 0); + }; + return + check_face(p, a, b, c, d) && + check_face(p, b, c, d, a) && + check_face(p, c, d, a, b) && + check_face(p, d, a, b, c); + } + +} // namespace barycoords +} // namespace admmpd \ No newline at end of file diff --git a/extern/softbody/src/admmpd_math.h b/extern/softbody/src/admmpd_math.h new file mode 100644 index 00000000000..d777df5ce2e --- /dev/null +++ b/extern/softbody/src/admmpd_math.h @@ -0,0 +1,30 @@ + + + +#ifndef _ADMMPD_MATH_H +#define _ADMMPD_MATH_H + +#include + +namespace admmpd { +namespace barycoords { + +Eigen::Matrix point_tet( + const Eigen::Matrix &p, + const Eigen::Matrix &a, + const Eigen::Matrix &b, + const Eigen::Matrix &c, + const Eigen::Matrix &d); + +bool point_in_tet( + const Eigen::Matrix &p, + const Eigen::Matrix &a, + const Eigen::Matrix &b, + const Eigen::Matrix &c, + const Eigen::Matrix &d); + +} // namespace barycoords + +} // namespace admmpd + +#endif //_ADMMPD_MATH_H diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp new file mode 100644 index 00000000000..eb2222ef3bb --- /dev/null +++ b/extern/softbody/src/admmpd_solver.cpp @@ -0,0 +1,200 @@ + + + +#include "admmpd_solver.h" +#include "admmpd_lattice.h" +#include "admmpd_energy.h" + +#include +#include + +#include + +namespace admmpd { +using namespace Eigen; +template using RowSparseMatrix = SparseMatrix; + +bool Solver::init( + const Eigen::MatrixXd &V, + const Eigen::MatrixXi &T, + const ADMMPD_Options *options, + ADMMPD_Data *data) +{ + if (!data || !options) + throw std::runtime_error("init: data/options null"); + + data->x = V; + data->tets = T; + compute_matrices(options,data); + return true; +} // end init + +void Solver::init_solve( + const ADMMPD_Options *options, + ADMMPD_Data *data) +{ + double dt = std::max(0.0, options->timestep_s); + int nx = data->x.rows(); + if (data->M_xbar.rows() != nx) + data->M_xbar.resize(nx,3); + + // velocity and position + data->x_start = data->x; + for( int i=0; iv.row(i) += options->grav; + data->M_xbar.row(i) = + data->m[i] * data->x.row(i) + + dt*data->m[i]*data->v.row(i); + } + + // ADMM variables + data->Dx.noalias() = data->D * data->x; + data->z = data->Dx; + data->z_prev = data->z; + data->u.setZero(); + data->u_prev.setZero(); + +} // end init solve + +int Solver::solve( + const ADMMPD_Options *options, + ADMMPD_Data *data) +{ + // Init the solve which computes + // quantaties like M_xbar and makes sure + // the variables are sized correctly. + init_solve(options,data); + + int ne = data->rest_volumes.size(); + Lame lame; + + // Begin solver loop + int iters = 0; + int max_iters = options->max_iters < 0 ? 10000 : options->max_iters; + for (; iters < max_iters; ++iters) + { + // Local step + data->Dx.noalias() = data->D * data->x; + data->z_prev.noalias() = data->z; + data->u_prev.noalias() = data->u; + for(int i=0; iindices[i][0], + lame, + data->rest_volumes[i], + data->weights[i], + &data->x, + &data->Dx, + &data->z, + &data->u ); + } + + // Global step + data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u); + data->x.noalias() = data->ldltA.solve(data->b); + + } // end solver iters + + double dt = std::max(0.0, options->timestep_s); + if (dt > 0.0) + data->v.noalias() = (data->x-data->x_start)*(1.0/dt); + + return iters; +} // end solve + +void Solver::compute_matrices( + const ADMMPD_Options *options, + ADMMPD_Data *data) +{ + // Allocate per-vertex data + int nx = data->x.rows(); + data->x_start = data->x; + data->M_xbar.resize(nx,3); + data->M_xbar.setZero(); + data->Dx.resize(nx,3); + data->Dx.setZero(); + if (data->v.rows() != nx) + { + data->v.resize(nx,3); + data->v.setZero(); + } + if (data->m.rows() != nx) + { // TODO get from BodyPoint + data->m.resize(nx); + data->m.setOnes(); + } + + // Add per-element energies to data + std::vector< Triplet > trips; + append_energies(options,data,trips); + int nw = trips.back().row()+1; + + // Global matrix + data->D.resize(nw,nx); + data->D.setFromTriplets(trips.begin(), trips.end()); + data->Dt = data->D.transpose(); + VectorXd w_diag = Map(data->weights.data(), data->weights.size()); + data->DtW2 = data->Dt * w_diag.asDiagonal() * w_diag.asDiagonal(); + data->A = data->DtW2 * data->D; + data->A.diagonal() += data->m; + data->ldltA.compute(data->A); + data->b.resize(nx,3); + data->b.setZero(); + + data->z.resize(nw,3); + data->z.setZero(); + data->z_prev.resize(nw,3); + data->z_prev.setZero(); + data->u.resize(nw,3); + data->u.setZero(); + data->u_prev.resize(nw,3); + data->u_prev.setZero(); + +} // end compute matrices + +void Solver::append_energies( + const ADMMPD_Options *options, + ADMMPD_Data *data, + std::vector > &D_triplets) +{ + int nt = data->tets.rows(); + if (nt==0) + return; + + data->indices.reserve(nt); + data->rest_volumes.reserve(nt); + data->weights.reserve(nt); + Lame lame; + + int energy_index = 0; + for (int i=0; irest_volumes.emplace_back(); + data->weights.emplace_back(); + int energy_dim = 0; + + RowVector4i ele = data->tets.row(i); + energy_dim = EnergyTerm().init_tet( + energy_index, + lame, + ele, + &data->x, + data->rest_volumes.back(), + data->weights.back(), + D_triplets ); + + // Error in initialization + if( energy_dim <= 0 ){ + data->rest_volumes.pop_back(); + data->weights.pop_back(); + continue; + } + + data->indices.emplace_back(energy_index, energy_dim); + energy_index += energy_dim; + } +} // end append energies + +} // namespace admmpd diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h new file mode 100644 index 00000000000..10dc103719c --- /dev/null +++ b/extern/softbody/src/admmpd_solver.h @@ -0,0 +1,91 @@ + + + +#ifndef _ADMMPD_H +#define _ADMMPD_H + +#include +#include +#include +#include + +namespace admmpd { + +struct ADMMPD_Options { + double timestep_s; + int max_iters; + Eigen::Vector3d grav; + ADMMPD_Options() : + timestep_s(1.0/100.0), // TODO: Figure out delta time from blender api! + max_iters(20), + grav(0,0,-9.8) + {} +}; + +struct ADMMPD_Data { + // Input: + Eigen::MatrixXi tets; // elements lattice, t x 4 + Eigen::MatrixXd x; // vertices of lattice, n x 3 + // Set in compute_matrices: + Eigen::MatrixXd x_start; // x at beginning of timestep, n x 3 + Eigen::MatrixXd v; // velocity of lattice mesh, n x 3 + Eigen::VectorXd m; // masses of lattice verts, n x 1 + Eigen::MatrixXd z, z_prev; // ADMM z variable + Eigen::MatrixXd u, u_prev; // ADMM u aug lag with W inv + Eigen::MatrixXd M_xbar; // M*(x + dt v) + Eigen::MatrixXd Dx; // D * x + Eigen::MatrixXd b; // M xbar + DtW2(z-u) + template using RowSparseMatrix = Eigen::SparseMatrix; + RowSparseMatrix D; // reduction matrix + RowSparseMatrix Dt; // transpose reduction matrix + RowSparseMatrix DtW2; // D'W^2 + RowSparseMatrix A; // M + D'W^2D + Eigen::SimplicialLDLT > ldltA; + // Set in append_energies: + std::vector indices; // per-energy index into D (row, num rows) + std::vector rest_volumes; // per-energy rest volume + std::vector weights; // per-energy weights +}; + +class Solver { +public: + + // Initialies solver data. If a per-vertex + // variable is resized it is initialized to zero. + // Returns true on success + bool init( + const Eigen::MatrixXd &V, // vertices + const Eigen::MatrixXi &T, // tets + const ADMMPD_Options *options, + ADMMPD_Data *data); + + // Solve a single time step. + // Returns number of iterations. + int solve( + const ADMMPD_Options *options, + ADMMPD_Data *data); + +protected: + + void init_solve( + const ADMMPD_Options *options, + ADMMPD_Data *data); + + void compute_lattice( + const ADMMPD_Options *options, + ADMMPD_Data *data); + + void compute_matrices( + const ADMMPD_Options *options, + ADMMPD_Data *data); + + void append_energies( + const ADMMPD_Options *options, + ADMMPD_Data *data, + std::vector > &D_triplets); + +}; // class ADMMPD_solver + +} // namespace admmpd + +#endif // _ADMMPD_H -- cgit v1.2.3