Cloth: Collision improvements

This commit includes several performance, stability, and reliability
improvements to cloth collisions.

Most notably:
* The implementation of a new self-collisions system.
* Multithreading of collision detection.
* Implementation of single sided collisions and normal overrides.
* Replacement of the `plNearestPoints` function from Bullet with a
dedicated solution.

Further, this also includes several bug fixes, and algorithmic
improvements.

Reviewed By: brecht

Differential Revision: http://developer.blender.org/D3712

1 files changed, 787 insertions, 591 deletions

diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c
index 183a4f9a181..d0eac3bb713 100644
--- a/source/blender/blenkernel/intern/collision.c
+++ b/source/blender/blenkernel/intern/collision.c
@@ -43,7 +43,8 @@
 #include "BLI_utildefines.h"
 #include "BLI_blenlib.h"
 #include "BLI_math.h"
-#include "BLI_edgehash.h"
+#include "BLI_task.h"
+#include "BLI_threads.h"
 
 #include "BKE_cloth.h"
 #include "BKE_collection.h"
@@ -52,9 +53,6 @@
 #include "BKE_modifier.h"
 #include "BKE_scene.h"
 
-#ifdef WITH_BULLET
-#include "Bullet-C-Api.h"
-#endif
 #include "BLI_kdopbvh.h"
 #include "BKE_collision.h"
 
@@ -67,6 +65,23 @@
 #endif
 
 
+typedef struct ColDetectData {
+	ClothModifierData *clmd;
+	CollisionModifierData *collmd;
+	BVHTreeOverlap *overlap;
+	CollPair *collisions;
+	bool culling;
+	bool use_normal;
+	bool collided;
+} ColDetectData;
+
+typedef struct SelfColDetectData {
+	ClothModifierData *clmd;
+	BVHTreeOverlap *overlap;
+	CollPair *collisions;
+	bool collided;
+} SelfColDetectData;
+
 /***********************************
 Collision modifier code start
 ***********************************/
