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