build system changes to eltopo, re-applied.
[blender.git] / source / blender / blenkernel / intern / collision.c
1 /*
2  * $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  *
20  * The Original Code is Copyright (C) Blender Foundation
21  * All rights reserved.
22  *
23  * The Original Code is: all of this file.
24  *
25  * Contributor(s): none yet.
26  *
27  * ***** END GPL LICENSE BLOCK *****
28  */
29
30 /** \file blender/blenkernel/intern/collision.c
31  *  \ingroup bke
32  */
33
34
35 #include "MEM_guardedalloc.h"
36
37 #include "BKE_cloth.h"
38
39 #include "DNA_cloth_types.h"
40 #include "DNA_group_types.h"
41 #include "DNA_mesh_types.h"
42 #include "DNA_object_types.h"
43 #include "DNA_object_force.h"
44 #include "DNA_scene_types.h"
45 #include "DNA_meshdata_types.h"
46
47 #include "BLI_blenlib.h"
48 #include "BLI_math.h"
49 #include "BLI_edgehash.h"
50 #include "BLI_utildefines.h"
51 #include "BLI_ghash.h"
52 #include "BLI_memarena.h"
53 #include "BLI_rand.h"
54
55 #include "BKE_DerivedMesh.h"
56 #include "BKE_global.h"
57 #include "BKE_scene.h"
58 #include "BKE_mesh.h"
59 #include "BKE_object.h"
60 #include "BKE_modifier.h"
61
62 #include "BKE_DerivedMesh.h"
63 #ifdef USE_BULLET
64 #include "Bullet-C-Api.h"
65 #endif
66 #include "BLI_kdopbvh.h"
67 #include "BKE_collision.h"
68
69 #ifdef WITH_ELTOPO
70 #include "eltopo-capi.h"
71 #endif
72
73
74 /***********************************
75 Collision modifier code start
76 ***********************************/
77
78 /* step is limited from 0 (frame start position) to 1 (frame end position) */
79 void collision_move_object ( CollisionModifierData *collmd, float step, float prevstep )
80 {
81         float tv[3] = {0, 0, 0};
82         unsigned int i = 0;
83
84         for ( i = 0; i < collmd->numverts; i++ )
85         {
86                 VECSUB ( tv, collmd->xnew[i].co, collmd->x[i].co );
87                 VECADDS ( collmd->current_x[i].co, collmd->x[i].co, tv, prevstep );
88                 VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step );
89                 VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co );
90         }
91
92         bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
93 }
94
95 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon )
96 {
97         BVHTree *tree;
98         float co[12];
99         unsigned int i;
100         MFace *tface = mfaces;
101
102         tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
103
104         // fill tree
105         for ( i = 0; i < numfaces; i++, tface++ )
106         {
107                 VECCOPY ( &co[0*3], x[tface->v1].co );
108                 VECCOPY ( &co[1*3], x[tface->v2].co );
109                 VECCOPY ( &co[2*3], x[tface->v3].co );
110                 if ( tface->v4 )
111                         VECCOPY ( &co[3*3], x[tface->v4].co );
112
113                 BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
114         }
115
116         // balance tree
117         BLI_bvhtree_balance ( tree );
118
119         return tree;
120 }
121
122 void bvhtree_update_from_mvert ( BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving )
123 {
124         int i;
125         MFace *mfaces = faces;
126         float co[12], co_moving[12];
127         int ret = 0;
128
129         if ( !bvhtree )
130                 return;
131
132         if ( x )
133         {
134                 for ( i = 0; i < numfaces; i++, mfaces++ )
135                 {
136                         VECCOPY ( &co[0*3], x[mfaces->v1].co );
137                         VECCOPY ( &co[1*3], x[mfaces->v2].co );
138                         VECCOPY ( &co[2*3], x[mfaces->v3].co );
139                         if ( mfaces->v4 )
140                                 VECCOPY ( &co[3*3], x[mfaces->v4].co );
141
142                         // copy new locations into array
143                         if ( moving && xnew )
144                         {
145                                 // update moving positions
146                                 VECCOPY ( &co_moving[0*3], xnew[mfaces->v1].co );
147                                 VECCOPY ( &co_moving[1*3], xnew[mfaces->v2].co );
148                                 VECCOPY ( &co_moving[2*3], xnew[mfaces->v3].co );
149                                 if ( mfaces->v4 )
150                                         VECCOPY ( &co_moving[3*3], xnew[mfaces->v4].co );
151
152                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
153                         }
154                         else
155                         {
156                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
157                         }
158
159                         // check if tree is already full
160                         if ( !ret )
161                                 break;
162                 }
163
164                 BLI_bvhtree_update_tree ( bvhtree );
165         }
166 }
167
168 /***********************************
169 Collision modifier code end
170 ***********************************/
171
172 /**
173 * gsl_poly_solve_cubic -
174 *
175 * copied from SOLVE_CUBIC.C --> GSL
176 */
177
178 #define mySWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0)
179 #if 0 /* UNUSED */
180 static int 
181 gsl_poly_solve_cubic (double a, double b, double c, 
182                                           double *x0, double *x1, double *x2)
183 {
184         double q = (a * a - 3 * b);
185         double r = (2 * a * a * a - 9 * a * b + 27 * c);
186
187         double Q = q / 9;
188         double R = r / 54;
189
190         double Q3 = Q * Q * Q;
191         double R2 = R * R;
192
193         double CR2 = 729 * r * r;
194         double CQ3 = 2916 * q * q * q;
195
196         if (R == 0 && Q == 0)
197         {
198                 *x0 = - a / 3 ;
199                 *x1 = - a / 3 ;
200                 *x2 = - a / 3 ;
201                 return 3 ;
202         }
203         else if (CR2 == CQ3) 
204         {
205                 /* this test is actually R2 == Q3, written in a form suitable
206                 for exact computation with integers */
207
208                 /* Due to finite precision some double roots may be missed, and
209                 considered to be a pair of complex roots z = x +/- epsilon i
210                 close to the real axis. */
211
212                 double sqrtQ = sqrt (Q);
213
214                 if (R > 0)
215                 {
216                         *x0 = -2 * sqrtQ  - a / 3;
217                         *x1 = sqrtQ - a / 3;
218                         *x2 = sqrtQ - a / 3;
219                 }
220                 else
221                 {
222                         *x0 = - sqrtQ  - a / 3;
223                         *x1 = - sqrtQ - a / 3;
224                         *x2 = 2 * sqrtQ - a / 3;
225                 }
226                 return 3 ;
227         }
228         else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
229         {
230                 double sqrtQ = sqrt (Q);
231                 double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
232                 double theta = acos (R / sqrtQ3);
233                 double norm = -2 * sqrtQ;
234                 *x0 = norm * cos (theta / 3) - a / 3;
235                 *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3;
236                 *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3;
237
238                 /* Sort *x0, *x1, *x2 into increasing order */
239
240                 if (*x0 > *x1)
241                         mySWAP(*x0, *x1) ;
242
243                 if (*x1 > *x2)
244                 {
245                         mySWAP(*x1, *x2) ;
246
247                         if (*x0 > *x1)
248                                 mySWAP(*x0, *x1) ;
249                 }
250
251                 return 3;
252         }
253         else
254         {
255                 double sgnR = (R >= 0 ? 1 : -1);
256                 double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0);
257                 double B = Q / A ;
258                 *x0 = A + B - a / 3;
259                 return 1;
260         }
261 }
262
263
264
265 /**
266 * gsl_poly_solve_quadratic
267 *
268 * copied from GSL
269 */
270 static int 
271 gsl_poly_solve_quadratic (double a, double b, double c, 
272                                                   double *x0, double *x1)
273 {
274         double disc = b * b - 4 * a * c;
275
276         if (a == 0) /* Handle linear case */
277         {
278                 if (b == 0)
279                 {
280                         return 0;
281                 }
282                 else
283                 {
284                         *x0 = -c / b;
285                         return 1;
286                 };
287         }
288
289         if (disc > 0)
290         {
291                 if (b == 0)
292                 {
293                         double r = fabs (0.5 * sqrt (disc) / a);
294                         *x0 = -r;
295                         *x1 =  r;
296                 }
297                 else
298                 {
299                         double sgnb = (b > 0 ? 1 : -1);
300                         double temp = -0.5 * (b + sgnb * sqrt (disc));
301                         double r1 = temp / a ;
302                         double r2 = c / temp ;
303
304                         if (r1 < r2) 
305                         {
306                                 *x0 = r1 ;
307                                 *x1 = r2 ;
308                         } 
309                         else 
310                         {
311                                 *x0 = r2 ;
312                                 *x1 = r1 ;
313                         }
314                 }
315                 return 2;
316         }
317         else if (disc == 0) 
318         {
319                 *x0 = -0.5 * b / a ;
320                 *x1 = -0.5 * b / a ;
321                 return 2 ;
322         }
323         else
324         {
325                 return 0;
326         }
327 }
328 #endif /* UNUSED */
329
330
331
332 /*
333 * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
334 *     page 4, left column
335 */
336 #if 0
337 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] )
338 {
339         int num_sols = 0;
340
341         // x^0 - checked 
342         double g =      a[0] * c[1] * e[2] - a[0] * c[2] * e[1] +
343                 a[1] * c[2] * e[0] - a[1] * c[0] * e[2] + 
344                 a[2] * c[0] * e[1] - a[2] * c[1] * e[0];
345
346         // x^1
347         double h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
348                 a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
349                 a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
350                 b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
351                 a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
352                 a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
353
354         // x^2
355         double i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
356                 b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
357                 b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
358                 b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
359                 a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
360                 b[2] * c[0] * f[1] - b[0] * c[2] * f[1] + 
361                 a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
362                 b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
363                 a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
364
365         // x^3 - checked
366         double j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
367                 b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
368                 b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
369
370         /*
371         printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]);
372         printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]);
373         printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]);
374
375         printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
376         printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]);
377         printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]);
378
379         printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]);
380         printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]);
381         printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
382
383         printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g);
384         
385 */
386         // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
387         if ( ABS ( j ) > DBL_EPSILON )
388         {
389                 i /= j;
390                 h /= j;
391                 g /= j;
392                 num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] );
393         }
394         else
395         {
396                 num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] );
397                 solution[2] = -1.0;
398         }
399
400         // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0],  solution[1],  solution[2]);
401
402         // Discard negative solutions
403         if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) )
404         {
405                 --num_sols;
406                 solution[0] = solution[num_sols];
407         }
408         if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) )
409         {
410                 --num_sols;
411                 solution[1] = solution[num_sols];
412         }
413         if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) )
414         {
415                 --num_sols;
416         }
417
418         // Sort
419         if ( num_sols == 2 )
420         {
421                 if ( solution[0] > solution[1] )
422                 {
423                         double tmp = solution[0];
424                         solution[0] = solution[1];
425                         solution[1] = tmp;
426                 }
427         }
428         else if ( num_sols == 3 )
429         {
430
431                 // Bubblesort
432                 if ( solution[0] > solution[1] )
433                 {
434                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
435                 }
436                 if ( solution[1] > solution[2] )
437                 {
438                         double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
439                 }
440                 if ( solution[0] > solution[1] )
441                 {
442                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
443                 }
444         }
445
446         return num_sols;
447 }
448 #endif
449
450
451 // w3 is not perfect
452 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
453 {
454         double  tempV1[3], tempV2[3], tempV4[3];
455         double  a,b,c,d,e,f;
456
457         VECSUB ( tempV1, p1, p3 );
458         VECSUB ( tempV2, p2, p3 );
459         VECSUB ( tempV4, pv, p3 );
460
461         a = INPR ( tempV1, tempV1 );
462         b = INPR ( tempV1, tempV2 );
463         c = INPR ( tempV2, tempV2 );
464         e = INPR ( tempV1, tempV4 );
465         f = INPR ( tempV2, tempV4 );
466
467         d = ( a * c - b * b );
468
469         if ( ABS ( d ) < ALMOST_ZERO )
470         {
471                 *w1 = *w2 = *w3 = 1.0 / 3.0;
472                 return;
473         }
474
475         w1[0] = ( float ) ( ( e * c - b * f ) / d );
476
477         if ( w1[0] < 0 )
478                 w1[0] = 0;
479
480         w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
481
482         if ( w2[0] < 0 )
483                 w2[0] = 0;
484
485         w3[0] = 1.0f - w1[0] - w2[0];
486 }
487
488 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
489 {
490         to[0] = to[1] = to[2] = 0;
491         VECADDMUL ( to, v1, w1 );
492         VECADDMUL ( to, v2, w2 );
493         VECADDMUL ( to, v3, w3 );
494 }
495
496 #ifndef WITH_ELTOPO
497 static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
498 {
499         int result = 0;
500         Cloth *cloth1;
501         float w1, w2, w3, u1, u2, u3;
502         float v1[3], v2[3], relativeVelocity[3];
503         float magrelVel;
504         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
505
506         cloth1 = clmd->clothObject;
507
508         for ( ; collpair != collision_end; collpair++ )
509         {
510                 // only handle static collisions here
511                 if ( collpair->flag & COLLISION_IN_FUTURE )
512                         continue;
513
514                 // compute barycentric coordinates for both collision points
515                 collision_compute_barycentric ( collpair->pa,
516                         cloth1->verts[collpair->ap1].txold,
517                         cloth1->verts[collpair->ap2].txold,
518                         cloth1->verts[collpair->ap3].txold,
519                         &w1, &w2, &w3 );
520
521                 // was: txold
522                 collision_compute_barycentric ( collpair->pb,
523                         collmd->current_x[collpair->bp1].co,
524                         collmd->current_x[collpair->bp2].co,
525                         collmd->current_x[collpair->bp3].co,
526                         &u1, &u2, &u3 );
527
528                 // Calculate relative "velocity".
529                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
530
531                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
532
533                 VECSUB ( relativeVelocity, v2, v1 );
534
535                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
536                 magrelVel = INPR ( relativeVelocity, collpair->normal );
537
538                 // printf("magrelVel: %f\n", magrelVel);
539
540                 // Calculate masses of points.
541                 // TODO
542
543                 // If v_n_mag < 0 the edges are approaching each other.
544                 if ( magrelVel > ALMOST_ZERO )
545                 {
546                         // Calculate Impulse magnitude to stop all motion in normal direction.
547                         float magtangent = 0, repulse = 0, d = 0;
548                         double impulse = 0.0;
549                         float vrel_t_pre[3];
550                         float temp[3], spf;
551
552                         // calculate tangential velocity
553                         VECCOPY ( temp, collpair->normal );
554                         mul_v3_fl( temp, magrelVel );
555                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
556
557                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
558                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
559                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
560
561                         // Apply friction impulse.
562                         if ( magtangent > ALMOST_ZERO )
563                         {
564                                 normalize_v3( vrel_t_pre );
565
566                                 impulse = magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // 2.0 * 
567                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
568                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
569                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
570                         }
571
572                         // Apply velocity stopping impulse
573                         // I_c = m * v_N / 2.0
574                         // no 2.0 * magrelVel normally, but looks nicer DG
575                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
576
577                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
578                         cloth1->verts[collpair->ap1].impulse_count++;
579
580                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
581                         cloth1->verts[collpair->ap2].impulse_count++;
582
583                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
584                         cloth1->verts[collpair->ap3].impulse_count++;
585
586                         // Apply repulse impulse if distance too short
587                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
588                         spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
589
590                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
591                         if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) )
592                         {
593                                 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
594
595                                 // stay on the safe side and clamp repulse
596                                 if ( impulse > ALMOST_ZERO )
597                                         repulse = MIN2 ( repulse, 5.0*impulse );
598                                 repulse = MAX2 ( impulse, repulse );
599
600                                 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
601                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
602                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
603                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
604                         }
605
606                         result = 1;
607                 }
608         }
609         return result;
610 }
611 #endif /* !WITH_ELTOPO */
612
613 #ifdef WITH_ELTOPO
614 typedef struct edgepairkey {
615         int a1, a2, b1, b2;
616 } edgepairkey;
617
618 unsigned int edgepair_hash(void *vkey)
619 {
620         edgepairkey *key = vkey;
621         int keys[4] = {key->a1, key->a2, key->b1, key->b2};
622         int i, j;
623         
624         for (i=0; i<4; i++) {
625                 for (j=0; j<3; j++) {
626                         if (keys[j] >= keys[j+1]) {
627                                 SWAP(int, keys[j], keys[j+1]);
628                         }
629                 }
630         }
631         
632         return keys[0]*101 + keys[1]*72 + keys[2]*53 + keys[3]*34;
633 }
634
635 int edgepair_cmp(const void *va, const void *vb)
636 {
637         edgepairkey *a = va, *b = vb;
638         int keysa[4] = {a->a1, a->a2, a->b1, a->b2};
639         int keysb[4] = {b->a1, b->a2, b->b1, b->b2};
640         int i;
641         
642         for (i=0; i<4; i++) {
643                 int j, ok=0;
644                 for (j=0; j<4; j++) {
645                         if (keysa[i] == keysa[j]) {
646                                 ok = 1;
647                                 break;
648                         }
649                 }
650                 if (!ok)
651                         return -1;
652         }
653         
654         return 0;
655 }
656
657 static void get_edgepairkey(edgepairkey *key, int a1, int a2, int b1, int b2)
658 {
659         key->a1 = a1;
660         key->a2 = a2;
661         key->b1 = b1;
662         key->b2 = b2;
663 }
664
665 /*an immense amount of duplication goes on here. . .a major performance hit, I'm sure*/
666 static CollPair* cloth_edge_collision ( ModifierData *md1, ModifierData *md2, 
667                                                                                 BVHTreeOverlap *overlap, CollPair *collpair,
668                                                                                 GHash *visithash, MemArena *arena)
669 {
670         ClothModifierData *clmd = ( ClothModifierData * ) md1;
671         CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
672         MFace *face1=NULL, *face2 = NULL;
673         ClothVertex *verts1 = clmd->clothObject->verts;
674         double distance = 0;
675         edgepairkey *key, tstkey;
676         float epsilon1 = clmd->coll_parms->epsilon;
677         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
678         float no[3], uv[3], t, relnor;
679         int i, i1, i2, i3, i4, i5, i6;
680         Cloth *cloth = clmd->clothObject;
681         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];
682         void **verts[] = {v1, v2, v3, v4, v5, v6};
683         int j, ret, bp1, bp2, bp3, ap1, ap2, ap3, table[6];
684         
685         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
686         face2 = & ( collmd->mfaces[overlap->indexB] );
687
688         // check all 4 possible collisions
689         for ( i = 0; i < 4; i++ )
690         {
691                 if ( i == 0 )
692                 {
693                         // fill faceA
694                         ap1 = face1->v1;
695                         ap2 = face1->v2;
696                         ap3 = face1->v3;
697
698                         // fill faceB
699                         bp1 = face2->v1;
700                         bp2 = face2->v2;
701                         bp3 = face2->v3;
702                 }
703                 else if ( i == 1 )
704                 {
705                         if ( face1->v4 )
706                         {
707                                 // fill faceA
708                                 ap1 = face1->v1;
709                                 ap2 = face1->v3;
710                                 ap3 = face1->v4;
711
712                                 // fill faceB
713                                 bp1 = face2->v1;
714                                 bp2 = face2->v2;
715                                 bp3 = face2->v3;
716                         }
717                         else {
718                                 continue;
719                         }
720                 }
721                 if ( i == 2 )
722                 {
723                         if ( face2->v4 )
724                         {
725                                 // fill faceA
726                                 ap1 = face1->v1;
727                                 ap2 = face1->v2;
728                                 ap3 = face1->v3;
729
730                                 // fill faceB
731                                 bp1 = face2->v1;
732                                 bp2 = face2->v3;
733                                 bp3 = face2->v4;
734                         }
735                         else {
736                                 continue;
737                         }
738                 }
739                 else if ( i == 3 )
740                 {
741                         if ( face1->v4 && face2->v4 )
742                         {
743                                 // fill faceA
744                                 ap1 = face1->v1;
745                                 ap2 = face1->v3;
746                                 ap3 = face1->v4;
747
748                                 // fill faceB
749                                 bp1 = face2->v1;
750                                 bp2 = face2->v3;
751                                 bp3 = face2->v4;
752                         }
753                         else {
754                                 continue;
755                         }
756                 }
757                 
758                 copy_v3_v3(v1[0], cloth->verts[ap1].txold); 
759                 copy_v3_v3(v1[1], cloth->verts[ap1].tx);
760                 copy_v3_v3(v2[0], cloth->verts[ap2].txold);
761                 copy_v3_v3(v2[1], cloth->verts[ap2].tx);
762                 copy_v3_v3(v3[0], cloth->verts[ap3].txold);
763                 copy_v3_v3(v3[1], cloth->verts[ap3].tx);
764                 
765                 copy_v3_v3(v4[0], collmd->current_x[bp1].co);
766                 copy_v3_v3(v4[1], collmd->current_xnew[bp1].co);
767                 copy_v3_v3(v5[0], collmd->current_x[bp2].co);
768                 copy_v3_v3(v5[1], collmd->current_xnew[bp2].co);
769                 copy_v3_v3(v6[0], collmd->current_x[bp3].co);
770                 copy_v3_v3(v6[1], collmd->current_xnew[bp3].co);
771                 
772                 normal_tri_v3(n2, v4[1], v5[1], v6[1]);
773
774                 /*offset new positions a bit, to account for margins*/
775                 i1 = ap1; i2 = ap2; i3 = ap3;
776                 i4 = bp1; i5 = bp2; i6 = bp3;
777
778                 for (j=0; j<3; j++) {
779                         int collp1, collp2, k, j2 = (j+1)%3;
780                         
781                         table[0] = ap1; table[1] = ap2; table[2] = ap3;
782                         table[3] = bp1; table[4] = bp2; table[5] = bp3;
783                         for (k=0; k<3; k++) {
784                                 float p1[3], p2[3];
785                                 int k2 = (k+1)%3;
786                                 
787                                 get_edgepairkey(&tstkey, table[j], table[j2], table[k+3], table[k2+3]);
788                                 //if (BLI_ghash_haskey(visithash, &tstkey))
789                                 //      continue;
790                                 
791                                 key = BLI_memarena_alloc(arena, sizeof(edgepairkey));
792                                 *key = tstkey;
793                                 BLI_ghash_insert(visithash, key, NULL);
794
795                                 sub_v3_v3v3(p1, verts[j], verts[j2]);
796                                 sub_v3_v3v3(p2, verts[k+3], verts[k2+3]);
797                                 
798                                 cross_v3_v3v3(off, p1, p2);
799                                 normalize_v3(off);
800
801                                 if (dot_v3v3(n2, off) < 0.0)
802                                         negate_v3(off);
803                                 
804                                 mul_v3_fl(off,  epsilon1 + epsilon2 + ALMOST_ZERO);
805                                 copy_v3_v3(p1, verts[k+3]);
806                                 copy_v3_v3(p2, verts[k2+3]);
807                                 add_v3_v3(p1, off);
808                                 add_v3_v3(p2, off);
809                                 
810                                 ret = eltopo_line_line_moving_isect_v3v3_f(verts[j], table[j], verts[j2], table[j2], 
811                                                                                                                    p1, table[k+3], p2, table[k2+3], 
812                                                                                                                    no, uv, &t, &relnor);
813                                 /*cloth vert versus coll face*/
814                                 if (ret) {
815                                         collpair->ap1 = table[j]; collpair->ap2 = table[j2]; 
816                                         collpair->bp1 = table[k+3]; collpair->bp2 = table[k2+3];
817                                         
818                                         /*I'm not sure if this is correct, but hopefully it's 
819                                           better then simply ignoring back edges*/
820                                         if (dot_v3v3(n2, no) < 0.0) {
821                                                 negate_v3(no);
822                                         }
823                                         
824                                         copy_v3_v3(collpair->normal, no);
825                                         mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
826                                         collpair->distance = relnor;
827                                         collpair->time = t;
828                                         
829                                         copy_v2_v2(collpair->bary, uv);
830                                         
831                                         collpair->flag = COLLISION_IS_EDGES;
832                                         collpair++;
833                                 }
834                         }
835                 }
836         }
837         
838         return collpair;
839 }
840
841 static int cloth_edge_collision_response_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
842 {
843         int result = 0;
844         Cloth *cloth1;
845         float w1, w2;
846         float v1[3], v2[3], relativeVelocity[3];
847         float magrelVel, pimpulse[3];
848
849         cloth1 = clmd->clothObject;
850
851         for ( ; collpair != collision_end; collpair++ )
852         {
853                 if (!(collpair->flag & COLLISION_IS_EDGES))
854                         continue;
855                 
856                 // was: txold
857                 w1 = collpair->bary[0]; w2 = collpair->bary[1];                 
858                 
859                 // Calculate relative "velocity".
860                 VECADDFAC(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, w1);
861                 VECADDFAC(v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, w2);
862                 
863                 VECSUB ( relativeVelocity, v2, v1);
864                 
865                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
866                 magrelVel = INPR ( relativeVelocity, collpair->normal );
867
868                 // If v_n_mag < 0 the edges are approaching each other.
869                 if ( magrelVel > ALMOST_ZERO )
870                 {
871                         // Calculate Impulse magnitude to stop all motion in normal direction.
872                         float magtangent = 0, repulse = 0, d = 0;
873                         double impulse = 0.0;
874                         float vrel_t_pre[3];
875                         float temp[3], spf;
876                         
877                         zero_v3(pimpulse);
878                         
879                         // calculate tangential velocity
880                         VECCOPY ( temp, collpair->normal );
881                         mul_v3_fl( temp, magrelVel );
882                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
883
884                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
885                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
886                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
887
888                         // Apply friction impulse.
889                         if ( magtangent > ALMOST_ZERO )
890                         {
891                                 normalize_v3( vrel_t_pre );
892
893                                 impulse = magtangent; 
894                                 VECADDMUL ( pimpulse, vrel_t_pre, impulse);
895                         }
896
897                         // Apply velocity stopping impulse
898                         // I_c = m * v_N / 2.0
899                         // no 2.0 * magrelVel normally, but looks nicer DG
900                         impulse =  magrelVel;
901                         
902                         mul_v3_fl(collpair->normal, 0.5);
903                         VECADDMUL ( pimpulse, collpair->normal, impulse);
904
905                         // Apply repulse impulse if distance too short
906                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
907                         spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
908
909                         d = collpair->distance;
910                         if ( ( magrelVel < 0.1*d*spf && ( d > ALMOST_ZERO ) ) )
911                         {
912                                 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
913
914                                 // stay on the safe side and clamp repulse
915                                 if ( impulse > ALMOST_ZERO )
916                                         repulse = MIN2 ( repulse, 5.0*impulse );
917                                 repulse = MAX2 ( impulse, repulse );
918
919                                 impulse = repulse / ( 5.0 ); // original 2.0 / 0.25
920                                 VECADDMUL ( pimpulse, collpair->normal, impulse);
921                         }
922                         
923                         w2 = 1.0f-w1;
924                         if (w1 < 0.5)
925                                 w1 *= 2.0;
926                         else
927                                 w2 *= 2.0;
928                         
929                         VECADDFAC(cloth1->verts[collpair->ap1].impulse, cloth1->verts[collpair->ap1].impulse, pimpulse, w1*2.0);
930                         VECADDFAC(cloth1->verts[collpair->ap2].impulse, cloth1->verts[collpair->ap2].impulse, pimpulse, w2*2.0);
931                         
932                         cloth1->verts[collpair->ap1].impulse_count++;
933                         cloth1->verts[collpair->ap2].impulse_count++;
934                         
935                         result = 1;
936                 }
937         } 
938         
939         return result;
940 }
941
942 static int cloth_collision_response_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
943 {
944         int result = 0;
945         Cloth *cloth1;
946         float w1, w2, w3, u1, u2, u3;
947         float v1[3], v2[3], relativeVelocity[3];
948         float magrelVel;
949         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
950         
951         cloth1 = clmd->clothObject;
952
953         for ( ; collpair != collision_end; collpair++ )
954         {
955                 if (collpair->flag & COLLISION_IS_EDGES)
956                         continue;
957                 
958                 if ( collpair->flag & COLLISION_USE_COLLFACE ) {
959                         // was: txold
960                         w1 = collpair->bary[0]; w2 = collpair->bary[1]; w3 = collpair->bary[2];                 
961
962                         // Calculate relative "velocity".
963                         collision_interpolateOnTriangle ( v1, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, w1, w2, w3);
964                         
965                         VECSUB ( relativeVelocity, v1, cloth1->verts[collpair->collp].tv);
966                         
967                         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
968                         magrelVel = INPR ( relativeVelocity, collpair->normal );
969         
970                         // If v_n_mag < 0 the edges are approaching each other.
971                         if ( magrelVel > ALMOST_ZERO )
972                         {
973                                 // Calculate Impulse magnitude to stop all motion in normal direction.
974                                 float magtangent = 0, repulse = 0, d = 0;
975                                 double impulse = 0.0;
976                                 float vrel_t_pre[3];
977                                 float temp[3], spf;
978         
979                                 // calculate tangential velocity
980                                 VECCOPY ( temp, collpair->normal );
981                                 mul_v3_fl( temp, magrelVel );
982                                 VECSUB ( vrel_t_pre, relativeVelocity, temp );
983         
984                                 // Decrease in magnitude of relative tangential velocity due to coulomb friction
985                                 // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
986                                 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
987         
988                                 // Apply friction impulse.
989                                 if ( magtangent > ALMOST_ZERO )
990                                 {
991                                         normalize_v3( vrel_t_pre );
992         
993                                         impulse = magtangent; // 2.0 * 
994                                         VECADDMUL ( cloth1->verts[collpair->collp].impulse, vrel_t_pre, impulse);
995                                 }
996         
997                                 // Apply velocity stopping impulse
998                                 // I_c = m * v_N / 2.0
999                                 // no 2.0 * magrelVel normally, but looks nicer DG
1000                                 impulse =  magrelVel/2.0;
1001         
1002                                 VECADDMUL ( cloth1->verts[collpair->collp].impulse, collpair->normal, impulse);
1003                                 cloth1->verts[collpair->collp].impulse_count++;
1004         
1005                                 // Apply repulse impulse if distance too short
1006                                 // I_r = -min(dt*kd, m(0,1d/dt - v_n))
1007                                 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
1008         
1009                                 d = -collpair->distance;
1010                                 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) )
1011                                 {
1012                                         repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
1013         
1014                                         // stay on the safe side and clamp repulse
1015                                         if ( impulse > ALMOST_ZERO )
1016                                                 repulse = MIN2 ( repulse, 5.0*impulse );
1017                                         repulse = MAX2 ( impulse, repulse );
1018         
1019                                         impulse = repulse / ( 5.0 ); // original 2.0 / 0.25
1020                                         VECADDMUL ( cloth1->verts[collpair->collp].impulse, collpair->normal, impulse);
1021                                 }
1022         
1023                                 result = 1;
1024                         }
1025                 } else {        
1026                         w1 = collpair->bary[0]; w2 = collpair->bary[1]; w3 = collpair->bary[2];                 
1027
1028                         // Calculate relative "velocity".
1029                         collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
1030         
1031                         VECSUB ( relativeVelocity, collmd->current_v[collpair->collp].co, v1);
1032                         
1033                         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
1034                         magrelVel = INPR ( relativeVelocity, collpair->normal );
1035         
1036                         // If v_n_mag < 0 the edges are approaching each other.
1037                         if ( magrelVel > ALMOST_ZERO )
1038                         {
1039                                 // Calculate Impulse magnitude to stop all motion in normal direction.
1040                                 float magtangent = 0, repulse = 0, d = 0;
1041                                 double impulse = 0.0;
1042                                 float vrel_t_pre[3], pimpulse[3] = {0.0f, 0.0f, 0.0f};
1043                                 float temp[3], spf;
1044         
1045                                 // calculate tangential velocity
1046                                 VECCOPY ( temp, collpair->normal );
1047                                 mul_v3_fl( temp, magrelVel );
1048                                 VECSUB ( vrel_t_pre, relativeVelocity, temp );
1049         
1050                                 // Decrease in magnitude of relative tangential velocity due to coulomb friction
1051                                 // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
1052                                 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
1053         
1054                                 // Apply friction impulse.
1055                                 if ( magtangent > ALMOST_ZERO )
1056                                 {
1057                                         normalize_v3( vrel_t_pre );
1058         
1059                                         impulse = magtangent; // 2.0 * 
1060                                         VECADDMUL ( pimpulse, vrel_t_pre, impulse);
1061                                 }
1062         
1063                                 // Apply velocity stopping impulse
1064                                 // I_c = m * v_N / 2.0
1065                                 // no 2.0 * magrelVel normally, but looks nicer DG
1066                                 impulse =  magrelVel/2.0;
1067         
1068                                 VECADDMUL ( pimpulse, collpair->normal, impulse);
1069         
1070                                 // Apply repulse impulse if distance too short
1071                                 // I_r = -min(dt*kd, m(0,1d/dt - v_n))
1072                                 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
1073         
1074                                 d = -collpair->distance;
1075                                 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) )
1076                                 {
1077                                         repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
1078         
1079                                         // stay on the safe side and clamp repulse
1080                                         if ( impulse > ALMOST_ZERO )
1081                                                 repulse = MIN2 ( repulse, 5.0*impulse );
1082                                         repulse = MAX2 ( impulse, repulse );
1083         
1084                                         impulse = repulse / ( 2.0 ); // original 2.0 / 0.25
1085                                         VECADDMUL ( pimpulse, collpair->normal, impulse);
1086                                 }
1087                                 
1088                                 if (w1 < 0.5) w1 *= 2.0;
1089                                 if (w2 < 0.5) w2 *= 2.0;
1090                                 if (w3 < 0.5) w3 *= 2.0;
1091                                 
1092                                 VECADDMUL(cloth1->verts[collpair->ap1].impulse, pimpulse, w1*2.0);
1093                                 VECADDMUL(cloth1->verts[collpair->ap2].impulse, pimpulse, w2*2.0);
1094                                 VECADDMUL(cloth1->verts[collpair->ap3].impulse, pimpulse, w3*2.0);;
1095                                 cloth1->verts[collpair->ap1].impulse_count++;
1096                                 cloth1->verts[collpair->ap2].impulse_count++;
1097                                 cloth1->verts[collpair->ap3].impulse_count++;
1098                                 
1099                                 result = 1;
1100                         }
1101                 }
1102         } 
1103         
1104         return result;
1105 }
1106
1107
1108 typedef struct tripairkey {
1109         int p, a1, a2, a3;
1110 } tripairkey;
1111
1112 unsigned int tripair_hash(void *vkey)
1113 {
1114         tripairkey *key = vkey;
1115         int keys[4] = {key->p, key->a1, key->a2, key->a3};
1116         int i, j;
1117         
1118         for (i=0; i<4; i++) {
1119                 for (j=0; j<3; j++) {
1120                         if (keys[j] >= keys[j+1]) {
1121                                 SWAP(int, keys[j], keys[j+1]);
1122                         }
1123                 }
1124         }
1125         
1126         return keys[0]*101 + keys[1]*72 + keys[2]*53 + keys[3]*34;
1127 }
1128
1129 int tripair_cmp(const void *va, const void *vb)
1130 {
1131         tripairkey *a = va, *b = vb;
1132         int keysa[4] = {a->p, a->a1, a->a2, a->a3};
1133         int keysb[4] = {b->p, b->a1, b->a2, b->a3};
1134         int i;
1135         
1136         for (i=0; i<4; i++) {
1137                 int j, ok=0;
1138                 for (j=0; j<4; j++) {
1139                         if (keysa[i] == keysa[j]) {
1140                                 ok = 1;
1141                                 break;
1142                         }
1143                 }
1144                 if (!ok)
1145                         return -1;
1146         }
1147         
1148         return 0;
1149 }
1150
1151 static void get_tripairkey(tripairkey *key, int p, int a1, int a2, int a3)
1152 {
1153         key->a1 = a1;
1154         key->a2 = a2;
1155         key->a3 = a3;
1156         key->p = p;
1157 }
1158
1159 static int checkvisit(MemArena *arena, GHash *gh, int p, int a1, int a2, int a3)
1160 {
1161         tripairkey key, *key2;
1162         
1163         get_tripairkey(&key, p, a1, a2, a3);
1164         if (BLI_ghash_haskey(gh, &key))
1165                 return 1;
1166         
1167         key2 = BLI_memarena_alloc(arena, sizeof(*key2));
1168         *key2 = key;
1169         BLI_ghash_insert(gh, key2, NULL);
1170         
1171         return 0;
1172 }
1173
1174 int cloth_point_tri_moving_v3v3_f(float v1[2][3], int i1, float v2[2][3], int i2,
1175                                    float v3[2][3],  int i3, float v4[2][3], int i4,
1176                                    float normal[3], float bary[3], float *t, 
1177                                                                    float *relnor, GHash *gh, MemArena *arena)
1178 {
1179         if (checkvisit(arena, gh, i1, i2, i3, i4))
1180                 return 0;
1181         
1182         return eltopo_point_tri_moving_v3v3_f(v1, i1, v2, i2, v3, i3, v4, i4, normal, bary, t, relnor);
1183 }
1184
1185 static CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, 
1186                                                                    CollPair *collpair, double dt, GHash *gh, MemArena *arena)
1187 {
1188         ClothModifierData *clmd = ( ClothModifierData * ) md1;
1189         CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
1190         MFace *face1=NULL, *face2 = NULL;
1191         ClothVertex *verts1 = clmd->clothObject->verts;
1192         double distance = 0;
1193         float epsilon1 = clmd->coll_parms->epsilon;
1194         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
1195         float no[3], uv[3], t, relnor;
1196         int i, i1, i2, i3, i4, i5, i6;
1197         Cloth *cloth = clmd->clothObject;
1198         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];
1199         int j, ret, bp1, bp2, bp3, ap1, ap2, ap3;
1200         
1201         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
1202         face2 = & ( collmd->mfaces[overlap->indexB] );
1203
1204         // check all 4 possible collisions
1205         for ( i = 0; i < 4; i++ )
1206         {
1207                 if ( i == 0 )
1208                 {
1209                         // fill faceA
1210                         ap1 = face1->v1;
1211                         ap2 = face1->v2;
1212                         ap3 = face1->v3;
1213
1214                         // fill faceB
1215                         bp1 = face2->v1;
1216                         bp2 = face2->v2;
1217                         bp3 = face2->v3;
1218                 }
1219                 else if ( i == 1 )
1220                 {
1221                         if ( face1->v4 )
1222                         {
1223                                 // fill faceA
1224                                 ap1 = face1->v1;
1225                                 ap2 = face1->v3;
1226                                 ap3 = face1->v4;
1227
1228                                 // fill faceB
1229                                 bp1 = face2->v1;
1230                                 bp2 = face2->v2;
1231                                 bp3 = face2->v3;
1232                         }
1233                         else {
1234                                 continue;
1235                         }
1236                 }
1237                 if ( i == 2 )
1238                 {
1239                         if ( face2->v4 )
1240                         {
1241                                 // fill faceA
1242                                 ap1 = face1->v1;
1243                                 ap2 = face1->v2;
1244                                 ap3 = face1->v3;
1245
1246                                 // fill faceB
1247                                 bp1 = face2->v1;
1248                                 bp2 = face2->v3;
1249                                 bp3 = face2->v4;
1250                         }
1251                         else {
1252                                 continue;
1253                         }
1254                 }
1255                 else if ( i == 3 )
1256                 {
1257                         if ( face1->v4 && face2->v4 )
1258                         {
1259                                 // fill faceA
1260                                 ap1 = face1->v1;
1261                                 ap2 = face1->v3;
1262                                 ap3 = face1->v4;
1263
1264                                 // fill faceB
1265                                 bp1 = face2->v1;
1266                                 bp2 = face2->v3;
1267                                 bp3 = face2->v4;
1268                         }
1269                         else {
1270                                 continue;
1271                         }
1272                 }
1273                 
1274                 copy_v3_v3(v1[0], cloth->verts[ap1].txold); 
1275                 copy_v3_v3(v1[1], cloth->verts[ap1].tx);
1276                 copy_v3_v3(v2[0], cloth->verts[ap2].txold);
1277                 copy_v3_v3(v2[1], cloth->verts[ap2].tx);
1278                 copy_v3_v3(v3[0], cloth->verts[ap3].txold);
1279                 copy_v3_v3(v3[1], cloth->verts[ap3].tx);
1280                 
1281                 copy_v3_v3(v4[0], collmd->current_x[bp1].co);
1282                 copy_v3_v3(v4[1], collmd->current_xnew[bp1].co);
1283                 copy_v3_v3(v5[0], collmd->current_x[bp2].co);
1284                 copy_v3_v3(v5[1], collmd->current_xnew[bp2].co);
1285                 copy_v3_v3(v6[0], collmd->current_x[bp3].co);
1286                 copy_v3_v3(v6[1], collmd->current_xnew[bp3].co);
1287                 
1288                 normal_tri_v3(n2, v4[1], v5[1], v6[1]);
1289                 
1290                 sdis = clmd->coll_parms->distance_repel + epsilon2 + FLT_EPSILON;
1291
1292                 /*apply a repulsion force, to help the solver along*/
1293                 copy_v3_v3(off, n2);
1294                 negate_v3(off);
1295                 if (isect_ray_plane_v3(v1[1], off, v4[1], v5[1], v6[1], &l, 0)) {
1296                         if (l >= 0.0 && l < sdis) {
1297                                 mul_v3_fl(off, (l-sdis)*cloth->verts[ap1].mass*dt*clmd->coll_parms->repel_force*0.1);
1298
1299                                 add_v3_v3(cloth->verts[ap1].tv, off);
1300                                 add_v3_v3(cloth->verts[ap2].tv, off);
1301                                 add_v3_v3(cloth->verts[ap3].tv, off);
1302                         }
1303                 }
1304
1305                 /*offset new positions a bit, to account for margins*/
1306                 copy_v3_v3(off, n2);
1307                 mul_v3_fl(off,  epsilon1 + epsilon2 + ALMOST_ZERO);
1308                 add_v3_v3(v4[1], off); add_v3_v3(v5[1], off); add_v3_v3(v6[1], off);
1309                 
1310                 i1 = ap1; i2 = ap2; i3 = ap3;
1311                 i4 = bp1+cloth->numverts; i5 = bp2+cloth->numverts; i6 = bp3+cloth->numverts;
1312                 
1313                 for (j=0; j<6; j++) {
1314                         int collp;
1315
1316                         switch (j) {
1317                         case 0:
1318                                 ret = cloth_point_tri_moving_v3v3_f(v1, i1, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
1319                                 collp = ap1;
1320                                 break;
1321                         case 1:
1322                                 collp = ap2;
1323                                 ret = cloth_point_tri_moving_v3v3_f(v2, i2, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
1324                                 break;
1325                         case 2:
1326                                 collp = ap3;
1327                                 ret = cloth_point_tri_moving_v3v3_f(v3, i3, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena);
1328                                 break;
1329                         case 3:
1330                                 collp = bp1;
1331                                 ret = cloth_point_tri_moving_v3v3_f(v4, i4, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
1332                                 break;
1333                         case 4:
1334                                 collp = bp2;                            
1335                                 ret = cloth_point_tri_moving_v3v3_f(v5, i5, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
1336                                 break;
1337                         case 5:
1338                                 collp = bp3;
1339                                 ret = cloth_point_tri_moving_v3v3_f(v6, i6, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena);
1340                                 break;
1341                         }
1342                         
1343                         /*cloth vert versus coll face*/
1344                         if (ret && j < 3) {
1345                                 collpair->bp1 = bp1; collpair->bp2 = bp2; collpair->bp3 = bp3;
1346                                 collpair->collp = collp;
1347                                 
1348                                 copy_v3_v3(collpair->normal, no);
1349                                 mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
1350                                 collpair->distance = relnor;
1351                                 collpair->time = t;
1352                                 
1353                                 copy_v3_v3(collpair->bary, uv);
1354                                 
1355                                 collpair->flag = COLLISION_USE_COLLFACE;
1356                                 collpair++;
1357                         } else if (ret && j >= 3) { /*coll vert versus cloth face*/
1358                                 collpair->ap1 = ap1; collpair->ap2 = ap2; collpair->ap3 = ap3;
1359                                 collpair->collp = collp;
1360                                 
1361                                 copy_v3_v3(collpair->normal, no);
1362                                 mul_v3_v3fl(collpair->vector, collpair->normal, relnor);
1363                                 collpair->distance = relnor;
1364                                 collpair->time = t;
1365                                 
1366                                 copy_v3_v3(collpair->bary, uv);
1367
1368                                 collpair->flag = 0;
1369                                 collpair++;
1370                         }
1371                 }
1372         }
1373         
1374         return collpair;
1375 }
1376
1377 static void machine_epsilon_offset(Cloth *cloth) {
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 necassary 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.0 && l < sdis) {
1497                                 mul_v3_fl(n2, (l-sdis)*cloth->verts[collpair->ap1].mass*dt*clmd->coll_parms->repel_force*0.1);
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 * ( 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]: %lf, sol2[k]: %lf\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                                                 VECCOPY ( 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, 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;
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;
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 }