@@ -74,7 +89,7 @@ Collision modifier code start
 /* step is limited from 0 (frame start position) to 1 (frame end position) */
 void collision_move_object(CollisionModifierData *collmd, float step, float prevstep)
 {
-	float tv[3] = {0, 0, 0};
+	float oldx[3];
 	unsigned int i = 0;
 
 	/* the collider doesn't move this frame */
@@ -87,15 +102,14 @@ void collision_move_object(CollisionModifierData *collmd, float step, float prev
 	}
 
 	for (i = 0; i < collmd->mvert_num; i++) {
-		sub_v3_v3v3(tv, collmd->xnew[i].co, collmd->x[i].co);
-		VECADDS(collmd->current_x[i].co, collmd->x[i].co, tv, prevstep);
-		VECADDS(collmd->current_xnew[i].co, collmd->x[i].co, tv, step);
-		sub_v3_v3v3(collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co);
+		interp_v3_v3v3(oldx, collmd->x[i].co, collmd->xnew[i].co, prevstep);
+		interp_v3_v3v3(collmd->current_x[i].co, collmd->x[i].co, collmd->xnew[i].co, step);
+		sub_v3_v3v3(collmd->current_v[i].co, collmd->current_x[i].co, oldx);
 	}
 
 	bvhtree_update_from_mvert(
-	        collmd->bvhtree, collmd->current_x, collmd->current_xnew,
-	        collmd->tri, collmd->tri_num, true);
+	        collmd->bvhtree, collmd->current_x, NULL,
+	        collmd->tri, collmd->tri_num, false);
 }
 
 BVHTree *bvhtree_build_from_mvert(
@@ -178,6 +192,264 @@ void bvhtree_update_from_mvert(
 Collision modifier code end
 ***********************************/
 
+static void clamp_point_seg(float a[3], float b[3], float p[3])
+{
+	float ap[3], bp[3], ab[3];
+
+	sub_v3_v3v3(ap, p, a);
+	sub_v3_v3v3(bp, p, b);
+	sub_v3_v3v3(ab, b, a);
+
+	if (dot_v3v3(ap, bp) > 0.0f) {
+		if (dot_v3v3(ap, ab) > 0.0f) {
+			copy_v3_v3(p, b);
+		}
+		else {
+			copy_v3_v3(p, a);
+		}
+	}
+}
+
+static bool isect_seg_seg(float a1[3], float a2[3], float b1[3], float b2[3], float r_a[3], float r_b[3])
+{
+	if (isect_line_line_epsilon_v3(a1, a2, b1, b2, r_a, r_b, 0.0f)) {
+		clamp_point_seg(a1, a2, r_a);
+		clamp_point_seg(b1, b2, r_b);
+
+		return true;
+	}
+
+	return false;
+}
+
+BLI_INLINE int next_ind(int i)
+{
+	return (++i < 3) ? i : 0;
+}
+
+static float compute_collision_point(float a1[3], float a2[3], float a3[3], float b1[3], float b2[3], float b3[3],
+                                     bool culling, bool use_normal, float r_a[3], float r_b[3], float r_vec[3])
+{
+	float a[3][3];
+	float b[3][3];
+	float dist = FLT_MAX;
+	float tmp_co1[3], tmp_co2[3];
+	float isect_a[3], isect_b[3];
+	int isect_count = 0;
+	float tmp, tmp_vec[3];
+	float normal[3], cent[3];
+	bool backside = false;
+
+	copy_v3_v3(a[0], a1);
+	copy_v3_v3(a[1], a2);
+	copy_v3_v3(a[2], a3);
+
+	copy_v3_v3(b[0], b1);
+	copy_v3_v3(b[1], b2);
+	copy_v3_v3(b[2], b3);
+
+	/* Find intersections. */
+	for (int i = 0; i < 3; i++) {
+		if (isect_line_segment_tri_v3(a[i], a[next_ind(i)], b[0], b[1], b[2], &tmp, NULL)) {
+			interp_v3_v3v3(isect_a, a[i], a[next_ind(i)], tmp);
+			isect_count++;
+		}
+	}
+
+	if (isect_count == 0) {
+		for (int i = 0; i < 3; i++) {
+			if (isect_line_segment_tri_v3(b[i], b[next_ind(i)], a[0], a[1], a[2], &tmp, NULL)) {
+				isect_count++;
+			}
+		}
+	}
+	else if (isect_count == 1) {
+		for (int i = 0; i < 3; i++) {
+			if (isect_line_segment_tri_v3(b[i], b[next_ind(i)], a[0], a[1], a[2], &tmp, NULL)) {
+				interp_v3_v3v3(isect_b, b[i], b[next_ind(i)], tmp);
+				break;
+			}
+		}
+	}
+
+	/* Determine collision side. */
+	if (culling) {
+		normal_tri_v3(normal, b[0], b[1], b[2]);
+		mid_v3_v3v3v3(cent, b[0], b[1], b[2]);
+
+		if (isect_count == 2) {
+			backside = true;
+		}
+		else if (isect_count == 0) {
+			for (int i = 0; i < 3; i++) {
+				sub_v3_v3v3(tmp_vec, a[i], cent);
+				if (dot_v3v3(tmp_vec, normal) < 0.0f) {
+					backside = true;
+					break;
+				}
+			}
+		}
+	}
+	else if (use_normal) {
+		normal_tri_v3(normal, b[0], b[1], b[2]);
+	}
+
+	if (isect_count == 1) {
+		/* Edge intersection. */
+		copy_v3_v3(r_a, isect_a);
+		copy_v3_v3(r_b, isect_b);
+
+		if (use_normal) {
+			copy_v3_v3(r_vec, normal);
+		}
+		else {
+			sub_v3_v3v3(r_vec, r_b, r_a);
+		}
+
+		return 0.0f;
+	}
+
+	if (backside) {
+		float maxdist = 0.0f;
+		bool found = false;
+
+		/* Point projections. */
+		for (int i = 0; i < 3; i++) {
+			if (isect_ray_tri_v3(a[i], normal, b[0], b[1], b[2], &tmp, NULL)) {
+				if (tmp > maxdist) {
+					maxdist = tmp;
+					copy_v3_v3(r_a, a[i]);
+					madd_v3_v3v3fl(r_b, a[i], normal, tmp);
+					found = true;
+				}
+			}
+		}
+
+		negate_v3(normal);
+
+		for (int i = 0; i < 3; i++) {
+			if (isect_ray_tri_v3(b[i], normal, a[0], a[1], a[2], &tmp, NULL)) {
+				if (tmp > maxdist) {
+					maxdist = tmp;
+					madd_v3_v3v3fl(r_a, b[i], normal, tmp);
+					copy_v3_v3(r_b, b[i]);
+					found = true;
+				}
+			}
+		}
+
+		negate_v3(normal);
+
+		/* Edge projections. */
+		for (int i = 0; i < 3; i++) {
+			float dir[3];
+
+			sub_v3_v3v3(tmp_vec, b[next_ind(i)], b[i]);
+			cross_v3_v3v3(dir, tmp_vec, normal);
+
+			for (int j = 0; j < 3; j++) {
+				if (isect_line_plane_v3(tmp_co1, a[j], a[next_ind(j)], b[i], dir) &&
+				    point_in_slice_seg(tmp_co1, a[j], a[next_ind(j)]) &&
+				    point_in_slice_seg(tmp_co1, b[i], b[next_ind(i)]))
+				{
+					closest_to_line_v3(tmp_co2, tmp_co1, b[i], b[next_ind(i)]);
+					sub_v3_v3v3(tmp_vec, tmp_co1, tmp_co2);
+					tmp = len_v3(tmp_vec);
+
+					if ((tmp > maxdist) && (dot_v3v3(tmp_vec, normal) < 0.0f)) {
+						maxdist = tmp;
+						copy_v3_v3(r_a, tmp_co1);
+						copy_v3_v3(r_b, tmp_co2);
+						found = true;
+					}
+				}
+			}
+		}
+
+		/* If no point is found, will fallback onto regular proximity test below. */
+		if (found) {
+			sub_v3_v3v3(r_vec, r_b, r_a);
+
+			if (use_normal) {
+				if (dot_v3v3(normal, r_vec) >= 0.0f) {
+					copy_v3_v3(r_vec, normal);
+				}
+				else {
+					negate_v3_v3(r_vec, normal);
+				}
+			}
+
+			return 0.0f;
+		}
+	}
+
+	/* Closest point. */
+	for (int i = 0; i < 3; i++) {
+		closest_on_tri_to_point_v3(tmp_co1, a[i], b[0], b[1], b[2]);
+		tmp = len_squared_v3v3(tmp_co1, a[i]);
+
+		if (tmp < dist) {
+			dist = tmp;
+			copy_v3_v3(r_a, a[i]);
+			copy_v3_v3(r_b, tmp_co1);
+		}
+	}
+
+	for (int i = 0; i < 3; i++) {
+		closest_on_tri_to_point_v3(tmp_co1, b[i], a[0], a[1], a[2]);
+		tmp = len_squared_v3v3(tmp_co1, b[i]);
+
+		if (tmp < dist) {
+			dist = tmp;
+			copy_v3_v3(r_a, tmp_co1);
+			copy_v3_v3(r_b, b[i]);
+		}
+	}
+
+	/* Closest edge. */
+	if (isect_count == 0) {
+		for (int i = 0; i < 3; i++) {
+			for (int j = 0; j < 3; j++) {
+				if (isect_seg_seg(a[i], a[next_ind(i)], b[j], b[next_ind(j)], tmp_co1, tmp_co2)) {
+					tmp = len_squared_v3v3(tmp_co1, tmp_co2);
+
+					if (tmp < dist) {
+						dist = tmp;
+						copy_v3_v3(r_a, tmp_co1);
+						copy_v3_v3(r_b, tmp_co2);
+					}
+				}
+			}
+		}
+	}
+
+	if (isect_count == 0) {
+		sub_v3_v3v3(r_vec, r_a, r_b);
+		dist = sqrtf(dist);
+	}
+	else {
+		sub_v3_v3v3(r_vec, r_b, r_a);
+		dist = 0.0f;
+	}
+
+	if (culling && use_normal) {
+		copy_v3_v3(r_vec, normal);
+	}
+	else if (use_normal) {
+		if (dot_v3v3(normal, r_vec) >= 0.0f) {
+			copy_v3_v3(r_vec, normal);
+		}
+		else {
+			negate_v3_v3(r_vec, normal);
+		}
+	}
+	else if (culling && (dot_v3v3(r_vec, normal) < 0.0f)) {
+		return FLT_MAX;
+	}
+
+	return dist;
+}
+
 // w3 is not perfect
 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
 {
@@ -232,141 +504,131 @@ DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float
 	VECADDMUL(to, v3, w3);
 }
 
-static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
+static int cloth_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, Object *collob,
+                                           CollPair *collpair, uint collision_count, const float dt)
 {
 	int result = 0;
 	Cloth *cloth1;
 	float w1, w2, w3, u1, u2, u3;
 	float v1[3], v2[3], relativeVelocity[3];
 	float magrelVel;
-	float epsilon2 = BLI_bvhtree_get_epsilon ( collmd->bvhtree );
+	float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree);
 
 	cloth1 = clmd->clothObject;
 
-	for ( ; collpair != collision_end; collpair++ ) {
+	for (int i = 0; i < collision_count; i++, 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 )
+		/* Only handle static collisions here. */
+		if (collpair->flag & (COLLISION_IN_FUTURE | COLLISION_INACTIVE)) {
 			continue;
+		}
 
-		/* compute barycentric coordinates for both collision points */
-		collision_compute_barycentric ( collpair->pa,
-			cloth1->verts[collpair->ap1].txold,
-			cloth1->verts[collpair->ap2].txold,
-			cloth1->verts[collpair->ap3].txold,
-			&w1, &w2, &w3 );
+		/* Compute barycentric coordinates for both collision points. */
+		collision_compute_barycentric(collpair->pa,
+		                              cloth1->verts[collpair->ap1].tx,
+		                              cloth1->verts[collpair->ap2].tx,
+		                              cloth1->verts[collpair->ap3].tx,
+		                              &w1, &w2, &w3);
 
-		/* 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 );
+		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". */
-		collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
+		collision_interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
 
-		collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
+		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 v_n_mag < 0 the edges are approaching each other. */
-		if ( magrelVel > ALMOST_ZERO ) {
+		/* If magrelVel < 0 the edges are approaching each other. */
+		if (magrelVel > 0.0f) {
 			/* 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;
+			float temp[3];
+			float time_multiplier;
 
-			/* calculate tangential velocity */
-			copy_v3_v3 ( temp, collpair->normal );
+			/* 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));
+			 * in original formula "magrelVel" should be the "change of relative velocity in normal direction". */
+			magtangent = min_ff(collob->pd->pdef_cfrict * 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 );
+				impulse = magtangent / 1.5;
+
+				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 );
+			/* Apply velocity stopping impulse. */
+			impulse =  magrelVel / 1.5f;
 
-			VECADDMUL ( i1, collpair->normal, w1 * impulse );
+			VECADDMUL(i1, collpair->normal, w1 * impulse);
 			cloth1->verts[collpair->ap1].impulse_count++;
 
-			VECADDMUL ( i2, collpair->normal, w2 * impulse );
+			VECADDMUL(i2, collpair->normal, w2 * impulse);
 			cloth1->verts[collpair->ap2].impulse_count++;
 
-			VECADDMUL ( i3, collpair->normal, w3 * impulse );
+			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;
+			time_multiplier = 1.0f / (clmd->sim_parms->dt * 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 );
+			if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) {
+				repulse = MIN2(d / time_multiplier, 0.1f * d * time_multiplier - magrelVel);
+
+				/* Stay on the safe side and clamp repulse. */
+				if (impulse > ALMOST_ZERO) {
+					repulse = min_ff(repulse, 5.0f * 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 );
+				impulse = repulse / 1.5f;
+
+				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 time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
+			float d;
+
+			d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;
 
-				float impulse = repulse / ( 3.0f * ( 1.0f + w1*w1 + w2*w2 + w3*w3 )); /* original 2.0 / 0.25 */
+			if (d > ALMOST_ZERO) {
+				/* Stay on the safe side and clamp repulse. */
+				float repulse = d / time_multiplier;
+				float impulse = repulse / 4.5f;
 
-				VECADDMUL ( i1, collpair->normal,  impulse );
-				VECADDMUL ( i2, collpair->normal,  impulse );
-				VECADDMUL ( i3, collpair->normal,  impulse );
+				VECADDMUL(i1, collpair->normal, w1 * impulse);
+				VECADDMUL(i2, collpair->normal, w2 * impulse);
+				VECADDMUL(i3, collpair->normal, w3 * impulse);
 
 				cloth1->verts[collpair->ap1].impulse_count++;
 				cloth1->verts[collpair->ap2].impulse_count++;
@@ -377,20 +639,190 @@ static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionM
 		}
 
 		if (result) {
-			int i = 0;
+			float clamp = clmd->coll_parms->clamp * dt;
 
-			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 ((clamp > 0.0f) &&
+			    ((len_v3(i1) > clamp) ||
+			     (len_v3(i2) > clamp) ||
+			     (len_v3(i3) > clamp)))
+			{
+				return 0;
+			}
+
+			for (int j = 0; j < 3; j++) {
+				if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j]))
+					cloth1->verts[collpair->ap1].impulse[j] = i1[j];
 
-				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->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j]))
+					cloth1->verts[collpair->ap2].impulse[j] = i2[j];
 
