working on interface

13 files changed, 410 insertions, 174 deletions

diff --git a/extern/softbody/src/admmpd_collision.cpp b/extern/softbody/src/admmpd_collision.cpp
index 2bce56fa9f0..0cd828011ad 100644
--- a/extern/softbody/src/admmpd_collision.cpp
+++ b/extern/softbody/src/admmpd_collision.cpp
@@ -17,16 +17,18 @@ void FloorCollider::jacobian(
 	std::vector<Eigen::Triplet<double> > *trips_z,
 	std::vector<double> *l)
 {
+	const double floor_z = -2.0;
+
 	int nx = x->rows();
 	for (int i=0; i<nx; ++i)
 	{
-		Eigen::Vector3d p = x->row(i);
-		if (p[2]>0)
+		Eigen::Vector3d xi = x->row(i);
+		if (xi[2]>floor_z)
 			continue;
-	
+
 		int c_idx = l->size();
 		trips_z->emplace_back(c_idx,i,1.0);
-		l->emplace_back(0);
+		l->emplace_back(floor_z);
 	}
 } // end floor collider Jacobian
 
diff --git a/extern/softbody/src/admmpd_energy.cpp b/extern/softbody/src/admmpd_energy.cpp
index ea7c68b6dca..3b90d506a2a 100644
--- a/extern/softbody/src/admmpd_energy.cpp
+++ b/extern/softbody/src/admmpd_energy.cpp
@@ -2,6 +2,7 @@
 
 
 #include "admmpd_energy.h"
