Moved most of the main cloth solver function out of implicit code core.

Force calculation is disabled, will follow shortly.

4 files changed, 393 insertions, 415 deletions

diff --git a/source/blender/physics/intern/BPH_mass_spring.cpp b/source/blender/physics/intern/BPH_mass_spring.cpp
index d9b3fe9fa46..ae68525a1e7 100644
--- a/source/blender/physics/intern/BPH_mass_spring.cpp
+++ b/source/blender/physics/intern/BPH_mass_spring.cpp
@@ -29,6 +29,8 @@
  *  \ingroup bph
  */
 
+extern "C" {
+#include "DNA_cloth_types.h"
 #include "DNA_scene_types.h"
 #include "DNA_object_types.h"
 #include "DNA_meshdata_types.h"
@@ -39,6 +41,9 @@
 #include "BLI_utildefines.h"
 
 #include "BKE_cloth.h"
+#include "BKE_collision.h"
+#include "BKE_effect.h"
+}
 
 #include "BPH_mass_spring.h"
 #include "implicit.h"
@@ -98,3 +103,242 @@ void BKE_cloth_solver_set_positions(ClothModifierData *clmd)
 		BPH_mass_spring_set_motion_state(id, i, verts[i].x, verts[i].v);
 	}
 }
+
+static bool collision_response(ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, float restitution, float r_impulse[3])
+{
+	Cloth *cloth = clmd->clothObject;
+	int index = collpair->ap1;
+	bool result = false;
+	
+	float v1[3], v2_old[3], v2_new[3], v_rel_old[3], v_rel_new[3];
+	float epsilon2 = BLI_bvhtree_getepsilon(collmd->bvhtree);
+
+	float margin_distance = collpair->distance - epsilon2;
+	float mag_v_rel;
+	
+	zero_v3(r_impulse);
+	
+	if (margin_distance > 0.0f)
+		return false; /* XXX tested before already? */
+	
+	/* only handle static collisions here */
+	if ( collpair->flag & COLLISION_IN_FUTURE )
+		return false;
+	
+	/* velocity */
+	copy_v3_v3(v1, cloth->verts[index].v);
+	collision_get_collider_velocity(v2_old, v2_new, collmd, collpair);
+	/* relative velocity = velocity of the cloth point relative to the collider */
+	sub_v3_v3v3(v_rel_old, v1, v2_old);
+	sub_v3_v3v3(v_rel_new, v1, v2_new);
+	/* normal component of the relative velocity */
+	mag_v_rel = dot_v3v3(v_rel_old, collpair->normal);
+	
+	/* only valid when moving toward the collider */
+	if (mag_v_rel < -ALMOST_ZERO) {
+		float v_nor_old, v_nor_new;
+		float v_tan_old[3], v_tan_new[3];
+		float bounce, repulse;
+		
+		/* Collision response based on
+		 * "Simulating Complex Hair with Robust Collision Handling" (Choe, Choi, Ko, ACM SIGGRAPH 2005)
+		 * http://graphics.snu.ac.kr/publications/2005-choe-HairSim/Choe_2005_SCA.pdf
+		 */
+		
+		v_nor_old = mag_v_rel;
+		v_nor_new = dot_v3v3(v_rel_new, collpair->normal);
+		
+		madd_v3_v3v3fl(v_tan_old, v_rel_old, collpair->normal, -v_nor_old);
+		madd_v3_v3v3fl(v_tan_new, v_rel_new, collpair->normal, -v_nor_new);
+		
+		/* TODO repulsion forces can easily destabilize the system,
+		 * have to clamp them or construct a linear spring instead
+		 */
+//		repulse = -margin_distance / dt + dot_v3v3(v1, collpair->normal);
+		repulse = 0.0f;
+		
+		if (margin_distance < -epsilon2) {
+			bounce = -(v_nor_new + v_nor_old * restitution);
+			mul_v3_v3fl(r_impulse, collpair->normal, max_ff(repulse, bounce));
+		}
+		else {
+			bounce = 0.0f;
+			mul_v3_v3fl(r_impulse, collpair->normal, repulse);
+		}
+		
+		result = true;
+	}
+	
+	return result;
+}
+
+/* Init constraint matrix
+ * This is part of the modified CG method suggested by Baraff/Witkin in
+ * "Large Steps in Cloth Simulation" (Siggraph 1998)
+ */
+static void cloth_setup_constraints(ClothModifierData *clmd, ColliderContacts *contacts, int totcolliders, float dt)
+{
+	Cloth *cloth = clmd->clothObject;
+	Implicit_Data *data = cloth->implicit;
+	ClothVertex *verts = cloth->verts;
+	int numverts = cloth->numverts;
+	int i, j, v;
+	
+	const float ZERO[3] = {0.0f, 0.0f, 0.0f};
+	
+	BPH_mass_spring_clear_constraints(data);
+	
+	for (v = 0; v < numverts; v++) {
+		if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) {
+			/* pinned vertex constraints */
+			BPH_mass_spring_add_constraint_ndof0(data, v, ZERO); /* velocity is defined externally */
+		}
+		
+		verts[v].impulse_count = 0;
+	}
+
+	for (i = 0; i < totcolliders; ++i) {
+		ColliderContacts *ct = &contacts[i];
+		for (j = 0; j < ct->totcollisions; ++j) {
+			CollPair *collpair = &ct->collisions[j];
+//			float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - ct->ob->pd->pdef_sbdamp);
+			float restitution = 0.0f;
+			int v = collpair->face1;
+			float impulse[3];
+			
+			/* pinned verts handled separately */
+			if (verts[v].flags & CLOTH_VERT_FLAG_PINNED)
+				continue;
+			
+			/* XXX cheap way of avoiding instability from multiple collisions in the same step
+			 * this should eventually be supported ...
+			 */
+			if (verts[v].impulse_count > 0)
+				continue;
+			
+			/* calculate collision response */
+			if (!collision_response(clmd, ct->collmd, collpair, restitution, impulse))
+				continue;
+			
+			BPH_mass_spring_add_constraint_ndof2(data, v, collpair->normal, impulse);
+			++verts[v].impulse_count;
+			
+			BKE_sim_debug_data_add_dot(clmd->debug_data, collpair->pa, 0, 1, 0, "collision", hash_collpair(936, collpair));
+			BKE_sim_debug_data_add_dot(clmd->debug_data, collpair->pb, 1, 0, 0, "collision", hash_collpair(937, collpair));
+			BKE_sim_debug_data_add_line(clmd->debug_data, collpair->pa, collpair->pb, 0.7, 0.7, 0.7, "collision", hash_collpair(938, collpair));
+			
+			{ /* DEBUG */
+//				float nor[3];
+//				mul_v3_v3fl(nor, collpair->normal, collpair->distance);
+//				BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, nor, 1, 1, 0, "collision", hash_collpair(939, collpair));
+				BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, impulse, 1, 1, 0, "collision", hash_collpair(940, collpair));
+//				BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, collpair->normal, 1, 1, 0, "collision", hash_collpair(941, collpair));
+			}
+		}
+	}
+}
+
+int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
+{
+	unsigned int i=0;
+	float step=0.0f, tf=clmd->sim_parms->timescale;
+	Cloth *cloth = clmd->clothObject;
+	ClothVertex *verts = cloth->verts/*, *cv*/;
+	unsigned int numverts = cloth->numverts;
+	float dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame;
+	Implicit_Data *id = cloth->implicit;
+	ColliderContacts *contacts = NULL;
+	int totcolliders = 0;
+	
+	BKE_sim_debug_data_clear_category(clmd->debug_data, "collision");
+	
+	if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { /* do goal stuff */
+		for (i = 0; i < numverts; i++) {
+			// update velocities with constrained velocities from pinned verts
+			if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
+				float v[3];
+				sub_v3_v3v3(v, verts[i].xconst, verts[i].xold);
+				// mul_v3_fl(v, clmd->sim_parms->stepsPerFrame);
+				BPH_mass_spring_set_velocity(id, i, v);
+			}
+		}
+	}
+	
+	if (clmd->debug_data) {
+		for (i = 0; i < numverts; i++) {
+			BKE_sim_debug_data_add_dot(clmd->debug_data, verts[i].x, 1.0f, 0.1f, 1.0f, "points", hash_vertex(583, i));
+		}
+	}
+	
+	while (step < tf) {
+		
+		/* copy velocities for collision */
+		for (i = 0; i < numverts; i++) {
+			BPH_mass_spring_get_motion_state(id, i, NULL, verts[i].tv);
+			copy_v3_v3(verts[i].v, verts[i].tv);
+		}
+		
+		/* determine contact points */
+		if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
+			if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_POINTS) {
+				cloth_find_point_contacts(ob, clmd, 0.0f, tf, &contacts, &totcolliders);
+			}
+		}
+		
+		/* setup vertex constraints for pinned vertices and contacts */
+		cloth_setup_constraints(clmd, contacts, totcolliders, dt);
+		
+		// damping velocity for artistic reasons
+		// this is a bad way to do it, should be removed imo - lukas_t
+		if (clmd->sim_parms->vel_damping != 1.0f) {
+			for (i = 0; i < numverts; i++) {
+				float v[3];
+				BPH_mass_spring_get_motion_state(id, i, NULL, v);
+				mul_v3_fl(v, clmd->sim_parms->vel_damping);
+				BPH_mass_spring_set_velocity(id, i, v);
+			}
+		}
+		
+		// calculate forces
+//		cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M);
+		
+		// calculate new velocity and position
+		BPH_mass_spring_solve(id, dt);
+		
+		BPH_mass_spring_apply_result(id);
+		
+		/* move pinned verts to correct position */
+		for (i = 0; i < numverts; i++) {
+			if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) {
+				if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
+					float x[3];
+					interp_v3_v3v3(x, verts[i].xold, verts[i].xconst, step + dt);
+					BPH_mass_spring_set_position(id, i, x);
+				}
+			}
+			
+			BPH_mass_spring_get_motion_state(id, i, verts[i].txold, NULL);
+			
+//			if (!(verts[i].flags & CLOTH_VERT_FLAG_PINNED) && i > 0) {
+//				BKE_sim_debug_data_add_line(clmd->debug_data, id->X[i], id->X[i-1], 0.6, 0.3, 0.3, "hair", hash_vertex(4892, i));
+//				BKE_sim_debug_data_add_line(clmd->debug_data, id->Xnew[i], id->Xnew[i-1], 1, 0.5, 0.5, "hair", hash_vertex(4893, i));
+//			}
+//			BKE_sim_debug_data_add_vector(clmd->debug_data, id->X[i], id->V[i], 0, 0, 1, "velocity", hash_vertex(3158, i));
+		}
+		
+		/* free contact points */
+		if (contacts) {
+			cloth_free_contacts(contacts, totcolliders);
+		}
+		
+		step += dt;
+	}
+	
+	/* copy results back to cloth data */
+	for (i = 0; i < numverts; i++) {
+		BPH_mass_spring_get_motion_state(id, i, verts[i].x, verts[i].v);
+		copy_v3_v3(verts[i].txold, verts[i].x);
+	}
+	
+	return 1;
+}
diff --git a/source/blender/physics/intern/implicit.h b/source/blender/physics/intern/implicit.h
index c674b4953b2..d2036712dc1 100644
--- a/source/blender/physics/intern/implicit.h
+++ b/source/blender/physics/intern/implicit.h
@@ -34,6 +34,8 @@
 
 #include "stdio.h"
 
