Use the S matrix of the modifier CG algorithm for implementing collision

responses.

The S matrix together with the z Vector encodes the degrees of freedom
of a colliding hair point and the target velocity change. In a collision
the hair vertex is restricted in the normal direction (when moving
toward the collider) and the collision dynamics define target velocity.

3 files changed, 126 insertions, 18 deletions

diff --git a/source/blender/blenkernel/BKE_cloth.h b/source/blender/blenkernel/BKE_cloth.h
index 151a2b58ad3..df118276809 100644
--- a/source/blender/blenkernel/BKE_cloth.h
+++ b/source/blender/blenkernel/BKE_cloth.h
@@ -44,6 +44,7 @@ struct ClothModifierData;
 struct CollisionModifierData;
 struct CollisionTree;
 struct VoxelData;
+struct PartDeflect;
 
 #define DO_INLINE MALWAYS_INLINE
 
@@ -201,6 +202,9 @@ void cloth_find_point_contacts(struct Object *ob, struct ClothModifierData *clmd
                                ColliderContacts **r_collider_contacts, int *r_totcolliders);
 void cloth_free_contacts(ColliderContacts *collider_contacts, int totcolliders);
 
+bool cloth_points_collpair_response(struct ClothModifierData *clmd, struct CollisionModifierData *collmd, struct PartDeflect *pd,
+                                    struct CollPair *collpair, float dt, float r_impulse[3]);
+
 ////////////////////////////////////////////////
 
 
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c
index 5fabfaf4632..78cb468fc54 100644
--- a/source/blender/blenkernel/intern/collision.c
+++ b/source/blender/blenkernel/intern/collision.c
@@ -958,6 +958,82 @@ BLI_INLINE void max_v3_v3v3(float r[3], const float a[3], const float b[3])
 	r[2] = max_ff(a[2], b[2]);
 }
 
+bool cloth_points_collpair_response(ClothModifierData *clmd, CollisionModifierData *collmd, PartDeflect *pd, CollPair *collpair, float dt, float r_impulse[3])
+{
+	bool result = false;
+	float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - pd->pdef_sbdamp);
+	Cloth *cloth1 = clmd->clothObject;
+	
+	float u1, u2, u3;
+	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;
+	
+	/* compute barycentric coordinates */
+	collision_compute_barycentric(collpair->pb,
+	                              collmd->current_x[collpair->bp1].co,
+	                              collmd->current_x[collpair->bp2].co,
+	                              collmd->current_x[collpair->bp3].co,
+	                              &u1, &u2, &u3 );
+	
+	/* Calculate relative velocity */
+	copy_v3_v3(v1, cloth1->verts[collpair->ap1].tv);
+	
+	collision_interpolateOnTriangle(v2_new, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3);
+	/* XXX assume constant velocity of the collider for now */
+	copy_v3_v3(v2_old, v2_new);
+	
+	/* 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);
+		
+		repulse = -margin_distance / dt + dot_v3v3(v1, collpair->normal);
+		
+		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;
+}
+
 static bool cloth_points_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, PartDeflect *pd,
                                                   CollPair *collpair, CollPair *collision_end, float dt)
 {
diff --git a/source/blender/blenkernel/intern/implicit.c b/source/blender/blenkernel/intern/implicit.c
index 27e1f6d500a..45a8ff5de93 100644
--- a/source/blender/blenkernel/intern/implicit.c
+++ b/source/blender/blenkernel/intern/implicit.c
@@ -349,8 +349,10 @@ static void print_sparse_matrix(fmatrix3x3 *m)
 {
 	if (m) {
 		unsigned int i;
-		for (i = 0; i < m[0].vcount + m[0].scount; i++)
+		for (i = 0; i < m[0].vcount + m[0].scount; i++) {
+			printf("%d:\n", i);
 			print_fmatrix(m[i].m);
+		}
 	}
 }
 #endif
@@ -1756,23 +1758,30 @@ bool implicit_hair_volume_get_texture_data(Object *UNUSED(ob), ClothModifierData
 
 /* ================================ */
 
