3 files changed, 168 insertions, 1 deletions

diff --git a/source/blender/blenkernel/BKE_effect.h b/source/blender/blenkernel/BKE_effect.h
index d5e54d849cd..d6991a473b0 100644
--- a/source/blender/blenkernel/BKE_effect.h
+++ b/source/blender/blenkernel/BKE_effect.h
@@ -136,6 +136,32 @@ int get_effector_data(struct EffectorCache *eff, struct EffectorData *efd, struc
 /* EffectorData->flag */
 #define PE_VELOCITY_TO_IMPULSE  1
 
+/* ======== Simulation Debugging ======== */
+
+typedef struct SimDebugElement {
+	int type;
+	int hash;
+	float color[3];
+	
+	float v1[3], v2[3];
+} SimDebugElement;
+
+typedef enum eSimDebugElement_Type {
+	SIM_DEBUG_ELEM_DOT,
+	SIM_DEBUG_ELEM_LINE,
+	SIM_DEBUG_ELEM_VECTOR,
+} eSimDebugElement_Type;
+
+typedef struct SimDebugData {
+	struct GHash *gh;
+} SimDebugData;
+
+struct SimDebugData *BKE_sim_debug_data_new(void);
+void BKE_sim_debug_data_add_dot(struct SimDebugData *debug_data, const float p[3], float r, float g, float b, int hash);
+void BKE_sim_debug_data_add_line(struct SimDebugData *debug_data, const float p1[3], const float p2[3], float r, float g, float b, int hash);
+void BKE_sim_debug_data_add_vector(struct SimDebugData *debug_data, const float p[3], const float d[3], float r, float g, float b, int hash);
+void BKE_sim_debug_data_clear(struct SimDebugData *debug_data);
+void BKE_sim_debug_data_free(struct SimDebugData *debug_data);
 
 #endif
 
diff --git a/source/blender/blenkernel/intern/effect.c b/source/blender/blenkernel/intern/effect.c
index ced9da8d0b1..94b4af10201 100644
--- a/source/blender/blenkernel/intern/effect.c
+++ b/source/blender/blenkernel/intern/effect.c
@@ -51,6 +51,7 @@
 #include "BLI_noise.h"
 #include "BLI_rand.h"
 #include "BLI_utildefines.h"
+#include "BLI_ghash.h"
 
 #include "PIL_time.h"
 
@@ -1024,3 +1025,107 @@ void pdDoEffectors(ListBase *effectors, ListBase *colliders, EffectorWeights *we
 		}
 	}
 }
+
+/* ======== Simulation Debugging ======== */
+
+static unsigned int debug_element_hash(const void *key)
+{
+	const SimDebugElement *elem = key;
+	return elem->hash;
+}
+
+static int debug_element_compare(const void *a, const void *b)
+{
+	const SimDebugElement *elem1 = a;
+	const SimDebugElement *elem2 = b;
+
+	if (elem1->hash == elem2->hash) {
+		return 0;
+	}
+	return 1;
+}
+
+static void debug_element_free(void *val)
+{
+	SimDebugElement *elem = val;
+	MEM_freeN(elem);
+}
+
+SimDebugData *BKE_sim_debug_data_new(void)
+{
+	SimDebugData *debug_data = MEM_callocN(sizeof(SimDebugData), "sim debug data");
+	debug_data->gh = BLI_ghash_new(debug_element_hash, debug_element_compare, "sim debug element hash");
+	return debug_data;
+	
+}
+
+static void debug_data_insert(SimDebugData *debug_data, SimDebugElement *elem)
+{
+	SimDebugElement *old_elem = BLI_ghash_lookup(debug_data->gh, elem);
+	if (old_elem) {
+		*old_elem = *elem;
+		MEM_freeN(elem);
+	}
+	else
+		BLI_ghash_insert(debug_data->gh, elem, elem);
+}
+
+void BKE_sim_debug_data_add_dot(struct SimDebugData *debug_data, const float p[3], float r, float g, float b, int hash)
+{
+	SimDebugElement *elem = MEM_callocN(sizeof(SimDebugElement), "sim debug data element");
+	elem->type = SIM_DEBUG_ELEM_DOT;
+	elem->hash = hash;
+	elem->color[0] = r;
+	elem->color[1] = g;
+	elem->color[2] = b;
+	copy_v3_v3(elem->v1, p);
+	
+	debug_data_insert(debug_data, elem);
+}
+
+void BKE_sim_debug_data_add_line(struct SimDebugData *debug_data, const float p1[3], const float p2[3], float r, float g, float b, int hash)
+{
+	SimDebugElement *elem = MEM_callocN(sizeof(SimDebugElement), "sim debug data element");
+	elem->type = SIM_DEBUG_ELEM_LINE;
+	elem->hash = hash;
+	elem->color[0] = r;
+	elem->color[1] = g;
+	elem->color[2] = b;
+	copy_v3_v3(elem->v1, p1);
+	copy_v3_v3(elem->v2, p2);
+	
+	debug_data_insert(debug_data, elem);
+}
+
+void BKE_sim_debug_data_add_vector(struct SimDebugData *debug_data, const float p[3], const float d[3], float r, float g, float b, int hash)
+{
+	SimDebugElement *elem = MEM_callocN(sizeof(SimDebugElement), "sim debug data element");
+	elem->type = SIM_DEBUG_ELEM_VECTOR;
+	elem->hash = hash;
+	elem->color[0] = r;
+	elem->color[1] = g;
+	elem->color[2] = b;
+	copy_v3_v3(elem->v1, p);
+	copy_v3_v3(elem->v2, d);
+	
+	debug_data_insert(debug_data, elem);
+}
+
+void BKE_sim_debug_data_clear(SimDebugData *debug_data)
+{
+	if (!debug_data)
+		return;
+	
+	if (debug_data->gh)
+		BLI_ghash_clear(debug_data->gh, NULL, debug_element_free);
+}
+
+void BKE_sim_debug_data_free(SimDebugData *debug_data)
+{
+	if (!debug_data)
+		return;
+	
+	if (debug_data->gh)
+		BLI_ghash_free(debug_data->gh, NULL, debug_element_free);
+	MEM_freeN(debug_data);
+}
diff --git a/source/blender/blenkernel/intern/implicit.c b/source/blender/blenkernel/intern/implicit.c
index 13403795745..d0f0bc9455a 100644
--- a/source/blender/blenkernel/intern/implicit.c
+++ b/source/blender/blenkernel/intern/implicit.c
@@ -109,6 +109,36 @@ 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);
+}
+/* ================ */
+
 /*
 #define C99
 #ifdef C99
@@ -1922,7 +1952,7 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 	unsigned int i=0;
 	float step=0.0f, tf=clmd->sim_parms->timescale;
 	Cloth *cloth = clmd->clothObject;
-	ClothVertex *verts = cloth->verts, *cv;
+	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;
@@ -1943,6 +1973,12 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 		}
 	}
 	
+	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, hash_vertex(583, i));
+		}
+	}
+	
 	while (step < tf) {
 		// damping velocity for artistic reasons
 		mul_lfvectorS(id->V, id->V, clmd->sim_parms->vel_damping, numverts);