+#include "BKE_collision.h"
+
 #include "BLI_utildefines.h"
 
 #ifdef __cplusplus
@@ -63,11 +65,59 @@ BLI_INLINE void implicit_print_matrix_elem(float v)
     printf("%-8.3f", v);
 }
 
+/* ==== hash functions for debugging ==== */
+BLI_INLINE unsigned int hash_int_2d(unsigned int kx, unsigned int ky)
+{
+#define rot(x,k) (((x)<<(k)) | ((x)>>(32-(k))))
+
+	unsigned int a, b, c;
+
+	a = b = c = 0xdeadbeef + (2 << 2) + 13;
+	a += kx;
+	b += ky;
+
+	c ^= b; c -= rot(b,14);
+	a ^= c; a -= rot(c,11);
+	b ^= a; b -= rot(a,25);
+	c ^= b; c -= rot(b,16);
+	a ^= c; a -= rot(c,4);
+	b ^= a; b -= rot(a,14);
+	c ^= b; c -= rot(b,24);
+
+	return c;
+
+#undef rot
+}
+
+BLI_INLINE int hash_vertex(int type, int vertex)
+{
+	return hash_int_2d((unsigned int)type, (unsigned int)vertex);
+}
+
+BLI_INLINE int hash_collpair(int type, CollPair *collpair)
+{
+	return hash_int_2d((unsigned int)type, hash_int_2d((unsigned int)collpair->face1, (unsigned int)collpair->face2));
+}
+/* ================ */
+
 void BPH_mass_spring_set_root_motion(struct Implicit_Data *data, int index, const float loc[3], const float vel[3], float rot[3][3], const float angvel[3]);
