style cleanup: function calls & whitespace.
[blender.git] / source / blender / blenkernel / intern / collision.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) Blender Foundation
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file blender/blenkernel/intern/collision.c
29  *  \ingroup bke
30  */
31
32
33 #include "MEM_guardedalloc.h"
34
35 #include "BKE_cloth.h"
36
37 #include "DNA_cloth_types.h"
38 #include "DNA_group_types.h"
39 #include "DNA_mesh_types.h"
40 #include "DNA_object_types.h"
41 #include "DNA_object_force.h"
42 #include "DNA_scene_types.h"
43 #include "DNA_meshdata_types.h"
44
45 #include "BLI_utildefines.h"
46 #include "BLI_blenlib.h"
47 #include "BLI_math.h"
48 #include "BLI_edgehash.h"
49 #include "BLI_utildefines.h"
50 #include "BLI_ghash.h"
51 #include "BLI_memarena.h"
52 #include "BLI_rand.h"
53
54 #include "BKE_DerivedMesh.h"
55 #include "BKE_global.h"
56 #include "BKE_scene.h"
57 #include "BKE_mesh.h"
58 #include "BKE_object.h"
59 #include "BKE_modifier.h"
60
61 #include "BKE_DerivedMesh.h"
62 #ifdef USE_BULLET
63 #include "Bullet-C-Api.h"
64 #endif
65 #include "BLI_kdopbvh.h"
66 #include "BKE_collision.h"
67
68 #ifdef WITH_ELTOPO
69 #include "eltopo-capi.h"
70 #endif
71
72
73 /***********************************
74 Collision modifier code start
75 ***********************************/
76
77 /* step is limited from 0 (frame start position) to 1 (frame end position) */
78 void collision_move_object(CollisionModifierData *collmd, float step, float prevstep)
79 {
80         float tv[3] = {0, 0, 0};
81         unsigned int i = 0;
82
83         for ( i = 0; i < collmd->numverts; i++ ) {
84                 sub_v3_v3v3(tv, collmd->xnew[i].co, collmd->x[i].co);
85                 VECADDS(collmd->current_x[i].co, collmd->x[i].co, tv, prevstep);
86                 VECADDS(collmd->current_xnew[i].co, collmd->x[i].co, tv, step);
87                 sub_v3_v3v3(collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co);
88         }
89
90         bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
91 }
92
93 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon )
94 {
95         BVHTree *tree;
96         float co[12];
97         unsigned int i;
98         MFace *tface = mfaces;
99
100         tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
101
102         // fill tree
103         for ( i = 0; i < numfaces; i++, tface++ ) {
104                 copy_v3_v3 ( &co[0*3], x[tface->v1].co );
105                 copy_v3_v3 ( &co[1*3], x[tface->v2].co );
106                 copy_v3_v3 ( &co[2*3], x[tface->v3].co );
107                 if ( tface->v4 )
108                         copy_v3_v3 ( &co[3*3], x[tface->v4].co );
109
110                 BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
111         }
112
113         // balance tree
114         BLI_bvhtree_balance ( tree );
115
116         return tree;
117 }
118
119 void bvhtree_update_from_mvert(BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving )
120 {
121         int i;
122         MFace *mfaces = faces;
123         float co[12], co_moving[12];
124         int ret = 0;
125
126         if ( !bvhtree )
127                 return;
128
129         if ( x ) {
130                 for ( i = 0; i < numfaces; i++, mfaces++ ) {
131                         copy_v3_v3 ( &co[0*3], x[mfaces->v1].co );
132                         copy_v3_v3 ( &co[1*3], x[mfaces->v2].co );
133                         copy_v3_v3 ( &co[2*3], x[mfaces->v3].co );
134                         if ( mfaces->v4 )
135                                 copy_v3_v3 ( &co[3*3], x[mfaces->v4].co );
136
137                         // copy new locations into array
138                         if ( moving && xnew ) {
139                                 // update moving positions
140                                 copy_v3_v3 ( &co_moving[0*3], xnew[mfaces->v1].co );
141                                 copy_v3_v3 ( &co_moving[1*3], xnew[mfaces->v2].co );
142                                 copy_v3_v3 ( &co_moving[2*3], xnew[mfaces->v3].co );
143                                 if ( mfaces->v4 )
144                                         copy_v3_v3 ( &co_moving[3*3], xnew[mfaces->v4].co );
145
146                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
147                         }
148                         else {
149                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
150                         }
151
152                         // check if tree is already full
153                         if ( !ret )
154                                 break;
155                 }
156
157                 BLI_bvhtree_update_tree ( bvhtree );
158         }
159 }
160
161 /***********************************
162 Collision modifier code end
163 ***********************************/
164
165 /**
166 * gsl_poly_solve_cubic -
167 *
168 * copied from SOLVE_CUBIC.C --> GSL
169 */
170
171 #define mySWAP(a, b) do { double tmp = b ; b = a ; a = tmp ; } while (0)
172 #if 0 /* UNUSED */
173 static int 
174 gsl_poly_solve_cubic (double a, double b, double c, 
175                                           double *x0, double *x1, double *x2)
176 {
177         double q = (a * a - 3 * b);
178         double r = (2 * a * a * a - 9 * a * b + 27 * c);
179
180         double Q = q / 9;
181         double R = r / 54;
182
183         double Q3 = Q * Q * Q;
184         double R2 = R * R;
185
186         double CR2 = 729 * r * r;
187         double CQ3 = 2916 * q * q * q;
188
189         if (R == 0 && Q == 0)
190         {
191                 *x0 = - a / 3;
192                 *x1 = - a / 3;
193                 *x2 = - a / 3;
194                 return 3;
195         }
196         else if (CR2 == CQ3) 
197         {
198                 /* this test is actually R2 == Q3, written in a form suitable
199                 for exact computation with integers */
200
201                 /* Due to finite precision some double roots may be missed, and
202                 considered to be a pair of complex roots z = x +/- epsilon i
203                 close to the real axis. */
204
205                 double sqrtQ = sqrt (Q);
206
207                 if (R > 0)
208                 {
209                         *x0 = -2 * sqrtQ  - a / 3;
210                         *x1 = sqrtQ - a / 3;
211                         *x2 = sqrtQ - a / 3;
212                 }
213                 else
214                 {
215                         *x0 = - sqrtQ  - a / 3;
216                         *x1 = - sqrtQ - a / 3;
217                         *x2 = 2 * sqrtQ - a / 3;
218                 }
219                 return 3;
220         }
221         else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
222         {
223                 double sqrtQ = sqrt (Q);
224                 double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
225                 double theta = acos (R / sqrtQ3);
226                 double norm = -2 * sqrtQ;
227                 *x0 = norm * cos (theta / 3) - a / 3;
228                 *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3;
229                 *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3;
230
231                 /* Sort *x0, *x1, *x2 into increasing order */
232
233                 if (*x0 > *x1)
234                         mySWAP(*x0, *x1);
235
236                 if (*x1 > *x2)
237                 {
238                         mySWAP(*x1, *x2);
239
240                         if (*x0 > *x1)
241                                 mySWAP(*x0, *x1);
242                 }
243
244                 return 3;
245         }
246         else
247         {
248                 double sgnR = (R >= 0 ? 1 : -1);
249                 double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0);
250                 double B = Q / A;
251                 *x0 = A + B - a / 3;
252                 return 1;
253         }
254 }
255
256
257
258 /**
259 * gsl_poly_solve_quadratic
260 *
261 * copied from GSL
262 */
263 static int 
264 gsl_poly_solve_quadratic (double a, double b, double c, 
265                                                   double *x0, double *x1)
266 {
267         double disc = b * b - 4 * a * c;
268
269         if (a == 0) /* Handle linear case */
270         {
271                 if (b == 0)
272                 {
273                         return 0;
274                 }
275                 else
276                 {
277                         *x0 = -c / b;
278                         return 1;
279                 };
280         }
281
282         if (disc > 0)
283         {
284                 if (b == 0)
285                 {
286                         double r = fabs (0.5 * sqrt (disc) / a);
287                         *x0 = -r;
288                         *x1 =  r;
289                 }
290                 else
291                 {
292                         double sgnb = (b > 0 ? 1 : -1);
293                         double temp = -0.5 * (b + sgnb * sqrt (disc));
294                         double r1 = temp / a;
295                         double r2 = c / temp;
296
297                         if (r1 < r2) 
298                         {
299                                 *x0 = r1;
300                                 *x1 = r2;
301                         } 
302                         else 
303                         {
304                                 *x0 = r2;
305                                 *x1 = r1;
306                         }
307                 }
308                 return 2;
309         }
310         else if (disc == 0) 
311         {
312                 *x0 = -0.5 * b / a;
313                 *x1 = -0.5 * b / a;
314                 return 2;
315         }
316         else
317         {
318                 return 0;
319         }
320 }
321 #endif /* UNUSED */
322
323
324
325 /*
326 * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
327 *     page 4, left column
328 */
329 #if 0
330 static int cloth_get_collision_time ( double a[3], double b[3], double c[3], double d[3], double e[3], double f[3], double solution[3] )
331 {
332         int num_sols = 0;
333
334         // x^0 - checked 
335         double g =      a[0] * c[1] * e[2] - a[0] * c[2] * e[1] +
336                 a[1] * c[2] * e[0] - a[1] * c[0] * e[2] + 
337                 a[2] * c[0] * e[1] - a[2] * c[1] * e[0];
338
339         // x^1
340         double h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
341                 a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
342                 a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
343                 b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
344                 a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
345                 a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
346
347         // x^2
348         double i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
349                 b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
350                 b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
351                 b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
352                 a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
353                 b[2] * c[0] * f[1] - b[0] * c[2] * f[1] + 
354                 a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
355                 b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
356                 a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
357
358         // x^3 - checked
359         double j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
360                 b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
361                 b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
362
363         /*
364         printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]);
365         printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]);
366         printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]);
367
368         printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
369         printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]);
370         printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]);
371
372         printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]);
373         printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]);
374         printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
375
376         printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g);
377         
378 */
379         // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
380         if ( ABS ( j ) > DBL_EPSILON )
381         {
382                 i /= j;
383                 h /= j;
384                 g /= j;
385                 num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] );
386         }
387         else
388         {
389                 num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] );
390                 solution[2] = -1.0;
391         }
392
393         // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0],  solution[1],  solution[2]);
394
395         // Discard negative solutions
396         if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) )
397         {
398                 --num_sols;
399                 solution[0] = solution[num_sols];
400         }
401         if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) )
402         {
403                 --num_sols;
404                 solution[1] = solution[num_sols];
405         }
406         if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) )
407         {
408                 --num_sols;
409         }
410
411         // Sort
412         if ( num_sols == 2 )
413         {
414                 if ( solution[0] > solution[1] )
415                 {
416                         double tmp = solution[0];
417                         solution[0] = solution[1];
418                         solution[1] = tmp;
419                 }
420         }
421         else if ( num_sols == 3 )
422         {
423
424                 // Bubblesort
425                 if ( solution[0] > solution[1] )
426                 {
427                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
428                 }
429                 if ( solution[1] > solution[2] )
430                 {
431                         double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
432                 }
433                 if ( solution[0] > solution[1] )
434                 {
435                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
436                 }
437         }
438
439         return num_sols;
440 }
441 #endif
442
443
444 // w3 is not perfect
445 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
446 {
447         double  tempV1[3], tempV2[3], tempV4[3];
448         double  a, b, c, d, e, f;
449
450         VECSUB ( tempV1, p1, p3 );
451         VECSUB ( tempV2, p2, p3 );
452         VECSUB ( tempV4, pv, p3 );
453
454         a = INPR ( tempV1, tempV1 );
455         b = INPR ( tempV1, tempV2 );
456         c = INPR ( tempV2, tempV2 );
457         e = INPR ( tempV1, tempV4 );
458         f = INPR ( tempV2, tempV4 );
459
460         d = ( a * c - b * b );
461
462         if ( ABS ( d ) < (double)ALMOST_ZERO ) {
463                 *w1 = *w2 = *w3 = 1.0 / 3.0;
464                 return;
465         }
466
467         w1[0] = ( float ) ( ( e * c - b * f ) / d );
468
469         if ( w1[0] < 0 )
470                 w1[0] = 0;
471
472         w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
473
474         if ( w2[0] < 0 )
475                 w2[0] = 0;
476
477         w3[0] = 1.0f - w1[0] - w2[0];
478 }
479
480 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
481 {
482         zero_v3(to);
483         VECADDMUL(to, v1, w1);
484         VECADDMUL(to, v2, w2);
485         VECADDMUL(to, v3, w3);
486 }
487
488 #ifndef WITH_ELTOPO
489 static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
490 {
491         int result = 0;
492         Cloth *cloth1;
493         float w1, w2, w3, u1, u2, u3;
494         float v1[3], v2[3], relativeVelocity[3];
495         float magrelVel;
496         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
497
498         cloth1 = clmd->clothObject;
499
500         for ( ; collpair != collision_end; collpair++ ) {
501                 // only handle static collisions here
502                 if ( collpair->flag & COLLISION_IN_FUTURE )
503                         continue;
504
505                 // compute barycentric coordinates for both collision points
506                 collision_compute_barycentric ( collpair->pa,
507                         cloth1->verts[collpair->ap1].txold,
508                         cloth1->verts[collpair->ap2].txold,
509                         cloth1->verts[collpair->ap3].txold,
510                         &w1, &w2, &w3 );
511
512                 // was: txold
513                 collision_compute_barycentric ( collpair->pb,
514                         collmd->current_x[collpair->bp1].co,
515                         collmd->current_x[collpair->bp2].co,
516                         collmd->current_x[collpair->bp3].co,
517                         &u1, &u2, &u3 );
518
519                 // Calculate relative "velocity".
520                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
521
522                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
523
524                 sub_v3_v3v3(relativeVelocity, v2, v1);
525
526                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
527                 magrelVel = dot_v3v3(relativeVelocity, collpair->normal);
528
529                 // printf("magrelVel: %f\n", magrelVel);
530
531                 // Calculate masses of points.
532                 // TODO
533
534                 // If v_n_mag < 0 the edges are approaching each other.
535                 if ( magrelVel > ALMOST_ZERO ) {
536                         // Calculate Impulse magnitude to stop all motion in normal direction.
537                         float magtangent = 0, repulse = 0, d = 0;
538                         double impulse = 0.0;
539                         float vrel_t_pre[3];
540                         float temp[3], spf;
541
542                         // calculate tangential velocity
543                         copy_v3_v3 ( temp, collpair->normal );
544                         mul_v3_fl(temp, magrelVel);
545                         sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
546
547                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
548                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
549                         magtangent = MIN2(clmd->coll_parms->friction * 0.01f * magrelVel, sqrtf(dot_v3v3(vrel_t_pre, vrel_t_pre)));
550
551                         // Apply friction impulse.
552                         if ( magtangent > ALMOST_ZERO ) {
553                                 normalize_v3(vrel_t_pre);
554
555                                 impulse = magtangent / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); // 2.0 *
556                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
557                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
558                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
559                         }
560
561                         // Apply velocity stopping impulse
562                         // I_c = m * v_N / 2.0
563                         // no 2.0 * magrelVel normally, but looks nicer DG
564                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
565
566                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
567                         cloth1->verts[collpair->ap1].impulse_count++;
568
569                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
570                         cloth1->verts[collpair->ap2].impulse_count++;
571
572                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
573                         cloth1->verts[collpair->ap3].impulse_count++;
574
575                         // Apply repulse impulse if distance too short
576                         // I_r = -min(dt*kd, m(0, 1d/dt - v_n))
577                         spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
578
579                         d = clmd->coll_parms->epsilon*8.0f/9.0f + epsilon2*8.0f/9.0f - collpair->distance;
580                         if ( ( magrelVel < 0.1f*d*spf ) && ( d > ALMOST_ZERO ) ) {
581                                 repulse = MIN2 ( d*1.0f/spf, 0.1f*d*spf - magrelVel );
582
583                                 // stay on the safe side and clamp repulse
584                                 if ( impulse > ALMOST_ZERO )
585                                         repulse = MIN2 ( repulse, 5.0*impulse );
586                                 repulse = MAX2 ( impulse, repulse );
587
588                                 impulse = repulse / ( 1.0f + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
589                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
590                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
591                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
592                         }
593
594                         result = 1;
595                 }
596         }
597         return result;
598 }
599 #endif /* !WITH_ELTOPO */
600
601 #ifdef WITH_ELTOPO
602 typedef struct edgepairkey {
603         int a1, a2, b1, b2;
604 } edgepairkey;
605
606 unsigned int edgepair_hash(void *vkey)
607 {
608         edgepairkey *key = vkey;
609         int keys[4] = {key->a1, key->a2, key->b1, key->b2};
610         int i, j;
611         
612         for (i=0; i<4; i++) {
613                 for (j=0; j<3; j++) {
614                         if (keys[j] >= keys[j+1]) {
615                                 SWAP(int, keys[j], keys[j+1]);
616                         }
617                 }
618         }
619         
620         return keys[0]*101 + keys[1]*72 + keys[2]*53 + keys[3]*34;
621 }
622
623 int edgepair_cmp(const void *va, const void *vb)
624 {
625         edgepairkey *a = va, *b = vb;
626         int keysa[4] = {a->a1, a->a2, a->b1, a->b2};
627         int keysb[4] = {b->a1, b->a2, b->b1, b->b2};
628         int i;
629         
630         for (i=0; i<4; i++) {
631                 int j, ok=0;
632                 for (j=0; j<4; j++) {
633                         if (keysa[i] == keysa[j]) {
634                                 ok = 1;
635                                 break;
636                         }
637                 }
638                 if (!ok)
639                         return -1;
640         }
641         
642         return 0;
643 }
644
645 static void get_edgepairkey(edgepairkey *key, int a1, int a2, int b1, int b2)
646 {
647         key->a1 = a1;
648         key->a2 = a2;
649         key->b1 = b1;
650         key->b2 = b2;
651 }
652
653 /*an immense amount of duplication goes on here. . .a major performance hit, I'm sure*/
654 static CollPair* cloth_edge_collision ( ModifierData *md1, ModifierData *md2, 
655                                                                                 BVHTreeOverlap *overlap, CollPair *collpair,
656                                                                                 GHash *visithash, MemArena *arena)
657 {
658         ClothModifierData *clmd = (ClothModifierData *)md1;
659         CollisionModifierData *collmd = (CollisionModifierData *) md2;
660         MFace *face1=NULL, *face2 = NULL;
661         ClothVertex *verts1 = clmd->clothObject->verts;
662         double distance = 0;
663         edgepairkey *key, tstkey;
664         float epsilon1 = clmd->coll_parms->epsilon;
665         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
666         float no[3], uv[3], t, relnor;
667         int i, i1, i2, i3, i4, i5, i6;
668         Cloth *cloth = clmd->clothObject;
669         float n1[3], n2[3], off[3], v1[2][3], v2[2][3], v3[2][3], v4[2][3], v5[2][3], v6[2][3];
670         void **verts[] = {v1, v2, v3, v4, v5, v6};
671         int j, ret, bp1, bp2, bp3, ap1, ap2, ap3, table[6];
672         
673         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
674         face2 = & ( collmd->mfaces[overlap->indexB] );
675
676         // check all 4 possible collisions
677         for ( i = 0; i < 4; i++ ) {
678                 if ( i == 0 ) {
679                         // fill faceA
680                         ap1 = face1->v1;
681                         ap2 = face1->v2;
682                         ap3 = face1->v3;
683
684                         // fill faceB
685                         bp1 = face2->v1;
686                         bp2 = face2->v2;
687                         bp3 = face2->v3;
688                 }
689                 else if ( i == 1 ) {
690                         if ( face1->v4 ) {
691                                 // fill faceA
692                                 ap1 = face1->v1;
693                                 ap2 = face1->v3;
694                                 ap3 = face1->v4;
695
696                                 // fill faceB
697                                 bp1 = face2->v1;
698                                 bp2 = face2->v2;
699                                 bp3 = face2->v3;
700                         }
701                         else {
702                                 continue;
703                         }
704                 }
705                 if ( i == 2 ) {
706                         if ( face2->v4 ) {
707                                 // fill faceA
708                                 ap1 = face1->v1;
709                                 ap2 = face1->v2;
710                                 ap3 = face1->v3;
711
712                                 // fill faceB
713                                 bp1 = face2->v1;
714                                 bp2 = face2->v3;
715                                 bp3 = face2->v4;
716                         }
717                         else {
718                                 continue;
719                         }
720                 }
721                 else if ( i == 3 ) {
722                         if ( face1->v4 && face2->v4 ) {
723                                 // fill faceA
724                                 ap1 = face1->v1;
725                                 ap2 = face1->v3;
726                                 ap3 = face1->v4;
727
728                                 // fill faceB
729                                 bp1 = face2->v1;
730                                 bp2 = face2->v3;
731                                 bp3 = face2->v4;
732                         }
733                         else {
734                                 continue;
735                         }
736                 }
737                 
738                 copy_v3_v3(v1[0], cloth->verts[ap1].txold); 
739                 copy_v3_v3(v1[1], cloth->verts[ap1].tx);
740                 copy_v3_v3(v2[0], cloth->verts[ap2].txold);
741                 copy_v3_v3(v2[1], cloth->verts[ap2].tx);
742                 copy_v3_v3(v3[0], cloth->verts[ap3].txold);
743                 copy_v3_v3(v3[1], cloth->verts[ap3].tx);
744                 
745                 copy_v3_v3(v4[0], collmd->current_x[bp1].co);
746                 copy_v3_v3(v4[1], collmd->current_xnew[bp1].co);
747                 copy_v3_v3(v5[0], collmd->current_x[bp2].co);
748                 copy_v3_v3(v5[1], collmd->current_xnew[bp2].co);
749                 copy_v3_v3(v6[0], collmd->current_x[bp3].co);
750                 copy_v3_v3(v6[1], collmd->current_xnew[bp3].co);
751                 
752                 normal_tri_v3(n2, v4[1], v5[1], v6[1]);
753
754                 /*offset new positions a bit, to account for margins*/
755                 i1 = ap1; i2 = ap2; i3 = ap3;
756                 i4 = bp1; i5 = bp2; i6 = bp3;
757
758                 for (j=0; j<3; j++) {
759                         int collp1, collp2, k, j2 = (j+1)%3;
760                         
761                         table[0] = ap1; table[1] = ap2; table[2] = ap3;
762                         table[3] = bp1; table[4] = bp2; table[5] = bp3;
763                         for (k=0; k<3; k++) {
764                                 float p1[3], p2[3];
765                                 int k2 = (k+1)%3;
766                                 
767                                 get_edgepairkey(&tstkey, table[j], table[j2], table[k+3], table[k2+3]);
768                                 //if (BLI_ghash_haskey(visithash, &tstkey))
769                                 //      continue;
770                                 
771                                 key = BLI_memarena_alloc(arena, sizeof(edgepairkey));
772                                 *key = tstkey;
773                                 BLI_ghash_insert(visithash, key, NULL);
774
775                                 sub_v3_v3v3(p1, verts[j], verts[j2]);
776                                 sub_v3_v3v3(p2, verts[k+3], verts[k2+3]);
777                                 
778                                 cross_v3_v3v3(off, p1, p2);
779                                 normalize_v3(off);
780
781                                 if (dot_v3v3(n2, off) < 0.0)
782                                         negate_v3(off);
783                                 
784                                 mul_v3_fl(off,  epsilon1 + epsilon2 + ALMOST_ZERO);
785                                 copy_v3_v3(p1, verts[k+3]);
786                                 copy_v3_v3(p2, verts[k2+3]);
787                                 add_v3_v3(p1, off);
788                                 add_v3_v3(p2, off);
789                                 
790                                 ret = eltopo_line_line_moving_isect_v3v3_f(verts[j], table[j], verts[j2], table[j2], 
791                                                                                                                    p1, table[k+3], p2, table[k2+3], 
792                                                                                                                    no, uv, &t, &relnor);
793                                 /*cloth vert versus coll face*/
794                                 if (ret) {
795                                         collpair->ap1 = table[j]; collpair->ap2 = table[j2]; 
796                                         collpair->bp1 = table[k+3]; collpair->bp2 = table[k2+3];
797                                         
798                                         /*I'm not sure if this is correct, but hopefully it's 
799                                           better then simply ignoring back edges*/
800                                         if (dot_v3v3(n2, no) < 0.0) {
801                                                 negate_v3(no);
802                                         }
803                                         
804                                         copy_v3_v3(collpair->normal, no);
805                                         mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
806                                         collpair->distance = relnor;
807                                         collpair->time = t;
808                                         
809                                         copy_v2_v2(collpair->bary, uv);
810                                         
811                                         collpair->flag = COLLISION_IS_EDGES;
812                                         collpair++;
813                                 }
814                         }
815                 }
816         }
817         
818         return collpair;
819 }
820
821 static int cloth_edge_collision_response_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
822 {
823         int result = 0;
824         Cloth *cloth1;
825         float w1, w2;
826         float v1[3], v2[3], relativeVelocity[3];
827         float magrelVel, pimpulse[3];
828
829         cloth1 = clmd->clothObject;
830
831         for ( ; collpair != collision_end; collpair++ ) {
832                 if (!(collpair->flag & COLLISION_IS_EDGES))
833                         continue;
834                 
835                 // was: txold
836                 w1 = collpair->bary[0]; w2 = collpair->bary[1];                 
837                 
838                 // Calculate relative "velocity".
839                 VECADDFAC(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, w1);
840                 VECADDFAC(v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, w2);
841                 
842                 sub_v3_v3v3(relativeVelocity, v2, v1);
843                 
844                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
845                 magrelVel = dot_v3v3 ( relativeVelocity, collpair->normal );
846
847                 // If v_n_mag < 0 the edges are approaching each other.
848                 if ( magrelVel > ALMOST_ZERO ) {
849                         // Calculate Impulse magnitude to stop all motion in normal direction.
850                         float magtangent = 0, repulse = 0, d = 0;
851                         double impulse = 0.0;
852                         float vrel_t_pre[3];
853                         float temp[3], spf;
854                         
855                         zero_v3(pimpulse);
856                         
857                         // calculate tangential velocity
858                         copy_v3_v3 ( temp, collpair->normal );
859                         mul_v3_fl(temp, magrelVel);
860                         sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
861
862                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
863                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
864                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel, sqrt ( dot_v3v3 ( vrel_t_pre, vrel_t_pre ) ) );
865
866                         // Apply friction impulse.
867                         if ( magtangent > ALMOST_ZERO ) {
868                                 normalize_v3(vrel_t_pre);
869
870                                 impulse = magtangent; 
871                                 VECADDMUL ( pimpulse, vrel_t_pre, impulse);
872                         }
873
874                         // Apply velocity stopping impulse
875                         // I_c = m * v_N / 2.0
876                         // no 2.0 * magrelVel normally, but looks nicer DG
877                         impulse =  magrelVel;
878                         
879                         mul_v3_fl(collpair->normal, 0.5);
880                         VECADDMUL ( pimpulse, collpair->normal, impulse);
881
882                         // Apply repulse impulse if distance too short
883                         // I_r = -min(dt*kd, m(0, 1d/dt - v_n))
884                         spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
885
886                         d = collpair->distance;
887                         if ( ( magrelVel < 0.1*d*spf && ( d > ALMOST_ZERO ) ) ) {
888                                 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
889
890                                 // stay on the safe side and clamp repulse
891                                 if ( impulse > ALMOST_ZERO )
892                                         repulse = MIN2 ( repulse, 5.0*impulse );
893                                 repulse = MAX2 ( impulse, repulse );
894
895                                 impulse = repulse / ( 5.0 ); // original 2.0 / 0.25
896                                 VECADDMUL ( pimpulse, collpair->normal, impulse);
897                         }
898                         
899                         w2 = 1.0f-w1;
900                         if (w1 < 0.5)
901                                 w1 *= 2.0;
902                         else
903                                 w2 *= 2.0;
904                         
905                         VECADDFAC(cloth1->verts[collpair->ap1].impulse, cloth1->verts[collpair->ap1].impulse, pimpulse, w1*2.0);
906                         VECADDFAC(cloth1->verts[collpair->ap2].impulse, cloth1->verts[collpair->ap2].impulse, pimpulse, w2*2.0);
907                         
908                         cloth1->verts[collpair->ap1].impulse_count++;
909                         cloth1->verts[collpair->ap2].impulse_count++;
910                         
911                         result = 1;
912                 }
913         } 
914         
915         return result;
916 }
917
918 static int cloth_collision_response_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
919 {
920         int result = 0;
921         Cloth *cloth1;
922         float w1, w2, w3, u1, u2, u3;
923         float v1[3], v2[3], relativeVelocity[3];
924         float magrelVel;
925         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
926         
927         cloth1 = clmd->clothObject;
928
929         for ( ; collpair != collision_end; collpair++ ) {
930                 if (collpair->flag & COLLISION_IS_EDGES)
931                         continue;
932                 
933                 if ( collpair->flag & COLLISION_USE_COLLFACE ) {
934                         // was: txold
935                         w1 = collpair->bary[0]; w2 = collpair->bary[1]; w3 = collpair->bary[2];                 
936
937                         // Calculate relative "velocity".
938                         collision_interpolateOnTriangle ( v1, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, w1, w2, w3);
939                         
940                         sub_v3_v3v3(relativeVelocity, v1, cloth1->verts[collpair->collp].tv);
941                         
942                         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
943                         magrelVel = dot_v3v3 ( relativeVelocity, collpair->normal );
944         
945                         // If v_n_mag < 0 the edges are approaching each other.
946                         if ( magrelVel > ALMOST_ZERO ) {
947                                 // Calculate Impulse magnitude to stop all motion in normal direction.
948                                 float magtangent = 0, repulse = 0, d = 0;
949                                 double impulse = 0.0;
950                                 float vrel_t_pre[3];
951                                 float temp[3], spf;
952         
953                                 // calculate tangential velocity
954                                 copy_v3_v3 ( temp, collpair->normal );
955                                 mul_v3_fl(temp, magrelVel);
956                                 sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
957         
958                                 // Decrease in magnitude of relative tangential velocity due to coulomb friction
959                                 // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
960                                 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel, sqrt ( dot_v3v3 ( vrel_t_pre, vrel_t_pre ) ) );
961         
962                                 // Apply friction impulse.
963                                 if ( magtangent > ALMOST_ZERO ) {
964                                         normalize_v3(vrel_t_pre);
965         
966                                         impulse = magtangent; // 2.0 * 
967                                         VECADDMUL ( cloth1->verts[collpair->collp].impulse, vrel_t_pre, impulse);
968                                 }
969         
970                                 // Apply velocity stopping impulse
971                                 // I_c = m * v_N / 2.0
972                                 // no 2.0 * magrelVel normally, but looks nicer DG
973                                 impulse =  magrelVel/2.0;
974         
975                                 VECADDMUL ( cloth1->verts[collpair->collp].impulse, collpair->normal, impulse);
976                                 cloth1->verts[collpair->collp].impulse_count++;
977         
978                                 // Apply repulse impulse if distance too short
979                                 // I_r = -min(dt*kd, m(0, 1d/dt - v_n))
980                                 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
981         
982                                 d = -collpair->distance;
983                                 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) ) {
984                                         repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
985         
986                                         // stay on the safe side and clamp repulse
987                                         if ( impulse > ALMOST_ZERO )
988                                                 repulse = MIN2 ( repulse, 5.0*impulse );
989                                         repulse = MAX2 ( impulse, repulse );
990         
991                                         impulse = repulse / ( 5.0 ); // original 2.0 / 0.25
992                                         VECADDMUL ( cloth1->verts[collpair->collp].impulse, collpair->normal, impulse);
993                                 }
994         
995                                 result = 1;
996                         }
997                 }
998                 else {
999                         w1 = collpair->bary[0]; w2 = collpair->bary[1]; w3 = collpair->bary[2];                 
1000
1001                         // Calculate relative "velocity".
1002                         collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
1003         
1004                         sub_v3_v3v3(relativeVelocity, collmd->current_v[collpair->collp].co, v1);
1005                         
1006                         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
1007                         magrelVel = dot_v3v3 ( relativeVelocity, collpair->normal );
1008         
1009                         // If v_n_mag < 0 the edges are approaching each other.
1010                         if ( magrelVel > ALMOST_ZERO ) {
1011                                 // Calculate Impulse magnitude to stop all motion in normal direction.
1012                                 float magtangent = 0, repulse = 0, d = 0;
1013                                 double impulse = 0.0;
1014                                 float vrel_t_pre[3], pimpulse[3] = {0.0f, 0.0f, 0.0f};
1015                                 float temp[3], spf;
1016         
1017                                 // calculate tangential velocity
1018                                 copy_v3_v3 ( temp, collpair->normal );
1019                                 mul_v3_fl(temp, magrelVel);
1020                                 sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
1021         
1022                                 // Decrease in magnitude of relative tangential velocity due to coulomb friction
1023                                 // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
1024                                 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel, sqrt ( dot_v3v3 ( vrel_t_pre, vrel_t_pre ) ) );
1025         
1026                                 // Apply friction impulse.
1027                                 if ( magtangent > ALMOST_ZERO ) {
1028                                         normalize_v3(vrel_t_pre);
1029         
1030                                         impulse = magtangent; // 2.0 * 
1031                                         VECADDMUL ( pimpulse, vrel_t_pre, impulse);
1032                                 }
1033         
1034                                 // Apply velocity stopping impulse
1035                                 // I_c = m * v_N / 2.0
1036                                 // no 2.0 * magrelVel normally, but looks nicer DG
1037                                 impulse =  magrelVel/2.0;
1038         
1039                                 VECADDMUL ( pimpulse, collpair->normal, impulse);
1040         
1041                                 // Apply repulse impulse if distance too short
1042                                 // I_r = -min(dt*kd, m(0, 1d/dt - v_n))
1043                                 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
1044         
1045                                 d = -collpair->distance;
1046                                 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) ) {
1047                                         repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
1048         
1049                                         // stay on the safe side and clamp repulse
1050                                         if ( impulse > ALMOST_ZERO )
1051                                                 repulse = MIN2 ( repulse, 5.0*impulse );
1052                                         repulse = MAX2 ( impulse, repulse );
1053         
1054                                         impulse = repulse / ( 2.0 ); // original 2.0 / 0.25
1055                                         VECADDMUL ( pimpulse, collpair->normal, impulse);
1056                                 }
1057                                 
1058                                 if (w1 < 0.5) w1 *= 2.0;
1059                                 if (w2 < 0.5) w2 *= 2.0;
1060                                 if (w3 < 0.5) w3 *= 2.0;
1061                                 
1062                                 VECADDMUL(cloth1->verts[collpair->ap1].impulse, pimpulse, w1*2.0);
1063                                 VECADDMUL(cloth1->verts[collpair->ap2].impulse, pimpulse, w2*2.0);
1064                                 VECADDMUL(cloth1->verts[collpair->ap3].impulse, pimpulse, w3*2.0);
1065                                 cloth1->verts[collpair->ap1].impulse_count++;
1066                                 cloth1->verts[collpair->ap2].impulse_count++;
1067                                 cloth1->verts[collpair->ap3].impulse_count++;
1068                                 
1069                                 result = 1;
1070                         }
1071                 }
1072         } 
1073         
1074         return result;
1075 }
1076
1077
1078 typedef struct tripairkey {
1079         int p, a1, a2, a3;
1080 } tripairkey;
1081
1082 unsigned int tripair_hash(void *vkey)
1083 {
1084         tripairkey *key = vkey;
1085         int keys[4] = {key->p, key->a1, key->a2, key->a3};
1086         int i, j;
1087         
1088         for (i=0; i<4; i++) {
1089                 for (j=0; j<3; j++) {
1090                         if (keys[j] >= keys[j+1]) {
1091                                 SWAP(int, keys[j], keys[j+1]);
1092                         }
1093                 }
1094         }
1095         
1096         return keys[0]*101 + keys[1]*72 + keys[2]*53 + keys[3]*34;
1097 }
1098
1099 int tripair_cmp(const void *va, const void *vb)
1100 {
1101         tripairkey *a = va, *b = vb;
1102         int keysa[4] = {a->p, a->a1, a->a2, a->a3};
1103         int keysb[4] = {b->p, b->a1, b->a2, b->a3};
1104         int i;
1105         
1106         for (i=0; i<4; i++) {
1107                 int j, ok=0;
1108                 for (j=0; j<4; j++) {
1109                         if (keysa[i] == keysa[j]) {
1110                                 ok = 1;
1111                                 break;
1112                         }
1113                 }
1114                 if (!ok)
1115                         return -1;
1116         }
1117         
1118         return 0;
1119 }
1120
1121 static void get_tripairkey(tripairkey *key, int p, int a1, int a2, int a3)
1122 {
1123         key->a1 = a1;
1124         key->a2 = a2;
1125         key->a3 = a3;
1126         key->p = p;
1127 }
1128
1129 static int checkvisit(MemArena *arena, GHash *gh, int p, int a1, int a2, int a3)
1130 {
1131         tripairkey key, *key2;
1132         
1133         get_tripairkey(&key, p, a1, a2, a3);
1134         if (BLI_ghash_haskey(gh, &key))
1135                 return 1;
1136         
1137         key2 = BLI_memarena_alloc(arena, sizeof(*key2));
1138         *key2 = key;
1139         BLI_ghash_insert(gh, key2, NULL);
1140         
1141         return 0;
1142 }
1143
1144 int cloth_point_tri_moving_v3v3_f(float v1[2][3], int i1, float v2[2][3], int i2,
1145                                    float v3[2][3],  int i3, float v4[2][3], int i4,
1146                                    float normal[3], float bary[3], float *t, 
1147                                                                    float *relnor, GHash *gh, MemArena *arena)
1148 {
1149         if (checkvisit(arena, gh, i1, i2, i3, i4))
1150                 return 0;
1151         
1152         return eltopo_point_tri_moving_v3v3_f(v1, i1, v2, i2, v3, i3, v4, i4, normal, bary, t, relnor);
1153 }
1154
1155 static CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, 
1156                                                                    CollPair *collpair, double dt, GHash *gh, MemArena *arena)
1157 {
1158         ClothModifierData *clmd = (ClothModifierData *)md1;
1159         CollisionModifierData *collmd = (CollisionModifierData *) md2;
1160         MFace *face1=NULL, *face2 = NULL;
1161         ClothVertex *verts1 = clmd->clothObject->verts;
1162         double distance = 0;
1163         float epsilon1 = clmd->coll_parms->epsilon;
1164         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
1165         float no[3], uv[3], t, relnor;
1166         int i, i1, i2, i3, i4, i5, i6;
1167         Cloth *cloth = clmd->clothObject;
1168         float n1[3], sdis, p[3], l, n2[3], off[3], v1[2][3], v2[2][3], v3[2][3], v4[2][3], v5[2][3], v6[2][3];
1169         int j, ret, bp1, bp2, bp3, ap1, ap2, ap3;
1170         
1171         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
1172         face2 = & ( collmd->mfaces[overlap->indexB] );
1173
1174         // check all 4 possible collisions
1175         for ( i = 0; i < 4; i++ ) {
1176                 if ( i == 0 ) {
1177                         // fill faceA
1178                         ap1 = face1->v1;
1179                         ap2 = face1->v2;
1180                         ap3 = face1->v3;
1181
1182                         // fill faceB
1183                         bp1 = face2->v1;
1184                         bp2 = face2->v2;
1185                         bp3 = face2->v3;
1186                 }
1187                 else if ( i == 1 ) {
1188                         if ( face1->v4 ) {
1189                                 // fill faceA
1190                                 ap1 = face1->v1;
1191                                 ap2 = face1->v3;
1192                                 ap3 = face1->v4;
1193
1194                                 // fill faceB
1195                                 bp1 = face2->v1;
1196                                 bp2 = face2->v2;
1197                                 bp3 = face2->v3;
1198                         }
1199                         else {
1200                                 continue;
1201                         }
1202                 }
1203                 if ( i == 2 ) {
1204                         if ( face2->v4 ) {
1205                                 // fill faceA
1206                                 ap1 = face1->v1;
1207                                 ap2 = face1->v2;
1208                                 ap3 = face1->v3;
1209
1210                                 // fill faceB
1211                                 bp1 = face2->v1;
1212                                 bp2 = face2->v3;
1213                                 bp3 = face2->v4;
1214                         }
1215                         else {
1216                                 continue;
1217                         }
1218                 }
1219                 else if ( i == 3 ) {
1220                         if ( face1->v4 && face2->v4 ) {
1221                                 // fill faceA
1222                                 ap1 = face1->v1;
1223                                 ap2 = face1->v3;
1224                                 ap3 = face1->v4;
1225
1226                                 // fill faceB
1227                                 bp1 = face2->v1;
1228                                 bp2 = face2->v3;
1229                                 bp3 = face2->v4;
1230                         }
1231                         else {
1232                                 continue;
1233                         }
1234                 }
1235                 
1236                 copy_v3_v3(v1[0], cloth->verts[ap1].txold); 
1237                 copy_v3_v3(v1[1], cloth->verts[ap1].tx);
1238                 copy_v3_v3(v2[0], cloth->verts[ap2].txold);
1239                 copy_v3_v3(v2[1], cloth->verts[ap2].tx);
1240                 copy_v3_v3(v3[0], cloth->verts[ap3].txold);
1241                 copy_v3_v3(v3[1], cloth->verts[ap3].tx);
1242                 
1243                 copy_v3_v3(v4[0], collmd->current_x[bp1].co);
1244                 copy_v3_v3(v4[1], collmd->current_xnew[bp1].co);
1245                 copy_v3_v3(v5[0], collmd->current_x[bp2].co);
1246                 copy_v3_v3(v5[1], collmd->current_xnew[bp2].co);
1247                 copy_v3_v3(v6[0], collmd->current_x[bp3].co);
1248                 copy_v3_v3(v6[1], collmd->current_xnew[bp3].co);
1249                 
1250                 normal_tri_v3(n2, v4[1], v5[1], v6[1]);
1251                 
1252                 sdis = clmd->coll_parms->distance_repel + epsilon2 + FLT_EPSILON;
1253
1254                 /*apply a repulsion force, to help the solver along*/
1255                 copy_v3_v3(off, n2);
1256                 negate_v3(off);
1257                 if (isect_ray_plane_v3(v1[1], off, v4[1], v5[1], v6[1], &l, 0)) {
1258                         if (l >= 0.0 && l < sdis) {
1259                                 mul_v3_fl(off, (l-sdis)*cloth->verts[ap1].mass*dt*clmd->coll_parms->repel_force*0.1);
1260
1261                                 add_v3_v3(cloth->verts[ap1].tv, off);
1262                                 add_v3_v3(cloth->verts[ap2].tv, off);
1263                                 add_v3_v3(cloth->verts[ap3].tv, off);
1264                         }
1265                 }
1266
1267                 /*offset new positions a bit, to account for margins*/
1268                 copy_v3_v3(off, n2);
1269                 mul_v3_fl(off,  epsilon1 + epsilon2 + ALMOST_ZERO);
1270                 add_v3_v3(v4[1], off); add_v3_v3(v5[1], off); add_v3_v3(v6[1], off);
1271                 
1272                 i1 = ap1; i2 = ap2; i3 = ap3;
1273                 i4 = bp1+cloth->numverts; i5 = bp2+cloth->numverts; i6 = bp3+cloth->numverts;
1274                 
1275                 for (j=0; j<6; j++) {
1276                         int collp;
1277
1278                         switch (j) {
1279                         case 0:
1280                                 ret = cloth_point_tri_moving_v3v3_f(v1, i1, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
1281                                 collp = ap1;
1282                                 break;
1283                         case 1:
1284                                 collp = ap2;
1285                                 ret = cloth_point_tri_moving_v3v3_f(v2, i2, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
1286                                 break;
1287                         case 2:
1288                                 collp = ap3;
1289                                 ret = cloth_point_tri_moving_v3v3_f(v3, i3, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
1290                                 break;
1291                         case 3:
1292                                 collp = bp1;
1293                                 ret = cloth_point_tri_moving_v3v3_f(v4, i4, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
1294                                 break;
1295                         case 4:
1296                                 collp = bp2;                            
1297                                 ret = cloth_point_tri_moving_v3v3_f(v5, i5, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
1298                                 break;
1299                         case 5:
1300                                 collp = bp3;
1301                                 ret = cloth_point_tri_moving_v3v3_f(v6, i6, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
1302                                 break;
1303                         }
1304                         
1305                         /*cloth vert versus coll face*/
1306                         if (ret && j < 3) {
1307                                 collpair->bp1 = bp1; collpair->bp2 = bp2; collpair->bp3 = bp3;
1308                                 collpair->collp = collp;
1309                                 
1310                                 copy_v3_v3(collpair->normal, no);
1311                                 mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
1312                                 collpair->distance = relnor;
1313                                 collpair->time = t;
1314                                 
1315                                 copy_v3_v3(collpair->bary, uv);
1316                                 
1317                                 collpair->flag = COLLISION_USE_COLLFACE;
1318                                 collpair++;
1319                         }
1320                         else if (ret && j >= 3) { /*coll vert versus cloth face*/
1321                                 collpair->ap1 = ap1; collpair->ap2 = ap2; collpair->ap3 = ap3;
1322                                 collpair->collp = collp;
1323                                 
1324                                 copy_v3_v3(collpair->normal, no);
1325                                 mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
1326                                 collpair->distance = relnor;
1327                                 collpair->time = t;
1328                                 
1329                                 copy_v3_v3(collpair->bary, uv);
1330
1331                                 collpair->flag = 0;
1332                                 collpair++;
1333                         }
1334                 }
1335         }
1336         
1337         return collpair;
1338 }
1339
1340 static void machine_epsilon_offset(Cloth *cloth)
1341 {
1342         ClothVertex *cv;
1343         int i, j;
1344         
1345         cv = cloth->verts;
1346         for (i=0; i<cloth->numverts; i++, cv++) {
1347                 /*aggrevatingly enough, it's necessary to offset the coordinates
1348                  by a multiple of the 32-bit floating point epsilon when switching
1349                  into doubles*/
1350                 #define RNDSIGN (float)(-1*(BLI_rand()%2==0)|1)
1351                 for (j=0; j<3; j++) {
1352                         cv->tx[j] += FLT_EPSILON*30.0f*RNDSIGN;
1353                         cv->txold[j] += FLT_EPSILON*30.0f*RNDSIGN;
1354                         cv->tv[j] += FLT_EPSILON*30.0f*RNDSIGN;
1355                 }               
1356         }
1357 }
1358
1359 #else /* !WITH_ELTOPO */
1360
1361 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
1362 static CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, 
1363         BVHTreeOverlap *overlap, CollPair *collpair, float dt )
1364 {
1365         ClothModifierData *clmd = (ClothModifierData *)md1;
1366         CollisionModifierData *collmd = (CollisionModifierData *) md2;
1367         Cloth *cloth = clmd->clothObject;
1368         MFace *face1=NULL, *face2 = NULL;
1369 #ifdef USE_BULLET
1370         ClothVertex *verts1 = clmd->clothObject->verts;
1371 #endif
1372         double distance = 0;
1373         float epsilon1 = clmd->coll_parms->epsilon;
1374         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
1375         float n2[3], sdis, l;
1376         int i;
1377
1378         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
1379         face2 = & ( collmd->mfaces[overlap->indexB] );
1380
1381         // check all 4 possible collisions
1382         for ( i = 0; i < 4; i++ ) {
1383                 if ( i == 0 ) {
1384                         // fill faceA
1385                         collpair->ap1 = face1->v1;
1386                         collpair->ap2 = face1->v2;
1387                         collpair->ap3 = face1->v3;
1388
1389                         // fill faceB
1390                         collpair->bp1 = face2->v1;
1391                         collpair->bp2 = face2->v2;
1392                         collpair->bp3 = face2->v3;
1393                 }
1394                 else if ( i == 1 ) {
1395                         if ( face1->v4 ) {
1396                                 // fill faceA
1397                                 collpair->ap1 = face1->v1;
1398                                 collpair->ap2 = face1->v4;
1399                                 collpair->ap3 = face1->v3;
1400
1401                                 // fill faceB
1402                                 collpair->bp1 = face2->v1;
1403                                 collpair->bp2 = face2->v2;
1404                                 collpair->bp3 = face2->v3;
1405                         }
1406                         else
1407                                 i++;
1408                 }
1409                 if ( i == 2 ) {
1410                         if ( face2->v4 ) {
1411                                 // fill faceA
1412                                 collpair->ap1 = face1->v1;
1413                                 collpair->ap2 = face1->v2;
1414                                 collpair->ap3 = face1->v3;
1415
1416                                 // fill faceB
1417                                 collpair->bp1 = face2->v1;
1418                                 collpair->bp2 = face2->v4;
1419                                 collpair->bp3 = face2->v3;
1420                         }
1421                         else
1422                                 break;
1423                 }
1424                 else if ( i == 3 ) {
1425                         if ( face1->v4 && face2->v4 ) {
1426                                 // fill faceA
1427                                 collpair->ap1 = face1->v1;
1428                                 collpair->ap2 = face1->v4;
1429                                 collpair->ap3 = face1->v3;
1430
1431                                 // fill faceB
1432                                 collpair->bp1 = face2->v1;
1433                                 collpair->bp2 = face2->v4;
1434                                 collpair->bp3 = face2->v3;
1435                         }
1436                         else
1437                                 break;
1438                 }
1439                 
1440                 normal_tri_v3(n2, collmd->current_xnew[collpair->bp1].co, 
1441                         collmd->current_xnew[collpair->bp2].co, 
1442                         collmd->current_xnew[collpair->bp3].co);
1443                 
1444                 sdis = clmd->coll_parms->distance_repel + epsilon2 + FLT_EPSILON;
1445                 
1446                 /* apply a repulsion force, to help the solver along.
1447                  * this is kindof crude, it only tests one vert of the triangle */
1448                 if (isect_ray_plane_v3(cloth->verts[collpair->ap1].tx, n2, collmd->current_xnew[collpair->bp1].co, 
1449                         collmd->current_xnew[collpair->bp2].co,
1450                         collmd->current_xnew[collpair->bp3].co, &l, 0))
1451                 {
1452                         if (l >= 0.0f && l < sdis) {
1453                                 mul_v3_fl(n2, (l-sdis)*cloth->verts[collpair->ap1].mass*dt*clmd->coll_parms->repel_force*0.1f);
1454
1455                                 add_v3_v3(cloth->verts[collpair->ap1].tv, n2);
1456                                 add_v3_v3(cloth->verts[collpair->ap2].tv, n2);
1457                                 add_v3_v3(cloth->verts[collpair->ap3].tv, n2);
1458                         }
1459                 }
1460                 
1461 #ifdef USE_BULLET
1462                 // calc distance + normal
1463                 distance = plNearestPoints (
1464                         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 );
1465 #else
1466                 // just be sure that we don't add anything
1467                 distance = 2.0 * (double)( epsilon1 + epsilon2 + ALMOST_ZERO );
1468 #endif
1469
1470                 if (distance <= (epsilon1 + epsilon2 + ALMOST_ZERO)) {
1471                         normalize_v3_v3(collpair->normal, collpair->vector);
1472
1473                         collpair->distance = distance;
1474                         collpair->flag = 0;
1475                         collpair++;
1476                 }/*
1477                 else
1478                 {
1479                         float w1, w2, w3, u1, u2, u3;
1480                         float v1[3], v2[3], relativeVelocity[3];
1481
1482                         // calc relative velocity
1483                         
1484                         // compute barycentric coordinates for both collision points
1485                         collision_compute_barycentric ( collpair->pa,
1486                         verts1[collpair->ap1].txold,
1487                         verts1[collpair->ap2].txold,
1488                         verts1[collpair->ap3].txold,
1489                         &w1, &w2, &w3 );
1490
1491                         // was: txold
1492                         collision_compute_barycentric ( collpair->pb,
1493                         collmd->current_x[collpair->bp1].co,
1494                         collmd->current_x[collpair->bp2].co,
1495                         collmd->current_x[collpair->bp3].co,
1496                         &u1, &u2, &u3 );
1497
1498                         // Calculate relative "velocity".
1499                         collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
1500
1501                         collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
1502
1503                         sub_v3_v3v3(relativeVelocity, v2, v1);
1504
1505                         if (sqrt(dot_v3v3(relativeVelocity, relativeVelocity)) >= distance)
1506                         {
1507                                 // check for collision in the future
1508                                 collpair->flag |= COLLISION_IN_FUTURE;
1509                                 collpair++;
1510                         }
1511                 }*/
1512         }
1513         return collpair;
1514 }
1515 #endif /* WITH_ELTOPO */
1516
1517
1518 #if 0
1519 static int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
1520 {
1521         int result = 0;
1522         Cloth *cloth1;
1523         float w1, w2, w3, u1, u2, u3;
1524         float v1[3], v2[3], relativeVelocity[3];
1525         float magrelVel;
1526
1527         cloth1 = clmd->clothObject;
1528
1529         for ( ; collpair != collision_end; collpair++ )
1530         {
1531                 // compute barycentric coordinates for both collision points
1532                 collision_compute_barycentric ( collpair->pa,
1533                         cloth1->verts[collpair->ap1].txold,
1534                         cloth1->verts[collpair->ap2].txold,
1535                         cloth1->verts[collpair->ap3].txold,
1536                         &w1, &w2, &w3 );
1537
1538                 // was: txold
1539                 collision_compute_barycentric ( collpair->pb,
1540                         collmd->current_x[collpair->bp1].co,
1541                         collmd->current_x[collpair->bp2].co,
1542                         collmd->current_x[collpair->bp3].co,
1543                         &u1, &u2, &u3 );
1544
1545                 // Calculate relative "velocity".
1546                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
1547
1548                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
1549
1550                 sub_v3_v3v3(relativeVelocity, v2, v1);
1551
1552                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
1553                 magrelVel = dot_v3v3 ( relativeVelocity, collpair->normal );
1554
1555                 // printf("magrelVel: %f\n", magrelVel);
1556
1557                 // Calculate masses of points.
1558                 // TODO
1559
1560                 // If v_n_mag < 0 the edges are approaching each other.
1561                 if ( magrelVel > ALMOST_ZERO )
1562                 {
1563                         // Calculate Impulse magnitude to stop all motion in normal direction.
1564                         float magtangent = 0;
1565                         double impulse = 0.0;
1566                         float vrel_t_pre[3];
1567                         float temp[3];
1568
1569                         // calculate tangential velocity
1570                         copy_v3_v3 ( temp, collpair->normal );
1571                         mul_v3_fl(temp, magrelVel);
1572                         sub_v3_v3v3(vrel_t_pre, relativeVelocity, temp);
1573
1574                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
1575                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
1576                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel, sqrt ( dot_v3v3 ( vrel_t_pre, vrel_t_pre ) ) );
1577
1578                         // Apply friction impulse.
1579                         if ( magtangent > ALMOST_ZERO )
1580                         {
1581                                 normalize_v3(vrel_t_pre);
1582
1583                                 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
1584                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
1585                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
1586                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
1587                         }
1588
1589                         // Apply velocity stopping impulse
1590                         // I_c = m * v_N / 2.0
1591                         // no 2.0 * magrelVel normally, but looks nicer DG
1592                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
1593
1594                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
1595                         cloth1->verts[collpair->ap1].impulse_count++;
1596
1597                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
1598                         cloth1->verts[collpair->ap2].impulse_count++;
1599
1600                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
1601                         cloth1->verts[collpair->ap3].impulse_count++;
1602
1603                         // Apply repulse impulse if distance too short
1604                         // I_r = -min(dt*kd, m(0, 1d/dt - v_n))
1605                         /*
1606                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
1607                         if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) )
1608                         {
1609                         repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
1610
1611                         // stay on the safe side and clamp repulse
1612                         if ( impulse > ALMOST_ZERO )
1613                         repulse = MIN2 ( repulse, 5.0*impulse );
1614                         repulse = MAX2 ( impulse, repulse );
1615
1616                         impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
1617                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
1618                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
1619                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
1620                         }
1621                         */
1622                         result = 1;
1623                 }
1624         }
1625         return result;
1626 }
1627 #endif
1628
1629 #if 0
1630 static float projectPointOntoLine(float *p, float *a, float *b) 
1631 {
1632         float ba[3], pa[3];
1633         sub_v3_v3v3(ba, b, a);
1634         sub_v3_v3v3(pa, p, a);
1635         return dot_v3v3(pa, ba) / dot_v3v3(ba, ba);
1636 }
1637
1638 static void calculateEENormal(float *np1, float *np2, float *np3, float *np4, float *out_normal)
1639 {
1640         float line1[3], line2[3];
1641         float length;
1642
1643         sub_v3_v3v3(line1, np2, np1);
1644         sub_v3_v3v3(line2, np3, np1);
1645
1646         // printf("l1: %f, l1: %f, l2: %f, l2: %f\n", line1[0], line1[1], line2[0], line2[1]);
1647
1648         cross_v3_v3v3(out_normal, line1, line2);
1649
1650         
1651
1652         length = normalize_v3(out_normal);
1653         if (length <= FLT_EPSILON)
1654         { // lines are collinear
1655                 sub_v3_v3v3(out_normal, np2, np1);
1656                 normalize_v3(out_normal);
1657         }
1658 }
1659
1660 static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, float *w1, float *w2)
1661 {
1662         float temp[3], temp2[3];
1663         
1664         double a, b, c, e, f; 
1665
1666         sub_v3_v3v3(temp, x2, x1);
1667         a = dot_v3v3(temp, temp);
1668
1669         sub_v3_v3v3(temp2, x4, x3);
1670         b = -dot_v3v3(temp, temp2);
1671
1672         c = dot_v3v3(temp2, temp2);
1673
1674         sub_v3_v3v3(temp2, x3, x1);
1675         e = dot_v3v3(temp, temp2);
1676
1677         sub_v3_v3v3(temp, x4, x3);
1678         f = -dot_v3v3(temp, temp2);
1679
1680         *w1 = (e * c - b * f) / (a * c - b * b);
1681         *w2 = (f - b * *w1) / c;
1682
1683 }
1684
1685 // calculates the distance of 2 edges
1686 static float edgedge_distance(float np11[3], float np12[3], float np21[3], float np22[3], float *out_a1, float *out_a2, float *out_normal)
1687 {
1688         float line1[3], line2[3], cross[3];
1689         float length;
1690         float temp[3], temp2[3];
1691         float dist_a1, dist_a2;
1692         
1693         sub_v3_v3v3(line1, np12, np11);
1694         sub_v3_v3v3(line2, np22, np21);
1695
1696         cross_v3_v3v3(cross, line1, line2);
1697         length = dot_v3v3(cross, cross);
1698
1699         if (length < FLT_EPSILON) 
1700         {
1701                 *out_a2 = projectPointOntoLine(np11, np21, np22);
1702                 if ((*out_a2 >= -FLT_EPSILON) && (*out_a2 <= 1.0 + FLT_EPSILON)) 
1703                 {
1704                         *out_a1 = 0;
1705                         calculateEENormal(np11, np12, np21, np22, out_normal);
1706                         sub_v3_v3v3(temp, np22, np21);
1707                         mul_v3_fl(temp, *out_a2);
1708                         VECADD(temp2, temp, np21);
1709                         VECADD(temp2, temp2, np11);
1710                         return dot_v3v3(temp2, temp2);
1711                 }
1712
1713                 CLAMP(*out_a2, 0.0, 1.0);
1714                 if (*out_a2 > .5) 
1715                 { // == 1.0
1716                         *out_a1 = projectPointOntoLine(np22, np11, np12);
1717                         if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) 
1718                         {
1719                                 calculateEENormal(np11, np12, np21, np22, out_normal);
1720
1721                                 // return (np22 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
1722                                 sub_v3_v3v3(temp, np12, np11);
1723                                 mul_v3_fl(temp, *out_a1);
1724                                 VECADD(temp2, temp, np11);
1725                                 sub_v3_v3v3(temp2, np22, temp2);
1726                                 return dot_v3v3(temp2, temp2);
1727                         }
1728                 } 
1729                 else 
1730                 { // == 0.0
1731                         *out_a1 = projectPointOntoLine(np21, np11, np12);
1732                         if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) 
1733                         {
1734                                 calculateEENormal(np11, np11, np21, np22, out_normal);
1735
1736                                 // return (np21 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
1737                                 sub_v3_v3v3(temp, np12, np11);
1738                                 mul_v3_fl(temp, *out_a1);
1739                                 VECADD(temp2, temp, np11);
1740                                 sub_v3_v3v3(temp2, np21, temp2);
1741                                 return dot_v3v3(temp2, temp2);
1742                         }
1743                 }
1744
1745                 CLAMP(*out_a1, 0.0, 1.0);
1746                 calculateEENormal(np11, np12, np21, np22, out_normal);
1747                 if (*out_a1 > .5)
1748                 {
1749                         if (*out_a2 > .5)
1750                         {
1751                                 sub_v3_v3v3(temp, np12, np22);
1752                         }
1753                         else
1754                         {
1755                                 sub_v3_v3v3(temp, np12, np21);
1756                         }
1757                 }
1758                 else
1759                 {
1760                         if (*out_a2 > .5)
1761                         {
1762                                 sub_v3_v3v3(temp, np11, np22);
1763                         }
1764                         else
1765                         {
1766                                 sub_v3_v3v3(temp, np11, np21);
1767                         }
1768                 }
1769
1770                 return dot_v3v3(temp, temp);
1771         }
1772         else
1773         {
1774                 
1775                 // If the lines aren't parallel (but coplanar) they have to intersect
1776
1777                 findClosestPointsEE(np11, np12, np21, np22, out_a1, out_a2);
1778
1779                 // If both points are on the finite edges, we're done.
1780                 if (*out_a1 >= 0.0 && *out_a1 <= 1.0 && *out_a2 >= 0.0 && *out_a2 <= 1.0) 
1781                 {
1782                         float p1[3], p2[3];
1783                         
1784                         // p1= np11 + (np12 - np11) * out_a1;
1785                         sub_v3_v3v3(temp, np12, np11);
1786                         mul_v3_fl(temp, *out_a1);
1787                         VECADD(p1, np11, temp);
1788                         
1789                         // p2 = np21 + (np22 - np21) * out_a2;
1790                         sub_v3_v3v3(temp, np22, np21);
1791                         mul_v3_fl(temp, *out_a2);
1792                         VECADD(p2, np21, temp);
1793
1794                         calculateEENormal(np11, np12, np21, np22, out_normal);
1795                         sub_v3_v3v3(temp, p1, p2);
1796                         return dot_v3v3(temp, temp);
1797                 }
1798
1799                 
1800                 /*
1801                 * Clamp both points to the finite edges.
1802                 * The one that moves most during clamping is one part of the solution.
1803                 */
1804                 dist_a1 = *out_a1;
1805                 CLAMP(dist_a1, 0.0, 1.0);
1806                 dist_a2 = *out_a2;
1807                 CLAMP(dist_a2, 0.0, 1.0);
1808
1809                 // Now project the "most clamped" point on the other line.
1810                 if (dist_a1 > dist_a2) 
1811                 { 
1812                         /* keep out_a1 */
1813                         float p1[3];
1814
1815                         // p1 = np11 + (np12 - np11) * out_a1;
1816                         sub_v3_v3v3(temp, np12, np11);
1817                         mul_v3_fl(temp, *out_a1);
1818                         VECADD(p1, np11, temp);
1819
1820                         *out_a2 = projectPointOntoLine(p1, np21, np22);
1821                         CLAMP(*out_a2, 0.0, 1.0);
1822
1823                         calculateEENormal(np11, np12, np21, np22, out_normal);
1824
1825                         // return (p1 - (np21 + (np22 - np21) * out_a2)).lengthSquared();
1826                         sub_v3_v3v3(temp, np22, np21);
1827                         mul_v3_fl(temp, *out_a2);
1828                         VECADD(temp, temp, np21);
1829                         sub_v3_v3v3(temp, p1, temp);
1830                         return dot_v3v3(temp, temp);
1831                 } 
1832                 else 
1833                 {       
1834                         /* keep out_a2 */
1835                         float p2[3];
1836                         
1837                         // p2 = np21 + (np22 - np21) * out_a2;
1838                         sub_v3_v3v3(temp, np22, np21);
1839                         mul_v3_fl(temp, *out_a2);
1840                         VECADD(p2, np21, temp);
1841
1842                         *out_a1 = projectPointOntoLine(p2, np11, np12);
1843                         CLAMP(*out_a1, 0.0, 1.0);
1844
1845                         calculateEENormal(np11, np12, np21, np22, out_normal);
1846                         
1847                         // return ((np11 + (np12 - np11) * out_a1) - p2).lengthSquared();
1848                         sub_v3_v3v3(temp, np12, np11);
1849                         mul_v3_fl(temp, *out_a1);
1850                         VECADD(temp, temp, np11);
1851                         sub_v3_v3v3(temp, temp, p2);
1852                         return dot_v3v3(temp, temp);
1853                 }
1854         }
1855         
1856         printf("Error in edgedge_distance: end of function\n");
1857         return 0;
1858 }
1859
1860 static int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
1861 {
1862         EdgeCollPair edgecollpair;
1863         Cloth *cloth1=NULL;
1864         ClothVertex *verts1=NULL;
1865         unsigned int i = 0, k = 0;
1866         int numsolutions = 0;
1867         double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
1868         double solution[3], solution2[3];
1869         MVert *verts2 = collmd->current_x; // old x
1870         MVert *velocity2 = collmd->current_v; // velocity
1871         float distance = 0;
1872         float triA[3][3], triB[3][3];
1873         int result = 0;
1874
1875         cloth1 = clmd->clothObject;
1876         verts1 = cloth1->verts;
1877
1878         for (i = 0; i < 9; i++)
1879         {
1880                 // 9 edge - edge possibilities
1881
1882                 if (i == 0) // cloth edge: 1-2; coll edge: 1-2
1883                 {
1884                         edgecollpair.p11 = collpair->ap1;
1885                         edgecollpair.p12 = collpair->ap2;
1886
1887                         edgecollpair.p21 = collpair->bp1;
1888                         edgecollpair.p22 = collpair->bp2;
1889                 }
1890                 else if (i == 1) // cloth edge: 1-2; coll edge: 2-3
1891                 {
1892                         edgecollpair.p11 = collpair->ap1;
1893                         edgecollpair.p12 = collpair->ap2;
1894
1895                         edgecollpair.p21 = collpair->bp2;
1896                         edgecollpair.p22 = collpair->bp3;
1897                 }
1898                 else if (i == 2) // cloth edge: 1-2; coll edge: 1-3
1899                 {
1900                         edgecollpair.p11 = collpair->ap1;
1901                         edgecollpair.p12 = collpair->ap2;
1902
1903                         edgecollpair.p21 = collpair->bp1;
1904                         edgecollpair.p22 = collpair->bp3;
1905                 }
1906                 else if (i == 3) // cloth edge: 2-3; coll edge: 1-2
1907                 {
1908                         edgecollpair.p11 = collpair->ap2;
1909                         edgecollpair.p12 = collpair->ap3;
1910
1911                         edgecollpair.p21 = collpair->bp1;
1912                         edgecollpair.p22 = collpair->bp2;
1913                 }
1914                 else if (i == 4) // cloth edge: 2-3; coll edge: 2-3
1915                 {
1916                         edgecollpair.p11 = collpair->ap2;
1917                         edgecollpair.p12 = collpair->ap3;
1918
1919                         edgecollpair.p21 = collpair->bp2;
1920                         edgecollpair.p22 = collpair->bp3;
1921                 }
1922                 else if (i == 5) // cloth edge: 2-3; coll edge: 1-3
1923                 {
1924                         edgecollpair.p11 = collpair->ap2;
1925                         edgecollpair.p12 = collpair->ap3;
1926
1927                         edgecollpair.p21 = collpair->bp1;
1928                         edgecollpair.p22 = collpair->bp3;
1929                 }
1930                 else if (i ==6) // cloth edge: 1-3; coll edge: 1-2
1931                 {
1932                         edgecollpair.p11 = collpair->ap1;
1933                         edgecollpair.p12 = collpair->ap3;
1934
1935                         edgecollpair.p21 = collpair->bp1;
1936                         edgecollpair.p22 = collpair->bp2;
1937                 }
1938                 else if (i ==7) // cloth edge: 1-3; coll edge: 2-3
1939                 {
1940                         edgecollpair.p11 = collpair->ap1;
1941                         edgecollpair.p12 = collpair->ap3;
1942
1943                         edgecollpair.p21 = collpair->bp2;
1944                         edgecollpair.p22 = collpair->bp3;
1945                 }
1946                 else if (i == 8) // cloth edge: 1-3; coll edge: 1-3
1947                 {
1948                         edgecollpair.p11 = collpair->ap1;
1949                         edgecollpair.p12 = collpair->ap3;
1950
1951                         edgecollpair.p21 = collpair->bp1;
1952                         edgecollpair.p22 = collpair->bp3;
1953                 }
1954                 /*
1955                 if ((edgecollpair.p11 == 3) && (edgecollpair.p12 == 16))
1956                         printf("Ahier!\n");
1957                 if ((edgecollpair.p11 == 16) && (edgecollpair.p12 == 3))
1958                         printf("Ahier!\n");
1959                 */
1960
1961                 // if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
1962                 {
1963                         // always put coll points in p21/p22
1964                         sub_v3_v3v3(x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold);
1965                         sub_v3_v3v3(v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv);
1966
1967                         sub_v3_v3v3(x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold);
1968                         sub_v3_v3v3(v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv);
1969
1970                         sub_v3_v3v3(x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold);
1971                         sub_v3_v3v3(v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv);
1972
1973                         numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
1974
1975                         if ((edgecollpair.p11 == 3 && edgecollpair.p12==16)|| (edgecollpair.p11==16 && edgecollpair.p12==3))
1976                         {
1977                                 if (edgecollpair.p21==6 || edgecollpair.p22 == 6)
1978                                 {
1979                                         printf("dist: %f, sol[k]: %f, sol2[k]: %f\n", distance, solution[k], solution2[k]);
1980                                         printf("a1: %f, a2: %f, b1: %f, b2: %f\n", x1[0], x2[0], x3[0], v1[0]);
1981                                         printf("b21: %d, b22: %d\n", edgecollpair.p21, edgecollpair.p22);
1982                                 }
1983                         }
1984
1985                         for ( k = 0; k < numsolutions; k++ )
1986                         {
1987                                 // printf("sol %d: %lf\n", k, solution[k]);
1988                                 if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) && ( solution[k] >  ALMOST_ZERO))
1989                                 {
1990                                         float a, b;
1991                                         float out_normal[3];
1992                                         float distance;
1993                                         float impulse = 0;
1994                                         float I_mag;
1995
1996                                         // move verts
1997                                         VECADDS(triA[0], verts1[edgecollpair.p11].txold, verts1[edgecollpair.p11].tv, solution[k]);
1998                                         VECADDS(triA[1], verts1[edgecollpair.p12].txold, verts1[edgecollpair.p12].tv, solution[k]);
1999
2000                                         VECADDS(triB[0], collmd->current_x[edgecollpair.p21].co, collmd->current_v[edgecollpair.p21].co, solution[k]);
2001                                         VECADDS(triB[1], collmd->current_x[edgecollpair.p22].co, collmd->current_v[edgecollpair.p22].co, solution[k]);
2002
2003                                         // TODO: check for collisions
2004                                         distance = edgedge_distance(triA[0], triA[1], triB[0], triB[1], &a, &b, out_normal);
2005                                         
2006                                         if ((distance <= clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree ) + ALMOST_ZERO) && (dot_v3v3(out_normal, out_normal) > 0))
2007                                         {
2008                                                 float vrel_1_to_2[3], temp[3], temp2[3], out_normalVelocity;
2009                                                 float desiredVn;
2010
2011                                                 copy_v3_v3(vrel_1_to_2, verts1[edgecollpair.p11].tv);
2012                                                 mul_v3_fl(vrel_1_to_2, 1.0 - a);
2013                                                 copy_v3_v3(temp, verts1[edgecollpair.p12].tv);
2014                                                 mul_v3_fl(temp, a);
2015
2016                                                 VECADD(vrel_1_to_2, vrel_1_to_2, temp);
2017
2018                                                 copy_v3_v3(temp, verts1[edgecollpair.p21].tv);
2019                                                 mul_v3_fl(temp, 1.0 - b);
2020                                                 copy_v3_v3(temp2, verts1[edgecollpair.p22].tv);
2021                                                 mul_v3_fl(temp2, b);
2022                                                 VECADD(temp, temp, temp2);
2023
2024                                                 sub_v3_v3v3(vrel_1_to_2, vrel_1_to_2, temp);
2025
2026                                                 out_normalVelocity = dot_v3v3(vrel_1_to_2, out_normal);
2027 /*
2028                                                 // this correction results in wrong normals sometimes?
2029                                                 if (out_normalVelocity < 0.0)
2030                                                 {
2031                                                         out_normalVelocity*= -1.0;
2032                                                         negate_v3(out_normal);
2033                                                 }
2034 */
2035                                                 /* Inelastic repulsion impulse. */
2036
2037                                                 // Calculate which normal velocity we need. 
2038                                                 desiredVn = (out_normalVelocity * (float)solution[k] - (.1 * (clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree )) - sqrt(distance)) - ALMOST_ZERO);
2039
2040                                                 // Now calculate what impulse we need to reach that velocity. 
2041                                                 I_mag = (out_normalVelocity - desiredVn) / 2.0; // / (1/m1 + 1/m2);
2042
2043                                                 // Finally apply that impulse. 
2044                                                 impulse = (2.0 * -I_mag) / (a*a + (1.0-a)*(1.0-a) + b*b + (1.0-b)*(1.0-b));
2045
2046                                                 VECADDMUL ( verts1[edgecollpair.p11].impulse, out_normal, (1.0-a) * impulse );
2047                                                 verts1[edgecollpair.p11].impulse_count++;
2048
2049                                                 VECADDMUL ( verts1[edgecollpair.p12].impulse, out_normal, a * impulse );
2050                                                 verts1[edgecollpair.p12].impulse_count++;
2051
2052                                                 // return true;
2053                                                 result = 1;
2054                                                 break;
2055                                         }
2056                                         else
2057                                         {
2058                                                 // missing from collision.hpp
2059                                         }
2060                                         // mintime = MIN2(mintime, (float)solution[k]);
2061
2062                                         break;
2063                                 }
2064                         }
2065                 }
2066         }
2067         return result;
2068 }
2069
2070 static int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
2071 {
2072         Cloth *cloth1;
2073         cloth1 = clmd->clothObject;
2074
2075         for ( ; collpair != collision_end; collpair++ )
2076         {
2077                 // only handle moving collisions here
2078                 if (!( collpair->flag & COLLISION_IN_FUTURE ))
2079                         continue;
2080
2081                 cloth_collision_moving_edges ( clmd, collmd, collpair);
2082                 // cloth_collision_moving_tris ( clmd, collmd, collpair);
2083         }
2084
2085         return 1;
2086 }
2087 #endif
2088
2089 static void add_collision_object(Object ***objs, unsigned int *numobj, unsigned int *maxobj, Object *ob, Object *self, int level, unsigned int modifier_type)
2090 {
2091         CollisionModifierData *cmd= NULL;
2092
2093         if (ob == self)
2094                 return;
2095
2096         /* only get objects with collision modifier */
2097         if (((modifier_type == eModifierType_Collision) && ob->pd && ob->pd->deflect) || (modifier_type != eModifierType_Collision))
2098                 cmd= (CollisionModifierData *)modifiers_findByType(ob, modifier_type);
2099         
2100         if (cmd) {      
2101                 /* extend array */
2102                 if (*numobj >= *maxobj) {
2103                         *maxobj *= 2;
2104                         *objs= MEM_reallocN(*objs, sizeof(Object*)*(*maxobj));
2105                 }
2106                 
2107                 (*objs)[*numobj] = ob;
2108                 (*numobj)++;
2109         }
2110
2111         /* objects in dupli groups, one level only for now */
2112         if (ob->dup_group && level == 0) {
2113                 GroupObject *go;
2114                 Group *group= ob->dup_group;
2115
2116                 /* add objects */
2117                 for (go= group->gobject.first; go; go= go->next)
2118                         add_collision_object(objs, numobj, maxobj, go->ob, self, level+1, modifier_type);
2119         }       
2120 }
2121
2122 // return all collision objects in scene
2123 // collision object will exclude self 
2124 Object **get_collisionobjects(Scene *scene, Object *self, Group *group, unsigned int *numcollobj, unsigned int modifier_type)
2125 {
2126         Base *base;
2127         Object **objs;
2128         GroupObject *go;
2129         unsigned int numobj= 0, maxobj= 100;
2130         
2131         objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray");
2132
2133         /* gather all collision objects */
2134         if (group) {
2135                 /* use specified group */
2136                 for (go= group->gobject.first; go; go= go->next)
2137                         add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0, modifier_type);
2138         }
2139         else {
2140                 Scene *sce_iter;
2141                 /* add objects in same layer in scene */
2142                 for (SETLOOPER(scene, sce_iter, base)) {
2143                         if (base->lay & self->lay)
2144                                 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0, modifier_type);
2145
2146                 }
2147         }
2148
2149         *numcollobj= numobj;
2150
2151         return objs;
2152 }
2153
2154 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level)
2155 {
2156         CollisionModifierData *cmd= NULL;
2157         ColliderCache *col;
2158
2159         if (ob == self)
2160                 return;
2161
2162         if (ob->pd && ob->pd->deflect)
2163                 cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
2164         
2165         if (cmd && cmd->bvhtree) {      
2166                 if (*objs == NULL)
2167                         *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array");
2168
2169                 col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
2170                 col->ob = ob;
2171                 col->collmd = cmd;
2172                 /* make sure collider is properly set up */
2173                 collision_move_object(cmd, 1.0, 0.0);
2174                 BLI_addtail(*objs, col);
2175         }
2176
2177         /* objects in dupli groups, one level only for now */
2178         if (ob->dup_group && level == 0) {
2179                 GroupObject *go;
2180                 Group *group= ob->dup_group;
2181
2182                 /* add objects */
2183                 for (go= group->gobject.first; go; go= go->next)
2184                         add_collider_cache_object(objs, go->ob, self, level+1);
2185         }
2186 }
2187
2188 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group)
2189 {
2190         GroupObject *go;
2191         ListBase *objs= NULL;
2192         
2193         /* add object in same layer in scene */
2194         if (group) {
2195                 for (go= group->gobject.first; go; go= go->next)
2196                         add_collider_cache_object(&objs, go->ob, self, 0);
2197         }
2198         else {
2199                 Scene *sce_iter;
2200                 Base *base;
2201
2202                 /* add objects in same layer in scene */
2203                 for (SETLOOPER(scene, sce_iter, base)) {
2204                         if (!self || (base->lay & self->lay))
2205                                 add_collider_cache_object(&objs, base->object, self, 0);
2206
2207                 }
2208         }
2209
2210         return objs;
2211 }
2212
2213 void free_collider_cache(ListBase **colliders)
2214 {
2215         if (*colliders) {
2216                 BLI_freelistN(*colliders);
2217                 MEM_freeN(*colliders);
2218                 *colliders = NULL;
2219         }
2220 }
2221
2222
2223 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd,
2224         CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap, double dt)
2225 {
2226         int i;
2227 #ifdef WITH_ELTOPO
2228         GHash *visithash = BLI_ghash_new(edgepair_hash, edgepair_cmp, "visthash, collision.c");
2229         GHash *tri_visithash = BLI_ghash_new(tripair_hash, tripair_cmp, "tri_visthash, collision.c");
2230         MemArena *arena = BLI_memarena_new(1<<16, "edge hash arena, collision.c");
2231 #endif
2232         
2233         *collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * numresult * 64, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
2234         *collisions_index = *collisions;
2235         
2236 #ifdef WITH_ELTOPO
2237         machine_epsilon_offset(clmd->clothObject);
2238
2239         for ( i = 0; i < numresult; i++ ) {
2240                 *collisions_index = cloth_collision ( (ModifierData *)clmd, (ModifierData *)collmd,
2241                                                                                           overlap+i, *collisions_index, dt, tri_visithash, arena );
2242         }
2243
2244         for ( i = 0; i < numresult; i++ ) {
2245                 *collisions_index = cloth_edge_collision ( (ModifierData *)clmd, (ModifierData *)collmd,
2246                                                                                                    overlap+i, *collisions_index, visithash, arena );
2247         }
2248         BLI_ghash_free(visithash, NULL, NULL);
2249         BLI_ghash_free(tri_visithash, NULL, NULL);
2250         BLI_memarena_free(arena);
2251 #else /* WITH_ELTOPO */
2252         for ( i = 0; i < numresult; i++ ) {
2253                 *collisions_index = cloth_collision ( (ModifierData *)clmd, (ModifierData *)collmd,
2254                                                                                           overlap+i, *collisions_index, dt );
2255         }
2256 #endif /* WITH_ELTOPO */
2257
2258 }
2259
2260 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
2261 {
2262         Cloth *cloth = clmd->clothObject;
2263         int i=0, j = 0, /*numfaces = 0, */ numverts = 0;
2264         ClothVertex *verts = NULL;
2265         int ret = 0;
2266         int result = 0;
2267         float tnull[3] = {0, 0, 0};
2268         
2269         /*numfaces = clmd->clothObject->numfaces;*/ /*UNUSED*/
2270         numverts = clmd->clothObject->numverts;
2271  
2272         verts = cloth->verts;
2273         
2274         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
2275         result = 1;
2276         for ( j = 0; j < 5; j++ ) { /* 5 is just a value that ensures convergence */
2277                 result = 0;
2278
2279                 if ( collmd->bvhtree ) {
2280 #ifdef WITH_ELTOPO
2281                         result += cloth_collision_response_moving(clmd, collmd, collisions, collisions_index);
2282                         result += cloth_edge_collision_response_moving(clmd, collmd, collisions, collisions_index);
2283 #else
2284                         result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
2285 #endif
2286 #ifdef WITH_ELTOPO
2287                         {
2288 #else
2289                         // apply impulses in parallel
2290                         if (result) {
2291 #endif
2292                                 for (i = 0; i < numverts; i++) {
2293                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
2294                                         if (verts[i].impulse_count) {
2295                                                 VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
2296                                                 copy_v3_v3 ( verts[i].impulse, tnull );
2297                                                 verts[i].impulse_count = 0;
2298
2299                                                 ret++;
2300                                         }
2301                                 }
2302                         }
2303                 }
2304         }
2305         return ret;
2306 }
2307
2308 // cloth - object collisions
2309 int cloth_bvh_objcollision(Object *ob, ClothModifierData * clmd, float step, float dt )
2310 {
2311         Cloth *cloth= clmd->clothObject;
2312         BVHTree *cloth_bvh= cloth->bvhtree;
2313         unsigned int i=0, /* numfaces = 0, */ /* UNUSED */ numverts = 0, k, l, j;
2314         int rounds = 0; // result counts applied collisions; ic is for debug output;
2315         ClothVertex *verts = NULL;
2316         int ret = 0, ret2 = 0;
2317         Object **collobjs = NULL;
2318         unsigned int numcollobj = 0;
2319
2320         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
2321                 return 0;
2322         
2323         verts = cloth->verts;
2324         /* numfaces = cloth->numfaces; */ /* UNUSED */
2325         numverts = cloth->numverts;
2326
2327         ////////////////////////////////////////////////////////////
2328         // static collisions
2329         ////////////////////////////////////////////////////////////
2330
2331         // update cloth bvh
2332         bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
2333         bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
2334         
2335         collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision);
2336         
2337         if (!collobjs)
2338                 return 0;
2339
2340         do
2341         {
2342                 CollPair **collisions, **collisions_index;
2343                 
2344                 ret2 = 0;
2345
2346                 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
2347                 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair");
2348                 
2349                 // check all collision objects
2350                 for (i = 0; i < numcollobj; i++) {
2351                         Object *collob= collobjs[i];
2352                         CollisionModifierData *collmd = (CollisionModifierData*)modifiers_findByType(collob, eModifierType_Collision);
2353                         BVHTreeOverlap *overlap = NULL;
2354                         unsigned int result = 0;
2355                         
2356                         if (!collmd->bvhtree)
2357                                 continue;
2358                         
2359                         /* move object to position (step) in time */
2360                         
2361                         collision_move_object ( collmd, step + dt, step );
2362                         
2363                         /* search for overlapping collision pairs */
2364                         overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
2365                                 
2366                         // go to next object if no overlap is there
2367                         if ( result && overlap ) {
2368                                 /* check if collisions really happen (costly near check) */
2369                                 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], 
2370                                         &collisions_index[i], result, overlap, dt/(float)clmd->coll_parms->loop_count);
2371                         
2372                                 // resolve nearby collisions
2373                                 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i],  collisions_index[i]);
2374                                 ret2 += ret;
2375                         }
2376
2377                         if ( overlap )
2378                                 MEM_freeN ( overlap );
2379                 }
2380                 rounds++;
2381                 
2382                 for (i = 0; i < numcollobj; i++) {
2383                         if ( collisions[i] ) MEM_freeN ( collisions[i] );
2384                 }
2385                         
2386                 MEM_freeN(collisions);
2387                 MEM_freeN(collisions_index);
2388
2389                 ////////////////////////////////////////////////////////////
2390                 // update positions
2391                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
2392                 ////////////////////////////////////////////////////////////
2393
2394                 // verts come from clmd
2395                 for ( i = 0; i < numverts; i++ ) {
2396                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
2397                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
2398                                         continue;
2399                                 }
2400                         }
2401
2402                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
2403                 }
2404                 ////////////////////////////////////////////////////////////
2405                 
2406                 
2407                 ////////////////////////////////////////////////////////////
2408                 // Test on *simple* selfcollisions
2409                 ////////////////////////////////////////////////////////////
2410                 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF ) {
2411                         for (l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++) {
2412                                 // TODO: add coll quality rounds again
2413                                 BVHTreeOverlap *overlap = NULL;
2414                                 unsigned int result = 0;
2415         
2416                                 // collisions = 1;
2417                                 verts = cloth->verts; // needed for openMP
2418         
2419                                 /* numfaces = cloth->numfaces; */ /* UNUSED */
2420                                 numverts = cloth->numverts;
2421         
2422                                 verts = cloth->verts;
2423         
2424                                 if ( cloth->bvhselftree ) {
2425                                         // search for overlapping collision pairs
2426                                         overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
2427         
2428         // #pragma omp parallel for private(k, i, j) schedule(static)
2429                                         for ( k = 0; k < result; k++ ) {
2430                                                 float temp[3];
2431                                                 float length = 0;
2432                                                 float mindistance;
2433         
2434                                                 i = overlap[k].indexA;
2435                                                 j = overlap[k].indexB;
2436         
2437                                                 mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
2438         
2439                                                 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) {
2440                                                         if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) &&
2441                                                              ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
2442                                                         {
2443                                                                 continue;
2444                                                         }
2445                                                 }
2446         
2447                                                 sub_v3_v3v3(temp, verts[i].tx, verts[j].tx);
2448         
2449                                                 if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
2450         
2451                                                 // check for adjacent points (i must be smaller j)
2452                                                 if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) ) {
2453                                                         continue;
2454                                                 }
2455         
2456                                                 length = normalize_v3(temp );
2457         
2458                                                 if ( length < mindistance ) {
2459                                                         float correction = mindistance - length;
2460         
2461                                                         if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) {
2462                                                                 mul_v3_fl(temp, -correction);
2463                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
2464                                                         }
2465                                                         else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) {
2466                                                                 mul_v3_fl(temp, correction);
2467                                                                 VECADD ( verts[i].tx, verts[i].tx, temp );
2468                                                         }
2469                                                         else {
2470                                                                 mul_v3_fl(temp, correction * -0.5);
2471                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
2472         
2473                                                                 sub_v3_v3v3(verts[i].tx, verts[i].tx, temp);
2474                                                         }
2475                                                         ret = 1;
2476                                                         ret2 += ret;
2477                                                 }
2478                                                 else {
2479                                                         // check for approximated time collisions
2480                                                 }
2481                                         }
2482         
2483                                         if ( overlap )
2484                                                 MEM_freeN ( overlap );
2485         
2486                                 }
2487                         }
2488                         ////////////////////////////////////////////////////////////
2489
2490                         ////////////////////////////////////////////////////////////
2491                         // SELFCOLLISIONS: update velocities
2492                         ////////////////////////////////////////////////////////////
2493                         if ( ret2 ) {
2494                                 for ( i = 0; i < cloth->numverts; i++ ) {
2495                                         if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) {
2496                                                 sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold);
2497                                         }
2498                                 }
2499                         }
2500                         ////////////////////////////////////////////////////////////
2501                 }
2502         }
2503         while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
2504         
2505         if (collobjs)
2506                 MEM_freeN(collobjs);
2507
2508         return 1|MIN2 ( ret, 1 );
2509 }