cache working but lots of copies

6 files changed, 254 insertions, 152 deletions

diff --git a/extern/softbody/src/admmpd_energy.h b/extern/softbody/src/admmpd_energy.h
index 4245c9cff9c..e58ba36a9e7 100644
--- a/extern/softbody/src/admmpd_energy.h
+++ b/extern/softbody/src/admmpd_energy.h
@@ -12,7 +12,7 @@ namespace admmpd {
 
 class Lame {
 public:
-	int m_model;
+	int m_model; // 0=ARAP
 	double m_mu;
 	double m_lambda;
 	double m_bulk_mod;
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index ad64e4ed277..4756cb76fac 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -52,15 +52,19 @@ int Solver::solve(
 	int iters = 0;
 	for (; iters < options->max_admm_iters; ++iters)
 	{
-
+		// Update ADMM z/u
 		solve_local_step(options,data);
+
+		// Perform collision detection and linearization
 		update_constraints(options,data);
 
+		// Solve Ax=b s.t. Kx=l
 		data->b.noalias() = data->M_xbar + data->DtW2*(data->z-data->u);
 		solve_conjugate_gradients(options,data);
 
 	} // end solver iters
 
+	// Update velocity (if not static solve)
 	double dt = options->timestep_s;
 	if (dt > 0.0)
 		data->v.noalias() = (data->x-data->x_start)*(1.0/dt);
@@ -99,8 +103,9 @@ static void parallel_zu_update(
 	const int i,
 	const TaskParallelTLS *__restrict UNUSED(tls))
 {
-	Lame lame; // TODO lame params as input
 	ThreadData *td = (ThreadData*)userdata;
+	Lame lame;
+	lame.set_from_youngs_poisson(td->options->youngs,td->options->poisson);
 	EnergyTerm().update(
 		td->data->indices[i][0],
 		lame,
@@ -222,47 +227,53 @@ void Solver::solve_conjugate_gradients(
 		return dot;
 	};
 
+	// Update CGData
+	admmpd::Data::CGData *cgdata = &data->cgdata;
 	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);
+	cgdata->b = data->b;
+	int nv = data->b.rows();
+	if (cgdata->r.rows() !=nv)
+	{
+		cgdata->r.resize(nv,3);
+		cgdata->z.resize(nv,3);
+		cgdata->p.resize(nv,3);
+		cgdata->Ap.resize(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);
+		cgdata->A[i] = data->A + data->spring_k*RowSparseMatrix<double>(Kt*data->K[i]);
+		cgdata->b.col(i).noalias() += data->spring_k*Kt*data->l;
+		cgdata->r.col(i).noalias() = cgdata->b.col(i) - cgdata->A[i]*data->x.col(i);
 	}
-	solve_Ax_b(data,&z,&r);
-	p = z;
+	solve_Ax_b(data,&cgdata->z,&cgdata->r);
+	cgdata->p = cgdata->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);
+			cgdata->Ap.col(i).noalias() = cgdata->A[i]*cgdata->p.col(i);
 
-		double p_dot_Ap = mat_inner(p,Ap);
+		double p_dot_Ap = mat_inner(cgdata->p,cgdata->Ap);
 		if( p_dot_Ap==0.0 )
 			break;
 
-		double zk_dot_rk = mat_inner(z,r);
+		double zk_dot_rk = mat_inner(cgdata->z,cgdata->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 )
+		data->x.noalias() += alpha * cgdata->p;
+		cgdata->r.noalias() -= alpha * cgdata->Ap;
+		if( cgdata->r.lpNorm<Infinity>() < eps )
 			break;
-		solve_Ax_b(data,&z,&r);
-		double beta = mat_inner(z,r) / zk_dot_rk;
-		p = z + beta*p;
+
+		solve_Ax_b(data,&cgdata->z,&cgdata->r);
+		double beta = mat_inner(cgdata->z,cgdata->r) / zk_dot_rk;
+		cgdata->p = cgdata->z + beta*cgdata->p;
 	}
+
 } // end solve conjugate gradients
 
 void Solver::compute_matrices(
@@ -285,14 +296,14 @@ void Solver::compute_matrices(
 		compute_masses(options,data);
 
 	// Add per-element energies to data
-	std::vector< Triplet<double> > trips;
+	std::vector<Triplet<double> > trips;
 	append_energies(options,data,trips);
 	int n_row_D = trips.back().row()+1;
 	double dt2 = options->timestep_s * options->timestep_s;
-	if (dt2 <= 0)
-		dt2 = 1.0; // static solve
+	if (options->timestep_s <= 0)
+		dt2 = 1.0; // static solve, use dt=1 to not scale matrices
 
-	// Weight matrix
+	// Diagonal weight matrix
 	RowSparseMatrix<double> W2(n_row_D,n_row_D);
 	VectorXi W_nnz = VectorXi::Ones(n_row_D);
 	W2.reserve(W_nnz);
@@ -301,30 +312,27 @@ void Solver::compute_matrices(
 	{
 		const Vector2i &idx = data->indices[i];
 		for (int j=0; j<idx[1]; ++j)
-		{
 			W2.coeffRef(idx[0]+j,idx[0]+j) = data->weights[i]*data->weights[i];
-		}
 	}
 
-	// Weighted Laplacian
+	// Mass weighted Laplacian
 	data->D.resize(n_row_D,nx);
 	data->D.setFromTriplets(trips.begin(), trips.end());
-	data->Dt = data->D.transpose();
-	data->DtW2 = dt2 * data->Dt * W2;
+	data->DtW2 = dt2 * data->D.transpose() * W2;
 	data->A = data->DtW2 * data->D;
 	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();
 
+	// Constraint data
 	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
+	// ADMM dual/lagrange
 	data->z.resize(n_row_D,3);
 	data->z.setZero();
 	data->u.resize(n_row_D,3);
@@ -338,26 +346,26 @@ void Solver::compute_masses(
 {
 	// 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 (e.g. soft rubber: 1100)
-	double density_kgm3 = 1100;
+	// 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; t<n_tets; ++t)
 	{
 		RowVector4i tet = data->tets.row(t);
+		RowVector3d tet0 = data->x.row(tet[0]);
 		Matrix3d edges;
-		edges.col(0) = data->x.row(tet[1]) - data->x.row(tet[0]);
-		edges.col(1) = data->x.row(tet[2]) - data->x.row(tet[0]);
-		edges.col(2) = data->x.row(tet[3]) - data->x.row(tet[0]);
+		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 = 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;
+		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;
 	}
-}
+} // end compute masses
 
 void Solver::append_energies(
 	const Options *options,
@@ -372,6 +380,7 @@ void Solver::append_energies(
 	data->rest_volumes.reserve(nt);
 	data->weights.reserve(nt);
 	Lame lame;
+	lame.set_from_youngs_poisson(options->youngs, options->poisson);
 
 	int energy_index = 0;
 	for (int i=0; i<nt; ++i)
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index 970cbfb6368..0305d535f39 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -17,6 +17,9 @@ 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
+    double poisson; // Poisson ratio
     Eigen::Vector3d grav;
     Options() :
         timestep_s(1.0/100.0), // TODO: Figure out delta time from blender api
@@ -24,18 +27,21 @@ struct Options {
         max_cg_iters(10),
         mult_k(1.0),
         min_res(1e-4),
+        density_kgm3(1100),
+        youngs(10000000),
+        poisson(0.399),
         grav(0,0,-9.8)
         {}
 };
 
 struct Data {
-    // Input:
+    // Set from input
     Eigen::MatrixXi tets; // elements t x 4
     Eigen::MatrixXd x; // vertices, n x 3
-    Eigen::MatrixXd v; // velocity, n x 3 TODO: from cache
+    Eigen::MatrixXd v; // velocity, n x 3
     // Set in compute_matrices: 
     Eigen::MatrixXd x_start; // x at beginning of timestep, n x 3
-    Eigen::VectorXd m; // masses, n x 1 TODO: from BodyPoint
+    Eigen::VectorXd m; // masses, n x 1
     Eigen::MatrixXd z; // ADMM z variable
     Eigen::MatrixXd u; // ADMM u aug lag with W inv
     Eigen::MatrixXd M_xbar; // M*(x + dt v)
@@ -43,13 +49,20 @@ struct Data {
     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> 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)
+    double spring_k; // constraint stiffness
 	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > ldltA;
-    double spring_k;
+    struct CGData { // Temporaries used in conjugate gradients
+        RowSparseMatrix<double> A[3]; // (M + D'W^2D) + k * Kt K
+        Eigen::MatrixXd b; // M xbar + DtW2(z-u) + Kt l
+        Eigen::MatrixXd r; // residual
+        Eigen::MatrixXd z;
+        Eigen::MatrixXd p;
+        Eigen::MatrixXd Ap; // A * p
+    } cgdata;
     // 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
diff --git a/intern/softbody/admmpd_api.cpp b/intern/softbody/admmpd_api.cpp
index 21e21c4cb9d..915b20b299a 100644
--- a/intern/softbody/admmpd_api.cpp
+++ b/intern/softbody/admmpd_api.cpp
@@ -84,11 +84,12 @@ void admmpd_dealloc(ADMMPDInterfaceData *iface)
     delete iface->data;
   }
 
-  iface->data = NULL;
   iface->in_verts = NULL;
   iface->in_vel = NULL;
   iface->in_faces = NULL;
   iface->out_verts = NULL;
+  iface->out_vel = NULL;
+  iface->data = NULL;
 }
 
 int admmpd_init(ADMMPDInterfaceData *iface)
@@ -180,6 +181,19 @@ void admmpd_solve(ADMMPDInterfaceData *iface)
   if (iface == NULL)
     return;
 
+  // Whatever is in out_verts and out_vel needs
+  // to be mapped to internal data, as it's used as input
+  // when reading from cached data.
+  int nv = iface->out_totverts;
+  for (int i=0; i<nv; ++i)
+  {
+    for (int j=0; j<3; ++j)
+    {
+      iface->data->data->x(i,j) = iface->out_verts[i*3+j];
+      iface->data->data->v(i,j) = iface->out_vel[i*3+j];
+    }
+  }
+
   try
   {
     admmpd::Solver().solve(iface->data->options,iface->data->data);
@@ -198,7 +212,7 @@ void admmpd_solve(ADMMPDInterfaceData *iface)
   }
 }
 
-void admmpd_map_vertices(ADMMPDInterfaceData *iface, float (*vertexCos)[3], int numVerts)
+void admmpd_get_vertices(ADMMPDInterfaceData *iface, float (*vertexCos)[3], int numVerts)
 {
   if (iface == NULL)
     return;
diff --git a/intern/softbody/admmpd_api.h b/intern/softbody/admmpd_api.h
index bcfe37403c0..3ee43cd5948 100644
--- a/intern/softbody/admmpd_api.h
+++ b/intern/softbody/admmpd_api.h
@@ -21,15 +21,13 @@
  * \ingroup admmpd
  */
 
-#ifndef __ADMMPD_API_H__
-#define __ADMMPD_API_H__
+#ifndef ADMMPD_API_H
+#define ADMMPD_API_H
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-//typedef struct Mesh Mesh_;
-
 typedef struct ADMMPDInterfaceData {
     float *in_verts;
     float *in_vel;
@@ -51,18 +49,10 @@ void admmpd_dealloc(ADMMPDInterfaceData*);
 int admmpd_init(ADMMPDInterfaceData*);
 int admmpd_cache_valid(ADMMPDInterfaceData*, int numVerts);
 void admmpd_solve(ADMMPDInterfaceData*);
-void admmpd_map_vertices(ADMMPDInterfaceData*, float (*vertexCos)[3], int numVerts); 
-
-//void admmpd_solve(ADMMPDInterfaceData*);
-
-// Copies the results of the solve (pos, vel) into BodyPoint
-//void admmpd_to_bodypoint(
-//    ADMMPD_Data *data,
-//    BodyPoint *bp,
-//    int numVerts);
+void admmpd_get_vertices(ADMMPDInterfaceData*, float (*vertexCos)[3], int numVerts); 
 
 #ifdef __cplusplus
 }
 #endif
 
-#endif /* __ADMMPD_API_H__ */
+#endif // ADMMPD_API_H
diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index 63c01e6ef3f..03df1878b00 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -945,6 +945,7 @@ static void free_softbody_intern(SoftBody *sb)
         }
       }
       MEM_freeN(sb->bpoint);
+      sb->bpoint = NULL;
     }
 
     if (sb->bspring) {
@@ -3545,23 +3546,23 @@ static void sbStoreLastFrame(struct Depsgraph *depsgraph, Object *object, float
   object_orig->soft->last_frame = framenr;
 }
 
-static void admm_resize_softbody(Object *ob, int totpoint)
+static void admmpd_resize_softbody(Object *ob, int totpoint)
 {
   if (totpoint<=0)
     return;
 
   SoftBody *sb = ob->soft;
-  if (sb->totpoint && sb->bpoint !=NULL)
+  if (sb->totpoint && sb->bpoint != NULL)
+  {
     MEM_freeN(sb->bpoint);
-  printf("bp ptr: %p\n",sb);
-  printf("bodypt: %d\n",sb->totpoint);
-
+    sb->bpoint = NULL;
+  }
   sb->totpoint = totpoint;
   sb->totspring = 0;
-//  sb->bpoint = MEM_mallocN(totpoint * sizeof(BodyPoint), "bodypoint");
+  sb->bpoint = MEM_mallocN(totpoint * sizeof(BodyPoint), "bodypoint");
 }
 
-static void admm_copy_to_softbody(Object *ob)
+static void admmpd_copy_to_softbody(Object *ob)
 {
   SoftBody *sb = ob->soft;
   ADMMPDInterfaceData *admmpd = sb->admmpd;
@@ -3570,7 +3571,7 @@ static void admm_copy_to_softbody(Object *ob)
 
   if (sb->totpoint != admmpd->out_totverts)
   {
-    printf("**admm_copy_to_softbody error: DOF missmatch");
+    printf("**admmpd_copy_to_softbody error: DOF missmatch");
     return;
   }
 
@@ -3585,8 +3586,80 @@ static void admm_copy_to_softbody(Object *ob)
   }
 }
 
+static void init_admmpd_interface(Object *ob, float (*vertexCos)[3])
+{
+  if(ob->type != OB_MESH)
+    return;
+
+  Mesh *me = ob->data;
+  SoftBody *sb = ob->soft;
+
+  sb->admmpd = MEM_callocN(sizeof(ADMMPDInterfaceData), "SoftBody_ADMMPD");
+
+  // Resize data
+  admmpd_dealloc(sb->admmpd);
+  int totfaces = poly_to_tri_count(me->totpoly, me->totloop);
+  admmpd_alloc(sb->admmpd, me->totvert, totfaces);
+
+  // Initialize input data
+  for (int i=0; i<me->totvert; ++i)
+  {
+    // Local to global coordinates
+    float vi[3];
+    vi[0] = vertexCos[i][0];
+    vi[1] = vertexCos[i][1];
+    vi[2] = vertexCos[i][2];
+    mul_m4_v3(ob->obmat, vi);
+    for (int j=0; j<3; ++j)
+    {
+      sb->admmpd->in_verts[i*3+j] = vi[j];
+      sb->admmpd->in_vel[i*3+j] = 0;
+    }
+  }
+  MLoopTri *looptri, *lt;
+  looptri = lt = MEM_mallocN(sizeof(*looptri)*totfaces, __func__);
+  BKE_mesh_recalc_looptri(me->mloop, me->mpoly, me->mvert, me->totloop, me->totpoly, looptri);
+  for (int i=0; i<totfaces; ++i, ++lt)
+  {
+      sb->admmpd->in_faces[i*3+0] = me->mloop[lt->tri[0]].v;
+      sb->admmpd->in_faces[i*3+1] = me->mloop[lt->tri[1]].v;
+      sb->admmpd->in_faces[i*3+2] = me->mloop[lt->tri[2]].v;
+  }
+  MEM_freeN(looptri);
+  looptri = NULL;
+
+  // Initalize solver
+  admmpd_init(sb->admmpd);
+
+  // Set up softbody to store defo verts
+  int num_defo_verts = sb->admmpd->out_totverts;
+  admmpd_resize_softbody(ob,num_defo_verts);
+  admmpd_copy_to_softbody(ob);
+}
+
+static void admmpd_update_from_softbody(Object *ob)
+{
+  SoftBody *sb = ob->soft;
+  ADMMPDInterfaceData *admmpd = sb->admmpd;
+  if (sb->totpoint != admmpd->out_totverts)
+  {
+    printf("**admmpd_update_from_softbody Error: DOF missmatch");
+    return;
+  }
+  BodyPoint *bp = sb->bpoint;
+  for (int i=0; i<sb->totpoint; ++i, ++bp)
+  {
+    for (int j=0; j<3; ++j)
+    {
+      admmpd->out_verts[i*3+j] = bp->pos[j];
+      admmpd->out_vel[i*3+j] = bp->vec[j];
+    }
+  }
+}
+
 /* simulates one step. framenr is in frames */
-void sbObjectStep_admmpd(struct Depsgraph *depsgraph,
+void sbObjectStep_admmpd(
+                  struct Depsgraph *depsgraph,
                   Scene *scene,
                   Object *ob,
                   float cfra,
@@ -3599,100 +3672,103 @@ void sbObjectStep_admmpd(struct Depsgraph *depsgraph,
   Mesh *me = ob->data;
   SoftBody *sb = ob->soft;
 
-//  PointCache *cache = sb->shared->pointcache;
+  PointCache *cache = sb->shared->pointcache;
+  PTCacheID pid;
   int framenr = (int)cfra;
-//  int framedelta = framenr - cache->simframe;
+  int framedelta = framenr - cache->simframe;
   int startframe = 1;
-
-//  PTCacheID pid;
-//  BKE_ptcache_id_from_softbody(&pid, ob, sb);
-//  float timescale;
-//  int startframe, endframe; // start and end frame of the cache
-//  BKE_ptcache_id_time(&pid, scene, framenr, &startframe, &endframe, &timescale);
-//  framenr = framenr < endframe ? framenr : endframe; // min(framenr,endframe)
-
-//  if (framenr < startframe)
-//  {
-//    BKE_ptcache_invalidate(cache);
-//    return;
-//  }
+  int endframe = 250;
+  float timescale = 1.f;
+  BKE_ptcache_id_from_softbody(&pid, ob, sb);
+  BKE_ptcache_id_time(&pid, scene, framenr, &startframe, &endframe, &timescale);
 
   // Reset simulation
   bool reset_sim =
     sb->admmpd == NULL ||
-    sb->admmpd->in_totverts != numVerts ||
-    framenr == startframe;
-
+    sb->admmpd->in_totverts != numVerts;
+  
+  // Reset cache
   if (reset_sim)
   {
-//      BKE_ptcache_invalidate(cache);
-
-      if (sb->admmpd == NULL)
-        sb->admmpd = MEM_callocN(sizeof(ADMMPDInterfaceData), "SoftBody_ADMMPD");
-
-      // Resize data
-      admmpd_dealloc(sb->admmpd);
-      int totfaces = poly_to_tri_count(me->totpoly, me->totloop);
-      admmpd_alloc(sb->admmpd, me->totvert, totfaces);
-
-      // Initialize input data
-      for (int i=0; i<me->totvert; ++i)
-      {
-        // Local to global coordinates
-        float vi[3];
-        vi[0] = vertexCos[i][0];
-        vi[1] = vertexCos[i][1];
-        vi[2] = vertexCos[i][2];
-        mul_m4_v3(ob->obmat, vi);
-        for (int j=0; j<3; ++j)
-        {
-          sb->admmpd->in_verts[i*3+j] = vi[j];
-          sb->admmpd->in_vel[i*3+j] = 0;
-        }
-      }
-      MLoopTri *looptri, *lt;
-      looptri = lt = MEM_mallocN(sizeof(*looptri)*totfaces, __func__);
-      BKE_mesh_recalc_looptri(me->mloop, me->mpoly, me->mvert, me->totloop, me->totpoly, looptri);
-      for (int i=0; i<totfaces; ++i, ++lt)
-      {
-          sb->admmpd->in_faces[i*3+0] = me->mloop[lt->tri[0]].v;
-          sb->admmpd->in_faces[i*3+1] = me->mloop[lt->tri[1]].v;
-          sb->admmpd->in_faces[i*3+2] = me->mloop[lt->tri[2]].v;
-      }
-      MEM_freeN(looptri);
-
-      // Initalize solver
-      admmpd_init(sb->admmpd);
+    cache->flag |= PTCACHE_OUTDATED;
+    BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
+    BKE_ptcache_validate(cache, 0);
+    cache->last_exact = 0;
+    cache->flag &= ~PTCACHE_REDO_NEEDED;
+  }
 
-      // Set up softbody to store defo verts
-      int num_defo_verts = sb->admmpd->out_totverts;
-      admm_resize_softbody(ob,num_defo_verts);
+  // Clamp simulation frame
+  if (framenr < startframe) {
+    BKE_ptcache_invalidate(cache);
+    return;
+  }
+  else if (framenr > endframe) {
+    framenr = endframe;
+  }
 
-  } // end reset ADMMPD data
+  // Initialize simulation data if needed.
+  if (sb->admmpd == NULL)
+    init_admmpd_interface(ob,vertexCos);
 
-  // Cache vertices at initializer
-  if (framenr == startframe)
-  {
-//    BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
-//    BKE_ptcache_validate(cache, framenr);
-//    cache->flag &= ~PTCACHE_REDO_NEEDED;
-//    sbStoreLastFrame(depsgraph, ob, framenr);
+  // If it's the first frame we reset the cache and data
+  if (framenr == startframe) {
+    BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
+    init_admmpd_interface(ob,vertexCos);
+    BKE_ptcache_validate(cache, framenr);
+    cache->flag &= ~PTCACHE_REDO_NEEDED;
+    sb->last_frame = framenr;
     return;
   }
 
+  // Read from cache
+  {
+    bool can_write_cache = DEG_is_active(depsgraph);
+    bool can_simulate = (framenr == sb->last_frame + 1) &&
+                        !(cache->flag & PTCACHE_BAKED) &&
+                        can_write_cache;
+    int cache_result = BKE_ptcache_read(&pid, (float)framenr + scene->r.subframe, can_simulate);
+
+    if (cache_result == PTCACHE_READ_EXACT || cache_result == PTCACHE_READ_INTERPOLATED ||
+        (!can_simulate && cache_result == PTCACHE_READ_OLD)) {
+      softbody_to_object(ob, vertexCos, numVerts, sb->local);
+      BKE_ptcache_validate(cache, framenr);
+      if (cache_result == PTCACHE_READ_INTERPOLATED &&
+        cache->flag & PTCACHE_REDO_NEEDED &&
+        can_write_cache) {
+        BKE_ptcache_write(&pid, framenr);
+      }
+      sbStoreLastFrame(depsgraph, ob, framenr);
+      return;
+    }
+    else if (cache_result == PTCACHE_READ_OLD) {} // pass
+    else if (cache->flag & PTCACHE_BAKED) {
+      // if baked and nothing in cache, do nothing
+      if (can_write_cache) {
+        BKE_ptcache_invalidate(cache);
+      }
+      return;
+    }
+    if (!can_simulate)
+      return;
+  } // end read from cache
+
   // if on second frame, write cache for first frame
-//  if (cache->simframe == startframe &&
-//      (cache->flag & PTCACHE_OUTDATED || cache->last_exact == 0)) {
-//    BKE_ptcache_write(&pid, startframe);
-//  }
+  if (cache->simframe == startframe &&
+      (cache->flag & PTCACHE_OUTDATED || cache->last_exact == 0)) {
+    BKE_ptcache_write(&pid, startframe);
+  }
 
+  // Update ADMMPD interface variables from cache
+  // and perform a solve.
+  admmpd_update_from_softbody(ob);
   admmpd_solve(sb->admmpd);
-  admmpd_map_vertices(sb->admmpd,vertexCos,numVerts);
-//  admm_copy_to_softbody(ob);
+  admmpd_get_vertices(sb->admmpd,vertexCos,numVerts);
+  admmpd_copy_to_softbody(ob);
 
-//  BKE_ptcache_validate(cache, framenr);
-//  BKE_ptcache_write(&pid, framenr);
-//  sbStoreLastFrame(depsgraph, ob, framenr);
+  // Write cache
+  BKE_ptcache_validate(cache, framenr);
+  BKE_ptcache_write(&pid, framenr);
+  sbStoreLastFrame(depsgraph, ob, framenr);
 
 } // end step object with ADMMPD