+
 void BPH_mass_spring_set_motion_state(struct Implicit_Data *data, int index, const float x[3], const float v[3]);
+void BPH_mass_spring_set_position(struct Implicit_Data *data, int index, const float x[3]);
+void BPH_mass_spring_set_velocity(struct Implicit_Data *data, int index, const float v[3]);
+void BPH_mass_spring_get_motion_state(struct Implicit_Data *data, int index, float x[3], float v[3]);
 void BPH_mass_spring_set_vertex_mass(struct Implicit_Data *data, int index, float mass);
+
 int BPH_mass_spring_init_spring(struct Implicit_Data *data, int index, int v1, int v2);
 
+void BPH_mass_spring_clear_constraints(struct Implicit_Data *data);
+void BPH_mass_spring_add_constraint_ndof0(struct Implicit_Data *data, int index, const float dV[3]);
+void BPH_mass_spring_add_constraint_ndof1(struct Implicit_Data *data, int index, const float c1[3], const float c2[3], const float dV[3]);
+void BPH_mass_spring_add_constraint_ndof2(struct Implicit_Data *data, int index, const float c1[3], const float dV[3]);
+
+bool BPH_mass_spring_solve(struct Implicit_Data *data, float dt);
+void BPH_mass_spring_apply_result(struct Implicit_Data *data);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/source/blender/physics/intern/implicit_blender.c b/source/blender/physics/intern/implicit_blender.c
index da7f5088523..ad7d2e14f92 100644
--- a/source/blender/physics/intern/implicit_blender.c
+++ b/source/blender/physics/intern/implicit_blender.c
@@ -113,41 +113,6 @@ static double itval(void)
 static float I[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
 static float ZERO[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
 
-/* ==== hash functions for debugging ==== */
-static unsigned int hash_int_2d(unsigned int kx, unsigned int ky)
-{
-#define rot(x,k) (((x)<<(k)) | ((x)>>(32-(k))))
-
-	unsigned int a, b, c;
-
-	a = b = c = 0xdeadbeef + (2 << 2) + 13;
-	a += kx;
-	b += ky;
-
-	c ^= b; c -= rot(b,14);
-	a ^= c; a -= rot(c,11);
-	b ^= a; b -= rot(a,25);
-	c ^= b; c -= rot(b,16);
-	a ^= c; a -= rot(c,4);
-	b ^= a; b -= rot(a,14);
-	c ^= b; c -= rot(b,24);
-
-	return c;
-
-#undef rot
-}
-
-static int hash_vertex(int type, int vertex)
-{
-	return hash_int_2d((unsigned int)type, (unsigned int)vertex);
-}
-
-static int hash_collpair(int type, CollPair *collpair)
-{
-	return hash_int_2d((unsigned int)type, hash_int_2d((unsigned int)collpair->face1, (unsigned int)collpair->face2));
-}
-/* ================ */
-
 /*
 #define C99
 #ifdef C99
@@ -2048,148 +2013,6 @@ bool implicit_hair_volume_get_texture_data(Object *UNUSED(ob), ClothModifierData
 
 /* ================================ */
 
-static bool collision_response(ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, float restitution, float r_impulse[3])
-{
-	Cloth *cloth = clmd->clothObject;
-	int index = collpair->ap1;
-	bool result = false;
-	
-	float v1[3], v2_old[3], v2_new[3], v_rel_old[3], v_rel_new[3];
-	float epsilon2 = BLI_bvhtree_getepsilon(collmd->bvhtree);
-
-	float margin_distance = collpair->distance - epsilon2;
-	float mag_v_rel;
-	
-	zero_v3(r_impulse);
-	
-	if (margin_distance > 0.0f)
-		return false; /* XXX tested before already? */
-	
-	/* only handle static collisions here */
-	if ( collpair->flag & COLLISION_IN_FUTURE )
-		return false;
-	
-	/* velocity */
-	copy_v3_v3(v1, cloth->verts[index].v);
-	collision_get_collider_velocity(v2_old, v2_new, collmd, collpair);
-	/* relative velocity = velocity of the cloth point relative to the collider */
-	sub_v3_v3v3(v_rel_old, v1, v2_old);
-	sub_v3_v3v3(v_rel_new, v1, v2_new);
-	/* normal component of the relative velocity */
-	mag_v_rel = dot_v3v3(v_rel_old, collpair->normal);
-	
-	/* only valid when moving toward the collider */
-	if (mag_v_rel < -ALMOST_ZERO) {
-		float v_nor_old, v_nor_new;
-		float v_tan_old[3], v_tan_new[3];
-		float bounce, repulse;
-		
-		/* Collision response based on
-		 * "Simulating Complex Hair with Robust Collision Handling" (Choe, Choi, Ko, ACM SIGGRAPH 2005)
-		 * http://graphics.snu.ac.kr/publications/2005-choe-HairSim/Choe_2005_SCA.pdf
-		 */
-		
-		v_nor_old = mag_v_rel;
-		v_nor_new = dot_v3v3(v_rel_new, collpair->normal);
-		
-		madd_v3_v3v3fl(v_tan_old, v_rel_old, collpair->normal, -v_nor_old);
-		madd_v3_v3v3fl(v_tan_new, v_rel_new, collpair->normal, -v_nor_new);
-		
-		/* TODO repulsion forces can easily destabilize the system,
-		 * have to clamp them or construct a linear spring instead
-		 */
-//		repulse = -margin_distance / dt + dot_v3v3(v1, collpair->normal);
-		repulse = 0.0f;
-		
-		if (margin_distance < -epsilon2) {
-			bounce = -(v_nor_new + v_nor_old * restitution);
-			mul_v3_v3fl(r_impulse, collpair->normal, max_ff(repulse, bounce));
-		}
-		else {
-			bounce = 0.0f;
-			mul_v3_v3fl(r_impulse, collpair->normal, repulse);
-		}
-		
-		result = true;
-	}
-	
-	return result;
-}
-
-/* Init constraint matrix
- * This is part of the modified CG method suggested by Baraff/Witkin in
- * "Large Steps in Cloth Simulation" (Siggraph 1998)
- */
-static void setup_constraint_matrix(ClothModifierData *clmd, ColliderContacts *contacts, int totcolliders, lfVector *X, lfVector *V, fmatrix3x3 *S, lfVector *z, float dt)
-{
-	Cloth *cloth = clmd->clothObject;
-	ClothVertex *verts = cloth->verts;
-	int numverts = cloth->numverts;
-	RootTransform *roots = cloth->implicit->root;
-	int i, j, v;
-
-	for (v = 0; v < numverts; v++) {
-		S[v].c = S[v].r = v;
-		if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) {
-			/* pinned vertex constraints */
-			zero_v3(z[v]); /* velocity is defined externally */
-			zero_m3(S[v].m);
-		}
-		else {
-			/* free vertex */
-			zero_v3(z[v]);
-			unit_m3(S[v].m);
-		}
-		
-		verts[v].impulse_count = 0;
-	}
-
-	for (i = 0; i < totcolliders; ++i) {
-		ColliderContacts *ct = &contacts[i];
-		for (j = 0; j < ct->totcollisions; ++j) {
-			CollPair *collpair = &ct->collisions[j];
-//			float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - ct->ob->pd->pdef_sbdamp);
-			float restitution = 0.0f;
-			int v = collpair->face1;
-			float cnor[3], cmat[3][3];
-			float impulse[3];
-			
-			/* pinned verts handled separately */
-			if (verts[v].flags & CLOTH_VERT_FLAG_PINNED)
-				continue;
-			
-			/* calculate collision response */
-			if (!collision_response(clmd, ct->collmd, collpair, restitution, impulse))
-				continue;
-			
-			vel_world_to_root(impulse, X[v], impulse, &roots[v]);
-			add_v3_v3(z[v], impulse);
-			++verts[v].impulse_count;
-			if (verts[v].impulse_count > 1)
-				continue;
-			
-			/* modify S to enforce velocity constraint in normal direction */
-			copy_v3_v3(cnor, collpair->normal);
-			mul_transposed_m3_v3(roots[v].rot, cnor);
-			mul_fvectorT_fvector(cmat, cnor, cnor);
-			
-			sub_m3_m3m3(S[v].m, I, cmat);
-			
-			BKE_sim_debug_data_add_dot(clmd->debug_data, collpair->pa, 0, 1, 0, "collision", hash_collpair(936, collpair));
-			BKE_sim_debug_data_add_dot(clmd->debug_data, collpair->pb, 1, 0, 0, "collision", hash_collpair(937, collpair));
-			BKE_sim_debug_data_add_line(clmd->debug_data, collpair->pa, collpair->pb, 0.7, 0.7, 0.7, "collision", hash_collpair(938, collpair));
-			
-			{ /* DEBUG */
-//				float nor[3];
-//				mul_v3_v3fl(nor, collpair->normal, collpair->distance);
-//				BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, nor, 1, 1, 0, "collision", hash_collpair(939, collpair));
-				BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, impulse, 1, 1, 0, "collision", hash_collpair(940, collpair));
-//				BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, collpair->normal, 1, 1, 0, "collision", hash_collpair(941, collpair));
-			}
-		}
-	}
-}
-
 static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVector *lF, lfVector *lX, lfVector *lV, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors, float time, fmatrix3x3 *M)
 {
 	/* Collect forces and derivatives:  F, dFdX, dFdV */
@@ -2371,38 +2194,48 @@ static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), lfVec
 	// printf("\n");
 }
 