-/* Init constraint matrix */
-static void setup_constraint_matrix(ClothVertex *verts, int numverts, ColliderContacts *contacts, int totcolliders, fmatrix3x3 *S, SimDebugData *debug_data)
+/* 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 *V, fmatrix3x3 *S, lfVector *z, float dt)
 {
-	int i, j;
+	ClothVertex *verts = clmd->clothObject->verts;
+	int numverts = clmd->clothObject->numverts;
+	int i, j, v;
 
 	/* Clear matrix from old vertex constraints */
-	for (i = 0; i < S[0].vcount; i++)
-		S[i].c = S[i].r = 0;
+	for (v = 0; v < S[0].vcount; v++)
+		S[v].c = S[v].r = 0;
 
 	/* pinned vertex constraints */
-	for (i = 0; i < numverts; i++) {
-		S[i].c = S[i].r = i;
-		if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
-			zero_m3(S[i].m);
+	for (v = 0; v < numverts; v++) {
+		S[v].c = S[v].r = v;
+		if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) {
+			negate_v3_v3(z[v], verts[v].v);
+			zero_m3(S[v].m);
 		}
 		else {
-			unit_m3(S[i].m);
+			zero_v3(z[v]);
+			unit_m3(S[v].m);
 		}
 	}
 
@@ -1782,21 +1791,34 @@ static void setup_constraint_matrix(ClothVertex *verts, int numverts, ColliderCo
 			CollPair *collpair = &ct->collisions[j];
 			int v = collpair->face1;
 			float cmat[3][3];
+			float impulse[3];
 			
 			/* pinned verts handled separately */
 			if (verts[v].flags & CLOTH_VERT_FLAG_PINNED)
 				continue;
 			
+			/* calculate collision response */
+//			if (!cloth_points_collpair_response(clmd, ct->collmd, ct->ob->pd, collpair, dt, impulse))
+//				continue;
+			cloth_points_collpair_response(clmd, ct->collmd, ct->ob->pd, collpair, dt, impulse);
+			
+//			add_v3_v3(z[v], impulse);
+			negate_v3_v3(z[v], V[v]);
+//			sub_v3_v3(z[v], verts[v].v);
+			
+			/* modify S to enforce velocity constraint in normal direction */
 			mul_fvectorT_fvector(cmat, collpair->normal, collpair->normal);
 			sub_m3_m3m3(S[v].m, I, cmat);
 			
-			BKE_sim_debug_data_add_dot(debug_data, collpair->pa, 0, 1, 0, "collision", hash_collpair(936, collpair));
-			BKE_sim_debug_data_add_dot(debug_data, collpair->pb, 1, 0, 0, "collision", hash_collpair(937, collpair));
-			BKE_sim_debug_data_add_line(debug_data, collpair->pa, collpair->pb, 0.7, 0.7, 0.7, "collision", hash_collpair(938, collpair));
+			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));
 			{
-				float nor[3];
-				mul_v3_v3fl(nor, collpair->normal, collpair->distance);
-				BKE_sim_debug_data_add_vector(debug_data, collpair->pb, nor, 1, 1, 0, "collision", hash_collpair(939, collpair));
+//				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));
 			}
 		}
 	}
@@ -2096,6 +2118,12 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 	
 	while (step < tf) {
 		
+		/* copy velocities for collision */
+		for (i = 0; i < numverts; i++) {
+			copy_v3_v3(verts[i].tv, id->V[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) {
@@ -2104,7 +2132,7 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 		}
 		
 		/* setup vertex constraints for pinned vertices and contacts */
-		setup_constraint_matrix(verts, cloth->numverts, contacts, totcolliders, id->S, clmd->debug_data);
+		setup_constraint_matrix(clmd, contacts, totcolliders, id->V, id->S, id->z, dt);
 		
 		// damping velocity for artistic reasons
 		mul_lfvectorS(id->V, id->V, clmd->sim_parms->vel_damping, numverts);