-				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];
+				if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j]))
+					cloth1->verts[collpair->ap3].impulse[j] = i3[j];
 			}
 		}
 	}
+
+	return result;
+}
+
+static int cloth_selfcollision_response_static(ClothModifierData *clmd, CollPair *collpair,
+                                               uint collision_count, const float dt)
+{
+	int result = 0;
+	Cloth *cloth1;
+	float w1, w2, w3, u1, u2, u3;
+	float v1[3], v2[3], relativeVelocity[3];
+	float magrelVel;
+
+	cloth1 = clmd->clothObject;
+
+	for (int i = 0; i < collision_count; i++, 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 | COLLISION_INACTIVE)) {
+			continue;
+		}
+
+		/* Compute barycentric coordinates for both collision points. */
+		collision_compute_barycentric(collpair->pa,
+		                              cloth1->verts[collpair->ap1].tx,
+		                              cloth1->verts[collpair->ap2].tx,
+		                              cloth1->verts[collpair->ap3].tx,
+		                              &w1, &w2, &w3);
+
+		collision_compute_barycentric(collpair->pb,
+		                              cloth1->verts[collpair->bp1].tx,
+		                              cloth1->verts[collpair->bp2].tx,
+		                              cloth1->verts[collpair->bp3].tx,
+		                              &u1, &u2, &u3);
+
+		/* Calculate relative "velocity". */
+		collision_interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
+
+		collision_interpolateOnTriangle(v2, cloth1->verts[collpair->bp1].tv, cloth1->verts[collpair->bp2].tv, cloth1->verts[collpair->bp3].tv, 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);
+
+		/* TODO: Impulses should be weighed by mass as this is self col,
+		 * this has to be done after mass distribution is implemented. */
+
+		/* If magrelVel < 0 the edges are approaching each other. */
+		if (magrelVel > 0.0f) {
+			/* 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], time_multiplier;
+
+			/* 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->self_friction * 0.01f * magrelVel, len_v3(vrel_t_pre));
+
+			/* Apply friction impulse. */
+			if (magtangent > ALMOST_ZERO) {
+				normalize_v3(vrel_t_pre);
+
+				impulse = magtangent / 1.5;
+
+				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. */
+			impulse = magrelVel / 3.0f;
+
+			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++;
+
+			time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
+
+			d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance;
+
+			if ((magrelVel < 0.1f * d * time_multiplier) && (d > ALMOST_ZERO)) {
+				repulse = MIN2 (d / time_multiplier, 0.1f * d * time_multiplier - magrelVel);
+
+				if (impulse > ALMOST_ZERO) {
+					repulse = min_ff(repulse, 5.0*impulse);
+				}
+
+				repulse = max_ff(impulse, repulse);
+
+				impulse = repulse / 1.5f;
+
+				VECADDMUL(i1, collpair->normal, w1 * impulse);
+				VECADDMUL(i2, collpair->normal, w2 * impulse);
+				VECADDMUL(i3, collpair->normal, w3 * impulse);
+			}
+
+			result = 1;
+		}
+		else {
+			float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
+			float d;
+
+			d = clmd->coll_parms->selfepsilon * 8.0f / 9.0f * 2.0f - collpair->distance;
+
+			if ( d > ALMOST_ZERO) {
+				/* Stay on the safe side and clamp repulse. */
+				float repulse = d*1.0f/time_multiplier;
+				float impulse = repulse / 9.0f;
+
+				VECADDMUL(i1, collpair->normal, w1 * impulse);
+				VECADDMUL(i2, collpair->normal, w2 * impulse);
+				VECADDMUL(i3, collpair->normal, w3 * impulse);
+
+				cloth1->verts[collpair->ap1].impulse_count++;
+				cloth1->verts[collpair->ap2].impulse_count++;
+				cloth1->verts[collpair->ap3].impulse_count++;
+
+				result = 1;
+			}
+		}
+
+		if (result) {
+			float clamp = clmd->coll_parms->self_clamp * dt;
+
+			if ((clamp > 0.0f) &&
+			    ((len_v3(i1) > clamp) ||
+			     (len_v3(i2) > clamp) ||
+			     (len_v3(i3) > clamp)))
+			{
+				return 0;
+			}
+
+			for (int j = 0; j < 3; j++) {
+				if (cloth1->verts[collpair->ap1].impulse_count > 0 && ABS(cloth1->verts[collpair->ap1].impulse[j]) < ABS(i1[j]))
+					cloth1->verts[collpair->ap1].impulse[j] = i1[j];
+
+				if (cloth1->verts[collpair->ap2].impulse_count > 0 && ABS(cloth1->verts[collpair->ap2].impulse[j]) < ABS(i2[j]))
+					cloth1->verts[collpair->ap2].impulse[j] = i2[j];
+
+				if (cloth1->verts[collpair->ap3].impulse_count > 0 && ABS(cloth1->verts[collpair->ap3].impulse[j]) < ABS(i3[j]))
+					cloth1->verts[collpair->ap3].impulse[j] = i3[j];
+			}
+		}
+	}
+
 	return result;
 }
 
