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