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