4d49ccc9eb3d5fd92dbb814a91dcd9c5ad3879e8
[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 int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
502 {
503         int result = 0;
504         Cloth *cloth1;
505         float w1, w2, w3, u1, u2, u3;
506         float v1[3], v2[3], relativeVelocity[3];
507         float magrelVel;
508         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
509
510         cloth1 = clmd->clothObject;
511
512         for ( ; collpair != collision_end; collpair++ )
513         {
514                 // only handle static collisions here
515                 if ( collpair->flag & COLLISION_IN_FUTURE )
516                         continue;
517
518                 // compute barycentric coordinates for both collision points
519                 collision_compute_barycentric ( collpair->pa,
520                                                 cloth1->verts[collpair->ap1].txold,
521                                                 cloth1->verts[collpair->ap2].txold,
522                                                 cloth1->verts[collpair->ap3].txold,
523                                                 &w1, &w2, &w3 );
524
525                 // was: txold
526                 collision_compute_barycentric ( collpair->pb,
527                                                 collmd->current_x[collpair->bp1].co,
528                                                 collmd->current_x[collpair->bp2].co,
529                                                 collmd->current_x[collpair->bp3].co,
530                                                 &u1, &u2, &u3 );
531
532                 // Calculate relative "velocity".
533                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
534
535                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
536
537                 VECSUB ( relativeVelocity, v2, v1 );
538
539                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
540                 magrelVel = INPR ( relativeVelocity, collpair->normal );
541
542                 // printf("magrelVel: %f\n", magrelVel);
543
544                 // Calculate masses of points.
545                 // TODO
546
547                 // If v_n_mag < 0 the edges are approaching each other.
548                 if ( magrelVel > ALMOST_ZERO )
549                 {
550                         // Calculate Impulse magnitude to stop all motion in normal direction.
551                         float magtangent = 0, repulse = 0, d = 0;
552                         double impulse = 0.0;
553                         float vrel_t_pre[3];
554                         float temp[3];
555
556                         // calculate tangential velocity
557                         VECCOPY ( temp, collpair->normal );
558                         VecMulf ( temp, magrelVel );
559                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
560
561                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
562                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
563                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
564
565                         // Apply friction impulse.
566                         if ( magtangent > ALMOST_ZERO )
567                         {
568                                 Normalize ( vrel_t_pre );
569
570                                 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
571                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
572                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
573                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
574                         }
575
576                         // Apply velocity stopping impulse
577                         // I_c = m * v_N / 2.0
578                         // no 2.0 * magrelVel normally, but looks nicer DG
579                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
580
581                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
582                         cloth1->verts[collpair->ap1].impulse_count++;
583
584                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
585                         cloth1->verts[collpair->ap2].impulse_count++;
586
587                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
588                         cloth1->verts[collpair->ap3].impulse_count++;
589
590                         // Apply repulse impulse if distance too short
591                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
592                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
593                         if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) )
594                         {
595                                 repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
596
597                                 // stay on the safe side and clamp repulse
598                                 if ( impulse > ALMOST_ZERO )
599                                         repulse = MIN2 ( repulse, 5.0*impulse );
600                                 repulse = MAX2 ( impulse, repulse );
601
602                                 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
603                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
604                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
605                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
606                         }
607
608                         result = 1;
609                 }
610         }
611         return result;
612 }
613
614 //Determines collisions on overlap, collisions are writen to collpair[i] and collision+number_collision_found is returned
615 CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, CollPair *collpair )
616 {
617         ClothModifierData *clmd = ( ClothModifierData * ) md1;
618         CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
619         MFace *face1=NULL, *face2 = NULL;
620         ClothVertex *verts1 = clmd->clothObject->verts;
621         double distance = 0;
622         float epsilon1 = clmd->coll_parms->epsilon;
623         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
624         int i;
625
626         face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
627         face2 = & ( collmd->mfaces[overlap->indexB] );
628
629         // check all 4 possible collisions
630         for ( i = 0; i < 4; i++ )
631         {
632                 if ( i == 0 )
633                 {
634                         // fill faceA
635                         collpair->ap1 = face1->v1;
636                         collpair->ap2 = face1->v2;
637                         collpair->ap3 = face1->v3;
638
639                         // fill faceB
640                         collpair->bp1 = face2->v1;
641                         collpair->bp2 = face2->v2;
642                         collpair->bp3 = face2->v3;
643                 }
644                 else if ( i == 1 )
645                 {
646                         if ( face1->v4 )
647                         {
648                                 // fill faceA
649                                 collpair->ap1 = face1->v1;
650                                 collpair->ap2 = face1->v4;
651                                 collpair->ap3 = face1->v3;
652
653                                 // fill faceB
654                                 collpair->bp1 = face2->v1;
655                                 collpair->bp2 = face2->v2;
656                                 collpair->bp3 = face2->v3;
657                         }
658                         else
659                                 i++;
660                 }
661                 if ( i == 2 )
662                 {
663                         if ( face2->v4 )
664                         {
665                                 // fill faceA
666                                 collpair->ap1 = face1->v1;
667                                 collpair->ap2 = face1->v2;
668                                 collpair->ap3 = face1->v3;
669
670                                 // fill faceB
671                                 collpair->bp1 = face2->v1;
672                                 collpair->bp2 = face2->v4;
673                                 collpair->bp3 = face2->v3;
674                         }
675                         else
676                                 break;
677                 }
678                 else if ( i == 3 )
679                 {
680                         if ( face1->v4 && face2->v4 )
681                         {
682                                 // fill faceA
683                                 collpair->ap1 = face1->v1;
684                                 collpair->ap2 = face1->v4;
685                                 collpair->ap3 = face1->v3;
686
687                                 // fill faceB
688                                 collpair->bp1 = face2->v1;
689                                 collpair->bp2 = face2->v4;
690                                 collpair->bp3 = face2->v3;
691                         }
692                         else
693                                 break;
694                 }
695
696 #ifdef WITH_BULLET
697                 // calc distance + normal
698                 distance = plNearestPoints (
699                                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 );
700 #else
701                 // just be sure that we don't add anything
702                 distance = 2.0 * ( epsilon1 + epsilon2 + ALMOST_ZERO );
703 #endif
704
705                 if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) )
706                 {
707                         VECCOPY ( collpair->normal, collpair->vector );
708                         Normalize ( collpair->normal );
709
710                         collpair->distance = distance;
711                         collpair->flag = 0;
712                 }
713                 else
714                 {
715                         // check for collision in the future
716                         collpair->flag |= COLLISION_IN_FUTURE;
717                 }
718                 collpair++;
719         }
720         return collpair;
721 }
722
723 int cloth_are_edges_adjacent ( ClothModifierData *clmd, CollisionModifierData *collmd, EdgeCollPair *edgecollpair )
724 {
725         Cloth *cloth1 = NULL;
726         ClothVertex *verts1 = NULL;
727         float temp[3];
728         MVert *verts2 = collmd->current_x; // old x
729
730         cloth1 = clmd->clothObject;
731         verts1 = cloth1->verts;
732
733         VECSUB ( temp, verts1[edgecollpair->p11].txold, verts2[edgecollpair->p21].co );
734         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
735                 return 1;
736
737         VECSUB ( temp, verts1[edgecollpair->p11].txold, verts2[edgecollpair->p22].co );
738         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
739                 return 1;
740
741         VECSUB ( temp, verts1[edgecollpair->p12].txold, verts2[edgecollpair->p21].co );
742         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
743                 return 1;
744
745         VECSUB ( temp, verts1[edgecollpair->p12].txold, verts2[edgecollpair->p22].co );
746         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
747                 return 1;
748         
749         VECSUB ( temp, verts1[edgecollpair->p11].txold, verts1[edgecollpair->p12].txold );
750         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
751                 return 1;
752         
753         VECSUB ( temp, verts2[edgecollpair->p21].co, verts2[edgecollpair->p22].co );
754         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
755                 return 1;
756         
757
758         return 0;
759 }
760
761 int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
762 {
763         int result = 0;
764         Cloth *cloth1;
765         float w1, w2, w3, u1, u2, u3;
766         float v1[3], v2[3], relativeVelocity[3];
767         float magrelVel;
768         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
769
770         cloth1 = clmd->clothObject;
771
772         for ( ; collpair != collision_end; collpair++ )
773         {
774                 // only handle static collisions here
775                 if ( collpair->flag & COLLISION_IN_FUTURE )
776                         continue;
777
778                 // compute barycentric coordinates for both collision points
779                 collision_compute_barycentric ( collpair->pa,
780                                                                                 cloth1->verts[collpair->ap1].txold,
781                   cloth1->verts[collpair->ap2].txold,
782         cloth1->verts[collpair->ap3].txold,
783  &w1, &w2, &w3 );
784
785                 // was: txold
786                 collision_compute_barycentric ( collpair->pb,
787                                                                                 collmd->current_x[collpair->bp1].co,
788                   collmd->current_x[collpair->bp2].co,
789         collmd->current_x[collpair->bp3].co,
790  &u1, &u2, &u3 );
791
792                 // Calculate relative "velocity".
793                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
794
795                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
796
797                 VECSUB ( relativeVelocity, v2, v1 );
798
799                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
800                 magrelVel = INPR ( relativeVelocity, collpair->normal );
801
802                 // printf("magrelVel: %f\n", magrelVel);
803
804                 // Calculate masses of points.
805                 // TODO
806
807                 // If v_n_mag < 0 the edges are approaching each other.
808                 if ( magrelVel > ALMOST_ZERO )
809                 {
810                         // Calculate Impulse magnitude to stop all motion in normal direction.
811                         float magtangent = 0, repulse = 0, d = 0;
812                         double impulse = 0.0;
813                         float vrel_t_pre[3];
814                         float temp[3];
815
816                         // calculate tangential velocity
817                         VECCOPY ( temp, collpair->normal );
818                         VecMulf ( temp, magrelVel );
819                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
820
821                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
822                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
823                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
824
825                         // Apply friction impulse.
826                         if ( magtangent > ALMOST_ZERO )
827                         {
828                                 Normalize ( vrel_t_pre );
829
830                                 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
831                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
832                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
833                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
834                         }
835
836                         // Apply velocity stopping impulse
837                         // I_c = m * v_N / 2.0
838                         // no 2.0 * magrelVel normally, but looks nicer DG
839                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
840
841                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
842                         cloth1->verts[collpair->ap1].impulse_count++;
843
844                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
845                         cloth1->verts[collpair->ap2].impulse_count++;
846
847                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
848                         cloth1->verts[collpair->ap3].impulse_count++;
849
850                         // Apply repulse impulse if distance too short
851                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
852                         /*
853                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
854                         if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) )
855                         {
856                                 repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
857
858                                 // stay on the safe side and clamp repulse
859                                 if ( impulse > ALMOST_ZERO )
860                                         repulse = MIN2 ( repulse, 5.0*impulse );
861                                 repulse = MAX2 ( impulse, repulse );
862
863                                 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
864                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
865                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
866                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
867                         }
868 */
869                         result = 1;
870                 }
871         }
872         return result;
873 }
874
875 int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
876 {
877         EdgeCollPair edgecollpair;
878         Cloth *cloth1=NULL;
879         ClothVertex *verts1=NULL;
880         unsigned int i = 0, j = 0, k = 0;
881         int numsolutions = 0;
882         double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
883         double solution[3];
884         MVert *verts2 = collmd->current_x; // old x
885         MVert *velocity2 = collmd->current_v; // velocity
886         float mintime = FLT_MAX;
887         float distance;
888         float triA[3][3], triB[3][3];
889         int result = 0;
890
891         cloth1 = clmd->clothObject;
892         verts1 = cloth1->verts;
893         
894         for(i = 0; i < 9; i++)
895         {
896                 // 9 edge - edge possibilities
897                 
898                 if(i == 0) // cloth edge: 1-2; coll edge: 1-2
899                 {
900                         edgecollpair.p11 = collpair->ap1;
901                         edgecollpair.p12 = collpair->ap2;
902                         
903                         edgecollpair.p21 = collpair->bp1;
904                         edgecollpair.p22 = collpair->bp2;
905                 }
906                 else if(i == 1) // cloth edge: 1-2; coll edge: 2-3
907                 {
908                         edgecollpair.p11 = collpair->ap1;
909                         edgecollpair.p12 = collpair->ap2;
910                         
911                         edgecollpair.p21 = collpair->bp2;
912                         edgecollpair.p22 = collpair->bp3;
913                 }
914                 else if(i == 2) // cloth edge: 1-2; coll edge: 1-3
915                 {
916                         edgecollpair.p11 = collpair->ap1;
917                         edgecollpair.p12 = collpair->ap2;
918                         
919                         edgecollpair.p21 = collpair->bp1;
920                         edgecollpair.p22 = collpair->bp3;
921                 }
922                 else if(i == 3) // cloth edge: 2-3; coll edge: 1-2
923                 {
924                         edgecollpair.p11 = collpair->ap2;
925                         edgecollpair.p12 = collpair->ap3;
926                         
927                         edgecollpair.p21 = collpair->bp1;
928                         edgecollpair.p22 = collpair->bp2;
929                 }
930                 else if(i == 4) // cloth edge: 2-3; coll edge: 2-3
931                 {
932                         edgecollpair.p11 = collpair->ap2;
933                         edgecollpair.p12 = collpair->ap3;
934                         
935                         edgecollpair.p21 = collpair->bp2;
936                         edgecollpair.p22 = collpair->bp3;
937                 }
938                 else if(i == 5) // cloth edge: 2-3; coll edge: 1-3
939                 {
940                         edgecollpair.p11 = collpair->ap2;
941                         edgecollpair.p12 = collpair->ap3;
942                         
943                         edgecollpair.p21 = collpair->bp1;
944                         edgecollpair.p22 = collpair->bp3;
945                 }
946                 else if(i ==6) // cloth edge: 1-3; coll edge: 1-2
947                 {
948                         edgecollpair.p11 = collpair->ap1;
949                         edgecollpair.p12 = collpair->ap3;
950                         
951                         edgecollpair.p21 = collpair->bp1;
952                         edgecollpair.p22 = collpair->bp2;
953                 }
954                 else if(i ==7) // cloth edge: 1-3; coll edge: 2-3
955                 {
956                         edgecollpair.p11 = collpair->ap1;
957                         edgecollpair.p12 = collpair->ap3;
958                         
959                         edgecollpair.p21 = collpair->bp2;
960                         edgecollpair.p22 = collpair->bp3;
961                 }
962                 else if(i == 8) // cloth edge: 1-3; coll edge: 1-3
963                 {
964                         edgecollpair.p11 = collpair->ap1;
965                         edgecollpair.p12 = collpair->ap3;
966                         
967                         edgecollpair.p21 = collpair->bp1;
968                         edgecollpair.p22 = collpair->bp3;
969                 }
970                 
971                 if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
972                 {
973                         // always put coll points in p21/p22
974                         VECSUB ( x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold );
975                         VECSUB ( v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv );
976                         
977                         VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold );
978                         VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv );
979                         
980                         VECSUB ( x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold );
981                         VECSUB ( v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv );
982                         
983                         numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
984         
985                         for ( k = 0; k < numsolutions; k++ )
986                         {
987                                 // printf("sol %d: %lf\n", k, solution[k]);
988                                 if ( ( solution[k] >= DBL_EPSILON ) && ( solution[k] <= 1.0 ) )
989                                 {
990                                         //float out_collisionTime = solution[k];
991         
992                                         // TODO: check for collisions
993         
994                                         // TODO: put into (edge) collision list
995                                         
996                                         mintime = MIN2(mintime, (float)solution[k]);
997                                         
998                                         result = 1;
999                                         break;
1000                                 }
1001                         }
1002                 }
1003         }
1004         
1005         if(result)
1006         {
1007                 // move triangles to collision point in time
1008                 VECADDS(triA[0], verts1[collpair->ap1].txold, verts1[collpair->ap1].tv, mintime);
1009                 VECADDS(triA[1], verts1[collpair->ap2].txold, verts1[collpair->ap2].tv, mintime);
1010                 VECADDS(triA[2], verts1[collpair->ap3].txold, verts1[collpair->ap3].tv, mintime);
1011                 
1012                 VECADDS(triB[0], collmd->current_x[collpair->bp1].co, collmd->current_v[collpair->bp1].co, mintime);
1013                 VECADDS(triB[1], collmd->current_x[collpair->bp2].co, collmd->current_v[collpair->bp2].co, mintime);
1014                 VECADDS(triB[2], collmd->current_x[collpair->bp3].co, collmd->current_v[collpair->bp3].co, mintime);
1015                 
1016                 // check distance there
1017                 distance = plNearestPoints (triA[0], triA[1], triA[2], triB[0], triB[1], triB[2], collpair->pa,collpair->pb,collpair->vector );
1018                 
1019                 if(distance <= (clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree ) + ALMOST_ZERO))
1020                 {
1021                         CollPair *next = collpair;
1022                         next++;
1023                         
1024                         collpair->distance = clmd->coll_parms->epsilon;
1025                         collpair->time = mintime;
1026                         
1027                         VECCOPY ( collpair->normal, collpair->vector );
1028                         Normalize ( collpair->normal );
1029                         
1030                         cloth_collision_response_moving ( clmd, collmd, collpair, next );
1031                 }
1032         }
1033         
1034         return result;
1035 }
1036
1037 /*
1038 void cloth_collision_moving_tris ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
1039 {
1040         CollPair collpair;
1041         Cloth *cloth1=NULL, *cloth2=NULL;
1042         MFace *face1=NULL, *face2=NULL;
1043         ClothVertex *verts1=NULL, *verts2=NULL;
1044         unsigned int i = 0, j = 0, k = 0;
1045         int numsolutions = 0;
1046         float a[3], b[3], c[3], d[3], e[3], f[3];
1047         double solution[3];
1048
1049         for ( i = 0; i < 2; i++ )
1050         {
1051                 cloth1 = clmd->clothObject;
1052                 cloth2 = coll_clmd->clothObject;
1053
1054                 verts1 = cloth1->verts;
1055                 verts2 = cloth2->verts;
1056
1057                 face1 = & ( cloth1->mfaces[tree1->tri_index] );
1058                 face2 = & ( cloth2->mfaces[tree2->tri_index] );
1059
1060                 // check all possible pairs of triangles
1061                 if ( i == 0 )
1062                 {
1063                         collpair.ap1 = face1->v1;
1064                         collpair.ap2 = face1->v2;
1065                         collpair.ap3 = face1->v3;
1066
1067                         collpair.pointsb[0] = face2->v1;
1068                         collpair.pointsb[1] = face2->v2;
1069                         collpair.pointsb[2] = face2->v3;
1070                         collpair.pointsb[3] = face2->v4;
1071                 }
1072
1073                 if ( i == 1 )
1074                 {
1075                         if ( face1->v4 )
1076                         {
1077                                 collpair.ap1 = face1->v3;
1078                                 collpair.ap2 = face1->v4;
1079                                 collpair.ap3 = face1->v1;
1080
1081                                 collpair.pointsb[0] = face2->v1;
1082                                 collpair.pointsb[1] = face2->v2;
1083                                 collpair.pointsb[2] = face2->v3;
1084                                 collpair.pointsb[3] = face2->v4;
1085                         }
1086                         else
1087                                 i++;
1088                 }
1089
1090                 // calc SIPcode (?)
1091
1092                 if ( i < 2 )
1093                 {
1094                         VECSUB ( a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold );
1095                         VECSUB ( b, verts1[collpair.ap2].v, verts1[collpair.ap1].v );
1096                         VECSUB ( c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold );
1097                         VECSUB ( d, verts1[collpair.ap3].v, verts1[collpair.ap1].v );
1098
1099                         for ( j = 0; j < 4; j++ )
1100                         {
1101                                 if ( ( j==3 ) && ! ( face2->v4 ) )
1102                                         break;
1103
1104                                 VECSUB ( e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold );
1105                                 VECSUB ( f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v );
1106
1107                                 numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution );
1108
1109                                 for ( k = 0; k < numsolutions; k++ )
1110                                 {
1111                                         if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) )
1112                                         {
1113                                                 //float out_collisionTime = solution[k];
1114
1115                                                 // TODO: check for collisions
1116
1117                                                 // TODO: put into (point-face) collision list
1118
1119                                                 // printf("Moving found!\n");
1120
1121                                         }
1122                                 }
1123
1124                                 // TODO: check borders for collisions
1125                         }
1126
1127                 }
1128         }
1129 }
1130 */
1131
1132 int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
1133 {
1134         int result = 0;
1135         Cloth *cloth1;
1136         float w1, w2, w3, u1, u2, u3;
1137         float v1[3], v2[3], relativeVelocity[3];
1138         float magrelVel;
1139         float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
1140
1141         cloth1 = clmd->clothObject;
1142
1143         for ( ; collpair != collision_end; collpair++ )
1144         {
1145                 // only handle moving collisions here
1146                 if (!( collpair->flag & COLLISION_IN_FUTURE ))
1147                         continue;
1148                 
1149                 cloth_collision_moving_edges ( clmd, collmd, collpair);
1150         }
1151         
1152         return 1;
1153 }
1154
1155 int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData *collmd, float step, float dt )
1156 {
1157         Cloth *cloth = clmd->clothObject;
1158         BVHTree *cloth_bvh= ( BVHTree * ) cloth->bvhtree;
1159         long i=0, j = 0, numfaces = 0, numverts = 0;
1160         ClothVertex *verts = NULL;
1161         CollPair *collisions = NULL, *collisions_index = NULL;
1162         int ret = 0;
1163         int result = 0;
1164         float tnull[3] = {0,0,0};
1165         BVHTreeOverlap *overlap = NULL;
1166
1167
1168         numfaces = clmd->clothObject->numfaces;
1169         numverts = clmd->clothObject->numverts;
1170
1171         verts = cloth->verts;
1172
1173         if ( collmd->bvhtree )
1174         {
1175                 /* get pointer to bounding volume hierarchy */
1176                 BVHTree *coll_bvh = collmd->bvhtree;
1177
1178                 /* move object to position (step) in time */
1179                 collision_move_object ( collmd, step + dt, step );
1180
1181                 /* search for overlapping collision pairs */
1182                 overlap = BLI_bvhtree_overlap ( cloth_bvh, coll_bvh, &result );
1183
1184                 collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * result*4, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
1185                 collisions_index = collisions;
1186
1187                 for ( i = 0; i < result; i++ )
1188                 {
1189                         collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, overlap+i, collisions_index );
1190                 }
1191
1192                 if ( overlap )
1193                         MEM_freeN ( overlap );
1194         }
1195         else
1196         {
1197                 if ( G.rt > 0 )
1198                         printf ( "cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n" );
1199         }
1200
1201         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
1202         result = 1;
1203         for ( j = 0; j < 5; j++ ) // 5 is just a value that ensures convergence
1204         {
1205                 result = 0;
1206
1207                 if ( collmd->bvhtree )
1208                 {
1209                         result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
1210
1211                         // apply impulses in parallel
1212                         if ( result )
1213                         {
1214                                 for ( i = 0; i < numverts; i++ )
1215                                 {
1216                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
1217                                         if ( verts[i].impulse_count )
1218                                         {
1219                                                 VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1220                                                 VECCOPY ( verts[i].impulse, tnull );
1221                                                 verts[i].impulse_count = 0;
1222         
1223                                                 ret++;
1224                                         }
1225                                 }
1226                         }
1227                         /*
1228                         result += cloth_collision_moving ( clmd, collmd, collisions, collisions_index );
1229                         
1230                         // apply impulses in parallel
1231                         if ( result )
1232                         {
1233                                 for ( i = 0; i < numverts; i++ )
1234                                 {
1235                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
1236                                         if ( verts[i].impulse_count )
1237                                         {
1238                                                 VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1239                                                 VECCOPY ( verts[i].impulse, tnull );
1240                                                 verts[i].impulse_count = 0;
1241         
1242                                                 ret++;
1243                                         }
1244                                 }
1245                         }
1246                 */
1247                 }
1248         }
1249
1250         if ( collisions ) MEM_freeN ( collisions );
1251
1252         return ret;
1253 }
1254
1255 // cloth - object collisions
1256 int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
1257 {
1258         Base *base=NULL;
1259         CollisionModifierData *collmd=NULL;
1260         Cloth *cloth=NULL;
1261         Object *coll_ob=NULL;
1262         BVHTree *cloth_bvh=NULL;
1263         long i=0, j = 0, k = 0, numfaces = 0, numverts = 0;
1264         unsigned int result = 0, rounds = 0; // result counts applied collisions; ic is for debug output;
1265         ClothVertex *verts = NULL;
1266         int ret = 0;
1267         ClothModifierData *tclmd;
1268         int collisions = 0, count = 0;
1269
1270         if ( ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ ) || ! ( ( ( Cloth * ) clmd->clothObject )->bvhtree ) )
1271         {
1272                 return 0;
1273         }
1274
1275         cloth = clmd->clothObject;
1276         verts = cloth->verts;
1277         cloth_bvh = ( BVHTree * ) cloth->bvhtree;
1278         numfaces = clmd->clothObject->numfaces;
1279         numverts = clmd->clothObject->numverts;
1280
1281         ////////////////////////////////////////////////////////////
1282         // static collisions
1283         ////////////////////////////////////////////////////////////
1284
1285         // update cloth bvh
1286         bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1287         bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1288
1289         do
1290         {
1291                 result = 0;
1292
1293                 // check all collision objects
1294                 for ( base = G.scene->base.first; base; base = base->next )
1295                 {
1296                         coll_ob = base->object;
1297                         collmd = ( CollisionModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Collision );
1298
1299                         if ( !collmd )
1300                         {
1301                                 if ( coll_ob->dup_group )
1302                                 {
1303                                         GroupObject *go;
1304                                         Group *group = coll_ob->dup_group;
1305
1306                                         for ( go= group->gobject.first; go; go= go->next )
1307                                         {
1308                                                 coll_ob = go->ob;
1309
1310                                                 collmd = ( CollisionModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Collision );
1311
1312                                                 if ( !collmd )
1313                                                         continue;
1314
1315                                                 tclmd = ( ClothModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Cloth );
1316                                                 if ( tclmd == clmd )
1317                                                         continue;
1318
1319                                                 ret += cloth_bvh_objcollisions_do ( clmd, collmd, step, dt );
1320                                         }
1321                                 }
1322                         }
1323                         else
1324                         {
1325                                 tclmd = ( ClothModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Cloth );
1326                                 if ( tclmd == clmd )
1327                                         continue;
1328
1329                                 ret += cloth_bvh_objcollisions_do ( clmd, collmd, step, dt );
1330                         }
1331                 }
1332                 rounds++;
1333
1334                 ////////////////////////////////////////////////////////////
1335                 // update positions
1336                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1337                 ////////////////////////////////////////////////////////////
1338
1339                 // verts come from clmd
1340                 for ( i = 0; i < numverts; i++ )
1341                 {
1342                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1343                         {
1344                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1345                                 {
1346                                         continue;
1347                                 }
1348                         }
1349
1350                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
1351                 }
1352                 ////////////////////////////////////////////////////////////
1353
1354
1355                 ////////////////////////////////////////////////////////////
1356                 // Test on *simple* selfcollisions
1357                 ////////////////////////////////////////////////////////////
1358                 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
1359                 {
1360
1361                         MFace *mface = cloth->mfaces;
1362                         BVHTreeOverlap *overlap = NULL;
1363
1364                         collisions = 1;
1365                         verts = cloth->verts; // needed for openMP
1366
1367                         numfaces = clmd->clothObject->numfaces;
1368                         numverts = clmd->clothObject->numverts;
1369
1370                         verts = cloth->verts;
1371
1372                         if ( cloth->bvhselftree )
1373                         {
1374                                 /* search for overlapping collision pairs */
1375                                 overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
1376
1377                                 for ( k = 0; k < result; k++ )
1378                                 {
1379                                         float temp[3];
1380                                         float length = 0;
1381                                         float mindistance;
1382                                         
1383                                         i = overlap[k].indexA;
1384                                         j = overlap[k].indexB;
1385                                         
1386                                         mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
1387
1388                                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1389                                         {
1390                                                 if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1391                                                                                                  && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
1392                                                 {
1393                                                         continue;
1394                                                 }
1395                                         }
1396
1397                                         VECSUB ( temp, verts[i].tx, verts[j].tx );
1398
1399                                         if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
1400
1401                                         // check for adjacent points (i must be smaller j)
1402                                         if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) )
1403                                         {
1404                                                 continue;
1405                                         }
1406
1407                                         length = Normalize ( temp );
1408
1409                                         if ( length < mindistance )
1410                                         {
1411                                                 float correction = mindistance - length;
1412
1413                                                 if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1414                                                 {
1415                                                         VecMulf ( temp, -correction );
1416                                                         VECADD ( verts[j].tx, verts[j].tx, temp );
1417                                                 }
1418                                                 else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
1419                                                 {
1420                                                         VecMulf ( temp, correction );
1421                                                         VECADD ( verts[i].tx, verts[i].tx, temp );
1422                                                 }
1423                                                 else
1424                                                 {
1425                                                         VecMulf ( temp, -correction*0.5 );
1426                                                         VECADD ( verts[j].tx, verts[j].tx, temp );
1427
1428                                                         VECSUB ( verts[i].tx, verts[i].tx, temp );
1429                                                 }
1430                                         }
1431                                 }
1432                                 
1433                                 if ( overlap )
1434                                         MEM_freeN ( overlap );
1435                                 
1436                         }
1437
1438                         /*
1439                         for ( count = 0; count < clmd->coll_parms->self_loop_count; count++ )
1440                         {
1441                         if ( collisions )
1442                         {
1443                         collisions = 0;
1444                         #pragma omp parallel for private(i,j, collisions) shared(verts, ret)
1445                         for ( i = 0; i < cloth->numverts; i++ )
1446                         {
1447                         for ( j = i + 1; j < cloth->numverts; j++ )
1448                         {
1449                         float temp[3];
1450                         float length = 0;
1451                         float mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
1452
1453                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1454                         {
1455                         if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1456                         && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
1457                         {
1458                         continue;
1459                         }
1460                         }
1461
1462                         VECSUB ( temp, verts[i].tx, verts[j].tx );
1463
1464                         if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
1465
1466                                                         // check for adjacent points (i must be smaller j)
1467                         if ( BLI_edgehash_haskey ( cloth->edgehash, i, j ) )
1468                         {
1469                         continue;
1470                         }
1471
1472                         length = Normalize ( temp );
1473
1474                         if ( length < mindistance )
1475                         {
1476                         float correction = mindistance - length;
1477
1478                         if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1479                         {
1480                         VecMulf ( temp, -correction );
1481                         VECADD ( verts[j].tx, verts[j].tx, temp );
1482                         }
1483                         else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
1484                         {
1485                         VecMulf ( temp, correction );
1486                         VECADD ( verts[i].tx, verts[i].tx, temp );
1487                         }
1488                         else
1489                         {
1490                         VecMulf ( temp, -correction*0.5 );
1491                         VECADD ( verts[j].tx, verts[j].tx, temp );
1492
1493                         VECSUB ( verts[i].tx, verts[i].tx, temp );
1494                         }
1495
1496                         collisions = 1;
1497
1498                         if ( !ret )
1499                         {
1500                         #pragma omp critical
1501                         {
1502                         ret = 1;
1503                         }
1504                         }
1505                         }
1506                         }
1507                         }
1508                         }
1509                         }
1510                         */
1511                         ////////////////////////////////////////////////////////////
1512
1513                         ////////////////////////////////////////////////////////////
1514                         // SELFCOLLISIONS: update velocities
1515                         ////////////////////////////////////////////////////////////
1516                         if ( ret )
1517                         {
1518                                 for ( i = 0; i < cloth->numverts; i++ )
1519                                 {
1520                                         if ( ! ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) )
1521                                                 VECSUB ( verts[i].tv, verts[i].tx, verts[i].txold );
1522                                 }
1523                         }
1524                         ////////////////////////////////////////////////////////////
1525                 }
1526         }
1527         while ( result && ( clmd->coll_parms->loop_count>rounds ) );
1528
1529         return MIN2 ( ret, 1 );
1530 }