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