1 files changed, 28 insertions, 9 deletions

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);