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