Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--extern/softbody/CMakeLists.txt2
-rw-r--r--extern/softbody/src/admmpd_collision.cpp33
-rw-r--r--extern/softbody/src/admmpd_collision.h42
-rw-r--r--extern/softbody/src/admmpd_energy.cpp2
-rw-r--r--extern/softbody/src/admmpd_lattice.cpp1
-rw-r--r--extern/softbody/src/admmpd_math.cpp40
-rw-r--r--extern/softbody/src/admmpd_math.h22
-rw-r--r--extern/softbody/src/admmpd_solver.cpp220
-rw-r--r--extern/softbody/src/admmpd_solver.h37
-rw-r--r--intern/softbody/admmpd_api.cpp2
10 files changed, 312 insertions, 89 deletions
diff --git a/extern/softbody/CMakeLists.txt b/extern/softbody/CMakeLists.txt
index 08a36882f28..8f6ec97d7dd 100644
--- a/extern/softbody/CMakeLists.txt
+++ b/extern/softbody/CMakeLists.txt
@@ -35,6 +35,8 @@ set(SRC
src/admmpd_lattice.cpp
src/admmpd_solver.h
src/admmpd_solver.cpp
+ src/admmpd_collision.h
+ src/admmpd_collision.cpp
)
set(LIB
diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
new file mode 100644
index 00000000000..2bce56fa9f0
--- /dev/null
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -0,0 +1,33 @@
+
+
+#include "admmpd_collision.h"
+
+namespace admmpd {
+
+void FloorCollider::detect(const Eigen::MatrixXd *x)
+{
+ (void)(x);
+ // Can just do detection in jacobian I guess.
+}
+
+void FloorCollider::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)
+{
+ int nx = x->rows();
+ for (int i=0; i<nx; ++i)
+ {
+ Eigen::Vector3d p = x->row(i);
+ if (p[2]>0)
+ continue;
+
+ int c_idx = l->size();
+ trips_z->emplace_back(c_idx,i,1.0);
+ l->emplace_back(0);
+ }
+} // end floor collider Jacobian
+
+} // namespace admmpd \ No newline at end of file
diff --git a/extern/softbody/src/admmpd_collision.h b/extern/softbody/src/admmpd_collision.h
new file mode 100644
index 00000000000..274be91be6d
--- /dev/null
+++ b/extern/softbody/src/admmpd_collision.h
@@ -0,0 +1,42 @@
+
+
+
+#ifndef _ADMMPD_COLLISION_H
+#define _ADMMPD_COLLISION_H
+
+#include <Eigen/Sparse>
+#include <vector>
+
+namespace admmpd {
+
+// 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 Collider {
+public:
+ virtual void detect(
+ const Eigen::MatrixXd *x) = 0;
+ virtual 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) = 0;
+};
+
+class FloorCollider : public Collider {
+public:
+ void detect(const Eigen::MatrixXd *x);
+ 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_energy.cpp b/extern/softbody/src/admmpd_energy.cpp
index 978fd565aa4..ea7c68b6dca 100644
--- a/extern/softbody/src/admmpd_energy.cpp
+++ b/extern/softbody/src/admmpd_energy.cpp
@@ -8,7 +8,7 @@ using namespace Eigen;
Lame::Lame() : m_model(0)
{
- set_from_youngs_poisson(10000000,0.299);
+ set_from_youngs_poisson(10000000,0.399);
}
void Lame::set_from_youngs_poisson(double youngs, double poisson)
diff --git a/extern/softbody/src/admmpd_lattice.cpp b/extern/softbody/src/admmpd_lattice.cpp
index 4323f8fce33..be7a5bc63d2 100644
--- a/extern/softbody/src/admmpd_lattice.cpp
+++ b/extern/softbody/src/admmpd_lattice.cpp
@@ -3,6 +3,7 @@
#include "admmpd_lattice.h"
#include "admmpd_math.h"
+#include <iostream>
//#include "vdb.h"
diff --git a/extern/softbody/src/admmpd_math.cpp b/extern/softbody/src/admmpd_math.cpp
index 98b1fcc2520..7c76dfb8b3a 100644
--- a/extern/softbody/src/admmpd_math.cpp
+++ b/extern/softbody/src/admmpd_math.cpp
@@ -6,26 +6,26 @@ namespace admmpd {
namespace barycoords {
Eigen::Matrix<double,4,1> point_tet(
- const Eigen::Matrix<double,3,1> &p,
- const Eigen::Matrix<double,3,1> &a,
- const Eigen::Matrix<double,3,1> &b,
- const Eigen::Matrix<double,3,1> &c,
- const Eigen::Matrix<double,3,1> &d)
+ const Eigen::Vector3d &p,
+ const Eigen::Vector3d &a,
+ const Eigen::Vector3d &b,
+ const Eigen::Vector3d &c,
+ const Eigen::Vector3d &d)
{
auto scalar_triple_product = [](
- const Eigen::Matrix<double,3,1> &u,
- const Eigen::Matrix<double,3,1> &v,
- const Eigen::Matrix<double,3,1> &w )
+ const Eigen::Vector3d &u,
+ const Eigen::Vector3d &v,
+ const Eigen::Vector3d &w )
{
return u.dot(v.cross(w));
};
- Eigen::Matrix<double,3,1> ap = p - a;
- Eigen::Matrix<double,3,1> bp = p - b;
- Eigen::Matrix<double,3,1> ab = b - a;
- Eigen::Matrix<double,3,1> ac = c - a;
- Eigen::Matrix<double,3,1> ad = d - a;
- Eigen::Matrix<double,3,1> bc = c - b;
- Eigen::Matrix<double,3,1> bd = d - b;
+ Eigen::Vector3d ap = p - a;
+ Eigen::Vector3d bp = p - b;
+ Eigen::Vector3d ab = b - a;
+ Eigen::Vector3d ac = c - a;
+ Eigen::Vector3d ad = d - a;
+ Eigen::Vector3d bc = c - b;
+ Eigen::Vector3d 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);
@@ -37,11 +37,11 @@ namespace 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<double,3,1> &p,
- const Eigen::Matrix<double,3,1> &a,
- const Eigen::Matrix<double,3,1> &b,
- const Eigen::Matrix<double,3,1> &c,
- const Eigen::Matrix<double,3,1> &d)
+ const Eigen::Vector3d &p,
+ const Eigen::Vector3d &a,
+ const Eigen::Vector3d &b,
+ const Eigen::Vector3d &c,
+ const Eigen::Vector3d &d)
{
using namespace Eigen;
auto check_face = [](
diff --git a/extern/softbody/src/admmpd_math.h b/extern/softbody/src/admmpd_math.h
index d777df5ce2e..25ff2d3114f 100644
--- a/extern/softbody/src/admmpd_math.h
+++ b/extern/softbody/src/admmpd_math.h
@@ -9,19 +9,19 @@
namespace admmpd {
namespace barycoords {
-Eigen::Matrix<double,4,1> point_tet(
- const Eigen::Matrix<double,3,1> &p,
- const Eigen::Matrix<double,3,1> &a,
- const Eigen::Matrix<double,3,1> &b,
- const Eigen::Matrix<double,3,1> &c,
- const Eigen::Matrix<double,3,1> &d);
+Eigen::Vector4d point_tet(
+ const Eigen::Vector3d &p,
+ const Eigen::Vector3d &a,
+ const Eigen::Vector3d &b,
+ const Eigen::Vector3d &c,
+ const Eigen::Vector3d &d);
bool point_in_tet(
- const Eigen::Matrix<double,3,1> &p,
- const Eigen::Matrix<double,3,1> &a,
- const Eigen::Matrix<double,3,1> &b,
- const Eigen::Matrix<double,3,1> &c,
- const Eigen::Matrix<double,3,1> &d);
+ const Eigen::Vector3d &p,
+ const Eigen::Vector3d &a,
+ const Eigen::Vector3d &b,
+ const Eigen::Vector3d &c,
+ const Eigen::Vector3d &d);
} // namespace barycoords
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index eb2222ef3bb..052008121b9 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -4,11 +4,13 @@
#include "admmpd_solver.h"
#include "admmpd_lattice.h"
#include "admmpd_energy.h"
+#include "admmpd_collision.h"
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <stdio.h>
+#include <iostream>
namespace admmpd {
using namespace Eigen;
@@ -29,34 +31,6 @@ bool Solver::init(
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; i<nx; ++i )
- {
- data->v.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)
@@ -67,17 +41,14 @@ int Solver::solve(
init_solve(options,data);
int ne = data->rest_volumes.size();
- Lame lame;
+ Lame lame; // TODO lame params as input
// Begin solver loop
int iters = 0;
- int max_iters = options->max_iters < 0 ? 10000 : options->max_iters;
- for (; iters < max_iters; ++iters)
+ for (; iters < options->max_admm_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; i<ne; ++i)
{
EnergyTerm().update(
@@ -92,18 +63,162 @@ int Solver::solve(
}
// Global step
+ update_constraints(options,data);
data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u);
- data->x.noalias() = data->ldltA.solve(data->b);
+ solve_conjugate_gradients(options,data);
} // end solver iters
- double dt = std::max(0.0, options->timestep_s);
+ double dt = options->timestep_s;
if (dt > 0.0)
data->v.noalias() = (data->x-data->x_start)*(1.0/dt);
return iters;
} // end solve
+void Solver::init_solve(
+ const ADMMPD_Options *options,
+ ADMMPD_Data *data)
+{
+ int nx = data->x.rows();
+ if (data->M_xbar.rows() != nx)
+ data->M_xbar.resize(nx,3);
+
+ // velocity and position
+ double dt = std::max(0.0, options->timestep_s);
+ data->x_start = data->x;
+ for( int i=0; i<nx; ++i )
+ {
+ data->v.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->u.setZero();
+
+} // end init solve
+
+void Solver::update_constraints(
+ const ADMMPD_Options *options,
+ ADMMPD_Data *data)
+{
+
+ 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(
+ &data->x,
+ &trips_x,
+ &trips_y,
+ &trips_z,
+ &l_coeffs);
+
+ // Check number of constraints.
+ // If no constraints, clear Jacobian.
+ int nx = data->x.rows();
+ int nc = l_coeffs.size();
+ if (nc==0)
+ {
+ data->l.setZero();
+ for (int i=0; i<3; ++i)
+ data->K[i].setZero();
+
+ return;
+ }
+
+ // Otherwise update the data.
+ data->l = Map<VectorXd>(l_coeffs.data(),nc);
+ data->K[0].resize(nc,nx);
+ data->K[0].setFromTriplets(trips_x.begin(),trips_x.end());
+ data->K[1].resize(nc,nx);
+ data->K[1].setFromTriplets(trips_y.begin(),trips_y.end());
+ data->K[2].resize(nc,nx);
+ data->K[2].setFromTriplets(trips_z.begin(),trips_z.end());
+
+} // end update constraints
+
+void Solver::solve_conjugate_gradients(
+ const ADMMPD_Options *options,
+ ADMMPD_Data *data)
+{
+ // If we don't have any constraints,
+ // we don't need to perform CG
+ int nnz = std::max(std::max(
+ data->K[0].nonZeros(),
+ data->K[1].nonZeros()),
+ data->K[2].nonZeros());
+ if (nnz==0)
+ {
+ data->x.noalias() = data->ldltA.solve(data->b);
+ return;
+ }
+
+ // Inner product of matrices interpreted
+ // if they were instead vectorized
+ auto mat_inner = [](
+ const MatrixXd &A,
+ const MatrixXd &B )
+ {
+ double dot = 0.0;
+ int nr = std::min(A.rows(), B.rows());
+ for( int i=0; i<nr; ++i )
+ for(int j=0; j<3; ++j)
+ dot += A(i,j)*B(i,j);
+
+ return dot;
+ };
+
+ double eps = options->min_res;
+ MatrixXd b = data->b;
+ int nv = b.rows();
+ RowSparseMatrix<double> A[3];
+ MatrixXd r(b.rows(),3);
+ MatrixXd z(nv,3);
+ MatrixXd p(nv,3);
+ MatrixXd Ap(nv,3);
+
+ for (int i=0; i<3; ++i)
+ {
+ RowSparseMatrix<double> Kt = data->K[i].transpose();
+ A[i] = data->A + data->spring_k*RowSparseMatrix<double>(Kt*data->K[i]);
+ b.col(i) += data->spring_k*Kt*data->l;
+ r.col(i) = b.col(i) - A[i]*data->x.col(i);
+ }
+ z = data->ldltA.solve(r);
+ p = z;
+
+ for (int iter=0; iter<options->max_cg_iters; ++iter)
+ {
+ for( int i=0; i<3; ++i )
+ Ap.col(i) = A[i]*p.col(i);
+
+ double p_dot_Ap = mat_inner(p,Ap);
+ if( p_dot_Ap==0.0 )
+ break;
+
+ double zk_dot_rk = mat_inner(z,r);
+ if( zk_dot_rk==0.0 )
+ break;
+
+ double alpha = zk_dot_rk / p_dot_Ap;
+ data->x += alpha * p;
+ r -= alpha * Ap;
+ if( r.lpNorm<Infinity>() < eps )
+ break;
+
+ z = data->ldltA.solve(r);
+ double beta = mat_inner(z,r) / zk_dot_rk;
+ p = z + beta*p;
+ }
+} // end solve conjugate gradients
+
void Solver::compute_matrices(
const ADMMPD_Options *options,
ADMMPD_Data *data)
@@ -124,33 +239,45 @@ void Solver::compute_matrices(
{ // TODO get from BodyPoint
data->m.resize(nx);
data->m.setOnes();
+ data->m *= 0.01;
}
// Add per-element energies to data
std::vector< Triplet<double> > trips;
append_energies(options,data,trips);
int nw = trips.back().row()+1;
+ double dt2 = std::max(0.0, options->timestep_s * options->timestep_s);
// Global matrix
data->D.resize(nw,nx);
data->D.setFromTriplets(trips.begin(), trips.end());
data->Dt = data->D.transpose();
- VectorXd w_diag = Map<VectorXd>(data->weights.data(), data->weights.size());
- data->DtW2 = data->Dt * w_diag.asDiagonal() * w_diag.asDiagonal();
+ VectorXd wd = Map<VectorXd>(data->weights.data(), data->weights.size());
+ RowSparseMatrix<double> W2(nw,nw);
+ VectorXi W_nnz = VectorXi::Ones(nw);
+ W2.reserve(W_nnz);
+ for(int i=0; i<nw; ++i)
+ W2.coeffRef(i,i) = data->weights[i]*data->weights[i];
+
+ data->DtW2 = dt2 * data->Dt * W2;
data->A = data->DtW2 * data->D;
- data->A.diagonal() += data->m;
+ for (int i=0; i<nx; ++i)
+ data->A.coeffRef(i,i) += data->m[i];
+
data->ldltA.compute(data->A);
data->b.resize(nx,3);
data->b.setZero();
+ data->spring_k = options->mult_k*data->A.diagonal().maxCoeff();
+ data->l = VectorXd::Zero(1);
+ for (int i=0; i<3; ++i)
+ data->K[i].resize(1,nx);
+
+ // ADMM variables
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
@@ -171,12 +298,11 @@ void Solver::append_energies(
int energy_index = 0;
for (int i=0; i<nt; ++i)
{
+ RowVector4i ele = data->tets.row(i);
+
data->rest_volumes.emplace_back();
data->weights.emplace_back();
- int energy_dim = 0;
-
- RowVector4i ele = data->tets.row(i);
- energy_dim = EnergyTerm().init_tet(
+ int energy_dim = EnergyTerm().init_tet(
energy_index,
lame,
ele,
@@ -197,4 +323,4 @@ void Solver::append_energies(
}
} // end append energies
-} // namespace admmpd
+} // namespace admmpd \ No newline at end of file
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index 10dc103719c..ec836d3c466 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -13,25 +13,31 @@ namespace admmpd {
struct ADMMPD_Options {
double timestep_s;
- int max_iters;
+ int max_admm_iters;
+ int max_cg_iters;
+ double mult_k; // stiffness multiplier for constraints
+ double min_res; // min residual for CG solver
Eigen::Vector3d grav;
ADMMPD_Options() :
- timestep_s(1.0/100.0), // TODO: Figure out delta time from blender api!
- max_iters(20),
+ timestep_s(1.0/100.0), // TODO: Figure out delta time from blender api
+ max_admm_iters(20),
+ max_cg_iters(10),
+ mult_k(3.0),
+ min_res(1e-4),
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
+ Eigen::MatrixXi tets; // elements t x 4
+ Eigen::MatrixXd x; // vertices, n x 3
+ Eigen::MatrixXd v; // velocity, n x 3 TODO: from cache
// 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::VectorXd m; // masses, n x 1 TODO: from BodyPoint
+ Eigen::MatrixXd z; // ADMM z variable
+ Eigen::MatrixXd u; // 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)
@@ -40,7 +46,10 @@ struct ADMMPD_Data {
RowSparseMatrix<double> Dt; // transpose reduction matrix
RowSparseMatrix<double> DtW2; // D'W^2
RowSparseMatrix<double> A; // M + D'W^2D
+ RowSparseMatrix<double> K[3]; // constraint Jacobian
+ Eigen::VectorXd l; // constraint rhs (Kx=l)
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > ldltA;
+ double spring_k;
// Set in append_energies:
std::vector<Eigen::Vector2i> indices; // per-energy index into D (row, num rows)
std::vector<double> rest_volumes; // per-energy rest volume
@@ -67,10 +76,20 @@ public:
protected:
+ void update_constraints(
+ const ADMMPD_Options *options,
+ ADMMPD_Data *data);
+
void init_solve(
const ADMMPD_Options *options,
ADMMPD_Data *data);
+ // Global step with CG:
+ // 1/2||Ax-b||^2 + k/2||Kx-l||^2
+ void solve_conjugate_gradients(
+ const ADMMPD_Options *options,
+ ADMMPD_Data *data);
+
void compute_lattice(
const ADMMPD_Options *options,
ADMMPD_Data *data);
diff --git a/intern/softbody/admmpd_api.cpp b/intern/softbody/admmpd_api.cpp
index 7f5a238214e..5963c4f71c8 100644
--- a/intern/softbody/admmpd_api.cpp
+++ b/intern/softbody/admmpd_api.cpp
@@ -128,7 +128,7 @@ void admmpd_to_bodypoint(
bpi.pos[0] = xi[0];
bpi.pos[1] = xi[1];
bpi.pos[2] = xi[2];
- bpi.vec[0] = vi[0];
+ bpi.vec[0] = vi[0]; // vec is velocity?
bpi.vec[1] = vi[1];
bpi.vec[2] = vi[2];
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-31 21:22:17 +0300