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