blender-archive/source/blender/blenkernel/intern/collision.c

/*
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The Original Code is Copyright (C) Blender Foundation
 * All rights reserved.
 */

/** \file
 * \ingroup bke
 */


#include "MEM_guardedalloc.h"

#include "DNA_cloth_types.h"
#include "DNA_collection_types.h"
#include "DNA_effect_types.h"
#include "DNA_object_types.h"
#include "DNA_object_force_types.h"
#include "DNA_scene_types.h"
#include "DNA_meshdata_types.h"

#include "BLI_utildefines.h"
#include "BLI_blenlib.h"
#include "BLI_math.h"
#include "BLI_task.h"
#include "BLI_threads.h"

#include "BKE_cloth.h"
#include "BKE_collection.h"
#include "BKE_effect.h"
#include "BKE_layer.h"
#include "BKE_modifier.h"
#include "BKE_scene.h"

#include "BLI_kdopbvh.h"
#include "BKE_collision.h"

#include "DEG_depsgraph.h"
#include "DEG_depsgraph_physics.h"
#include "DEG_depsgraph_query.h"

#ifdef WITH_ELTOPO
#include "eltopo-capi.h"
#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
***********************************/

/* step is limited from 0 (frame start position) to 1 (frame end position) */
void collision_move_object(CollisionModifierData *collmd, float step, float prevstep)
{
	float oldx[3];
	unsigned int i = 0;

	/* the collider doesn't move this frame */
	if (collmd->is_static) {
		for (i = 0; i < collmd->mvert_num; i++) {
			zero_v3(collmd->current_v[i].co);
		}

		return;
	}

	for (i = 0; i < collmd->mvert_num; i++) {
		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, NULL,
	        collmd->tri, collmd->tri_num, false);
}

BVHTree *bvhtree_build_from_mvert(
        const MVert *mvert,
        const struct MVertTri *tri, int tri_num,
        float epsilon)
{
	BVHTree *tree;
	const MVertTri *vt;
	int i;

	tree = BLI_bvhtree_new(tri_num, epsilon, 4, 26);

	/* fill tree */
	for (i = 0, vt = tri; i < tri_num; i++, vt++) {
		float co[3][3];

		copy_v3_v3(co[0], mvert[vt->tri[0]].co);
		copy_v3_v3(co[1], mvert[vt->tri[1]].co);
		copy_v3_v3(co[2], mvert[vt->tri[2]].co);

		BLI_bvhtree_insert(tree, i, co[0], 3);
	}

	/* balance tree */
	BLI_bvhtree_balance(tree);

	return tree;
}

void bvhtree_update_from_mvert(
        BVHTree *bvhtree,
        const MVert *mvert, const MVert *mvert_moving,
        const MVertTri *tri, int tri_num,
        bool moving)
{
	const MVertTri *vt;
	int i;

	if ((bvhtree == NULL) || (mvert == NULL)) {
		return;
	}

	if (mvert_moving == NULL) {
		moving = false;
	}

	for (i = 0, vt = tri; i < tri_num; i++, vt++) {
		float co[3][3];
		bool ret;

		copy_v3_v3(co[0], mvert[vt->tri[0]].co);
		copy_v3_v3(co[1], mvert[vt->tri[1]].co);
		copy_v3_v3(co[2], mvert[vt->tri[2]].co);

		/* copy new locations into array */
		if (moving) {
			float co_moving[3][3];
			/* update moving positions */
			copy_v3_v3(co_moving[0], mvert_moving[vt->tri[0]].co);
			copy_v3_v3(co_moving[1], mvert_moving[vt->tri[1]].co);
			copy_v3_v3(co_moving[2], mvert_moving[vt->tri[2]].co);

			ret = BLI_bvhtree_update_node(bvhtree, i, &co[0][0], &co_moving[0][0], 3);
		}
		else {
			ret = BLI_bvhtree_update_node(bvhtree, i, &co[0][0], NULL, 3);
		}

		/* check if tree is already full */
		if (ret == false) {
			break;
		}
	}

	BLI_bvhtree_update_tree(bvhtree);
}