@@ -398,89 +830,118 @@ static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionM
 #  pragma GCC diagnostic pop
 #endif
 
-//Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
-static CollPair* cloth_collision(ModifierData *md1, ModifierData *md2,
-                                 BVHTreeOverlap *overlap, CollPair *collpair, float UNUSED(dt))
+static void cloth_collision(
+        void *__restrict userdata,
+        const int index,
+        const ParallelRangeTLS *__restrict UNUSED(tls))
 {
-	ClothModifierData *clmd = (ClothModifierData *)md1;
-	CollisionModifierData *collmd = (CollisionModifierData *) md2;
-	/* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
+	ColDetectData *data = (ColDetectData *)userdata;
+
+	ClothModifierData *clmd = data->clmd;
+	CollisionModifierData *collmd = data->collmd;
+	CollPair *collpair = data->collisions;
 	const MVertTri *tri_a, *tri_b;
-#ifdef WITH_BULLET
 	ClothVertex *verts1 = clmd->clothObject->verts;
-#endif
-	double distance = 0;
+	float distance = 0.0f;
 	float epsilon1 = clmd->coll_parms->epsilon;
-	float epsilon2 = BLI_bvhtree_get_epsilon ( collmd->bvhtree );
+	float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree);
+	float pa[3], pb[3], vect[3];
 
-	tri_a = &clmd->clothObject->tri[overlap->indexA];
-	tri_b = &collmd->tri[overlap->indexB];
+	tri_a = &clmd->clothObject->tri[data->overlap[index].indexA];
+	tri_b = &collmd->tri[data->overlap[index].indexB];
 
-	/* fill face_a */
-	collpair->ap1 = tri_a->tri[0];
-	collpair->ap2 = tri_a->tri[1];
-	collpair->ap3 = tri_a->tri[2];
+	/* Compute distance and normal. */
+	distance = compute_collision_point(verts1[tri_a->tri[0]].tx, verts1[tri_a->tri[1]].tx, verts1[tri_a->tri[2]].tx,
+	                                   collmd->current_x[tri_b->tri[0]].co, collmd->current_x[tri_b->tri[1]].co, collmd->current_x[tri_b->tri[2]].co,
+	                                   data->culling, data->use_normal, pa, pb, vect);
 
-	/* fill face_b */
-	collpair->bp1 = tri_b->tri[0];
-	collpair->bp2 = tri_b->tri[1];
-	collpair->bp3 = tri_b->tri[2];
+	if ((distance <= (epsilon1 + epsilon2 + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) {
+		collpair[index].ap1 = tri_a->tri[0];
+		collpair[index].ap2 = tri_a->tri[1];
+		collpair[index].ap3 = tri_a->tri[2];
 
-	{
+		collpair[index].bp1 = tri_b->tri[0];
+		collpair[index].bp2 = tri_b->tri[1];
+		collpair[index].bp3 = tri_b->tri[2];
 
-#ifdef WITH_BULLET
-		// calc distance + normal
-		distance = plNearestPoints (
-			verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, collmd->current_x[collpair->bp1].co, collmd->current_x[collpair->bp2].co, collmd->current_x[collpair->bp3].co, collpair->pa, collpair->pb, collpair->vector );
-#else
-		// just be sure that we don't add anything
-		distance = 2.0 * (double)( epsilon1 + epsilon2 + ALMOST_ZERO );
-#endif
+		copy_v3_v3(collpair[index].pa, pa);
+		copy_v3_v3(collpair[index].pb, pb);
+		copy_v3_v3(collpair[index].vector, vect);
 
-		// distance -1 means no collision result
-		if (distance != -1.0 && (distance <= (double)(epsilon1 + epsilon2 + ALMOST_ZERO))) {
-			normalize_v3_v3(collpair->normal, collpair->vector);
+		normalize_v3_v3(collpair[index].normal, collpair[index].vector);
 
-			collpair->distance = distance;
-			collpair->flag = 0;
-			collpair++;
-		}/*
-		else {
-			float w1, w2, w3, u1, u2, u3;
-			float v1[3], v2[3], relativeVelocity[3];
+		collpair[index].distance = distance;
+		collpair[index].flag = 0;
 
-			// calc relative velocity
+		data->collided = true;
+	}
+	else {
+		collpair[index].flag = COLLISION_INACTIVE;
+	}
+}
 
-			// compute barycentric coordinates for both collision points
-			collision_compute_barycentric ( collpair->pa,
-			verts1[collpair->ap1].txold,
-			verts1[collpair->ap2].txold,
-			verts1[collpair->ap3].txold,
-			&w1, &w2, &w3 );
+static void cloth_selfcollision(
+        void *__restrict userdata,
+        const int index,
+        const ParallelRangeTLS *__restrict UNUSED(tls))
+{
+	SelfColDetectData *data = (SelfColDetectData *)userdata;
 
-			// 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 );
+	ClothModifierData *clmd = data->clmd;
+	CollPair *collpair = data->collisions;
+	const MVertTri *tri_a, *tri_b;
+	ClothVertex *verts1 = clmd->clothObject->verts;
+	float distance = 0.0f;
+	float epsilon = clmd->coll_parms->selfepsilon;
+	float pa[3], pb[3], vect[3];
+
+	tri_a = &clmd->clothObject->tri[data->overlap[index].indexA];
+	tri_b = &clmd->clothObject->tri[data->overlap[index].indexB];
+
+	for (uint i = 0; i < 3; i++) {
+		for (uint j = 0; j < 3; j++) {
+			if (tri_a->tri[i] == tri_b->tri[j]) {
+				collpair[index].flag = COLLISION_INACTIVE;
+				return;
+			}
+		}
+	}
 
-			// Calculate relative "velocity".
-			collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
+	if (((verts1[tri_a->tri[0]].flags & verts1[tri_a->tri[1]].flags & verts1[tri_a->tri[2]].flags) |
+	     (verts1[tri_b->tri[0]].flags & verts1[tri_b->tri[1]].flags & verts1[tri_b->tri[2]].flags)) & CLOTH_VERT_FLAG_NOSELFCOLL)
+	{
+		collpair[index].flag = COLLISION_INACTIVE;
+		return;
+	}
 
-			collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
+	/* Compute distance and normal. */
+	distance = compute_collision_point(verts1[tri_a->tri[0]].tx, verts1[tri_a->tri[1]].tx, verts1[tri_a->tri[2]].tx,
+	                                   verts1[tri_b->tri[0]].tx, verts1[tri_b->tri[1]].tx, verts1[tri_b->tri[2]].tx,
+	                                   false, false, pa, pb, vect);
 
-			sub_v3_v3v3(relativeVelocity, v2, v1);
+	if ((distance <= (epsilon * 2.0f + ALMOST_ZERO)) && (len_squared_v3(vect) > ALMOST_ZERO)) {
+		collpair[index].ap1 = tri_a->tri[0];
+		collpair[index].ap2 = tri_a->tri[1];
+		collpair[index].ap3 = tri_a->tri[2];
 
-			if (sqrt(dot_v3v3(relativeVelocity, relativeVelocity)) >= distance)
-			{
-				// check for collision in the future
-				collpair->flag |= COLLISION_IN_FUTURE;
-				collpair++;
-			}
-		}*/
+		collpair[index].bp1 = tri_b->tri[0];
+		collpair[index].bp2 = tri_b->tri[1];
+		collpair[index].bp3 = tri_b->tri[2];
+
+		copy_v3_v3(collpair[index].pa, pa);
+		copy_v3_v3(collpair[index].pb, pb);
+		copy_v3_v3(collpair[index].vector, vect);
+
+		normalize_v3_v3(collpair[index].normal, collpair[index].vector);
+
+		collpair[index].distance = distance;
+		collpair[index].flag = 0;
+
+		data->collided = true;
+	}
+	else {
+		collpair[index].flag = COLLISION_INACTIVE;
 	}
-	return collpair;
 }
 
 static void add_collision_object(ListBase *relations, Object *ob, int level, unsigned int modifier_type)
@@ -628,25 +1089,49 @@ void BKE_collider_cache_free(ListBase **colliders)
 	}
 }
 
+static bool cloth_bvh_objcollisions_nearcheck(ClothModifierData * clmd, CollisionModifierData *collmd,
+                                              CollPair **collisions, int numresult,
+                                              BVHTreeOverlap *overlap, bool culling, bool use_normal)
+{
+	*collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * numresult, "collision array");
+
+	ColDetectData data = {.clmd = clmd,
+	                      .collmd = collmd,
+	                      .overlap = overlap,
+	                      .collisions = *collisions,
+	                      .culling = culling,
+	                      .use_normal = use_normal,
+	                      .collided = false};
+
+	ParallelRangeSettings settings;
+	BLI_parallel_range_settings_defaults(&settings);
+	settings.use_threading = true;
+	BLI_task_parallel_range(0, numresult, &data, cloth_collision, &settings);
+
+	return data.collided;
+}
 