-static bool simulate_implicit_euler(Implicit_Data *id, float dt)
+bool BPH_mass_spring_solve(Implicit_Data *data, float dt)
 {
-	unsigned int numverts = id->dFdV[0].vcount;
+	unsigned int numverts = data->dFdV[0].vcount;
 	bool ok;
 
 	lfVector *dFdXmV = create_lfvector(numverts);
-	zero_lfvector(id->dV, numverts);
+	zero_lfvector(data->dV, numverts);
 
-	cp_bfmatrix(id->A, id->M);
+	cp_bfmatrix(data->A, data->M);
 
-	subadd_bfmatrixS_bfmatrixS(id->A, id->dFdV, dt, id->dFdX, (dt*dt));
+	subadd_bfmatrixS_bfmatrixS(data->A, data->dFdV, dt, data->dFdX, (dt*dt));
 
-	mul_bfmatrix_lfvector(dFdXmV, id->dFdX, id->V);
+	mul_bfmatrix_lfvector(dFdXmV, data->dFdX, data->V);
 
-	add_lfvectorS_lfvectorS(id->B, id->F, dt, dFdXmV, (dt*dt), numverts);
+	add_lfvectorS_lfvectorS(data->B, data->F, dt, dFdXmV, (dt*dt), numverts);
 
 	// itstart();
 
-	ok = cg_filtered(id->dV, id->A, id->B, id->z, id->S); /* conjugate gradient algorithm to solve Ax=b */
+	ok = cg_filtered(data->dV, data->A, data->B, data->z, data->S); /* conjugate gradient algorithm to solve Ax=b */
 	// cg_filtered_pre(id->dV, id->A, id->B, id->z, id->S, id->P, id->Pinv, id->bigI);
 
 	// itend();
 	// printf("cg_filtered calc time: %f\n", (float)itval());
 
 	// advance velocities
-	add_lfvector_lfvector(id->Vnew, id->V, id->dV, numverts);
+	add_lfvector_lfvector(data->Vnew, data->V, data->dV, numverts);
+	// advance positions
+	add_lfvector_lfvectorS(data->Xnew, data->X, data->Vnew, dt, numverts);
 
 	del_lfvector(dFdXmV);
 	
 	return ok;
 }
 
