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