-static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd,
-	CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap, double dt)
+static bool cloth_bvh_selfcollisions_nearcheck(ClothModifierData * clmd, CollPair *collisions,
+                                               int numresult, BVHTreeOverlap *overlap)
 {
-	int i;
+	SelfColDetectData data = {.clmd = clmd,
+	                          .overlap = overlap,
+	                          .collisions = collisions,
+	                          .collided = false};
 
-	*collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 4, "collision array" ); // * 4 since cloth_collision_static can return more than 1 collision
-	*collisions_index = *collisions;
+	ParallelRangeSettings settings;
+	BLI_parallel_range_settings_defaults(&settings);
+	settings.use_threading = true;
+	BLI_task_parallel_range(0, numresult, &data, cloth_selfcollision, &settings);
 
-	for ( i = 0; i < numresult; i++ ) {
-		*collisions_index = cloth_collision((ModifierData *)clmd, (ModifierData *)collmd,
-		                                    overlap+i, *collisions_index, dt);
-	}
+	return data.collided;
 }
 
-static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
+static int cloth_bvh_objcollisions_resolve(ClothModifierData * clmd, Object **collobjs, CollPair **collisions,
+                                           uint *collision_counts, const uint numcollobj, const float dt)
 {
 	Cloth *cloth = clmd->clothObject;
-	int i=0, j = 0, /*numfaces = 0, */ mvert_num = 0;
+	int i = 0, j = 0, mvert_num = 0;
 	ClothVertex *verts = NULL;
 	int ret = 0;
 	int result = 0;
@@ -654,26 +1139,68 @@ static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, Collision
 	mvert_num = clmd->clothObject->mvert_num;
 	verts = cloth->verts;
 
-	// 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 */
+
+	for (j = 0; j < 2; j++) {
 		result = 0;
 
-		if ( collmd->bvhtree ) {
-			result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
+		for (i = 0; i < numcollobj; i++) {
+			Object *collob= collobjs[i];
+			CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
 
-			// apply impulses in parallel
-			if (result) {
-				for (i = 0; i < mvert_num; 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;
+			if ( collmd->bvhtree ) {
+				result += cloth_collision_response_static(clmd, collmd, collob, collisions[i], collision_counts[i], dt);
+			}
+		}
 
-						ret++;
-					}
+		/* Apply impulses in parallel. */
+		if (result) {
+			for (i = 0; i < mvert_num; i++) {
+				// calculate "velocities" (just xnew = xold + v; no dt in v)
+				if (verts[i].impulse_count) {
+					VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse);
+					VECADD ( verts[i].dcvel, verts[i].dcvel, verts[i].impulse);
+					zero_v3(verts[i].impulse);
+					verts[i].impulse_count = 0;
+
+					ret++;
+				}
+			}
+		}
+		else {
+			break;
+		}
+	}
+	return ret;
+}
+
+static int cloth_bvh_selfcollisions_resolve(ClothModifierData * clmd, CollPair *collisions, int collision_count, const float dt)
+{
+	Cloth *cloth = clmd->clothObject;
+	int i = 0, j = 0, mvert_num = 0;
+	ClothVertex *verts = NULL;
+	int ret = 0;
+	int result = 0;
+
+	mvert_num = clmd->clothObject->mvert_num;
+	verts = cloth->verts;
+
+	for (j = 0; j < 2; j++) {
+		result = 0;
+
+		result += cloth_selfcollision_response_static(clmd, collisions, collision_count, dt);
+
+		/* Apply impulses in parallel. */
+		if (result) {
+			for (i = 0; i < mvert_num; i++) {
+				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);
+					VECADD ( verts[i].dcvel, verts[i].dcvel, verts[i].impulse);
+					zero_v3(verts[i].impulse);
+					verts[i].impulse_count = 0;
+
+					ret++;
 				}
 			}
 		}
@@ -685,91 +1212,86 @@ static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, Collision
 	return ret;
 }
 
