Preparation for collision code fixing.

Instead of handling contact tests and collision response in the same
function in collision.c, first generate contact points and return them
as a list, then free at the end of the stepping function. This way the
contact response can be integrated into the conjugate gradient method
properly instead of using the hackish and unstable double evaluation
that is currently used.

3 files changed, 167 insertions, 1 deletions

diff --git a/source/blender/blenkernel/BKE_cloth.h b/source/blender/blenkernel/BKE_cloth.h
index e4fa0b0648b..151a2b58ad3 100644
--- a/source/blender/blenkernel/BKE_cloth.h
+++ b/source/blender/blenkernel/BKE_cloth.h
@@ -41,6 +41,7 @@ struct Scene;
 struct MFace;
 struct DerivedMesh;
 struct ClothModifierData;
+struct CollisionModifierData;
 struct CollisionTree;
 struct VoxelData;
 
@@ -182,10 +183,24 @@ typedef enum {
 // collision.c
 ////////////////////////////////////////////////
 
+struct CollPair;
+
+typedef struct ColliderContacts {
+	struct Object *ob;
+	struct CollisionModifierData *collmd;
+	
+	struct CollPair *collisions;
+	int totcollisions;
+} ColliderContacts;
+
 // needed for implicit.c
 int cloth_bvh_objcollision (struct Object *ob, struct ClothModifierData *clmd, float step, float dt );
 int cloth_points_objcollision(struct Object *ob, struct ClothModifierData *clmd, float step, float dt);
 
+void cloth_find_point_contacts(struct Object *ob, struct ClothModifierData *clmd, float step, float dt,
+                               ColliderContacts **r_collider_contacts, int *r_totcolliders);
+void cloth_free_contacts(ColliderContacts *collider_contacts, int totcolliders);
+
 ////////////////////////////////////////////////
 
 
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c
index 2c0939359e0..427b76ab778 100644
--- a/source/blender/blenkernel/intern/collision.c
+++ b/source/blender/blenkernel/intern/collision.c
@@ -1364,3 +1364,135 @@ int cloth_points_objcollision(Object *ob, ClothModifierData *clmd, float step, f
 
 	return 1|MIN2 ( ret, 1 );
 }
+
+void cloth_find_point_contacts(Object *ob, ClothModifierData *clmd, float step, float dt,
+                               ColliderContacts **r_collider_contacts, int *r_totcolliders)
+{
+	Cloth *cloth= clmd->clothObject;
+	BVHTree *cloth_bvh;
+	unsigned int i=0, numverts = 0;
+	ClothVertex *verts = NULL;
+	
+	ColliderContacts *collider_contacts;
+	
+	Object **collobjs = NULL;
+	unsigned int numcollobj = 0;
+	
+	verts = cloth->verts;
+	numverts = cloth->numverts;
+	
+	////////////////////////////////////////////////////////////
+	// static collisions
+	////////////////////////////////////////////////////////////
+	
+	// create temporary cloth points bvh
+	cloth_bvh = BLI_bvhtree_new(numverts, MAX2(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6);
+	/* fill tree */
+	for (i = 0; i < numverts; i++) {
+		float co[6];
+		
+		copy_v3_v3(&co[0*3], verts[i].x);
+		copy_v3_v3(&co[1*3], verts[i].tx);
+		
+		BLI_bvhtree_insert(cloth_bvh, i, co, 2);
+	}
+	/* balance tree */
+	BLI_bvhtree_balance(cloth_bvh);
+	
+	collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
+	if (!collobjs) {
+		*r_collider_contacts = NULL;
+		*r_totcolliders = 0;
+		return;
+	}
+	
+	/* move object to position (step) in time */
+	for (i = 0; i < numcollobj; i++) {
+		Object *collob= collobjs[i];
+		CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
+		if (!collmd->bvhtree)
+			continue;
+		
+		/* move object to position (step) in time */
+		collision_move_object ( collmd, step + dt, step );
+	}
+	
+	collider_contacts = MEM_callocN(sizeof(ColliderContacts) * numcollobj, "CollPair");
+	
+	// check all collision objects
+	for (i = 0; i < numcollobj; i++) {
+		ColliderContacts *ct = collider_contacts + i;
+		Object *collob= collobjs[i];
+		CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
+		BVHTreeOverlap *overlap;
+		unsigned int result = 0;
+		float epsilon;
+		
+		ct->ob = collob;
+		ct->collmd = collmd;
+		ct->collisions = NULL;
+		ct->totcollisions = 0;
+		
+		if (!collmd->bvhtree)
+			continue;
+		
+		/* search for overlapping collision pairs */
+		overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result);
+		epsilon = BLI_bvhtree_getepsilon(collmd->bvhtree);
+		
+		// go to next object if no overlap is there
+		if (result && overlap) {
+			CollPair *collisions_index;
+			
+			/* check if collisions really happen (costly near check) */
+			cloth_points_objcollisions_nearcheck(clmd, collmd, &ct->collisions, &collisions_index,
+			                                     result, overlap, epsilon, dt);
+			ct->totcollisions = (int)(collisions_index - ct->collisions);
+			
+			// resolve nearby collisions
+//			ret += cloth_points_objcollisions_resolve(clmd, collmd, collob->pd, collisions[i], collisions_index[i], dt);
+		}
+		
+		if (overlap)
+			MEM_freeN(overlap);
+	}
+	
+	if (collobjs)
+		MEM_freeN(collobjs);
+
+	BLI_bvhtree_free(cloth_bvh);
+	
+	////////////////////////////////////////////////////////////
+	// update positions
+	// this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
+	////////////////////////////////////////////////////////////
+	
+	// verts come from clmd
+	for (i = 0; i < numverts; i++) {
+		if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) {
+			if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
+				continue;
+			}
+		}
+		
+		VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
+	}
+	////////////////////////////////////////////////////////////
+	
+	*r_collider_contacts = collider_contacts;
+	*r_totcolliders = numcollobj;
+}
+
+void cloth_free_contacts(ColliderContacts *collider_contacts, int totcolliders)
+{
+	if (collider_contacts) {
+		int i;
+		for (i = 0; i < totcolliders; ++i) {
+			ColliderContacts *ct = collider_contacts + i;
+			if (ct->collisions) {
+				MEM_freeN(ct->collisions);
+			}
+		}
+		MEM_freeN(collider_contacts);
+	}
+}
diff --git a/source/blender/blenkernel/intern/implicit.c b/source/blender/blenkernel/intern/implicit.c
index c0058d361ae..2a50cde9e66 100644
--- a/source/blender/blenkernel/intern/implicit.c
+++ b/source/blender/blenkernel/intern/implicit.c
@@ -1968,6 +1968,8 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 	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");
 
@@ -1991,6 +1993,13 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 		}
 	}
 	
+	/* 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);
+		}
+	}
+	
 	while (step < tf) {
 		// damping velocity for artistic reasons
 		mul_lfvectorS(id->V, id->V, clmd->sim_parms->vel_damping, numverts);
@@ -2019,6 +2028,7 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 			copy_v3_v3(verts[i].txold, id->X[i]);
 		}
 
+#if 0
 		if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
 			bool do_extra_solve = false;
 			
@@ -2090,13 +2100,17 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 		
 		// 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
@@ -2110,6 +2124,11 @@ int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *
 		}
 	}
 	
+	/* free contact points */
+	if (contacts) {
+		cloth_free_contacts(contacts, totcolliders);
+	}
+	
 	/* unused */
 	/*MEM_freeN(initial_cos);*/