+#include <iostream>
 
 namespace admmpd {
 using namespace Eigen;
diff --git a/extern/softbody/src/admmpd_solver.cpp b/extern/softbody/src/admmpd_solver.cpp
index 052008121b9..008db80720e 100644
--- a/extern/softbody/src/admmpd_solver.cpp
+++ b/extern/softbody/src/admmpd_solver.cpp
@@ -19,8 +19,8 @@ template <typename T> using RowSparseMatrix = SparseMatrix<T,RowMajor>;
 bool Solver::init(
     const Eigen::MatrixXd &V,
 	const Eigen::MatrixXi &T,
-    const ADMMPD_Options *options,
-    ADMMPD_Data *data)
+    const Options *options,
+    Data *data)
 {
 	if (!data || !options)
 		throw std::runtime_error("init: data/options null");
@@ -32,9 +32,10 @@ bool Solver::init(
 } // end init
 
 int Solver::solve(
-	const ADMMPD_Options *options,
-	ADMMPD_Data *data)
+	const Options *options,
+	Data *data)
 {
+
 	// Init the solve which computes
 	// quantaties like M_xbar and makes sure
 	// the variables are sized correctly.
@@ -77,8 +78,8 @@ int Solver::solve(
 } // end solve
 
 void Solver::init_solve(
-	const ADMMPD_Options *options,
-	ADMMPD_Data *data)
+	const Options *options,
+	Data *data)
 {
 	int nx = data->x.rows();
 	if (data->M_xbar.rows() != nx)
@@ -87,7 +88,7 @@ void Solver::init_solve(
 	// velocity and position
 	double dt = std::max(0.0, options->timestep_s);
 	data->x_start = data->x;
-	for( int i=0; i<nx; ++i )
+	for (int i=0; i<nx; ++i)
 	{
 		data->v.row(i) += options->grav;
 		data->M_xbar.row(i) =
@@ -103,8 +104,8 @@ void Solver::init_solve(
 } // end init solve
 
 void Solver::update_constraints(
-	const ADMMPD_Options *options,
-	ADMMPD_Data *data)
+	const Options *options,
+	Data *data)
 {
 
 	std::vector<double> l_coeffs;
@@ -145,8 +146,8 @@ void Solver::update_constraints(
 } // end update constraints
 
 void Solver::solve_conjugate_gradients(
-	const ADMMPD_Options *options,
-	ADMMPD_Data *data)
+	const Options *options,
+	Data *data)
 {
 	// If we don't have any constraints,
 	// we don't need to perform CG
@@ -220,8 +221,8 @@ void Solver::solve_conjugate_gradients(
 } // end solve conjugate gradients
 
 void Solver::compute_matrices(
-	const ADMMPD_Options *options,
-	ADMMPD_Data *data)
+	const Options *options,
+	Data *data)
 {
 	// Allocate per-vertex data
 	int nx = data->x.rows();
@@ -245,20 +246,29 @@ void Solver::compute_matrices(
 	// 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);
+	int n_row_D = trips.back().row()+1;
+	double dt2 = options->timestep_s * options->timestep_s;
+	if (dt2 <= 0)
+		dt2 = 1.0; // static solve
+
+	// Weight matrix
+	RowSparseMatrix<double> W2(n_row_D,n_row_D);
+	VectorXi W_nnz = VectorXi::Ones(n_row_D);
+	W2.reserve(W_nnz);
+	int ne = data->indices.size();
+	for (int i=0; i<ne; ++i)
+	{
+		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];
+		}
+	}
 
-	// Global matrix
-	data->D.resize(nw,nx);
+	// Weighted Laplacian
+	data->D.resize(n_row_D,nx);
 	data->D.setFromTriplets(trips.begin(), trips.end());
 	data->Dt = data->D.transpose();
-	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;
 	for (int i=0; i<nx; ++i)
@@ -274,16 +284,16 @@ void Solver::compute_matrices(
 		data->K[i].resize(1,nx);
 
 	// ADMM variables
-	data->z.resize(nw,3);
+	data->z.resize(n_row_D,3);
 	data->z.setZero();
-	data->u.resize(nw,3);
+	data->u.resize(n_row_D,3);
 	data->u.setZero();
 
 } // end compute matrices
 
 void Solver::append_energies(
-	const ADMMPD_Options *options,
-	ADMMPD_Data *data,
+	const Options *options,
+	Data *data,
 	std::vector<Triplet<double> > &D_triplets)
 {
 	int nt = data->tets.rows();
diff --git a/extern/softbody/src/admmpd_solver.h b/extern/softbody/src/admmpd_solver.h
index ec836d3c466..6b397bee406 100644
--- a/extern/softbody/src/admmpd_solver.h
+++ b/extern/softbody/src/admmpd_solver.h
@@ -11,24 +11,24 @@
 
 namespace admmpd {
 
-struct ADMMPD_Options {
+struct Options {
     double timestep_s;
     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() :
+    Options() :
         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),
+        mult_k(1.0),
         min_res(1e-4),
         grav(0,0,-9.8)
         {}
 };
 
-struct ADMMPD_Data {
+struct Data {
     // Input:
     Eigen::MatrixXi tets; // elements t x 4
     Eigen::MatrixXd x; // vertices, n x 3
@@ -65,42 +65,42 @@ public:
     bool init(
         const Eigen::MatrixXd &V, // vertices
         const Eigen::MatrixXi &T, // tets
-        const ADMMPD_Options *options,
-        ADMMPD_Data *data);
+        const Options *options,
+        Data *data);
 
     // Solve a single time step.
     // Returns number of iterations.
     int solve(
-        const ADMMPD_Options *options,
-        ADMMPD_Data *data);
+        const Options *options,
+        Data *data);
 
 protected:
 
     void update_constraints(
-        const ADMMPD_Options *options,
-        ADMMPD_Data *data);
+        const Options *options,
+        Data *data);
 
     void init_solve(
-        const ADMMPD_Options *options,
-        ADMMPD_Data *data);
+        const Options *options,
+        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);
+        const Options *options,
+        Data *data);
 
     void compute_lattice(
-        const ADMMPD_Options *options,
-        ADMMPD_Data *data);
+        const Options *options,
+        Data *data);
 
     void compute_matrices(
-        const ADMMPD_Options *options,
-        ADMMPD_Data *data);
+        const Options *options,
+        Data *data);
 
 	void append_energies(
-		const ADMMPD_Options *options,
-		ADMMPD_Data *data,
+		const Options *options,
+		Data *data,
 		std::vector<Eigen::Triplet<double> > &D_triplets);
 
 }; // class ADMMPD_solver