/***********************************
Collision modifier code end
***********************************/

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++) {
				isect_seg_seg_v3(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 )
{
	/* dot_v3v3 */
#define INPR(v1, v2) ( (v1)[0] * (v2)[0] + (v1)[1] * (v2)[1] + (v1)[2] * (v2)[2])

	double tempV1[3], tempV2[3], tempV4[3];
	double a, b, c, d, e, f;

	sub_v3db_v3fl_v3fl(tempV1, p1, p3);
	sub_v3db_v3fl_v3fl(tempV2, p2, p3);
	sub_v3db_v3fl_v3fl(tempV4, pv, p3);

	a = INPR ( tempV1, tempV1 );
	b = INPR ( tempV1, tempV2 );
	c = INPR ( tempV2, tempV2 );
	e = INPR ( tempV1, tempV4 );
	f = INPR ( tempV2, tempV4 );

	d = ( a * c - b * b );

	if ( ABS ( d ) < (double)ALMOST_ZERO ) {
		*w1 = *w2 = *w3 = 1.0 / 3.0;
		return;
	}

	w1[0] = ( float ) ( ( e * c - b * f ) / d );

	if ( w1[0] < 0 )
		w1[0] = 0;

	w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );

	if ( w2[0] < 0 )
		w2[0] = 0;

	w3[0] = 1.0f - w1[0] - w2[0];

#undef INPR
}

#ifdef __GNUC__
#  pragma GCC diagnostic push
#  pragma GCC diagnostic ignored "-Wdouble-promotion"
#endif

DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
{
	zero_v3(to);
	VECADDMUL(to, v1, w1);
	VECADDMUL(to, v2, w2);
	VECADDMUL(to, v3, w3);
}

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);

	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,
		                              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(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);

		/* 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];
			float 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(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.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 / 1.5f;

			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->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;

			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.5f;

				VECADDMUL(i1, collpair->normal, impulse);
				VECADDMUL(i2, collpair->normal, impulse);
				VECADDMUL(i3, collpair->normal, impulse);
			}

			result = 1;
		}
		else {
			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;

			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, 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->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;
}

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;
}

#ifdef __GNUC__
#  pragma GCC diagnostic pop
#endif

static void cloth_collision(
        void *__restrict userdata,
        const int index,
        const ParallelRangeTLS *__restrict UNUSED(tls))
{
	ColDetectData *data = (ColDetectData *)userdata;

	ClothModifierData *clmd = data->clmd;
	CollisionModifierData *collmd = data->collmd;
	CollPair *collpair = data->collisions;
	const MVertTri *tri_a, *tri_b;
	ClothVertex *verts1 = clmd->clothObject->verts;
	float distance = 0.0f;
	float epsilon1 = clmd->coll_parms->epsilon;
	float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree);
	float pa[3], pb[3], vect[3];

	tri_a = &clmd->clothObject->tri[data->overlap[index].indexA];
	tri_b = &collmd->tri[data->overlap[index].indexB];

	/* 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);

	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];

		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;
	}
}

static void cloth_selfcollision(
        void *__restrict userdata,
        const int index,
        const ParallelRangeTLS *__restrict UNUSED(tls))
{
	SelfColDetectData *data = (SelfColDetectData *)userdata;

	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;
			}
		}
	}

	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;
	}

	/* 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);

	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];

		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;
	}
}

static void add_collision_object(ListBase *relations, Object *ob, int level, unsigned int modifier_type)
{
	CollisionModifierData *cmd= NULL;

	/* only get objects with collision modifier */
	if (((modifier_type == eModifierType_Collision) && ob->pd && ob->pd->deflect) || (modifier_type != eModifierType_Collision))
		cmd= (CollisionModifierData *)modifiers_findByType(ob, modifier_type);

	if (cmd) {
		CollisionRelation *relation = MEM_callocN(sizeof(CollisionRelation), "CollisionRelation");
		relation->ob = ob;
		BLI_addtail(relations, relation);
	}

	/* objects in dupli groups, one level only for now */
	/* TODO: this doesn't really work, we are not taking into account the
	 * dupli transforms and can get objects in the list multiple times. */
	if (ob->instance_collection && level == 0) {
		Collection *collection= ob->instance_collection;

		/* add objects */
		FOREACH_COLLECTION_OBJECT_RECURSIVE_BEGIN(collection, object)
		{
			add_collision_object(relations, object, level+1, modifier_type);
		}
		FOREACH_COLLECTION_OBJECT_RECURSIVE_END;
	}
}

