623ba26e86de6596ee1cbc5b1dcdb23148d28734
[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 #include "MEM_guardedalloc.h"
31
32 #include "BKE_cloth.h"
33
34 #include "DNA_cloth_types.h"
35 #include "DNA_group_types.h"
36 #include "DNA_mesh_types.h"
37 #include "DNA_object_types.h"
38 #include "DNA_object_force.h"
39 #include "DNA_scene_types.h"
40 #include "DNA_meshdata_types.h"
41
42 #include "BLI_blenlib.h"
43 #include "BLI_math.h"
44 #include "BLI_edgehash.h"
45 #include "BLI_utildefines.h"
46
47 #include "BKE_DerivedMesh.h"
48 #include "BKE_global.h"
49 #include "BKE_scene.h"
50 #include "BKE_mesh.h"
51 #include "BKE_object.h"
52 #include "BKE_modifier.h"
53
54 #include "BKE_DerivedMesh.h"
55 #ifdef USE_BULLET
56 #include "Bullet-C-Api.h"
57 #endif
58 #include "BLI_kdopbvh.h"
59 #include "BKE_collision.h"
60
61
62 /***********************************
63 Collision modifier code start
64 ***********************************/
65
66 /* step is limited from 0 (frame start position) to 1 (frame end position) */
67 void collision_move_object ( CollisionModifierData *collmd, float step, float prevstep )
68 {
69         float tv[3] = {0, 0, 0};
70         unsigned int i = 0;
71
72         for ( i = 0; i < collmd->numverts; i++ )
73         {
74                 VECSUB ( tv, collmd->xnew[i].co, collmd->x[i].co );
75                 VECADDS ( collmd->current_x[i].co, collmd->x[i].co, tv, prevstep );
76                 VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step );
77                 VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co );
78         }
79
80         bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
81 }
82
83 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon )
84 {
85         BVHTree *tree;
86         float co[12];
87         unsigned int i;
88         MFace *tface = mfaces;
89
90         tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
91
92         // fill tree
93         for ( i = 0; i < numfaces; i++, tface++ )
94         {
95                 VECCOPY ( &co[0*3], x[tface->v1].co );
96                 VECCOPY ( &co[1*3], x[tface->v2].co );
97                 VECCOPY ( &co[2*3], x[tface->v3].co );
98                 if ( tface->v4 )
99                         VECCOPY ( &co[3*3], x[tface->v4].co );
100
101                 BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
102         }
103
104         // balance tree
105         BLI_bvhtree_balance ( tree );
106
107         return tree;
108 }
109
110 void bvhtree_update_from_mvert ( BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving )
111 {
112         int i;
113         MFace *mfaces = faces;
114         float co[12], co_moving[12];
115         int ret = 0;
116
117         if ( !bvhtree )
118                 return;
119
120         if ( x )
121         {
122                 for ( i = 0; i < numfaces; i++, mfaces++ )
123                 {
124                         VECCOPY ( &co[0*3], x[mfaces->v1].co );
125                         VECCOPY ( &co[1*3], x[mfaces->v2].co );
126                         VECCOPY ( &co[2*3], x[mfaces->v3].co );
127                         if ( mfaces->v4 )
128                                 VECCOPY ( &co[3*3], x[mfaces->v4].co );
129
130                         // copy new locations into array
131                         if ( moving && xnew )
132                         {
133                                 // update moving positions
134                                 VECCOPY ( &co_moving[0*3], xnew[mfaces->v1].co );
135                                 VECCOPY ( &co_moving[1*3], xnew[mfaces->v2].co );
136                                 VECCOPY ( &co_moving[2*3], xnew[mfaces->v3].co );
137                                 if ( mfaces->v4 )
138                                         VECCOPY ( &co_moving[3*3], xnew[mfaces->v4].co );
139
140                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
141                         }
142                         else
143                         {
144                                 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
145                         }
146
147                         // check if tree is already full
148                         if ( !ret )
149                                 break;
150                 }
151
152                 BLI_bvhtree_update_tree ( bvhtree );
153         }
154 }
155
156 /***********************************
157 Collision modifier code end
158 ***********************************/
159
160 /**
161 * gsl_poly_solve_cubic -
162 *
163 * copied from SOLVE_CUBIC.C --> GSL
164 */
165
166 #define mySWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0)
167 #if 0 /* UNUSED */
168 static int 
169 gsl_poly_solve_cubic (double a, double b, double c, 
170                                           double *x0, double *x1, double *x2)
171 {
172         double q = (a * a - 3 * b);
173         double r = (2 * a * a * a - 9 * a * b + 27 * c);
174
175         double Q = q / 9;
176         double R = r / 54;
177
178         double Q3 = Q * Q * Q;
179         double R2 = R * R;
180
181         double CR2 = 729 * r * r;
182         double CQ3 = 2916 * q * q * q;
183
184         if (R == 0 && Q == 0)
185         {
186                 *x0 = - a / 3 ;
187                 *x1 = - a / 3 ;
188                 *x2 = - a / 3 ;
189                 return 3 ;
190         }
191         else if (CR2 == CQ3) 
192         {
193                 /* this test is actually R2 == Q3, written in a form suitable
194                 for exact computation with integers */
195
196                 /* Due to finite precision some double roots may be missed, and
197                 considered to be a pair of complex roots z = x +/- epsilon i
198                 close to the real axis. */
199
200                 double sqrtQ = sqrt (Q);
201
202                 if (R > 0)
203                 {
204                         *x0 = -2 * sqrtQ  - a / 3;
205                         *x1 = sqrtQ - a / 3;
206                         *x2 = sqrtQ - a / 3;
207                 }
208                 else
209                 {
210                         *x0 = - sqrtQ  - a / 3;
211                         *x1 = - sqrtQ - a / 3;
212                         *x2 = 2 * sqrtQ - a / 3;
213                 }
214                 return 3 ;
215         }
216         else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
217         {
218                 double sqrtQ = sqrt (Q);
219                 double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
220                 double theta = acos (R / sqrtQ3);
221                 double norm = -2 * sqrtQ;
222                 *x0 = norm * cos (theta / 3) - a / 3;
223                 *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3;
224                 *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3;
225
226                 /* Sort *x0, *x1, *x2 into increasing order */
227
228                 if (*x0 > *x1)
229                         mySWAP(*x0, *x1) ;
230
231                 if (*x1 > *x2)
232                 {
233                         mySWAP(*x1, *x2) ;
234
235                         if (*x0 > *x1)
236                                 mySWAP(*x0, *x1) ;
237                 }
238
239                 return 3;
240         }
241         else
242         {
243                 double sgnR = (R >= 0 ? 1 : -1);
244                 double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0);
245                 double B = Q / A ;
246                 *x0 = A + B - a / 3;
247                 return 1;
248         }
249 }
250
251
252
253 /**
254 * gsl_poly_solve_quadratic
255 *
256 * copied from GSL
257 */
258 static int 
259 gsl_poly_solve_quadratic (double a, double b, double c, 
260                                                   double *x0, double *x1)
261 {
262         double disc = b * b - 4 * a * c;
263
264         if (a == 0) /* Handle linear case */
265         {
266                 if (b == 0)
267                 {
268                         return 0;
269                 }
270                 else
271                 {
272                         *x0 = -c / b;
273                         return 1;
274                 };
275         }
276
277         if (disc > 0)
278         {
279                 if (b == 0)
280                 {
281                         double r = fabs (0.5 * sqrt (disc) / a);
282                         *x0 = -r;
283                         *x1 =  r;
284                 }
285                 else
286                 {
287                         double sgnb = (b > 0 ? 1 : -1);
288                         double temp = -0.5 * (b + sgnb * sqrt (disc));
289                         double r1 = temp / a ;
290                         double r2 = c / temp ;
291
292                         if (r1 < r2) 
293                         {
294                                 *x0 = r1 ;
295                                 *x1 = r2 ;
296                         } 
297                         else 
298                         {
299                                 *x0 = r2 ;
300                                 *x1 = r1 ;
301                         }
302                 }
303                 return 2;
304         }
305         else if (disc == 0) 
306         {
307                 *x0 = -0.5 * b / a ;
308                 *x1 = -0.5 * b / a ;
309                 return 2 ;
310         }
311         else
312         {
313                 return 0;
314         }
315 }
316 #endif /* UNUSED */
317
318
319
320 /*
321 * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
322 *     page 4, left column
323 */
324 #if 0
325 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] )
326 {
327         int num_sols = 0;
328
329         // x^0 - checked 
330         double g =      a[0] * c[1] * e[2] - a[0] * c[2] * e[1] +
331                 a[1] * c[2] * e[0] - a[1] * c[0] * e[2] + 
332                 a[2] * c[0] * e[1] - a[2] * c[1] * e[0];
333
334         // x^1
335         double h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
336                 a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
337                 a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
338                 b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
339                 a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
340                 a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
341
342         // x^2
343         double i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
344                 b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
345                 b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
346                 b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
347                 a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
348                 b[2] * c[0] * f[1] - b[0] * c[2] * f[1] + 
349                 a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
350                 b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
351                 a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
352
353         // x^3 - checked
354         double j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
355                 b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
356                 b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
357
358         /*
359         printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]);
360         printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]);
361         printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]);
362
363         printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
364         printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]);
365         printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]);
366
367         printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]);
368         printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]);
369         printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
370
371         printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g);
372         
373 */
374         // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
375         if ( ABS ( j ) > DBL_EPSILON )
376         {
377                 i /= j;
378                 h /= j;
379                 g /= j;
380                 num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] );
381         }
382         else
383         {
384                 num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] );
385                 solution[2] = -1.0;
386         }
387
388         // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0],  solution[1],  solution[2]);
389
390         // Discard negative solutions
391         if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) )
392         {
393                 --num_sols;
394                 solution[0] = solution[num_sols];
395         }
396         if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) )
397         {
398                 --num_sols;
399                 solution[1] = solution[num_sols];
400         }
401         if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) )
402         {
403                 --num_sols;
404         }
405
406         // Sort
407         if ( num_sols == 2 )
408         {
409                 if ( solution[0] > solution[1] )
410                 {
411                         double tmp = solution[0];
412                         solution[0] = solution[1];
413                         solution[1] = tmp;
414                 }
415         }
416         else if ( num_sols == 3 )
417         {
418
419                 // Bubblesort
420                 if ( solution[0] > solution[1] )
421                 {
422                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
423                 }
424                 if ( solution[1] > solution[2] )
425                 {
426                         double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
427                 }
428                 if ( solution[0] > solution[1] )
429                 {
430                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
431                 }
432         }
433
434         return num_sols;
435 }
436 #endif
437
438
439 // w3 is not perfect
440 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
441 {
442         double  tempV1[3], tempV2[3], tempV4[3];
443         double  a,b,c,d,e,f;
444
445         VECSUB ( tempV1, p1, p3 );
446         VECSUB ( tempV2, p2, p3 );
447         VECSUB ( tempV4, pv, p3 );
448
449         a = INPR ( tempV1, tempV1 );
450         b = INPR ( tempV1, tempV2 );
451         c = INPR ( tempV2, tempV2 );
452         e = INPR ( tempV1, tempV4 );
453         f = INPR ( tempV2, tempV4 );
454
455         d = ( a * c - b * b );
456
457         if ( ABS ( d ) < ALMOST_ZERO )
458         {
459                 *w1 = *w2 = *w3 = 1.0 / 3.0;
460                 return;
461         }
462
463         w1[0] = ( float ) ( ( e * c - b * f ) / d );
464
465         if ( w1[0] < 0 )
466                 w1[0] = 0;
467
468         w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
469
470         if ( w2[0] < 0 )
471                 w2[0] = 0;
472
473         w3[0] = 1.0f - w1[0] - w2[0];
474 }
475
476 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
477 {
478         to[0] = to[1] = to[2] = 0;
479         VECADDMUL ( to, v1, w1 );
480         VECADDMUL ( to, v2, w2 );
481         VECADDMUL ( to, v3, w3 );
482 }
483
484
485 static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
486 {
487         int result = 0;
488         Cloth *cloth1;
489         float w1, w2, w3, u1, u2, u3;
490         float v1[3], v2[3], relativeVelocity[3];
491         float magrelVel;
492         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
493
494         cloth1 = clmd->clothObject;
495
496         for ( ; collpair != collision_end; collpair++ )
497         {
498                 // only handle static collisions here
499                 if ( collpair->flag & COLLISION_IN_FUTURE )
500                         continue;
501
502                 // compute barycentric coordinates for both collision points
503                 collision_compute_barycentric ( collpair->pa,
504                         cloth1->verts[collpair->ap1].txold,
505                         cloth1->verts[collpair->ap2].txold,
506                         cloth1->verts[collpair->ap3].txold,
507                         &w1, &w2, &w3 );
508
509                 // was: txold
510                 collision_compute_barycentric ( collpair->pb,
511                         collmd->current_x[collpair->bp1].co,
512                         collmd->current_x[collpair->bp2].co,
513                         collmd->current_x[collpair->bp3].co,
514                         &u1, &u2, &u3 );
515
516                 // Calculate relative "velocity".
517                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
518
519                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
520
521                 VECSUB ( relativeVelocity, v2, v1 );
522
523                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
524                 magrelVel = INPR ( relativeVelocity, collpair->normal );
525
526                 // printf("magrelVel: %f\n", magrelVel);
527
528                 // Calculate masses of points.
529                 // TODO
530
531                 // If v_n_mag < 0 the edges are approaching each other.
532                 if ( magrelVel > ALMOST_ZERO )
533                 {
534                         // Calculate Impulse magnitude to stop all motion in normal direction.
535                         float magtangent = 0, repulse = 0, d = 0;
536                         double impulse = 0.0;
537                         float vrel_t_pre[3];
538                         float temp[3], spf;
539
540                         // calculate tangential velocity
541                         VECCOPY ( temp, collpair->normal );
542                         mul_v3_fl( temp, magrelVel );
543                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
544
545                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
546                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
547                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
548
549                         // Apply friction impulse.
550                         if ( magtangent > ALMOST_ZERO )
551                         {
552                                 normalize_v3( vrel_t_pre );
553
554                                 impulse = magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // 2.0 * 
555                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
556                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
557                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
558                         }
559
560                         // Apply velocity stopping impulse
561                         // I_c = m * v_N / 2.0
562                         // no 2.0 * magrelVel normally, but looks nicer DG
563                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
564
565                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
566                         cloth1->verts[collpair->ap1].impulse_count++;
567
568                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
569                         cloth1->verts[collpair->ap2].impulse_count++;
570
571                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
572                         cloth1->verts[collpair->ap3].impulse_count++;
573
574                         // Apply repulse impulse if distance too short
575                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
576                         spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
577
578                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
579                         if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) )
580                         {
581                                 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
582
583                                 // stay on the safe side and clamp repulse
584                                 if ( impulse > ALMOST_ZERO )
585                                         repulse = MIN2 ( repulse, 5.0*impulse );
586                                 repulse = MAX2 ( impulse, repulse );
587
588                                 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
589                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
590                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
591                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
592                         }
593
594                         result = 1;
595                 }
596         }
597         return result;
598 }
599
600 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
601 static CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, CollPair *collpair )
602 {
603         ClothModifierData *clmd = ( ClothModifierData * ) md1;
604         CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
605         MFace *face1=NULL, *face2 = NULL;
606 #ifdef USE_BULLET
607         ClothVertex *verts1 = clmd->clothObject->verts;
608 #endif
609         double distance = 0;
610         float epsilon1 = clmd->coll_parms->epsilon;
611         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
612         int i;
613
614         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
615         face2 = & ( collmd->mfaces[overlap->indexB] );
616
617         // check all 4 possible collisions
618         for ( i = 0; i < 4; i++ )
619         {
620                 if ( i == 0 )
621                 {
622                         // fill faceA
623                         collpair->ap1 = face1->v1;
624                         collpair->ap2 = face1->v2;
625                         collpair->ap3 = face1->v3;
626
627                         // fill faceB
628                         collpair->bp1 = face2->v1;
629                         collpair->bp2 = face2->v2;
630                         collpair->bp3 = face2->v3;
631                 }
632                 else if ( i == 1 )
633                 {
634                         if ( face1->v4 )
635                         {
636                                 // fill faceA
637                                 collpair->ap1 = face1->v1;
638                                 collpair->ap2 = face1->v4;
639                                 collpair->ap3 = face1->v3;
640
641                                 // fill faceB
642                                 collpair->bp1 = face2->v1;
643                                 collpair->bp2 = face2->v2;
644                                 collpair->bp3 = face2->v3;
645                         }
646                         else
647                                 i++;
648                 }
649                 if ( i == 2 )
650                 {
651                         if ( face2->v4 )
652                         {
653                                 // fill faceA
654                                 collpair->ap1 = face1->v1;
655                                 collpair->ap2 = face1->v2;
656                                 collpair->ap3 = face1->v3;
657
658                                 // fill faceB
659                                 collpair->bp1 = face2->v1;
660                                 collpair->bp2 = face2->v4;
661                                 collpair->bp3 = face2->v3;
662                         }
663                         else
664                                 break;
665                 }
666                 else if ( i == 3 )
667                 {
668                         if ( face1->v4 && face2->v4 )
669                         {
670                                 // fill faceA
671                                 collpair->ap1 = face1->v1;
672                                 collpair->ap2 = face1->v4;
673                                 collpair->ap3 = face1->v3;
674
675                                 // fill faceB
676                                 collpair->bp1 = face2->v1;
677                                 collpair->bp2 = face2->v4;
678                                 collpair->bp3 = face2->v3;
679                         }
680                         else
681                                 break;
682                 }
683
684 #ifdef USE_BULLET
685                 // calc distance + normal
686                 distance = plNearestPoints (
687                         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 );
688 #else
689                 // just be sure that we don't add anything
690                 distance = 2.0 * ( epsilon1 + epsilon2 + ALMOST_ZERO );
691 #endif
692
693                 if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) )
694                 {
695                         normalize_v3_v3( collpair->normal, collpair->vector );
696
697                         collpair->distance = distance;
698                         collpair->flag = 0;
699                         collpair++;
700                 }/*
701                 else
702                 {
703                         float w1, w2, w3, u1, u2, u3;
704                         float v1[3], v2[3], relativeVelocity[3];
705
706                         // calc relative velocity
707                         
708                         // compute barycentric coordinates for both collision points
709                         collision_compute_barycentric ( collpair->pa,
710                         verts1[collpair->ap1].txold,
711                         verts1[collpair->ap2].txold,
712                         verts1[collpair->ap3].txold,
713                         &w1, &w2, &w3 );
714
715                         // was: txold
716                         collision_compute_barycentric ( collpair->pb,
717                         collmd->current_x[collpair->bp1].co,
718                         collmd->current_x[collpair->bp2].co,
719                         collmd->current_x[collpair->bp3].co,
720                         &u1, &u2, &u3 );
721
722                         // Calculate relative "velocity".
723                         collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
724
725                         collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
726
727                         VECSUB ( relativeVelocity, v2, v1 );
728
729                         if(sqrt(INPR(relativeVelocity, relativeVelocity)) >= distance)
730                         {
731                                 // check for collision in the future
732                                 collpair->flag |= COLLISION_IN_FUTURE;
733                                 collpair++;
734                         }
735                 }*/
736         }
737         return collpair;
738 }
739
740 #if 0
741 static int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
742 {
743         int result = 0;
744         Cloth *cloth1;
745         float w1, w2, w3, u1, u2, u3;
746         float v1[3], v2[3], relativeVelocity[3];
747         float magrelVel;
748
749         cloth1 = clmd->clothObject;
750
751         for ( ; collpair != collision_end; collpair++ )
752         {
753                 // compute barycentric coordinates for both collision points
754                 collision_compute_barycentric ( collpair->pa,
755                         cloth1->verts[collpair->ap1].txold,
756                         cloth1->verts[collpair->ap2].txold,
757                         cloth1->verts[collpair->ap3].txold,
758                         &w1, &w2, &w3 );
759
760                 // was: txold
761                 collision_compute_barycentric ( collpair->pb,
762                         collmd->current_x[collpair->bp1].co,
763                         collmd->current_x[collpair->bp2].co,
764                         collmd->current_x[collpair->bp3].co,
765                         &u1, &u2, &u3 );
766
767                 // Calculate relative "velocity".
768                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
769
770                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
771
772                 VECSUB ( relativeVelocity, v2, v1 );
773
774                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
775                 magrelVel = INPR ( relativeVelocity, collpair->normal );
776
777                 // printf("magrelVel: %f\n", magrelVel);
778
779                 // Calculate masses of points.
780                 // TODO
781
782                 // If v_n_mag < 0 the edges are approaching each other.
783                 if ( magrelVel > ALMOST_ZERO )
784                 {
785                         // Calculate Impulse magnitude to stop all motion in normal direction.
786                         float magtangent = 0;
787                         double impulse = 0.0;
788                         float vrel_t_pre[3];
789                         float temp[3];
790
791                         // calculate tangential velocity
792                         VECCOPY ( temp, collpair->normal );
793                         mul_v3_fl( temp, magrelVel );
794                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
795
796                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
797                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
798                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
799
800                         // Apply friction impulse.
801                         if ( magtangent > ALMOST_ZERO )
802                         {
803                                 normalize_v3( vrel_t_pre );
804
805                                 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
806                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
807                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
808                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
809                         }
810
811                         // Apply velocity stopping impulse
812                         // I_c = m * v_N / 2.0
813                         // no 2.0 * magrelVel normally, but looks nicer DG
814                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
815
816                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
817                         cloth1->verts[collpair->ap1].impulse_count++;
818
819                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
820                         cloth1->verts[collpair->ap2].impulse_count++;
821
822                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
823                         cloth1->verts[collpair->ap3].impulse_count++;
824
825                         // Apply repulse impulse if distance too short
826                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
827                         /*
828                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
829                         if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) )
830                         {
831                         repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
832
833                         // stay on the safe side and clamp repulse
834                         if ( impulse > ALMOST_ZERO )
835                         repulse = MIN2 ( repulse, 5.0*impulse );
836                         repulse = MAX2 ( impulse, repulse );
837
838                         impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
839                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
840                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
841                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
842                         }
843                         */
844                         result = 1;
845                 }
846         }
847         return result;
848 }
849 #endif
850
851 #if 0
852 static float projectPointOntoLine(float *p, float *a, float *b) 
853 {
854    float ba[3], pa[3];
855    VECSUB(ba, b, a);
856    VECSUB(pa, p, a);
857    return INPR(pa, ba) / INPR(ba, ba);
858 }
859
860 static void calculateEENormal(float *np1, float *np2, float *np3, float *np4,float *out_normal) 
861 {
862         float line1[3], line2[3];
863         float length;
864
865         VECSUB(line1, np2, np1);
866         VECSUB(line2, np3, np1);
867
868         // printf("l1: %f, l1: %f, l2: %f, l2: %f\n", line1[0], line1[1], line2[0], line2[1]);
869
870         cross_v3_v3v3(out_normal, line1, line2);
871
872         
873
874         length = normalize_v3(out_normal);
875         if (length <= FLT_EPSILON)
876         { // lines are collinear
877                 VECSUB(out_normal, np2, np1);
878                 normalize_v3(out_normal);
879         }
880 }
881
882 static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, float *w1, float *w2)
883 {
884         float temp[3], temp2[3];
885         
886         double a, b, c, e, f; 
887
888         VECSUB(temp, x2, x1);
889         a = INPR(temp, temp);
890
891         VECSUB(temp2, x4, x3);
892         b = -INPR(temp, temp2);
893
894         c = INPR(temp2, temp2);
895
896         VECSUB(temp2, x3, x1);
897         e = INPR(temp, temp2);
898
899         VECSUB(temp, x4, x3);
900         f = -INPR(temp, temp2);
901
902         *w1 = (e * c - b * f) / (a * c - b * b);
903         *w2 = (f - b * *w1) / c;
904
905 }
906
907 // calculates the distance of 2 edges
908 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)
909 {
910         float line1[3], line2[3], cross[3];
911         float length;
912         float temp[3], temp2[3];
913         float dist_a1, dist_a2;
914         
915         VECSUB(line1, np12, np11);
916         VECSUB(line2, np22, np21);
917
918         cross_v3_v3v3(cross, line1, line2);
919         length = INPR(cross, cross);
920
921         if (length < FLT_EPSILON) 
922         {
923                 *out_a2 = projectPointOntoLine(np11, np21, np22);
924                 if ((*out_a2 >= -FLT_EPSILON) && (*out_a2 <= 1.0 + FLT_EPSILON)) 
925                 {
926                         *out_a1 = 0;
927                         calculateEENormal(np11, np12, np21, np22, out_normal);
928                         VECSUB(temp, np22, np21);
929                         mul_v3_fl(temp, *out_a2);
930                         VECADD(temp2, temp, np21);
931                         VECADD(temp2, temp2, np11);
932                         return INPR(temp2, temp2);
933                 }
934
935                 CLAMP(*out_a2, 0.0, 1.0);
936                 if (*out_a2 > .5) 
937                 { // == 1.0
938                         *out_a1 = projectPointOntoLine(np22, np11, np12);
939                         if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) 
940                         {
941                                 calculateEENormal(np11, np12, np21, np22, out_normal);
942
943                                 // return (np22 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
944                                 VECSUB(temp, np12, np11);
945                                 mul_v3_fl(temp, *out_a1);
946                                 VECADD(temp2, temp, np11);
947                                 VECSUB(temp2, np22, temp2);
948                                 return INPR(temp2, temp2);
949                         }
950                 } 
951                 else 
952                 { // == 0.0
953                         *out_a1 = projectPointOntoLine(np21, np11, np12);
954                         if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) 
955                         {
956                                 calculateEENormal(np11, np11, np21, np22, out_normal);
957
958                                 // return (np21 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
959                                 VECSUB(temp, np12, np11);
960                                 mul_v3_fl(temp, *out_a1);
961                                 VECADD(temp2, temp, np11);
962                                 VECSUB(temp2, np21, temp2);
963                                 return INPR(temp2, temp2);
964                         }
965                 }
966
967                 CLAMP(*out_a1, 0.0, 1.0);
968                 calculateEENormal(np11, np12, np21, np22, out_normal);
969                 if(*out_a1 > .5)
970                 {
971                         if(*out_a2 > .5)
972                         {
973                                 VECSUB(temp, np12, np22);
974                         }
975                         else
976                         {
977                                 VECSUB(temp, np12, np21);
978                         }
979                 }
980                 else
981                 {
982                         if(*out_a2 > .5)
983                         {
984                                 VECSUB(temp, np11, np22);
985                         }
986                         else
987                         {
988                                 VECSUB(temp, np11, np21);
989                         }
990                 }
991
992                 return INPR(temp, temp);
993         }
994         else
995         {
996                 
997                 // If the lines aren't parallel (but coplanar) they have to intersect
998
999                 findClosestPointsEE(np11, np12, np21, np22, out_a1, out_a2);
1000
1001                 // If both points are on the finite edges, we're done.
1002                 if (*out_a1 >= 0.0 && *out_a1 <= 1.0 && *out_a2 >= 0.0 && *out_a2 <= 1.0) 
1003                 {
1004                         float p1[3], p2[3];
1005                         
1006                         // p1= np11 + (np12 - np11) * out_a1;
1007                         VECSUB(temp, np12, np11);
1008                         mul_v3_fl(temp, *out_a1);
1009                         VECADD(p1, np11, temp);
1010                         
1011                         // p2 = np21 + (np22 - np21) * out_a2;
1012                         VECSUB(temp, np22, np21);
1013                         mul_v3_fl(temp, *out_a2);
1014                         VECADD(p2, np21, temp);
1015
1016                         calculateEENormal(np11, np12, np21, np22, out_normal);
1017                         VECSUB(temp, p1, p2);
1018                         return INPR(temp, temp);
1019                 }
1020
1021                 
1022                 /*
1023                 * Clamp both points to the finite edges.
1024                 * The one that moves most during clamping is one part of the solution.
1025                 */
1026                 dist_a1 = *out_a1;
1027                 CLAMP(dist_a1, 0.0, 1.0);
1028                 dist_a2 = *out_a2;
1029                 CLAMP(dist_a2, 0.0, 1.0);
1030
1031                 // Now project the "most clamped" point on the other line.
1032                 if (dist_a1 > dist_a2) 
1033                 { 
1034                         /* keep out_a1 */
1035                         float p1[3];
1036
1037                         // p1 = np11 + (np12 - np11) * out_a1;
1038                         VECSUB(temp, np12, np11);
1039                         mul_v3_fl(temp, *out_a1);
1040                         VECADD(p1, np11, temp);
1041
1042                         *out_a2 = projectPointOntoLine(p1, np21, np22);
1043                         CLAMP(*out_a2, 0.0, 1.0);
1044
1045                         calculateEENormal(np11, np12, np21, np22, out_normal);
1046
1047                         // return (p1 - (np21 + (np22 - np21) * out_a2)).lengthSquared();
1048                         VECSUB(temp, np22, np21);
1049                         mul_v3_fl(temp, *out_a2);
1050                         VECADD(temp, temp, np21);
1051                         VECSUB(temp, p1, temp);
1052                         return INPR(temp, temp);
1053                 } 
1054                 else 
1055                 {       
1056                         /* keep out_a2 */
1057                         float p2[3];
1058                         
1059                         // p2 = np21 + (np22 - np21) * out_a2;
1060                         VECSUB(temp, np22, np21);
1061                         mul_v3_fl(temp, *out_a2);
1062                         VECADD(p2, np21, temp);
1063
1064                         *out_a1 = projectPointOntoLine(p2, np11, np12);
1065                         CLAMP(*out_a1, 0.0, 1.0);
1066
1067                         calculateEENormal(np11, np12, np21, np22, out_normal);
1068                         
1069                         // return ((np11 + (np12 - np11) * out_a1) - p2).lengthSquared();
1070                         VECSUB(temp, np12, np11);
1071                         mul_v3_fl(temp, *out_a1);
1072                         VECADD(temp, temp, np11);
1073                         VECSUB(temp, temp, p2);
1074                         return INPR(temp, temp);
1075                 }
1076         }
1077         
1078         printf("Error in edgedge_distance: end of function\n");
1079         return 0;
1080 }
1081
1082 static int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
1083 {
1084         EdgeCollPair edgecollpair;
1085         Cloth *cloth1=NULL;
1086         ClothVertex *verts1=NULL;
1087         unsigned int i = 0, k = 0;
1088         int numsolutions = 0;
1089         double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
1090         double solution[3], solution2[3];
1091         MVert *verts2 = collmd->current_x; // old x
1092         MVert *velocity2 = collmd->current_v; // velocity
1093         float distance = 0;
1094         float triA[3][3], triB[3][3];
1095         int result = 0;
1096
1097         cloth1 = clmd->clothObject;
1098         verts1 = cloth1->verts;
1099
1100         for(i = 0; i < 9; i++)
1101         {
1102                 // 9 edge - edge possibilities
1103
1104                 if(i == 0) // cloth edge: 1-2; coll edge: 1-2
1105                 {
1106                         edgecollpair.p11 = collpair->ap1;
1107                         edgecollpair.p12 = collpair->ap2;
1108
1109                         edgecollpair.p21 = collpair->bp1;
1110                         edgecollpair.p22 = collpair->bp2;
1111                 }
1112                 else if(i == 1) // cloth edge: 1-2; coll edge: 2-3
1113                 {
1114                         edgecollpair.p11 = collpair->ap1;
1115                         edgecollpair.p12 = collpair->ap2;
1116
1117                         edgecollpair.p21 = collpair->bp2;
1118                         edgecollpair.p22 = collpair->bp3;
1119                 }
1120                 else if(i == 2) // cloth edge: 1-2; coll edge: 1-3
1121                 {
1122                         edgecollpair.p11 = collpair->ap1;
1123                         edgecollpair.p12 = collpair->ap2;
1124
1125                         edgecollpair.p21 = collpair->bp1;
1126                         edgecollpair.p22 = collpair->bp3;
1127                 }
1128                 else if(i == 3) // cloth edge: 2-3; coll edge: 1-2
1129                 {
1130                         edgecollpair.p11 = collpair->ap2;
1131                         edgecollpair.p12 = collpair->ap3;
1132
1133                         edgecollpair.p21 = collpair->bp1;
1134                         edgecollpair.p22 = collpair->bp2;
1135                 }
1136                 else if(i == 4) // cloth edge: 2-3; coll edge: 2-3
1137                 {
1138                         edgecollpair.p11 = collpair->ap2;
1139                         edgecollpair.p12 = collpair->ap3;
1140
1141                         edgecollpair.p21 = collpair->bp2;
1142                         edgecollpair.p22 = collpair->bp3;
1143                 }
1144                 else if(i == 5) // cloth edge: 2-3; coll edge: 1-3
1145                 {
1146                         edgecollpair.p11 = collpair->ap2;
1147                         edgecollpair.p12 = collpair->ap3;
1148
1149                         edgecollpair.p21 = collpair->bp1;
1150                         edgecollpair.p22 = collpair->bp3;
1151                 }
1152                 else if(i ==6) // cloth edge: 1-3; coll edge: 1-2
1153                 {
1154                         edgecollpair.p11 = collpair->ap1;
1155                         edgecollpair.p12 = collpair->ap3;
1156
1157                         edgecollpair.p21 = collpair->bp1;
1158                         edgecollpair.p22 = collpair->bp2;
1159                 }
1160                 else if(i ==7) // cloth edge: 1-3; coll edge: 2-3
1161                 {
1162                         edgecollpair.p11 = collpair->ap1;
1163                         edgecollpair.p12 = collpair->ap3;
1164
1165                         edgecollpair.p21 = collpair->bp2;
1166                         edgecollpair.p22 = collpair->bp3;
1167                 }
1168                 else if(i == 8) // cloth edge: 1-3; coll edge: 1-3
1169                 {
1170                         edgecollpair.p11 = collpair->ap1;
1171                         edgecollpair.p12 = collpair->ap3;
1172
1173                         edgecollpair.p21 = collpair->bp1;
1174                         edgecollpair.p22 = collpair->bp3;
1175                 }
1176                 /*
1177                 if((edgecollpair.p11 == 3) && (edgecollpair.p12 == 16))
1178                         printf("Ahier!\n");
1179                 if((edgecollpair.p11 == 16) && (edgecollpair.p12 == 3))
1180                         printf("Ahier!\n");
1181                 */
1182
1183                 // if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
1184                 {
1185                         // always put coll points in p21/p22
1186                         VECSUB ( x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold );
1187                         VECSUB ( v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv );
1188
1189                         VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold );
1190                         VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv );
1191
1192                         VECSUB ( x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold );
1193                         VECSUB ( v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv );
1194
1195                         numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
1196
1197                         if((edgecollpair.p11 == 3 && edgecollpair.p12==16)|| (edgecollpair.p11==16 && edgecollpair.p12==3))
1198                         {
1199                                 if(edgecollpair.p21==6 || edgecollpair.p22 == 6)
1200                                 {
1201                                         printf("dist: %f, sol[k]: %lf, sol2[k]: %lf\n", distance, solution[k], solution2[k]);
1202                                         printf("a1: %f, a2: %f, b1: %f, b2: %f\n", x1[0], x2[0], x3[0], v1[0]);
1203                                         printf("b21: %d, b22: %d\n", edgecollpair.p21, edgecollpair.p22);
1204                                 }
1205                         }
1206
1207                         for ( k = 0; k < numsolutions; k++ )
1208                         {
1209                                 // printf("sol %d: %lf\n", k, solution[k]);
1210                                 if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) && ( solution[k] >  ALMOST_ZERO))
1211                                 {
1212                                         float a,b;
1213                                         float out_normal[3];
1214                                         float distance;
1215                                         float impulse = 0;
1216                                         float I_mag;
1217
1218                                         // move verts
1219                                         VECADDS(triA[0], verts1[edgecollpair.p11].txold, verts1[edgecollpair.p11].tv, solution[k]);
1220                                         VECADDS(triA[1], verts1[edgecollpair.p12].txold, verts1[edgecollpair.p12].tv, solution[k]);
1221
1222                                         VECADDS(triB[0], collmd->current_x[edgecollpair.p21].co, collmd->current_v[edgecollpair.p21].co, solution[k]);
1223                                         VECADDS(triB[1], collmd->current_x[edgecollpair.p22].co, collmd->current_v[edgecollpair.p22].co, solution[k]);
1224
1225                                         // TODO: check for collisions
1226                                         distance = edgedge_distance(triA[0], triA[1], triB[0], triB[1], &a, &b, out_normal);
1227                                         
1228                                         if ((distance <= clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree ) + ALMOST_ZERO) && (INPR(out_normal, out_normal) > 0))
1229                                         {
1230                                                 float vrel_1_to_2[3], temp[3], temp2[3], out_normalVelocity;
1231                                                 float desiredVn;
1232
1233                                                 VECCOPY(vrel_1_to_2, verts1[edgecollpair.p11].tv);
1234                                                 mul_v3_fl(vrel_1_to_2, 1.0 - a);
1235                                                 VECCOPY(temp, verts1[edgecollpair.p12].tv);
1236                                                 mul_v3_fl(temp, a);
1237
1238                                                 VECADD(vrel_1_to_2, vrel_1_to_2, temp);
1239
1240                                                 VECCOPY(temp, verts1[edgecollpair.p21].tv);
1241                                                 mul_v3_fl(temp, 1.0 - b);
1242                                                 VECCOPY(temp2, verts1[edgecollpair.p22].tv);
1243                                                 mul_v3_fl(temp2, b);
1244                                                 VECADD(temp, temp, temp2);
1245
1246                                                 VECSUB(vrel_1_to_2, vrel_1_to_2, temp);
1247
1248                                                 out_normalVelocity = INPR(vrel_1_to_2, out_normal);
1249 /*
1250                                                 // this correction results in wrong normals sometimes?
1251                                                 if(out_normalVelocity < 0.0)
1252                                                 {
1253                                                         out_normalVelocity*= -1.0;
1254                                                         negate_v3(out_normal);
1255                                                 }
1256 */
1257                                                 /* Inelastic repulsion impulse. */
1258
1259                                                 // Calculate which normal velocity we need. 
1260                                                 desiredVn = (out_normalVelocity * (float)solution[k] - (.1 * (clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree )) - sqrt(distance)) - ALMOST_ZERO);
1261
1262                                                 // Now calculate what impulse we need to reach that velocity. 
1263                                                 I_mag = (out_normalVelocity - desiredVn) / 2.0; // / (1/m1 + 1/m2);
1264
1265                                                 // Finally apply that impulse. 
1266                                                 impulse = (2.0 * -I_mag) / (a*a + (1.0-a)*(1.0-a) + b*b + (1.0-b)*(1.0-b));
1267
1268                                                 VECADDMUL ( verts1[edgecollpair.p11].impulse, out_normal, (1.0-a) * impulse );
1269                                                 verts1[edgecollpair.p11].impulse_count++;
1270
1271                                                 VECADDMUL ( verts1[edgecollpair.p12].impulse, out_normal, a * impulse );
1272                                                 verts1[edgecollpair.p12].impulse_count++;
1273
1274                                                 // return true;
1275                                                 result = 1;
1276                                                 break;
1277                                         }
1278                                         else
1279                                         {
1280                                                 // missing from collision.hpp
1281                                         }
1282                                         // mintime = MIN2(mintime, (float)solution[k]);
1283
1284                                         break;
1285                                 }
1286                         }
1287                 }
1288         }
1289         return result;
1290 }
1291
1292 static int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
1293 {
1294         Cloth *cloth1;
1295         cloth1 = clmd->clothObject;
1296
1297         for ( ; collpair != collision_end; collpair++ )
1298         {
1299                 // only handle moving collisions here
1300                 if (!( collpair->flag & COLLISION_IN_FUTURE ))
1301                         continue;
1302
1303                 cloth_collision_moving_edges ( clmd, collmd, collpair);
1304                 // cloth_collision_moving_tris ( clmd, collmd, collpair);
1305         }
1306
1307         return 1;
1308 }
1309 #endif
1310
1311 static void add_collision_object(Object ***objs, unsigned int *numobj, unsigned int *maxobj, Object *ob, Object *self, int level)
1312 {
1313         CollisionModifierData *cmd= NULL;
1314
1315         if(ob == self)
1316                 return;
1317
1318         /* only get objects with collision modifier */
1319         if(ob->pd && ob->pd->deflect)
1320                 cmd= (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
1321         
1322         if(cmd) {       
1323                 /* extend array */
1324                 if(*numobj >= *maxobj) {
1325                         *maxobj *= 2;
1326                         *objs= MEM_reallocN(*objs, sizeof(Object*)*(*maxobj));
1327                 }
1328                 
1329                 (*objs)[*numobj] = ob;
1330                 (*numobj)++;
1331         }
1332
1333         /* objects in dupli groups, one level only for now */
1334         if(ob->dup_group && level == 0) {
1335                 GroupObject *go;
1336                 Group *group= ob->dup_group;
1337
1338                 /* add objects */
1339                 for(go= group->gobject.first; go; go= go->next)
1340                         add_collision_object(objs, numobj, maxobj, go->ob, self, level+1);
1341         }       
1342 }
1343
1344 // return all collision objects in scene
1345 // collision object will exclude self 
1346 Object **get_collisionobjects(Scene *scene, Object *self, Group *group, unsigned int *numcollobj)
1347 {
1348         Base *base;
1349         Object **objs;
1350         GroupObject *go;
1351         unsigned int numobj= 0, maxobj= 100;
1352         
1353         objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray");
1354
1355         /* gather all collision objects */
1356         if(group) {
1357                 /* use specified group */
1358                 for(go= group->gobject.first; go; go= go->next)
1359                         add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0);
1360         }
1361         else {
1362                 Scene *sce_iter;
1363                 /* add objects in same layer in scene */
1364                 for(SETLOOPER(scene, sce_iter, base)) {
1365                         if(base->lay & self->lay)
1366                                 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0);
1367
1368                 }
1369         }
1370
1371         *numcollobj= numobj;
1372
1373         return objs;
1374 }
1375
1376 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level)
1377 {
1378         CollisionModifierData *cmd= NULL;
1379         ColliderCache *col;
1380
1381         if(ob == self)
1382                 return;
1383
1384         if(ob->pd && ob->pd->deflect)
1385                 cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
1386         
1387         if(cmd && cmd->bvhtree) {       
1388                 if(*objs == NULL)
1389                         *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array");
1390
1391                 col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
1392                 col->ob = ob;
1393                 col->collmd = cmd;
1394                 /* make sure collider is properly set up */
1395                 collision_move_object(cmd, 1.0, 0.0);
1396                 BLI_addtail(*objs, col);
1397         }
1398
1399         /* objects in dupli groups, one level only for now */
1400         if(ob->dup_group && level == 0) {
1401                 GroupObject *go;
1402                 Group *group= ob->dup_group;
1403
1404                 /* add objects */
1405                 for(go= group->gobject.first; go; go= go->next)
1406                         add_collider_cache_object(objs, go->ob, self, level+1);
1407         }
1408 }
1409
1410 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group)
1411 {
1412         GroupObject *go;
1413         ListBase *objs= NULL;
1414         
1415         /* add object in same layer in scene */
1416         if(group) {
1417                 for(go= group->gobject.first; go; go= go->next)
1418                         add_collider_cache_object(&objs, go->ob, self, 0);
1419         }
1420         else {
1421                 Scene *sce_iter;
1422                 Base *base;
1423
1424                 /* add objects in same layer in scene */
1425                 for(SETLOOPER(scene, sce_iter, base)) {
1426                         if(!self || (base->lay & self->lay))
1427                                 add_collider_cache_object(&objs, base->object, self, 0);
1428
1429                 }
1430         }
1431
1432         return objs;
1433 }
1434
1435 void free_collider_cache(ListBase **colliders)
1436 {
1437         if(*colliders) {
1438                 BLI_freelistN(*colliders);
1439                 MEM_freeN(*colliders);
1440                 *colliders = NULL;
1441         }
1442 }
1443
1444 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap)
1445 {
1446         int i;
1447         
1448         *collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * numresult * 4, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
1449         *collisions_index = *collisions;
1450
1451         for ( i = 0; i < numresult; i++ )
1452         {
1453                 *collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, overlap+i, *collisions_index );
1454         }
1455 }
1456
1457 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
1458 {
1459         Cloth *cloth = clmd->clothObject;
1460         int i=0, j = 0, /*numfaces = 0,*/ numverts = 0;
1461         ClothVertex *verts = NULL;
1462         int ret = 0;
1463         int result = 0;
1464         float tnull[3] = {0,0,0};
1465         
1466         /*numfaces = clmd->clothObject->numfaces;*/ /*UNUSED*/
1467         numverts = clmd->clothObject->numverts;
1468  
1469         verts = cloth->verts;
1470         
1471         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
1472         result = 1;
1473         for ( j = 0; j < 5; j++ ) // 5 is just a value that ensures convergence
1474         {
1475                 result = 0;
1476
1477                 if ( collmd->bvhtree )
1478                 {
1479                         result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
1480
1481                         // apply impulses in parallel
1482                         if ( result )
1483                         {
1484                                 for ( i = 0; i < numverts; i++ )
1485                                 {
1486                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
1487                                         if ( verts[i].impulse_count )
1488                                         {
1489                                                 VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1490                                                 VECCOPY ( verts[i].impulse, tnull );
1491                                                 verts[i].impulse_count = 0;
1492
1493                                                 ret++;
1494                                         }
1495                                 }
1496                         }
1497                 }
1498         }
1499         return ret;
1500 }
1501
1502 // cloth - object collisions
1503 int cloth_bvh_objcollision (Object *ob, ClothModifierData * clmd, float step, float dt )
1504 {
1505         Cloth *cloth= clmd->clothObject;
1506         BVHTree *cloth_bvh= cloth->bvhtree;
1507         unsigned int i=0, numfaces = 0, numverts = 0, k, l, j;
1508         int rounds = 0; // result counts applied collisions; ic is for debug output;
1509         ClothVertex *verts = NULL;
1510         int ret = 0, ret2 = 0;
1511         Object **collobjs = NULL;
1512         unsigned int numcollobj = 0;
1513
1514         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
1515                 return 0;
1516
1517         verts = cloth->verts;
1518         numfaces = cloth->numfaces;
1519         numverts = cloth->numverts;
1520
1521         ////////////////////////////////////////////////////////////
1522         // static collisions
1523         ////////////////////////////////////////////////////////////
1524
1525         // update cloth bvh
1526         bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1527         bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1528         
1529         collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj);
1530         
1531         if(!collobjs)
1532                 return 0;
1533
1534         do
1535         {
1536                 CollPair **collisions, **collisions_index;
1537                 
1538                 ret2 = 0;
1539
1540                 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
1541                 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
1542                 
1543                 // check all collision objects
1544                 for(i = 0; i < numcollobj; i++)
1545                 {
1546                         Object *collob= collobjs[i];
1547                         CollisionModifierData *collmd = (CollisionModifierData*)modifiers_findByType(collob, eModifierType_Collision);
1548                         BVHTreeOverlap *overlap = NULL;
1549                         unsigned int result = 0;
1550                         
1551                         if(!collmd->bvhtree)
1552                                 continue;
1553                         
1554                         /* move object to position (step) in time */
1555                         collision_move_object ( collmd, step + dt, step );
1556                         
1557                         /* search for overlapping collision pairs */
1558                         overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
1559                                 
1560                         // go to next object if no overlap is there
1561                         if( result && overlap ) {
1562                                 /* check if collisions really happen (costly near check) */
1563                                 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], &collisions_index[i], result, overlap);
1564                         
1565                                 // resolve nearby collisions
1566                                 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i],  collisions_index[i]);
1567                                 ret2 += ret;
1568                         }
1569
1570                         if ( overlap )
1571                                 MEM_freeN ( overlap );
1572                 }
1573                 rounds++;
1574                 
1575                 for(i = 0; i < numcollobj; i++)
1576                 {
1577                         if ( collisions[i] ) MEM_freeN ( collisions[i] );
1578                 }
1579                         
1580                 MEM_freeN(collisions);
1581                 MEM_freeN(collisions_index);
1582
1583                 ////////////////////////////////////////////////////////////
1584                 // update positions
1585                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1586                 ////////////////////////////////////////////////////////////
1587
1588                 // verts come from clmd
1589                 for ( i = 0; i < numverts; i++ )
1590                 {
1591                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1592                         {
1593                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1594                                 {
1595                                         continue;
1596                                 }
1597                         }
1598
1599                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
1600                 }
1601                 ////////////////////////////////////////////////////////////
1602                 
1603                 
1604                 ////////////////////////////////////////////////////////////
1605                 // Test on *simple* selfcollisions
1606                 ////////////////////////////////////////////////////////////
1607                 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
1608                 {
1609                         for(l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++)
1610                         {
1611                                 // TODO: add coll quality rounds again
1612                                 BVHTreeOverlap *overlap = NULL;
1613                                 unsigned int result = 0;
1614         
1615                                 // collisions = 1;
1616                                 verts = cloth->verts; // needed for openMP
1617         
1618                                 numfaces = cloth->numfaces;
1619                                 numverts = cloth->numverts;
1620         
1621                                 verts = cloth->verts;
1622         
1623                                 if ( cloth->bvhselftree )
1624                                 {
1625                                         // search for overlapping collision pairs 
1626                                         overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
1627         
1628         // #pragma omp parallel for private(k, i, j) schedule(static)
1629                                         for ( k = 0; k < result; k++ )
1630                                         {
1631                                                 float temp[3];
1632                                                 float length = 0;
1633                                                 float mindistance;
1634         
1635                                                 i = overlap[k].indexA;
1636                                                 j = overlap[k].indexB;
1637         
1638                                                 mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
1639         
1640                                                 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1641                                                 {
1642                                                         if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1643                                                                                 && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
1644                                                         {
1645                                                                 continue;
1646                                                         }
1647                                                 }
1648         
1649                                                 VECSUB ( temp, verts[i].tx, verts[j].tx );
1650         
1651                                                 if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
1652         
1653                                                 // check for adjacent points (i must be smaller j)
1654                                                 if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) )
1655                                                 {
1656                                                         continue;
1657                                                 }
1658         
1659                                                 length = normalize_v3( temp );
1660         
1661                                                 if ( length < mindistance )
1662                                                 {
1663                                                         float correction = mindistance - length;
1664         
1665                                                         if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1666                                                         {
1667                                                                 mul_v3_fl( temp, -correction );
1668                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
1669                                                         }
1670                                                         else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
1671                                                         {
1672                                                                 mul_v3_fl( temp, correction );
1673                                                                 VECADD ( verts[i].tx, verts[i].tx, temp );
1674                                                         }
1675                                                         else
1676                                                         {
1677                                                                 mul_v3_fl( temp, -correction*0.5 );
1678                                                                 VECADD ( verts[j].tx, verts[j].tx, temp );
1679         
1680                                                                 VECSUB ( verts[i].tx, verts[i].tx, temp );
1681                                                         }
1682                                                         ret = 1;
1683                                                         ret2 += ret;
1684                                                 }
1685                                                 else
1686                                                 {
1687                                                         // check for approximated time collisions
1688                                                 }
1689                                         }
1690         
1691                                         if ( overlap )
1692                                                 MEM_freeN ( overlap );
1693         
1694                                 }
1695                         }
1696                         ////////////////////////////////////////////////////////////
1697
1698                         ////////////////////////////////////////////////////////////
1699                         // SELFCOLLISIONS: update velocities
1700                         ////////////////////////////////////////////////////////////
1701                         if ( ret2 )
1702                         {
1703                                 for ( i = 0; i < cloth->numverts; i++ )
1704                                 {
1705                                         if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) )
1706                                         {
1707                                                 VECSUB ( verts[i].tv, verts[i].tx, verts[i].txold );
1708                                         }
1709                                 }
1710                         }
1711                         ////////////////////////////////////////////////////////////
1712                 }
1713         }
1714         while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
1715         
1716         if(collobjs)
1717                 MEM_freeN(collobjs);
1718
1719         return MIN2 ( ret, 1 );
1720 }