diff --git a/intern/softbody/CMakeLists.txt b/intern/softbody/CMakeLists.txt
index 5c8a2da0e44..1bb705e1d1e 100644
--- a/intern/softbody/CMakeLists.txt
+++ b/intern/softbody/CMakeLists.txt
@@ -24,8 +24,11 @@ set(INC
 
 set(INC_SYS
   ../../extern/softbody/src
+  ../tetgen
   ../../source/blender/makesdna
   ../../source/blender/blenkernel
+  ../../source/blender/blenlib
+  ../../intern/guardedalloc
   ${EIGEN3_INCLUDE_DIRS}
 )
 
diff --git a/intern/softbody/admmpd_api.cpp b/intern/softbody/admmpd_api.cpp
index 5963c4f71c8..21e21c4cb9d 100644
--- a/intern/softbody/admmpd_api.cpp
+++ b/intern/softbody/admmpd_api.cpp
@@ -24,77 +24,173 @@
 #include "admmpd_api.h"
 #include "admmpd_solver.h"
 #include "admmpd_lattice.h"
-#include "DNA_object_types.h" // Object
-#include "BKE_softbody.h"
+#include "tetgen_api.h"
+#include "DNA_mesh_types.h" // Mesh
+#include "DNA_meshdata_types.h" // MVert
+#include "BKE_mesh_remesh_voxel.h" // TetGen
+#include "BKE_mesh.h" // BKE_mesh_free
+#include "MEM_guardedalloc.h" // 
+
 #include <iostream>
 
-struct ADMMPD_Data {
-  admmpd::ADMMPD_Options *options;
-  admmpd::ADMMPD_Data *data;
-  admmpd::Lattice *lattice;
+
+struct ADMMPDInternalData {
+  admmpd::Options *options;
+  admmpd::Data *data;
+//  admmpd::Lattice *lattice;
 };
 
-ADMMPD_Data* admmpd_init(
-    BodyPoint *bp,
-    int numVerts)
+
+void admmpd_alloc(ADMMPDInterfaceData *iface, int in_verts, int in_faces)
+{
+  if (iface==NULL)
+    return;
+
+  iface->in_totverts = in_verts;
+  iface->in_verts = (float *)MEM_mallocN(in_verts*3*sizeof(float), "admmpd_verts");
+  iface->in_vel = (float *)MEM_mallocN(in_verts*3*sizeof(float), "admmpd_vel");
+
+  iface->in_totfaces = in_faces;
+  iface->in_faces = (unsigned int *)MEM_mallocN(in_faces*3*sizeof(unsigned int), "admmpd_faces");
+}
+
+void admmpd_dealloc(ADMMPDInterfaceData *iface)
 {
-  if (!bp)
-    return NULL;
-
-  ADMMPD_Data *admmpd_data = new ADMMPD_Data();
-  admmpd_data->options = new admmpd::ADMMPD_Options();
-  admmpd::ADMMPD_Options *options = admmpd_data->options;
-  admmpd_data->data = new admmpd::ADMMPD_Data();
-  admmpd::ADMMPD_Data *data = admmpd_data->data;
-  admmpd_data->lattice = new admmpd::Lattice();
-  admmpd::Lattice *lattice = admmpd_data->lattice;
-
-  // Create initializer
-  Eigen::MatrixXd V(numVerts,3);
+  if (iface==NULL)
+    return;
+
+  iface->in_totverts = 0;
+  if (iface->in_verts != NULL)
+    MEM_freeN(iface->in_verts);
+  if (iface->in_vel != NULL)
+    MEM_freeN(iface->in_vel);
+
+  iface->in_totfaces = 0;
+  if (iface->in_faces != NULL)
+    MEM_freeN(iface->in_faces);
+
+  iface->out_totverts = 0;
+  if (iface->out_verts != NULL)
+    MEM_freeN(iface->out_verts);
+  if (iface->out_vel != NULL)
+    MEM_freeN(iface->out_vel);
+
+  if (iface->data)
+  {
+    if(iface->data->options)
+        delete iface->data->options;
+    if(iface->data->data)
+        delete iface->data->data;
+    delete iface->data;
+  }
+
+  iface->data = NULL;
+  iface->in_verts = NULL;
+  iface->in_vel = NULL;
+  iface->in_faces = NULL;
+  iface->out_verts = NULL;
+}
+
+int admmpd_init(ADMMPDInterfaceData *iface)
+{
+  if (iface==NULL)
+    return 0;
+  if (iface->in_verts==NULL || iface->in_vel==NULL || iface->in_faces==NULL)
+    return 0;
+  if (iface->in_totverts<=0 || iface->in_totfaces<=0)
+    return 0;
+
+  // Generate tets
+  TetGenRemeshData tg;
+  init_tetgenremeshdata(&tg);
+  tg.in_verts = iface->in_verts;
+  tg.in_totverts = iface->in_totverts;
+  tg.in_faces = iface->in_faces;
+  tg.in_totfaces = iface->in_totfaces;
+  bool success = tetgen_resmesh(&tg);
+  if (!success || tg.out_tottets==0)
+    return 0;
+
+  // Resize data
+  iface->out_totverts = tg.out_totverts;
+  iface->out_verts = (float *)MEM_callocN(
+      iface->out_totverts*3*sizeof(float), "ADMMPD_out_verts");
+  iface->out_vel = (float *)MEM_callocN(
+      iface->out_totverts*3*sizeof(float), "ADMMPD_out_verts");
+
+  // Create initializer for ADMMPD
+  int nv = tg.out_totverts;
+  int nt = tg.out_tottets;
+  Eigen::MatrixXd V(nv,3);
+  Eigen::MatrixXi T(nt,4);
   V.setZero();
-  for (int i = 0; i < numVerts; ++i)
+  for (int i=0; i<nv; ++i)
   {
-    BodyPoint &bpi = bp[i];
-    V(i,0) = bpi.pos[0];
-    V(i,1) = bpi.pos[1];
-    V(i,2) = bpi.pos[2];
+    for (int j=0; j<3; ++j)
+    {
+      V(i,j) = tg.out_verts[i*3+j];
+      iface->out_verts[i*3+j] = tg.out_verts[i*3+j];
+      iface->out_vel[i*3+j] = 0;
+    }
+  }
+  T.setZero();
+  for (int i=0; i<nt; ++i)
+  {
+    T(i,0) = tg.out_tets[i*4+0];
+    T(i,1) = tg.out_tets[i*4+1];
+    T(i,2) = tg.out_tets[i*4+2];
+    T(i,3) = tg.out_tets[i*4+3];
   }
 
-  // Generate a mapping from x -> V
-  // Usually, dim(x) < dim(V)
-  Eigen::MatrixXd x;
-  Eigen::MatrixXi T;
-  lattice->generate(V, &x, &T);
+  // Generate solver data
+  iface->data = new ADMMPDInternalData();
+  iface->data->options = new admmpd::Options();
+  admmpd::Options *options = iface->data->options;
+  iface->data->data = new admmpd::Data();
+  admmpd::Data *data = iface->data->data;
 
+  // Initialize
+  bool init_success = false;
   try
   {
-    admmpd::Solver().init(x, T, options, data);
+    init_success = admmpd::Solver().init(V, T, options, data);
   }
   catch(const std::exception &e)
   {
     printf("**ADMMPD Error on init: %s\n", e.what());
-    // Should probably return nullptr
   }
 
-  return admmpd_data;
+  // Clean up tetgen data
+  MEM_freeN(tg.out_tets);
+  MEM_freeN(tg.out_facets);
+  MEM_freeN(tg.out_verts);
+  return int(init_success);
 }
 
-void admmpd_cleanup(ADMMPD_Data *admmpd_data)
+int admmpd_cache_valid(ADMMPDInterfaceData *iface, int numVerts)
 {
-  if (!admmpd_data)
-    return;
-  
-  delete admmpd_data->data;
-  delete admmpd_data->options;
-  delete admmpd_data->lattice;
-  delete admmpd_data;
+  if (iface == NULL) // we haven't initialized yet
+    return true;
+
+  return iface->in_totverts == numVerts;
 }
 
-void admmpd_solve(ADMMPD_Data *admmpd_data)
+void admmpd_solve(ADMMPDInterfaceData *iface)
 {
+  if (iface == NULL)
+    return;
+
   try
   {
-    admmpd::Solver().solve(admmpd_data->options,admmpd_data->data);
+    admmpd::Solver().solve(iface->data->options,iface->data->data);
+    for (int i=0; i<iface->data->data->x.rows(); ++i)
+    {
+      for (int j=0; j<3; ++j)
+      {
+        iface->out_verts[i*3+j] = iface->data->data->x(i,j);
+        iface->out_vel[i*3+j] = iface->data->data->v(i,j);
+      }
+    }
   }
   catch(const std::exception &e)
   {
@@ -102,35 +198,24 @@ void admmpd_solve(ADMMPD_Data *admmpd_data)
   }
 }
 
-void admmpd_to_bodypoint(
-    ADMMPD_Data* data,
-    BodyPoint *bp,
-    int numVerts)
+void admmpd_map_vertices(ADMMPDInterfaceData *iface, float (*vertexCos)[3], int numVerts)
 {
-  if (!data || !bp)
-  {
-    printf("DATA OR BP NULL!?\n");
+  if (iface == NULL)
     return;
-  }
 
-  if (!data->data || !data->lattice)
+  if (numVerts != iface->in_totverts || numVerts > iface->out_totverts)
+  {
+    printf("**ADMMPD TODO: PROPER VERTEX MAPPINGS\n");
     return;
-
-  // Map x -> BodyPoint
-  // Usually, dim(x) < dim(V)
+  }
+  // TODO double check this, but I believe the first n verts
+  // created by tetgen are the same as the input. I hope. We'll find out I guess.
+  // If not, this function will be a placeholder for the mapping that
+  // will have to occur.
   for (int i=0; i<numVerts; ++i)
   {
-    BodyPoint &bpi = bp[i];
-    Eigen::Vector3d xi = data->lattice->get_mapped_vertex(
-      i, &data->data->x, &data->data->tets);
-    Eigen::Vector3d vi = data->lattice->get_mapped_vertex(
-      i, &data->data->v, &data->data->tets);
-    bpi.pos[0] = xi[0];
-    bpi.pos[1] = xi[1];
-    bpi.pos[2] = xi[2];
-    bpi.vec[0] = vi[0]; // vec is velocity?
-    bpi.vec[1] = vi[1];
-    bpi.vec[2] = vi[2];
+    vertexCos[i][0] = iface->out_verts[i*3+0];
+    vertexCos[i][1] = iface->out_verts[i*3+1];
+    vertexCos[i][2] = iface->out_verts[i*3+2];
   }
-
-} // end map to object
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/intern/softbody/admmpd_api.h b/intern/softbody/admmpd_api.h
index c43f0b62375..bcfe37403c0 100644
--- a/intern/softbody/admmpd_api.h
+++ b/intern/softbody/admmpd_api.h
@@ -28,23 +28,38 @@
 extern "C" {
 #endif
 
-typedef struct ADMMPD_Data ADMMPD_Data;
-typedef struct Object Object;
-typedef struct BodyPoint BodyPoint;
+//typedef struct Mesh Mesh_;
 
-ADMMPD_Data* admmpd_init(
-    BodyPoint *bp,
-    int numVerts);
+typedef struct ADMMPDInterfaceData {
+    float *in_verts;
+    float *in_vel;
+    unsigned int *in_faces;
+    int in_totfaces;
+    int in_totverts;
+    // Num output verts might be different than num input verts.
+    // This is due to the lattice/tetmesh that is generated
+    // in init. They need to be cached by the system
+    float *out_verts;
+    float *out_vel;
+    int out_totverts;
+    // Solver data used internally
+    struct ADMMPDInternalData *data;
+} ADMMPDInterfaceData;
 
-void admmpd_cleanup(ADMMPD_Data*);
+void admmpd_alloc(ADMMPDInterfaceData*, int in_verts, int in_faces);
+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(ADMMPD_Data*);
+//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_to_bodypoint(
+//    ADMMPD_Data *data,
+//    BodyPoint *bp,
+//    int numVerts);
 
 #ifdef __cplusplus
 }
diff --git a/intern/tetgen/tetgen_api.cpp b/intern/tetgen/tetgen_api.cpp
index c2edcfd650f..16d403975a0 100644
--- a/intern/tetgen/tetgen_api.cpp
+++ b/intern/tetgen/tetgen_api.cpp
@@ -100,7 +100,6 @@ static void make_tetgenio(
 
 bool tetgen_resmesh(TetGenRemeshData *tg)
 {
-    printf("\n\n\n\nCALLING TETGEN\n");
 //	float maxvol = compute_maxvol(tg->in_verts, tg->in_faces, tg->in_totfaces);
 //    float quality = 1.4;
 
diff --git a/source/blender/blenkernel/BKE_mesh_remesh_voxel.h b/source/blender/blenkernel/BKE_mesh_remesh_voxel.h
index 15569406680..f44276adac1 100644
--- a/source/blender/blenkernel/BKE_mesh_remesh_voxel.h
+++ b/source/blender/blenkernel/BKE_mesh_remesh_voxel.h
@@ -57,7 +57,9 @@ struct Mesh *BKE_mesh_remesh_quadriflow_to_mesh_nomain(struct Mesh *mesh,
                                                        bool adaptive_scale,
                                                        void *update_cb,
                                                        void *update_cb_data);
-struct Mesh *BKE_mesh_remesh_tetgen_to_mesh_nomain(struct Mesh *mesh);
+struct Mesh *BKE_mesh_remesh_tetgen_to_mesh_nomain(struct Mesh *mesh,
+													unsigned int **tets,
+													int *numtets);
 
 /* Data reprojection functions */
 void BKE_mesh_remesh_reproject_paint_mask(struct Mesh *target, struct Mesh *source);
diff --git a/source/blender/blenkernel/intern/mesh_remesh_voxel.c b/source/blender/blenkernel/intern/mesh_remesh_voxel.c
index c3423c682c9..018c13b65a7 100644
--- a/source/blender/blenkernel/intern/mesh_remesh_voxel.c
+++ b/source/blender/blenkernel/intern/mesh_remesh_voxel.c
@@ -265,7 +265,9 @@ static Mesh *BKE_mesh_remesh_quadriflow(Mesh *input_mesh,
 #endif
 
 #ifdef WITH_TETGEN
-static Mesh *BKE_mesh_remesh_tetgen(Mesh *input_mesh)
+static Mesh *BKE_mesh_remesh_tetgen(Mesh *input_mesh,
+                                  unsigned int **tets,
+                                  int *numtets)
 {
   // Ensure that the triangulated mesh data is up to data
   BKE_mesh_runtime_looptri_recalc(input_mesh);
@@ -310,6 +312,19 @@ static Mesh *BKE_mesh_remesh_tetgen(Mesh *input_mesh)
   MEM_freeN(faces);
   MEM_freeN(verttri);
 
+{
+    if (tg.out_verts)
+    MEM_freeN(tg.out_verts);
+
+  if (tg.out_facets)
+    MEM_freeN(tg.out_facets);
+
+  if (tg.out_tets)
+    MEM_freeN(tg.out_tets);
+    
+  return NULL;
+}
+
   Mesh *mesh = NULL;
   if (success)
   {
@@ -333,9 +348,10 @@ static Mesh *BKE_mesh_remesh_tetgen(Mesh *input_mesh)
     BKE_mesh_calc_edges(mesh, false, false);
     BKE_mesh_calc_normals(mesh);
 
-//    mesh->tottet = tg.out_tottets;
-//    mesh->mtet = (unsigned int *)MEM_malloc_arrayN(tg.out_tottets*4, sizeof(unsigned int), "remesh_output_tets");
-//    memcpy(mesh->mtet,tg.out_tets,tg.out_tottets*4*sizeof(unsigned int));
+    *numtets = tg.out_tottets;
+    //*tets = (unsigned int *)MEM_malloc_arrayN(tg.out_tottets*4, sizeof(unsigned int), "remesh_output_tets");
+    *tets = (unsigned int *)malloc(tg.out_tottets*4*sizeof(unsigned int));
+    memcpy(*tets,tg.out_tets,tg.out_tottets*4*sizeof(unsigned int));
 
   } // end success
 
@@ -352,11 +368,12 @@ static Mesh *BKE_mesh_remesh_tetgen(Mesh *input_mesh)
 }
 #endif
 
-struct Mesh *BKE_mesh_remesh_tetgen_to_mesh_nomain(struct Mesh *mesh)
+struct Mesh *BKE_mesh_remesh_tetgen_to_mesh_nomain(struct Mesh *mesh,
+                                                unsigned int **tets,
+                                                int *numtets)
 {
   #ifdef WITH_TETGEN
-  Mesh *new_mesh = BKE_mesh_remesh_tetgen(mesh);  
-  return new_mesh;
+  return BKE_mesh_remesh_tetgen(mesh,tets,numtets);  
   #else
   UNUSED_VARS(mesh,
               tets,
diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index 90ef6519e33..a14cb41a4af 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -81,7 +81,6 @@
 
 #include "PIL_time.h"
 
-
 static CLG_LogRef LOG = {"bke.softbody"};
 
 /* callbacks for errors and interrupts and some goo */
@@ -843,6 +842,11 @@ static void renew_softbody(Scene *scene, Object *ob, int totpoint, int totspring
   sb = ob->soft;
   softflag = ob->softflag;
 
+  if (sb->admmpd == NULL)
+  {
+    sb->admmpd = MEM_callocN(sizeof(ADMMPDInterfaceData), "SoftBody_ADMMPD");
+  }
+
   if (totpoint) {
     sb->totpoint = totpoint;
     sb->totspring = totspring;
@@ -852,6 +856,13 @@ static void renew_softbody(Scene *scene, Object *ob, int totpoint, int totspring
       sb->bspring = MEM_mallocN(totspring * sizeof(BodySpring), "bodyspring");
     }
 
+    if (ob->type == OB_MESH)
+    {
+      const Mesh *me = ob->data;
+      int totfaces = poly_to_tri_count(me->totpoly, me->totloop);
+      admmpd_alloc(sb->admmpd, totpoint, totfaces);
+    }  
+
     /* initialize BodyPoint array */
     for (i = 0; i < totpoint; i++) {
       BodyPoint *bp = &sb->bpoint[i];
@@ -944,9 +955,10 @@ static void free_softbody_intern(SoftBody *sb)
     sb->bpoint = NULL;
     sb->bspring = NULL;
 
-    if (sb->admmpd_data) {
-      admmpd_cleanup(sb->admmpd_data);
-      sb->admmpd_data = NULL;
+    if (sb->admmpd) {
+      admmpd_dealloc(sb->admmpd);
+      MEM_freeN(sb->admmpd);
+      sb->admmpd = NULL;
     }
 
     free_scratch(sb);
@@ -3121,7 +3133,6 @@ static void sb_new_scratch(SoftBody *sb)
 SoftBody *sbNew(Scene *scene)
 {
   SoftBody *sb;
-
   sb = MEM_callocN(sizeof(SoftBody), "softbody");
 
   sb->mediafrict = 0.5f;
@@ -3168,9 +3179,7 @@ SoftBody *sbNew(Scene *scene)
     sb->effector_weights = BKE_effector_add_weights(NULL);
   }
 
-  // ADMMPD_Data created in sbObjectStep
-  sb->admmpd_data = NULL;
-
+  sb->admmpd = NULL;
   sb->last_frame = MINFRAME - 1;
 
   return sb;
@@ -3537,6 +3546,110 @@ static void sbStoreLastFrame(struct Depsgraph *depsgraph, Object *object, float
 }
 
 /* simulates one step. framenr is in frames */
+void sbObjectStep_admmpd(struct Depsgraph *depsgraph,
+                  Scene *scene,
+                  Object *ob,
+                  float cfra,
+                  float (*vertexCos)[3],
+                  int numVerts)
+{
+  if(ob->type != OB_MESH)
+    return;
+
+  Mesh *me = ob->data;
+  SoftBody *sb = ob->soft;
+  PointCache *cache = sb->shared->pointcache;
+  int framenr = (int)cfra;
+  int framedelta = framenr - cache->simframe;
+
+  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;
+  }
+
+  // Reset simulation
+  bool reset_sim =
+    sb->admmpd == NULL ||
+    sb->admmpd->in_totverts != numVerts ||
+    framenr == startframe;
+
+  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);
+
+  } // end reset ADMMPD data
+
+  // 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);
+    return;
+  }
+
+  // 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);
+//  }
+
+  admmpd_solve(sb->admmpd);
+  admmpd_map_vertices(sb->admmpd,vertexCos,numVerts);
+
+//  BKE_ptcache_validate(cache, framenr);
+//  BKE_ptcache_write(&pid, framenr);
+  //sbStoreLastFrame(depsgraph, ob, framenr);
+
+} // end step object with ADMMPD
+
+/* simulates one step. framenr is in frames */
 void sbObjectStep(struct Depsgraph *depsgraph,
                   Scene *scene,
                   Object *ob,
@@ -3544,6 +3657,9 @@ void sbObjectStep(struct Depsgraph *depsgraph,
                   float (*vertexCos)[3],
                   int numVerts)
 {
+ sbObjectStep_admmpd(depsgraph,scene,ob,cfra,vertexCos,numVerts);
+ return;
+
   SoftBody *sb = ob->soft;
   PointCache *cache;
   PTCacheID pid;
@@ -3596,12 +3712,6 @@ void sbObjectStep(struct Depsgraph *depsgraph,
 
     softbody_update_positions(ob, sb, vertexCos, numVerts);
     softbody_reset(ob, sb, vertexCos, numVerts);
-
-    if (sb->admmpd_data)
-      admmpd_cleanup(sb->admmpd_data);
-
-    BodyPoint *bp = sb->bpoint;
-    sb->admmpd_data = admmpd_init(bp, numVerts);
   }
 
   /* still no points? go away */
@@ -3614,10 +3724,6 @@ void sbObjectStep(struct Depsgraph *depsgraph,
     /* first frame, no simulation to do, just set the positions */
     softbody_update_positions(ob, sb, vertexCos, numVerts);
 
-    // sb->bpoint is NULL here :/
-    //if (sb->admmpd_data)
-    //  bodypoint_to_admmpd(sb->admmpd_data,bp,numVerts)
-
     BKE_ptcache_validate(cache, framenr);
     cache->flag &= ~PTCACHE_REDO_NEEDED;
 
@@ -3677,16 +3783,7 @@ void sbObjectStep(struct Depsgraph *depsgraph,
   dtime = framedelta * timescale;
 
   /* do simulation */
-//  softbody_step(depsgraph, scene, ob, sb, dtime);
-
-  {
-    if (sb->admmpd_data)
-    {
-      admmpd_solve(sb->admmpd_data);
-      BodyPoint *bp = sb->bpoint;
-      admmpd_to_bodypoint(sb->admmpd_data,bp,numVerts);
-    }
-  }
+  softbody_step(depsgraph, scene, ob, sb, dtime);
 
   softbody_to_object(ob, vertexCos, numVerts, 0);
 
diff --git a/source/blender/editors/object/object_remesh.c b/source/blender/editors/object/object_remesh.c
index 10e41ebe946..1b0d373633c 100644
--- a/source/blender/editors/object/object_remesh.c
+++ b/source/blender/editors/object/object_remesh.c
@@ -1188,7 +1188,12 @@ static int tetgen_remesh_exec(bContext *C, wmOperator *op)
   Mesh *mesh = ob->data;
   Mesh *new_mesh = NULL;
 
-  new_mesh = BKE_mesh_remesh_tetgen_to_mesh_nomain(mesh);
+  unsigned int *tets;
+  int numtets;
+  new_mesh = BKE_mesh_remesh_tetgen_to_mesh_nomain(mesh,&tets,&numtets);
+  if (tets) {
+    MEM_freeN(tets);
+  }
 
   if (!new_mesh) {
     BKE_report(op->reports, RPT_ERROR, "TetGen remesher failed to create mesh");
diff --git a/source/blender/makesdna/DNA_object_force_types.h b/source/blender/makesdna/DNA_object_force_types.h
index 183681c500e..bf0d26ef1b6 100644
--- a/source/blender/makesdna/DNA_object_force_types.h
+++ b/source/blender/makesdna/DNA_object_force_types.h
@@ -304,7 +304,7 @@ typedef struct SoftBody_Shared {
   struct ListBase ptcaches;
 } SoftBody_Shared;
 
-typedef struct ADMMPD_Data ADMMPD_Data;
+typedef struct ADMMPDInterfaceData ADMMPDInterfaceData;
 typedef struct SoftBody {
   /* dynamic data */
   int totpoint, totspring;
@@ -313,7 +313,7 @@ typedef struct SoftBody {
   /** Not saved in file. */
   struct BodySpring *bspring;
 
-  struct ADMMPD_Data *admmpd_data;
+  struct ADMMPDInterfaceData *admmpd;
 
   char _pad;
   char msg_lock;