/* Create list of collision relations in the collection or entire scene.
 * This is used by the depsgraph to build relations, as well as faster
 * lookup of colliders during evaluation. */
ListBase *BKE_collision_relations_create(Depsgraph *depsgraph, Collection *collection, unsigned int modifier_type)
{
	ViewLayer *view_layer = DEG_get_input_view_layer(depsgraph);
	Base *base = BKE_collection_or_layer_objects(view_layer, collection);
	const bool for_render = (DEG_get_mode(depsgraph) == DAG_EVAL_RENDER);
	const int base_flag = (for_render) ? BASE_ENABLED_RENDER : BASE_ENABLED_VIEWPORT;

	ListBase *relations = MEM_callocN(sizeof(ListBase), "CollisionRelation list");

	for (; base; base = base->next) {
		if (base->flag & base_flag) {
			add_collision_object(relations, base->object, 0, modifier_type);
		}
	}

	return relations;
}

void BKE_collision_relations_free(ListBase *relations)
{
	if (relations) {
		BLI_freelistN(relations);
		MEM_freeN(relations);
	}
}

/* Create effective list of colliders from relations built beforehand.
 * Self will be excluded. */
Object **BKE_collision_objects_create(Depsgraph *depsgraph, Object *self, Collection *collection, unsigned int *numcollobj, unsigned int modifier_type)
{
	ListBase *relations = DEG_get_collision_relations(depsgraph, collection, modifier_type);

	if (!relations) {
		*numcollobj = 0;
		return NULL;
	}

	int maxnum = BLI_listbase_count(relations);
	int num = 0;
	Object **objects = MEM_callocN(sizeof(Object*) * maxnum, __func__);

	for (CollisionRelation *relation = relations->first; relation; relation = relation->next) {
		/* Get evaluated object. */
		Object *ob = (Object*)DEG_get_evaluated_id(depsgraph, &relation->ob->id);

		if (ob != self) {
			objects[num] = ob;
			num++;
		}
	}

	if (num == 0) {
		MEM_freeN(objects);
		objects = NULL;
	}

	*numcollobj = num;
	return objects;
}

void BKE_collision_objects_free(Object **objects)
{
	if (objects) {
		MEM_freeN(objects);
	}
}

/* Create effective list of colliders from relations built beforehand.
 * Self will be excluded. */
ListBase *BKE_collider_cache_create(Depsgraph *depsgraph, Object *self, Collection *collection)
{
	ListBase *relations = DEG_get_collision_relations(depsgraph, collection, eModifierType_Collision);
	ListBase *cache = NULL;

	if (!relations) {
		return NULL;
	}

	for (CollisionRelation *relation = relations->first; relation; relation = relation->next) {
		/* Get evaluated object. */
		Object *ob = (Object*)DEG_get_evaluated_id(depsgraph, &relation->ob->id);

		if (ob == self) {
			continue;
		}

		CollisionModifierData *cmd = (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
		if (cmd && cmd->bvhtree) {
			if (cache == NULL) {
				cache = MEM_callocN(sizeof(ListBase), "ColliderCache array");
			}

			ColliderCache *col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
			col->ob = ob;
			col->collmd = cmd;
			/* make sure collider is properly set up */
			collision_move_object(cmd, 1.0, 0.0);
			BLI_addtail(cache, col);
		}
	}

	return cache;
}

void BKE_collider_cache_free(ListBase **colliders)
{
	if (*colliders) {
		BLI_freelistN(*colliders);
		MEM_freeN(*colliders);
		*colliders = NULL;
	}
}

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 bool cloth_bvh_selfcollisions_nearcheck(ClothModifierData * clmd, CollPair *collisions,
                                               int numresult, BVHTreeOverlap *overlap)
{
	SelfColDetectData data = {
		.clmd = clmd,
		.overlap = overlap,
		.collisions = collisions,
		.collided = false,
	};

	ParallelRangeSettings settings;
	BLI_parallel_range_settings_defaults(&settings);
	settings.use_threading = true;
	BLI_task_parallel_range(0, numresult, &data, cloth_selfcollision, &settings);

	return data.collided;
}

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, mvert_num = 0;
	ClothVertex *verts = NULL;
	int ret = 0;
	int result = 0;

	mvert_num = clmd->clothObject->mvert_num;
	verts = cloth->verts;

	result = 1;

	for (j = 0; j < 2; j++) {
		result = 0;

		for (i = 0; i < numcollobj; i++) {
			Object *collob= collobjs[i];
			CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);

			if ( collmd->bvhtree ) {
				result += cloth_collision_response_static(clmd, collmd, collob, collisions[i], collision_counts[i], 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) {
					add_v3_v3(verts[i].tv, verts[i].impulse);
					add_v3_v3(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 );
					add_v3_v3(verts[i].tv, verts[i].impulse);
					add_v3_v3(verts[i].dcvel, verts[i].impulse);
					zero_v3(verts[i].impulse);
					verts[i].impulse_count = 0;

					ret++;
				}
			}
		}

		if (!result) {
			break;
		}
	}
	return ret;
}