+void BPH_mass_spring_apply_result(Implicit_Data *data)
+{
+	int numverts = data->M[0].vcount;
+	cp_lfvector(data->X, data->Xnew, numverts);
+	cp_lfvector(data->V, data->Vnew, numverts);
+}
+
+
 /* computes where the cloth would be if it were subject to perfectly stiff edges
  * (edge distance constraints) in a lagrangian solver.  then add forces to help
  * guide the implicit solver to that state.  this function is called after
@@ -2482,198 +2315,6 @@ static int UNUSED_FUNCTION(cloth_calc_helper_forces)(Object *UNUSED(ob), ClothMo
 	return 1;
 }
 
-int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
-{
-	unsigned int i=0;
-	float step=0.0f, tf=clmd->sim_parms->timescale;
-	Cloth *cloth = clmd->clothObject;
-	ClothVertex *verts = cloth->verts/*, *cv*/;
-	unsigned int numverts = cloth->numverts;
-	float dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame;
-	float spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
-	/*float (*initial_cos)[3] = MEM_callocN(sizeof(float)*3*cloth->numverts, "initial_cos implicit.c");*/ /* UNUSED */
-	Implicit_Data *id = cloth->implicit;
-	ColliderContacts *contacts = NULL;
-	int totcolliders = 0;
-	
-	BKE_sim_debug_data_clear_category(clmd->debug_data, "collision");
-	
-	if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { /* do goal stuff */
-		for (i = 0; i < numverts; i++) {
-			// update velocities with constrained velocities from pinned verts
-			if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
-				float v[3];
-				sub_v3_v3v3(v, verts[i].xconst, verts[i].xold);
-				// mul_v3_fl(id->V[i], clmd->sim_parms->stepsPerFrame);
-				vel_world_to_root(id->V[i], id->X[i], v, &id->root[i]);
-			}
-		}
-	}
-	
-	if (clmd->debug_data) {
-		for (i = 0; i < numverts; i++) {
-			BKE_sim_debug_data_add_dot(clmd->debug_data, verts[i].x, 1.0f, 0.1f, 1.0f, "points", hash_vertex(583, i));
-		}
-	}
-	
-	while (step < tf) {
-		
-		/* copy velocities for collision */
-		for (i = 0; i < numverts; i++) {
-			vel_root_to_world(verts[i].tv, id->X[i], id->V[i], &id->root[i]);
-			copy_v3_v3(verts[i].v, verts[i].tv);
-		}
-		
-		/* determine contact points */
-		if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
-			if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_POINTS) {
-				cloth_find_point_contacts(ob, clmd, 0.0f, tf, &contacts, &totcolliders);
-			}
-		}
-		
-		/* setup vertex constraints for pinned vertices and contacts */
-		setup_constraint_matrix(clmd, contacts, totcolliders, id->X, id->V, id->S, id->z, dt);
-		
-		// damping velocity for artistic reasons
-		mul_lfvectorS(id->V, id->V, clmd->sim_parms->vel_damping, numverts);
-		
-		// calculate forces
-		cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M);
-		
-		// calculate new velocity
-		simulate_implicit_euler(id, dt);
-		
-		// advance positions
-		add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
-		
-		/* move pinned verts to correct position */
-		for (i = 0; i < numverts; i++) {
-			if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) {
-				if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
-					float x[3];
-					interp_v3_v3v3(x, verts[i].xold, verts[i].xconst, step + dt);
-					loc_world_to_root(id->Xnew[i], x, &id->root[i]);
-				}
-			}
-			
-			loc_root_to_world(verts[i].txold, id->X[i], &id->root[i]);
-			
-			if (!(verts[i].flags & CLOTH_VERT_FLAG_PINNED) && i > 0) {
-				BKE_sim_debug_data_add_line(clmd->debug_data, id->X[i], id->X[i-1], 0.6, 0.3, 0.3, "hair", hash_vertex(4892, i));
-				BKE_sim_debug_data_add_line(clmd->debug_data, id->Xnew[i], id->Xnew[i-1], 1, 0.5, 0.5, "hair", hash_vertex(4893, i));
-			}
-			BKE_sim_debug_data_add_vector(clmd->debug_data, id->X[i], id->V[i], 0, 0, 1, "velocity", hash_vertex(3158, i));
-		}
-		
-		/* free contact points */
-		if (contacts) {
-			cloth_free_contacts(contacts, totcolliders);
-		}
-
-#if 0
-		if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
-			bool do_extra_solve = false;
-			
-			// collisions 
-			// itstart();
-			
-			// update verts to current positions
-			for (i = 0; i < numverts; i++) {
-				copy_v3_v3(verts[i].tx, id->Xnew[i]);
-				
-				sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold);
-				copy_v3_v3(verts[i].v, verts[i].tv);
-			}
-			
-			/* unused */
-			/*for (i=0, cv=cloth->verts; i<cloth->numverts; i++, cv++) {
-				copy_v3_v3(initial_cos[i], cv->tx);
-			}*/
-			
-			if (clmd->clothObject->bvhtree) {
-				// call collision function
-				// TODO: check if "step" or "step+dt" is correct - dg
-				do_extra_solve = cloth_bvh_objcollision(ob, clmd, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale);
-			}
-			else if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_POINTS) {
-				do_extra_solve = cloth_points_objcollision(ob, clmd, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale);
-			}
-			
-			// copy corrected positions back to simulation
-			for (i = 0; i < numverts; i++) {
-				// correct velocity again, just to be sure we had to change it due to adaptive collisions
-				sub_v3_v3v3(verts[i].tv, verts[i].tx, id->X[i]);
-			}
-			
-			/* unused */
-			/*if (do_extra_solve)
-				cloth_calc_helper_forces(ob, clmd, initial_cos, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale);*/
-			
-			if (do_extra_solve) {
-				for (i = 0; i < numverts; i++) {				
-					if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED))
-						continue;
-					
-					copy_v3_v3(id->Xnew[i], verts[i].tx);
-					copy_v3_v3(id->Vnew[i], verts[i].tv);
-					mul_v3_fl(id->Vnew[i], spf);
-				}
-			}
-			
-			// X = Xnew;
-			cp_lfvector(id->X, id->Xnew, numverts);
-			
-			// if there were collisions, advance the velocity from v_n+1/2 to v_n+1
-			
-			if (do_extra_solve) {
-				// V = Vnew;
-				cp_lfvector(id->V, id->Vnew, numverts);
-				
-				// calculate 
-				cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step+dt, id->M);
-				
-				simulate_implicit_euler(id, dt / 2.0f);
-			}
-		}
-		else {
-			// X = Xnew;
-			cp_lfvector(id->X, id->Xnew, numverts);
-		}
-		
-		// itend();
-		// printf("collision time: %f\n", (float)itval());
-#else
-		// X = Xnew;
-		cp_lfvector(id->X, id->Xnew, numverts);
-#endif
-		
-		// V = Vnew;
-		cp_lfvector(id->V, id->Vnew, numverts);
-		
-		step += dt;
-	}
-	
-	for (i = 0; i < numverts; i++) {
-		if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED)) {
-			copy_v3_v3(verts[i].txold, verts[i].xconst); // TODO: test --> should be .x
-			copy_v3_v3(verts[i].x, verts[i].xconst);
-			
-			vel_root_to_world(verts[i].v, id->X[i], id->V[i], &id->root[i]);
-		}
-		else {
-			loc_root_to_world(verts[i].txold, id->X[i], &id->root[i]);
-			copy_v3_v3(verts[i].x, verts[i].txold);
-			
-			vel_root_to_world(verts[i].v, id->X[i], id->V[i], &id->root[i]);
-		}
-	}
-	
-	/* unused */
-	/*MEM_freeN(initial_cos);*/
-	
-	return 1;
-}
-
 void BPH_mass_spring_set_root_motion(Implicit_Data *data, int index, const float loc[3], const float vel[3], float rot[3][3], const float angvel[3])
 {
 	RootTransform *root = &data->root[index];
@@ -2703,11 +2344,29 @@ void BPH_mass_spring_set_root_motion(Implicit_Data *data, int index, const float
 void BPH_mass_spring_set_motion_state(Implicit_Data *data, int index, const float x[3], const float v[3])
 {
 	RootTransform *root = &data->root[index];
-	
 	loc_world_to_root(data->X[index], x, root);
 	vel_world_to_root(data->V[index], data->X[index], v, root);
 }
 
+void BPH_mass_spring_set_position(Implicit_Data *data, int index, const float x[3])
+{
+	RootTransform *root = &data->root[index];
+	loc_world_to_root(data->X[index], x, root);
+}
+
+void BPH_mass_spring_set_velocity(Implicit_Data *data, int index, const float v[3])
+{
+	RootTransform *root = &data->root[index];
+	vel_world_to_root(data->V[index], data->X[index], v, root);
+}
+
+void BPH_mass_spring_get_motion_state(struct Implicit_Data *data, int index, float x[3], float v[3])
+{
+	RootTransform *root = &data->root[index];
+	if (x) loc_root_to_world(x, data->X[index], root);
+	if (v) vel_root_to_world(v, data->X[index], data->V[index], root);
+}
+
 void BPH_mass_spring_set_vertex_mass(Implicit_Data *data, int index, float mass)
 {
 	unit_m3(data->M[index].m);
@@ -2730,4 +2389,65 @@ int BPH_mass_spring_init_spring(Implicit_Data *data, int index, int v1, int v2)
 	return s;
 }
 
+void BPH_mass_spring_clear_constraints(Implicit_Data *data)
+{
+	int i, numverts = data->S[0].vcount;
+	for (i = 0; i < numverts; ++i) {
+		unit_m3(data->S[i].m);
+		zero_v3(data->z[i]);
+	}
+}
+
+void BPH_mass_spring_add_constraint_ndof0(Implicit_Data *data, int index, const float dV[3])
+{
+	RootTransform *root = &data->root[index];
+	
+	zero_m3(data->S[index].m);
+	
+	copy_v3_v3(data->z[index], dV);
+	mul_transposed_m3_v3(root->rot, data->z[index]);
+}
+
+void BPH_mass_spring_add_constraint_ndof1(Implicit_Data *data, int index, const float c1[3], const float c2[3], const float dV[3])
+{
+	RootTransform *root = &data->root[index];
+	float m[3][3], p[3], q[3], u[3], cmat[3][3];
+	
+	copy_v3_v3(p, c1);
+	mul_transposed_m3_v3(root->rot, p);
+	mul_fvectorT_fvector(cmat, p, p);
+	sub_m3_m3m3(m, I, cmat);
+	
+	copy_v3_v3(q, c2);
+	mul_transposed_m3_v3(root->rot, q);
+	mul_fvectorT_fvector(cmat, q, q);
+	sub_m3_m3m3(m, m, cmat);
+	
+	/* XXX not sure but multiplication should work here */
+	copy_m3_m3(data->S[index].m, m);
+//	mul_m3_m3m3(data->S[index].m, data->S[index].m, m);
+	
+	copy_v3_v3(u, dV);
+	mul_transposed_m3_v3(root->rot, u);
+	add_v3_v3(data->z[index], u);
+}
+
+void BPH_mass_spring_add_constraint_ndof2(Implicit_Data *data, int index, const float c1[3], const float dV[3])
+{
+	RootTransform *root = &data->root[index];
+	float m[3][3], p[3], u[3], cmat[3][3];
+	
+	copy_v3_v3(p, c1);
+	mul_transposed_m3_v3(root->rot, p);
+	mul_fvectorT_fvector(cmat, p, p);
+	sub_m3_m3m3(m, I, cmat);
+	
+	copy_m3_m3(data->S[index].m, m);
+//	mul_m3_m3m3(data->S[index].m, data->S[index].m, m);
+	
+	copy_v3_v3(u, dV);
+	mul_transposed_m3_v3(root->rot, u);
+	add_v3_v3(data->z[index], u);
+}
+
 #endif /* IMPLICIT_SOLVER_BLENDER */
diff --git a/source/blender/physics/intern/implicit_eigen.cpp b/source/blender/physics/intern/implicit_eigen.cpp
index 230ca6f4f6a..d391907743a 100644
--- a/source/blender/physics/intern/implicit_eigen.cpp
+++ b/source/blender/physics/intern/implicit_eigen.cpp
@@ -79,42 +79,6 @@ extern "C" {
 #include "BPH_mass_spring.h"
 }
 
-/* ==== hash functions for debugging ==== */
-static unsigned int hash_int_2d(unsigned int kx, unsigned int ky)
-{
-#define rot(x,k) (((x)<<(k)) | ((x)>>(32-(k))))
-
-	unsigned int a, b, c;
-
-	a = b = c = 0xdeadbeef + (2 << 2) + 13;
-	a += kx;
-	b += ky;
-
-	c ^= b; c -= rot(b,14);
-	a ^= c; a -= rot(c,11);
-	b ^= a; b -= rot(a,25);
-	c ^= b; c -= rot(b,16);
-	a ^= c; a -= rot(c,4);
-	b ^= a; b -= rot(a,14);
-	c ^= b; c -= rot(b,24);
-
-	return c;
-
-#undef rot
-}
-
-static int hash_vertex(int type, int vertex)
-{
-	return hash_int_2d((unsigned int)type, (unsigned int)vertex);
-}
-
-static int hash_collpair(int type, CollPair *collpair)
-{
-	return hash_int_2d((unsigned int)type, hash_int_2d((unsigned int)collpair->face1, (unsigned int)collpair->face2));
-}
-/* ================ */
-
-
 typedef float Scalar;
 
 typedef Eigen::SparseMatrix<Scalar> lMatrix;