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