int cloth_bvh_collision(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt)
{
	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;
	mvert_num = cloth->mvert_num;

	if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
		bvhtree_update_from_cloth(clmd, false, false);

		collobjs = BKE_collision_objects_create(depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);

		if (collobjs) {
			coll_counts_obj = MEM_callocN(sizeof(uint) * numcollobj, "CollCounts");
			overlap_obj = MEM_callocN(sizeof(*overlap_obj) * numcollobj, "BVHOverlap");

			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);

				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) {
		bvhtree_update_from_cloth(clmd, false, true);

		overlap_self = BLI_bvhtree_overlap(cloth->bvhselftree, cloth->bvhselftree, &coll_count_self, NULL, NULL);
	}

	do {
		ret2 = 0;

		/* Object collisions. */
		if ((clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) && collobjs) {
			CollPair **collisions;
			bool collided = false;

			collisions = MEM_callocN(sizeof(CollPair *) * numcollobj, "CollPair");

			for (i = 0; i < numcollobj; i++) {
				Object *collob = collobjs[i];
				CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision);

				if (!collmd->bvhtree) {
					continue;
				}

				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;
				}
			}

			if (collided) {
				ret += cloth_bvh_objcollisions_resolve(clmd, collobjs, collisions, coll_counts_obj, numcollobj, dt);
				ret2 += ret;
			}

			for (i = 0; i < numcollobj; i++) {
				MEM_SAFE_FREE(collisions[i]);
			}

			MEM_freeN(collisions);
		}

		/* 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;
					}
				}

			}

			MEM_SAFE_FREE(collisions);
		}

		/* 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) {
						continue;
					}
				}

				add_v3_v3v3(verts[i].tx, verts[i].txold, verts[i].tv);
			}
		}

		rounds++;
	}
	while (ret2 && (clmd->coll_parms->loop_count > rounds));

	if (overlap_obj) {
		for (i = 0; i < numcollobj; i++) {
			MEM_SAFE_FREE(overlap_obj[i]);
		}

		MEM_freeN(overlap_obj);
	}

	MEM_SAFE_FREE(coll_counts_obj);

	MEM_SAFE_FREE(overlap_self);

	BKE_collision_objects_free(collobjs);

	return MIN2(ret, 1);
}

BLI_INLINE void max_v3_v3v3(float r[3], const float a[3], const float b[3])
{
	r[0] = max_ff(a[0], b[0]);
	r[1] = max_ff(a[1], b[1]);
	r[2] = max_ff(a[2], b[2]);
}

void collision_get_collider_velocity(float vel_old[3], float vel_new[3], CollisionModifierData *collmd, CollPair *collpair)
{
	float u1, u2, u3;

	/* 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);

	collision_interpolateOnTriangle(vel_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(vel_old, vel_new);
}

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])
{
	float edge1[3], edge2[3], p2face[3], p1p2[3], v0p2[3];
	float nor_v0p2, nor_p1p2;

	sub_v3_v3v3(edge1, v1, v0);
	sub_v3_v3v3(edge2, v2, v0);
	cross_v3_v3v3(r_nor, edge1, edge2);
	normalize_v3(r_nor);

	sub_v3_v3v3(v0p2, p2, v0);
	nor_v0p2 = dot_v3v3(v0p2, r_nor);
	madd_v3_v3v3fl(p2face, p2, r_nor, -nor_v0p2);
	interp_weights_tri_v3(r_w, v0, v1, v2, p2face);

	sub_v3_v3v3(p1p2, p2, p1);
	nor_p1p2 = dot_v3v3(p1p2, r_nor);
	*r_lambda = (nor_p1p2 != 0.0f ? nor_v0p2 / nor_p1p2 : 0.0f);

	return r_w[1] >= 0.0f && r_w[2] >= 0.0f && r_w[1] + r_w[2] <= 1.0f;
}

static CollPair *cloth_point_collpair(
        float p1[3], float p2[3], const MVert *mverts, int bp1, int bp2, int bp3,
        int index_cloth, int index_coll, float epsilon, CollPair *collpair)
{
	const float *co1 = mverts[bp1].co, *co2 = mverts[bp2].co, *co3 = mverts[bp3].co;
	float lambda /*, distance1 */, distance2;
	float facenor[3], v1p1[3], v1p2[3];
	float w[3];

	if (!cloth_point_face_collision_params(p1, p2, co1, co2, co3, facenor, &lambda, w))
		return collpair;

	sub_v3_v3v3(v1p1, p1, co1);