-// cloth - object collisions
-int cloth_bvh_objcollision(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt )
+int cloth_bvh_collision(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt)
 {
-	Cloth *cloth= clmd->clothObject;
-	BVHTree *cloth_bvh= cloth->bvhtree;
-	unsigned int i=0, /* numfaces = 0, */ /* UNUSED */ mvert_num = 0, k, l, j;
-	int rounds = 0; // result counts applied collisions; ic is for debug output;
+	Cloth *cloth = clmd->clothObject;
+	BVHTree *cloth_bvh = cloth->bvhtree;
+	uint i = 0, mvert_num = 0;
+	int rounds = 0;
 	ClothVertex *verts = NULL;
 	int ret = 0, ret2 = 0;
 	Object **collobjs = NULL;
 	unsigned int numcollobj = 0;
+	uint *coll_counts_obj = NULL;
+	BVHTreeOverlap **overlap_obj = NULL;
+	uint coll_count_self = 0;
+	BVHTreeOverlap *overlap_self = NULL;
 
 	if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
 		return 0;
 
 	verts = cloth->verts;
-	/* numfaces = cloth->numfaces; */ /* UNUSED */
 	mvert_num = cloth->mvert_num;
 
-	////////////////////////////////////////////////////////////
-	// static collisions
-	////////////////////////////////////////////////////////////
-
 	if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
-		bvhtree_update_from_cloth(clmd, true);
+		bvhtree_update_from_cloth(clmd, false, false);
 
 		collobjs = BKE_collision_objects_create(depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
 
-		if (!collobjs) {
-			return 0;
-		}
+		if (collobjs) {
+			coll_counts_obj = MEM_callocN(sizeof(uint) * numcollobj, "CollCounts");
+			overlap_obj = MEM_callocN(sizeof(*overlap_obj) * numcollobj, "BVHOverlap");
 
-		/* 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);
+			for (i = 0; i < numcollobj; i++) {
+				Object *collob = collobjs[i];
+				CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);
 
-			if (!collmd->bvhtree) {
-				continue;
-			}
+				if (!collmd->bvhtree) {
+					continue;
+				}
+
+				/* Move object to position (step) in time. */
+				collision_move_object(collmd, step + dt, step);
 
-			/* Move object to position (step) in time. */
-			collision_move_object(collmd, step + dt, step);
+				overlap_obj[i] = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &coll_counts_obj[i], NULL, NULL);
+			}
 		}
 	}
 
 	if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) {
-		bvhselftree_update_from_cloth(clmd, false);
+		bvhtree_update_from_cloth(clmd, false, true);
+
+		overlap_self = BLI_bvhtree_overlap(cloth->bvhselftree, cloth->bvhselftree, &coll_count_self, NULL, NULL);
 	}
 
 	do {
 		ret2 = 0;
 
-		if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
-			CollPair **collisions, **collisions_index;
+		/* Object collisions. */
+		if ((clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) && collobjs) {
+			CollPair **collisions;
+			bool collided = false;
 
-			collisions = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
-			collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
+			collisions = MEM_callocN(sizeof(CollPair *) * numcollobj, "CollPair");
 
-			/* Check all collision objects. */
 			for (i = 0; i < numcollobj; i++) {
-				Object *collob= collobjs[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, NULL, NULL);
-
-				/* Go to next object if no overlap is there. */
-				if (result && overlap) {
-					/* Check if collisions really happen (costly near check). */
-					cloth_bvh_objcollisions_nearcheck(clmd, collmd, &collisions[i], &collisions_index[i],
-					                                  result, overlap, dt/(float)clmd->coll_parms->loop_count);
-
-					/* Resolve nearby collisions. */
-					ret += cloth_bvh_objcollisions_resolve(clmd, collmd, collisions[i], collisions_index[i]);
-					ret2 += ret;
+				if (coll_counts_obj[i] && overlap_obj[i]) {
+					collided = cloth_bvh_objcollisions_nearcheck(clmd, collmd, &collisions[i], coll_counts_obj[i], overlap_obj[i],
+					                                             (collob->pd->flag & PFIELD_CLOTH_USE_CULLING),
+					                                             (collob->pd->flag & PFIELD_CLOTH_USE_NORMAL)) || collided;
 				}
+			}
 
-				MEM_SAFE_FREE(overlap);
+			if (collided) {
+				ret += cloth_bvh_objcollisions_resolve(clmd, collobjs, collisions, coll_counts_obj, numcollobj, dt);
+				ret2 += ret;
 			}
 
 			for (i = 0; i < numcollobj; i++) {
@@ -777,14 +1299,32 @@ int cloth_bvh_objcollision(Depsgraph *depsgraph, Object *ob, ClothModifierData *
 			}
 
 			MEM_freeN(collisions);
-			MEM_freeN(collisions_index);
+		}
+
+		/* Self collisions. */
+		if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF) {
+			CollPair *collisions = NULL;
+
+			verts = cloth->verts;
+			mvert_num = cloth->mvert_num;
+
+			if (cloth->bvhselftree) {
+				if (coll_count_self && overlap_self) {
+					collisions = (CollPair *)MEM_mallocN(sizeof(CollPair) * coll_count_self, "collision array");
+
+					if (cloth_bvh_selfcollisions_nearcheck(clmd, collisions, coll_count_self, overlap_self)) {
+						ret += cloth_bvh_selfcollisions_resolve(clmd, collisions,  coll_count_self, dt);
+						ret2 += ret;
+					}
+				}
+
+			}
 
-			////////////////////////////////////////////////////////////
-			// update positions
-			// this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
-			////////////////////////////////////////////////////////////
+			MEM_SAFE_FREE(collisions);
+		}
 
-			/* Verts come from clmd. */
+		/* Apply all collision resolution. */
+		if (ret2) {
 			for (i = 0; i < mvert_num; i++) {
 				if (clmd->sim_parms->vgroup_mass > 0) {
 					if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
@@ -794,117 +1334,27 @@ int cloth_bvh_objcollision(Depsgraph *depsgraph, Object *ob, ClothModifierData *
 
 				VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
 			}
-			////////////////////////////////////////////////////////////
 		}
 
 		rounds++;
+	}
+	while (ret2 && (clmd->coll_parms->loop_count > rounds));
 
-		////////////////////////////////////////////////////////////
-		// Test on *simple* selfcollisions
-		////////////////////////////////////////////////////////////
-		if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF ) {
-			for (l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++) {
-				/* TODO: add coll quality rounds again */
-				BVHTreeOverlap *overlap = NULL;
-				unsigned int result = 0;
-
-				// collisions = 1;
-				verts = cloth->verts; // needed for openMP
-
-				/* numfaces = cloth->numfaces; */ /* UNUSED */
-				mvert_num = cloth->mvert_num;
-
-				verts = cloth->verts;
-
-				if ( cloth->bvhselftree ) {
-					// search for overlapping collision pairs
-					overlap = BLI_bvhtree_overlap(cloth->bvhselftree, cloth->bvhselftree, &result, NULL, NULL);
-
-					/* Could be parallelized (using BLI_task)... */
-					for ( k = 0; k < result; k++ ) {
-						float temp[3];
-						float length = 0;
-						float mindistance;
-
-						i = overlap[k].indexA;
-						j = overlap[k].indexB;
-
-						mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
-
-						if (clmd->sim_parms->vgroup_mass > 0) {
-							if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) &&
-							     ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
-							{
-								continue;
-							}
-						}
-
-						if ((cloth->verts[i].flags & CLOTH_VERT_FLAG_NOSELFCOLL) ||
-						    (cloth->verts[j].flags & CLOTH_VERT_FLAG_NOSELFCOLL))
-						{
-							continue;
-						}
-
-						sub_v3_v3v3(temp, verts[i].tx, verts[j].tx);
-
-						if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
-
-						if (BLI_edgeset_haskey(cloth->edgeset, i, j)) {
-							continue;
-						}
-
-						length = normalize_v3(temp );
-
-						if ( length < mindistance ) {
-							float correction = mindistance - length;
-
-							if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
-								mul_v3_fl(temp, -correction);
-								VECADD ( verts[j].tx, verts[j].tx, temp );
-							}
-							else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) {
-								mul_v3_fl(temp, correction);
-								VECADD ( verts[i].tx, verts[i].tx, temp );
-							}
-							else {
-								mul_v3_fl(temp, correction * -0.5f);
-								VECADD ( verts[j].tx, verts[j].tx, temp );
-
-								sub_v3_v3v3(verts[i].tx, verts[i].tx, temp);
-							}
-							ret = 1;
-							ret2 += ret;
-						}
-						else {
-							// check for approximated time collisions
-						}
-					}
-
-					if ( overlap )
-						MEM_freeN ( overlap );
-
-				}
-			}
-			////////////////////////////////////////////////////////////
-
-			////////////////////////////////////////////////////////////
-			// SELFCOLLISIONS: update velocities
-			////////////////////////////////////////////////////////////
-			if (ret2) {
-				for (i = 0; i < cloth->mvert_num; i++) {
-					if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) {
-						sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold);
-					}
-				}
-			}
-			////////////////////////////////////////////////////////////
+	if (overlap_obj) {
+		for (i = 0; i < numcollobj; i++) {
+			MEM_SAFE_FREE(overlap_obj[i]);
 		}
+
+		MEM_freeN(overlap_obj);
 	}
