Some initial collision code, without actual response forces still.

This is still using the old BVH tree collision methods to generate
contact points, similar to what cloth does. This should be replaced
by a Bullet collision check, but generating contacts in this way is
easier for now, and lets us test responses and stability (although in
more complex collision cases the BVH method fails utterly, beside being
terribly inefficient with many colliders).

1 files changed, 432 insertions, 2 deletions

diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c
index 3030ee4b4a4..b38609e3a31 100644
--- a/source/blender/blenkernel/intern/collision.c
+++ b/source/blender/blenkernel/intern/collision.c
@@ -652,8 +652,8 @@ static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, Collis
 	*collisions_index = *collisions;
 
 	for ( i = 0; i < numresult; i++ ) {
-		*collisions_index = cloth_collision ( (ModifierData *)clmd, (ModifierData *)collmd,
-		                                      overlap+i, *collisions_index, dt );
+		*collisions_index = cloth_collision((ModifierData *)clmd, (ModifierData *)collmd,
+		                                    overlap+i, *collisions_index, dt);
 	}
 }
 
@@ -913,3 +913,433 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa
 
 	return 1|MIN2 ( ret, 1 );
 }
+
+
+static int cloth_points_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end)
+{
+	int result = 0;
+	Cloth *cloth1;
+	float w1, w2, u1, u2, u3;
+	float v1[3], v2[3], relativeVelocity[3];
+	float magrelVel;
+	float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
+
+	cloth1 = clmd->clothObject;
+
+	for ( ; collpair != collision_end; collpair++ ) {
+//		float i1[3], i2[3], i3[3];
+
+//		zero_v3(i1);
+//		zero_v3(i2);
+//		zero_v3(i3);
+
+		/* only handle static collisions here */
+		if ( collpair->flag & COLLISION_IN_FUTURE )
+			continue;
+
+		/* compute barycentric coordinates for both collision points */
+		w1 = 1.0f - collpair->time;
+		w2 = collpair->time;
+
+		/* was: txold */
+		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, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
+
+		sub_v3_v3v3(relativeVelocity, v2, v1);
+
+		/* Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). */
+		magrelVel = dot_v3v3(relativeVelocity, collpair->normal);
+
+		/* printf("magrelVel: %f\n", magrelVel); */
+
+		/* Calculate masses of points.
+		 * TODO */
+
+#if 0
+		/* If v_n_mag < 0 the edges are approaching each other. */
+		if ( magrelVel > ALMOST_ZERO ) {
+			/* Calculate Impulse magnitude to stop all motion in normal direction. */
+			float magtangent = 0, repulse = 0, d = 0;
+			double impulse = 0.0;
+			float vrel_t_pre[3];
+			float temp[3], spf;
+
+			/* calculate tangential velocity */
+			copy_v3_v3 ( temp, collpair->normal );
+			mul_v3_fl(temp, magrelVel);
+			sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
+
+			/* Decrease in magnitude of relative tangential velocity due to coulomb friction
+			 * in original formula "magrelVel" should be the "change of relative velocity in normal direction" */
+			magtangent = min_ff(clmd->coll_parms->friction * 0.01f * magrelVel, len_v3(vrel_t_pre));
+
+			/* Apply friction impulse. */
+			if ( magtangent > ALMOST_ZERO ) {
+				normalize_v3(vrel_t_pre);
+
+				impulse = magtangent / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); /* 2.0 * */
+				VECADDMUL ( i1, vrel_t_pre, w1 * impulse );
+				VECADDMUL ( i2, vrel_t_pre, w2 * impulse );
+				VECADDMUL ( i3, vrel_t_pre, w3 * impulse );
+			}
+
+			/* Apply velocity stopping impulse
+			 * I_c = m * v_N / 2.0
+			 * no 2.0 * magrelVel normally, but looks nicer DG */
+			impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
+
+			VECADDMUL ( i1, collpair->normal, w1 * impulse );
+			cloth1->verts[collpair->ap1].impulse_count++;
+
+			VECADDMUL ( i2, collpair->normal, w2 * impulse );
+			cloth1->verts[collpair->ap2].impulse_count++;
+
+			VECADDMUL ( i3, collpair->normal, w3 * impulse );
+			cloth1->verts[collpair->ap3].impulse_count++;
+
+			/* Apply repulse impulse if distance too short
+			 * I_r = -min(dt*kd, m(0, 1d/dt - v_n))
+			 * DG: this formula ineeds to be changed for this code since we apply impulses/repulses like this:
+			 * v += impulse; x_new = x + v;
+			 * We don't use dt!!
+			 * DG TODO: Fix usage of dt here! */
+			spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
+
+			d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;
+			if ( ( magrelVel < 0.1f*d*spf ) && ( d > ALMOST_ZERO ) ) {
+				repulse = MIN2 ( d*1.0f/spf, 0.1f*d*spf - magrelVel );
+
+				/* stay on the safe side and clamp repulse */
+				if ( impulse > ALMOST_ZERO )
+					repulse = min_ff( repulse, 5.0*impulse );
+				repulse = max_ff(impulse, repulse);
+
+				impulse = repulse / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); /* original 2.0 / 0.25 */
+				VECADDMUL ( i1, collpair->normal,  impulse );
+				VECADDMUL ( i2, collpair->normal,  impulse );
+				VECADDMUL ( i3, collpair->normal,  impulse );
+			}
+
+			result = 1;
+		}
+		else {
+			/* Apply repulse impulse if distance too short
+			 * I_r = -min(dt*kd, max(0, 1d/dt - v_n))
+			 * DG: this formula ineeds to be changed for this code since we apply impulses/repulses like this:
+			 * v += impulse; x_new = x + v;
+			 * We don't use dt!! */
+			float spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
+
+			float d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - (float)collpair->distance;
+			if ( d > ALMOST_ZERO) {
+				/* stay on the safe side and clamp repulse */
+				float repulse = d*1.0f/spf;
+
+				float impulse = repulse / ( 3.0f * ( 1.0f + w1*w1 + w2*w2 + w3*w3 )); /* original 2.0 / 0.25 */
+
+				VECADDMUL ( i1, collpair->normal,  impulse );
+				VECADDMUL ( i2, collpair->normal,  impulse );
+				VECADDMUL ( i3, collpair->normal,  impulse );
+
+				cloth1->verts[collpair->ap1].impulse_count++;
+				cloth1->verts[collpair->ap2].impulse_count++;
+				cloth1->verts[collpair->ap3].impulse_count++;
+
+				result = 1;
+			}
+		}
+
+		if (result) {
+			int i = 0;
+
+			for (i = 0; i < 3; i++) {
+				if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[i]) < ABS(i1[i]))
+					cloth1->verts[collpair->ap1].impulse[i] = i1[i];
+
+				if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[i]) < ABS(i2[i]))
+					cloth1->verts[collpair->ap2].impulse[i] = i2[i];
+
+				if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[i]) < ABS(i3[i]))
+					cloth1->verts[collpair->ap3].impulse[i] = i3[i];
+			}
+		}
+#endif
+	}
+	return result;
+}
+
+BLI_INLINE void face_normal(float co1[3], float co2[3], float co3[3], float nor[3])
+{
+	float p[3], q[3];
+	sub_v3_v3v3(p, co2, co1);
+	sub_v3_v3v3(q, co3, co1);
+	cross_v3_v3v3(nor, p, q);
+	normalize_v3(nor);
+}
+
+static CollPair *cloth_point_collpair(float p1[3], float p2[3], MVert *mverts, int bp1, int bp2, int bp3,
+                                      int index_cloth, int index_coll, CollPair *collpair)
+{
+	float *co1 = mverts[bp1].co, *co2 = mverts[bp2].co, *co3 = mverts[bp3].co;
+	float lambda, uv[2];
+	float fnor[3], vec[3];
+	float w[3];
+	
+	if (!isect_line_tri_v3(p1, p2, co1, co2, co3, &lambda, uv))
+		return collpair;
+	
+	face_normal(co1, co2, co3, fnor);
+	sub_v3_v3v3(vec, p2, p1);
+	
+	collpair->face1 = index_cloth; /* XXX actually not a face, but equivalent index for point */
+	collpair->face2 = index_coll;
+	collpair->ap1 = index_cloth;
+	collpair->ap2 = collpair->ap3 = -1; /* unused */
+	collpair->bp1 = bp1;
+	collpair->bp2 = bp2;
+	collpair->bp3 = bp3;
+	
+	copy_v3_v3(collpair->pa, p1);
+	w[0] = 1.0f - uv[0] - uv[1];
+	w[1] = uv[0];
+	w[2] = uv[1];
+	interp_v3_v3v3v3(collpair->pb, co1, co2, co3, w);
+	
+	/* note: face normal smoothing is ignored here,
+	 * it would probably not work with collision response
+	 */
+	copy_v3_v3(collpair->normal, fnor);
+	collpair->distance = -dot_v3v3(fnor, vec) * (1.0f - lambda);
+	copy_v3_v3(collpair->vector, vec);
+	collpair->time = lambda;
+	collpair->flag = 0;
+	
+	collpair++;
+	return collpair;
+}
+
+//Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
+static CollPair* cloth_point_collision(ModifierData *md1, ModifierData *md2,
+                                       BVHTreeOverlap *overlap, CollPair *collpair, float UNUSED(dt))
+{
+	ClothModifierData *clmd = (ClothModifierData *)md1;
+	CollisionModifierData *collmd = (CollisionModifierData *) md2;
+	/* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
+	ClothVertex *vert = NULL;
+	MFace *face = NULL;
+	MVert *mverts = collmd->xnew;
+
+	vert = &clmd->clothObject->verts[overlap->indexA];
+	face = &collmd->mfaces[overlap->indexB];
+
+	collpair = cloth_point_collpair(vert->x, vert->tx, mverts, face->v1, face->v2, face->v3, overlap->indexA, overlap->indexB, collpair);
+	if (face->v4)
+		collpair = cloth_point_collpair(vert->x, vert->tx, mverts, face->v3, face->v4, face->v1, overlap->indexA, overlap->indexB, collpair);
+
+	return collpair;
+}
+
+static void cloth_points_objcollisions_nearcheck(ClothModifierData * clmd, CollisionModifierData *collmd,
+                                                     CollPair **collisions, CollPair **collisions_index,
+                                                     int numresult, BVHTreeOverlap *overlap, double dt)
+{
+	int i;
+	
+	/* can return 2 collisions in total */
+	*collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 2, "collision array" );
+	*collisions_index = *collisions;
+
+	for ( i = 0; i < numresult; i++ ) {
+		*collisions_index = cloth_point_collision((ModifierData *)clmd, (ModifierData *)collmd,
+		                                          overlap+i, *collisions_index, dt);
+	}
+}
+
+static int cloth_points_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
+{
+	Cloth *cloth = clmd->clothObject;
+	int i=0, numverts = clmd->clothObject->numverts;
+	ClothVertex *verts = cloth->verts;
+	int ret = 0;
+	
+	// process all collisions
+	if ( collmd->bvhtree ) {
+		int result = cloth_points_collision_response_static(clmd, collmd, collisions, collisions_index);
+		
+		// apply impulses in parallel
+		if (result) {
+			for (i = 0; i < numverts; i++) {
+				// calculate "velocities" (just xnew = xold + v; no dt in v)
+				if (verts[i].impulse_count) {
+					// VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
+					VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
+					zero_v3(verts[i].impulse);
+					verts[i].impulse_count = 0;
+					
+					ret++;
+				}
+			}
+		}
+	}
+#if 0
+	// process all collisions (calculate impulses, TODO: also repulses if distance too short)
+	result = 1;
+	for ( j = 0; j < 2; j++ ) { /* 5 is just a value that ensures convergence */
+		result = 0;
+
+		if ( collmd->bvhtree ) {
+			result += cloth_points_collision_response_static(clmd, collmd, collisions, collisions_index);
+
+			// apply impulses in parallel
+			if (result) {
+				for (i = 0; i < numverts; i++) {
+					// calculate "velocities" (just xnew = xold + v; no dt in v)
+					if (verts[i].impulse_count) {
+						// VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
+						VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
+						zero_v3(verts[i].impulse);
+						verts[i].impulse_count = 0;
+
+						ret++;
+					}
+				}
+			}
+		}
+
+		if (!result) {
+			break;
+		}
+	}
+#endif
+	
+	return ret;
+}
+
+// cloth - object collisions
+int cloth_points_objcollision(Object *ob, ClothModifierData *clmd, float step, float dt)
+{
+	Cloth *cloth= clmd->clothObject;
+	BVHTree *cloth_bvh;
+	int rounds = 0; // result counts applied collisions; ic is for debug output;
+	float round_dt = dt / (float)clmd->coll_parms->loop_count;
+	unsigned int i=0, numverts = 0;
+	ClothVertex *verts = NULL;
+	int ret = 0, ret2 = 0;
+	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), 2, 26);
+	/* 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)
+		return 0;
+	
+	/* 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 );
+	}
+
+	do {
+		CollPair **collisions, **collisions_index;
+		
+		ret2 = 0;
+		
+		collisions = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
+		collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
+		
+		// check all collision objects
+		for (i = 0; i < numcollobj; i++) {
+			Object *collob= collobjs[i];
+			CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
+			BVHTreeOverlap *overlap = NULL;
+			unsigned int result = 0;
+			
+			if (!collmd->bvhtree)
+				continue;
+			
+			/* search for overlapping collision pairs */
+			overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
+			
+			// go to next object if no overlap is there
+			if (result && overlap) {
+				/* check if collisions really happen (costly near check) */
+				cloth_points_objcollisions_nearcheck(clmd, collmd, &collisions[i], &collisions_index[i],
+				                                     result, overlap, round_dt);
+				
+				// resolve nearby collisions
+				ret += cloth_points_objcollisions_resolve(clmd, collmd, collisions[i], collisions_index[i]);
+				ret2 += ret;
+			}
+			
+			if (overlap)
+				MEM_freeN ( overlap );
+		}
+		rounds++;
+		
+		for (i = 0; i < numcollobj; i++) {
+			if (collisions[i])
+				MEM_freeN(collisions[i]);
+		}
+			
+		MEM_freeN(collisions);
+		MEM_freeN(collisions_index);
+
+		////////////////////////////////////////////////////////////
+		// 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 );
+		}
+		////////////////////////////////////////////////////////////
+	}
+	while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
+	
+	if (collobjs)
+		MEM_freeN(collobjs);
+
+	BLI_bvhtree_free(cloth_bvh);
+
+	return 1|MIN2 ( ret, 1 );
+}