//	distance1 = dot_v3v3(v1p1, facenor);
	sub_v3_v3v3(v1p2, p2, co1);
	distance2 = dot_v3v3(v1p2, facenor);
//	if (distance2 > epsilon || (distance1 < 0.0f && distance2 < 0.0f))
	if (distance2 > epsilon)
		return collpair;

	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;

	/* note: using the second point here, which is
	 * the current updated position that needs to be corrected
	 */
	copy_v3_v3(collpair->pa, p2);
	collpair->distance = distance2;
	mul_v3_v3fl(collpair->vector, facenor, -distance2);

	interp_v3_v3v3v3(collpair->pb, co1, co2, co3, w);

	copy_v3_v3(collpair->normal, facenor);
	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, float epsilon, CollPair *collpair, float UNUSED(dt))
{
	ClothModifierData *clmd = (ClothModifierData *)md1;
	CollisionModifierData *collmd = (CollisionModifierData *) md2;
	/* Cloth *cloth = clmd->clothObject; */ /* UNUSED */
	ClothVertex *vert = NULL;
	const MVertTri *vt;
	const MVert *mverts = collmd->current_x;

	vert = &clmd->clothObject->verts[overlap->indexA];
	vt = &collmd->tri[overlap->indexB];

	collpair = cloth_point_collpair(
	        vert->tx, vert->x, mverts,
	        vt->tri[0], vt->tri[1], vt->tri[2],
	        overlap->indexA, overlap->indexB,
	        epsilon, collpair);

	return collpair;
}

static void cloth_points_objcollisions_nearcheck(
        ClothModifierData *clmd, CollisionModifierData *collmd,
        CollPair **collisions, CollPair **collisions_index,
        int numresult, BVHTreeOverlap *overlap, float epsilon, 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, epsilon, *collisions_index, dt);
	}
}

void cloth_find_point_contacts(Depsgraph *depsgraph, 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, mvert_num = 0;
	ClothVertex *verts = NULL;

	ColliderContacts *collider_contacts;

	Object **collobjs = NULL;
	unsigned int numcollobj = 0;

	verts = cloth->verts;
	mvert_num = cloth->mvert_num;

	////////////////////////////////////////////////////////////
	// static collisions
	////////////////////////////////////////////////////////////

	/* Check we do have collision objects to test against, before doing anything else. */
	collobjs = BKE_collision_objects_create(depsgraph, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
	if (!collobjs) {
		*r_collider_contacts = NULL;
		*r_totcolliders = 0;
		return;
	}

	// create temporary cloth points bvh
	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];

		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);

	/* 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, NULL, NULL);
		epsilon = BLI_bvhtree_get_epsilon(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);
	}

	BKE_collision_objects_free(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 < mvert_num; i++) {
		if (clmd->sim_parms->vgroup_mass > 0) {
			if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
				continue;
			}
		}

		add_v3_v3v3(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);
	}
}