-	while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
+
+	MEM_SAFE_FREE(coll_counts_obj);
+
+	MEM_SAFE_FREE(overlap_self);
 
 	BKE_collision_objects_free(collobjs);
 
-	return 1|MIN2 ( ret, 1 );
+	return MIN2(ret, 1);
 }
 
 BLI_INLINE void max_v3_v3v3(float r[3], const float a[3], const float b[3])
@@ -930,106 +1380,6 @@ void collision_get_collider_velocity(float vel_old[3], float vel_new[3], Collisi
 	copy_v3_v3(vel_old, vel_new);
 }
 
-static bool cloth_points_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, PartDeflect *pd,
-                                                  CollPair *collpair, CollPair *collision_end, float dt)
-{
-	bool result = false;
-	float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - pd->pdef_sbdamp);
-	float inv_dt = 1.0f / dt;
-	Cloth *cloth1 = clmd->clothObject;
-
-	// float w1, w2;
-	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_get_epsilon ( collmd->bvhtree );
-
-	for ( ; collpair != collision_end; collpair++ ) {
-		float margin_distance = (float)(collpair->distance - (double)epsilon2);
-		float impulse[3];
-		float mag_v_rel;
-
-		if (margin_distance > 0.0f)
-			continue;
-
-		zero_v3(impulse);
-
-		/* 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_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);
-
-		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);
-
-		/**** DEBUG ****/
-		BKE_sim_debug_data_add_dot(collpair->pa, 0.9, 0.2, 0.2, "collision", 833, collpair->face1, collpair->face2);
-		BKE_sim_debug_data_add_dot(collpair->pb, 0.2, 0.9, 0.2, "collision", 834, collpair->face1, collpair->face2);
-		BKE_sim_debug_data_add_line(collpair->pa, collpair->pb, 0.8, 0.8, 0.8, "collision", 835, collpair->face1, collpair->face2);
-		/********/
-
-		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 * inv_dt + dot_v3v3(v1, collpair->normal);
-
-			if (margin_distance < -epsilon2) {
-				bounce = -v_nor_new + v_nor_old * restitution;
-				mul_v3_v3fl(impulse, collpair->normal, max_ff(repulse, bounce));
-			}
-			else {
-				bounce = 0.0f;
-				mul_v3_v3fl(impulse, collpair->normal, repulse);
-			}
-			cloth1->verts[collpair->ap1].impulse_count++;
-
-			result = true;
-		}
-
-		if (result) {
-			int i = 0;
-
-			for (i = 0; i < 3; i++) {
-				if (cloth1->verts[collpair->ap1].impulse_count > 0 && fabsf(cloth1->verts[collpair->ap1].impulse[i]) < fabsf(impulse[i]))
-					cloth1->verts[collpair->ap1].impulse[i] = impulse[i];
-			}
-		}
-	}
-	return result;
-}
-
 BLI_INLINE bool cloth_point_face_collision_params(const float p1[3], const float p2[3], const float v0[3], const float v1[3], const float v2[3],
                                                   float r_nor[3], float *r_lambda, float r_w[3])
 {
@@ -1181,160 +1531,6 @@ static void cloth_points_objcollisions_nearcheck(
 	}
 }
 
-static int cloth_points_objcollisions_resolve(
-        ClothModifierData *clmd, CollisionModifierData *collmd, PartDeflect *pd,
-        CollPair *collisions, CollPair *collisions_index, float dt)
-{
-	Cloth *cloth = clmd->clothObject;
-	int i = 0, mvert_num = clmd->clothObject->mvert_num;
-	ClothVertex *verts = cloth->verts;
-	int ret = 0;
-
-	// process all collisions
-	if ( collmd->bvhtree ) {
-		bool result = cloth_points_collision_response_static(clmd, collmd, pd, collisions, collisions_index, dt);
-
-		// apply impulses in parallel
-		if (result) {
-			for (i = 0; i < mvert_num; 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++;
-				}
-			}
-		}
-	}
-
-	return ret;
-}
-
-// cloth - object collisions
-int cloth_points_objcollision(Depsgraph *depsgraph, 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, mvert_num = 0;
-	ClothVertex *verts = NULL;
-	int ret = 0, ret2 = 0;
-	Object **collobjs = NULL;
-	unsigned int numcollobj = 0;
-
-	verts = cloth->verts;
-	mvert_num = cloth->mvert_num;
-
-	////////////////////////////////////////////////////////////
-	// static collisions
-	////////////////////////////////////////////////////////////
-
-	// create temporary cloth points bvh
-	cloth_bvh = BLI_bvhtree_new(mvert_num, max_ff(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6);
-	/* fill tree */
-	for (i = 0; i < mvert_num; i++) {
-		float co[2][3];
-
-		copy_v3_v3(co[0], verts[i].x);
-		copy_v3_v3(co[1], verts[i].tx);
-
-		BLI_bvhtree_insert(cloth_bvh, i, co[0], 2);
-	}
-	/* balance tree */
-	BLI_bvhtree_balance(cloth_bvh);
-
-	collobjs = BKE_collision_objects_create(depsgraph, 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;
-			float epsilon;
-
-			if (!collmd->bvhtree)
-				continue;
-
-			/* search for overlapping collision pairs */
-			overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL);
-			epsilon = BLI_bvhtree_get_epsilon(collmd->bvhtree);
-
-			// 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, epsilon, round_dt);
-
-				// resolve nearby collisions
-				ret += cloth_points_objcollisions_resolve(clmd, collmd, collob->pd, collisions[i], collisions_index[i], round_dt);
-				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 < mvert_num; i++) {
-			if (clmd->sim_parms->vgroup_mass > 0) {
-				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 ) );
-
-	BKE_collision_objects_free(collobjs);
-
-	BLI_bvhtree_free(cloth_bvh);
-
-	return 1|MIN2 ( ret, 1 );
-}
-
 void cloth_find_point_contacts(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt,
                                ColliderContacts **r_collider_contacts, int *r_totcolliders)
 {
@@ -1364,7 +1560,7 @@ void cloth_find_point_contacts(Depsgraph *depsgraph, Object *ob, ClothModifierDa
 	}
 
 	// create temporary cloth points bvh
-	cloth_bvh = BLI_bvhtree_new(mvert_num, max_ff(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6);
+	cloth_bvh = BLI_bvhtree_new(mvert_num, clmd->coll_parms->epsilon, 4, 6);
 	/* fill tree */
 	for (i = 0; i < mvert_num; i++) {
 		float co[6];