svn merge -r 13452:14721 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[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_cloth.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 /***********************************
53 Collision modifier code start
54 ***********************************/
55
56 /* step is limited from 0 (frame start position) to 1 (frame end position) */
57 void collision_move_object ( CollisionModifierData *collmd, float step, float prevstep )
58 {
59         float tv[3] = {0,0,0};
60         unsigned int i = 0;
61
62         for ( i = 0; i < collmd->numverts; i++ )
63         {
64                 VECSUB ( tv, collmd->xnew[i].co, collmd->x[i].co );
65                 VECADDS ( collmd->current_x[i].co, collmd->x[i].co, tv, prevstep );
66                 VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step );
67                 VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co );
68         }
69         bvh_update_from_mvert ( collmd->bvh, collmd->current_x, collmd->numverts, collmd->current_xnew, 1 );
70 }
71
72 /* build bounding volume hierarchy from mverts (see kdop.c for whole BVH code) */
73 BVH *bvh_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int numverts, float epsilon )
74 {
75         BVH *bvh=NULL;
76
77         bvh = MEM_callocN ( sizeof ( BVH ), "BVH" );
78         if ( bvh == NULL )
79         {
80                 printf ( "bvh: Out of memory.\n" );
81                 return NULL;
82         }
83
84         // in the moment, return zero if no faces there
85         if ( !numfaces )
86                 return NULL;
87
88         bvh->epsilon = epsilon;
89         bvh->numfaces = numfaces;
90         bvh->mfaces = mfaces;
91
92         // we have no faces, we save seperate points
93         if ( !mfaces )
94         {
95                 bvh->numfaces = numverts;
96         }
97
98         bvh->numverts = numverts;
99         bvh->current_x = MEM_dupallocN ( x );
100
101         bvh_build ( bvh );
102
103         return bvh;
104 }
105
106 void bvh_update_from_mvert ( BVH * bvh, MVert *x, unsigned int numverts, MVert *xnew, int moving )
107 {
108         if ( !bvh )
109                 return;
110
111         if ( numverts!=bvh->numverts )
112                 return;
113
114         if ( x )
115                 memcpy ( bvh->current_xold, x, sizeof ( MVert ) * numverts );
116
117         if ( xnew )
118                 memcpy ( bvh->current_x, xnew, sizeof ( MVert ) * numverts );
119
120         bvh_update ( bvh, moving );
121 }
122
123 /***********************************
124 Collision modifier code end
125 ***********************************/
126
127 /**
128  * gsl_poly_solve_cubic -
129  *
130  * copied from SOLVE_CUBIC.C --> GSL
131  */
132
133 /* DG: debug hint! don't forget that all functions were "fabs", "sinf", etc before */
134 #define mySWAP(a,b) { float tmp = b ; b = a ; a = tmp ; }
135
136 int gsl_poly_solve_cubic ( float a, float b, float c, float *x0, float *x1, float *x2 )
137 {
138         float q = ( a * a - 3 * b );
139         float r = ( 2 * a * a * a - 9 * a * b + 27 * c );
140
141         float Q = q / 9;
142         float R = r / 54;
143
144         float Q3 = Q * Q * Q;
145         float R2 = R * R;
146
147         float CR2 = 729 * r * r;
148         float CQ3 = 2916 * q * q * q;
149
150         if ( R == 0 && Q == 0 )
151         {
152                 *x0 = - a / 3 ;
153                 *x1 = - a / 3 ;
154                 *x2 = - a / 3 ;
155                 return 3 ;
156         }
157         else if ( CR2 == CQ3 )
158         {
159                 /* this test is actually R2 == Q3, written in a form suitable
160                   for exact computation with integers */
161
162                 /* Due to finite precision some float roots may be missed, and
163                   considered to be a pair of complex roots z = x +/- epsilon i
164                   close to the real axis. */
165
166                 float sqrtQ = sqrt ( Q );
167
168                 if ( R > 0 )
169                 {
170                         *x0 = -2 * sqrtQ  - a / 3;
171                         *x1 = sqrtQ - a / 3;
172                         *x2 = sqrtQ - a / 3;
173                 }
174                 else
175                 {
176                         *x0 = - sqrtQ  - a / 3;
177                         *x1 = - sqrtQ - a / 3;
178                         *x2 = 2 * sqrtQ - a / 3;
179                 }
180                 return 3 ;
181         }
182         else if ( CR2 < CQ3 ) /* equivalent to R2 < Q3 */
183         {
184                 float sqrtQ = sqrt ( Q );
185                 float sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
186                 float theta = acos ( R / sqrtQ3 );
187                 float norm = -2 * sqrtQ;
188                 *x0 = norm * cos ( theta / 3 ) - a / 3;
189                 *x1 = norm * cos ( ( theta + 2.0 * M_PI ) / 3 ) - a / 3;
190                 *x2 = norm * cos ( ( theta - 2.0 * M_PI ) / 3 ) - a / 3;
191
192                 /* Sort *x0, *x1, *x2 into increasing order */
193
194                 if ( *x0 > *x1 )
195                         mySWAP ( *x0, *x1 ) ;
196
197                 if ( *x1 > *x2 )
198                 {
199                         mySWAP ( *x1, *x2 ) ;
200
201                         if ( *x0 > *x1 )
202                                 mySWAP ( *x0, *x1 ) ;
203                 }
204
205                 return 3;
206         }
207         else
208         {
209                 float sgnR = ( R >= 0 ? 1 : -1 );
210                 float A = -sgnR * pow ( ABS ( R ) + sqrt ( R2 - Q3 ), 1.0/3.0 );
211                 float B = Q / A ;
212                 *x0 = A + B - a / 3;
213                 return 1;
214         }
215 }
216
217
218 /**
219  * gsl_poly_solve_quadratic
220  *
221  * copied from GSL
222  */
223 int gsl_poly_solve_quadratic ( float a, float b, float c,  float *x0, float *x1 )
224 {
225         float disc = b * b - 4 * a * c;
226
227         if ( disc > 0 )
228         {
229                 if ( b == 0 )
230                 {
231                         float r = ABS ( 0.5 * sqrt ( disc ) / a );
232                         *x0 = -r;
233                         *x1 =  r;
234                 }
235                 else
236                 {
237                         float sgnb = ( b > 0 ? 1 : -1 );
238                         float temp = -0.5 * ( b + sgnb * sqrt ( disc ) );
239                         float r1 = temp / a ;
240                         float r2 = c / temp ;
241
242                         if ( r1 < r2 )
243                         {
244                                 *x0 = r1 ;
245                                 *x1 = r2 ;
246                         }
247                         else
248                         {
249                                 *x0 = r2 ;
250                                 *x1 = r1 ;
251                         }
252                 }
253                 return 2;
254         }
255         else if ( disc == 0 )
256         {
257                 *x0 = -0.5 * b / a ;
258                 *x1 = -0.5 * b / a ;
259                 return 2 ;
260         }
261         else
262         {
263                 return 0;
264         }
265 }
266
267
268
269 /*
270  * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
271  *     page 4, left column
272  */
273
274 int cloth_get_collision_time ( float a[3], float b[3], float c[3], float d[3], float e[3], float f[3], float solution[3] )
275 {
276         int num_sols = 0;
277
278         float g = -a[2] * c[1] * e[0] + a[1] * c[2] * e[0] +
279                   a[2] * c[0] * e[1] - a[0] * c[2] * e[1] -
280                   a[1] * c[0] * e[2] + a[0] * c[1] * e[2];
281
282         float h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
283                   a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
284                   a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
285                   b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
286                   a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
287                   a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
288
289         float i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
290                   b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
291                   b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
292                   b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
293                   a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
294                   b[2] * c[0] * f[1] - b[0] * c[2] * f[1] +
295                   a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
296                   b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
297                   a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
298
299         float j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
300                   b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
301                   b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
302
303         // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
304         if ( ABS ( j ) > ALMOST_ZERO )
305         {
306                 i /= j;
307                 h /= j;
308                 g /= j;
309
310                 num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] );
311         }
312         else if ( ABS ( i ) > ALMOST_ZERO )
313         {
314                 num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] );
315                 solution[2] = -1.0;
316         }
317         else if ( ABS ( h ) > ALMOST_ZERO )
318         {
319                 solution[0] = -g / h;
320                 solution[1] = solution[2] = -1.0;
321                 num_sols = 1;
322         }
323         else if ( ABS ( g ) > ALMOST_ZERO )
324         {
325                 solution[0] = 0;
326                 solution[1] = solution[2] = -1.0;
327                 num_sols = 1;
328         }
329
330         // Discard negative solutions
331         if ( ( num_sols >= 1 ) && ( solution[0] < 0 ) )
332         {
333                 --num_sols;
334                 solution[0] = solution[num_sols];
335         }
336         if ( ( num_sols >= 2 ) && ( solution[1] < 0 ) )
337         {
338                 --num_sols;
339                 solution[1] = solution[num_sols];
340         }
341         if ( ( num_sols == 3 ) && ( solution[2] < 0 ) )
342         {
343                 --num_sols;
344         }
345
346         // Sort
347         if ( num_sols == 2 )
348         {
349                 if ( solution[0] > solution[1] )
350                 {
351                         double tmp = solution[0];
352                         solution[0] = solution[1];
353                         solution[1] = tmp;
354                 }
355         }
356         else if ( num_sols == 3 )
357         {
358
359                 // Bubblesort
360                 if ( solution[0] > solution[1] )
361                 {
362                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
363                 }
364                 if ( solution[1] > solution[2] )
365                 {
366                         double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
367                 }
368                 if ( solution[0] > solution[1] )
369                 {
370                         double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
371                 }
372         }
373
374         return num_sols;
375 }
376
377 // w3 is not perfect
378 void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
379 {
380         double  tempV1[3], tempV2[3], tempV4[3];
381         double  a,b,c,d,e,f;
382
383         VECSUB ( tempV1, p1, p3 );
384         VECSUB ( tempV2, p2, p3 );
385         VECSUB ( tempV4, pv, p3 );
386
387         a = INPR ( tempV1, tempV1 );
388         b = INPR ( tempV1, tempV2 );
389         c = INPR ( tempV2, tempV2 );
390         e = INPR ( tempV1, tempV4 );
391         f = INPR ( tempV2, tempV4 );
392
393         d = ( a * c - b * b );
394
395         if ( ABS ( d ) < ALMOST_ZERO )
396         {
397                 *w1 = *w2 = *w3 = 1.0 / 3.0;
398                 return;
399         }
400
401         w1[0] = ( float ) ( ( e * c - b * f ) / d );
402
403         if ( w1[0] < 0 )
404                 w1[0] = 0;
405
406         w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
407
408         if ( w2[0] < 0 )
409                 w2[0] = 0;
410
411         w3[0] = 1.0f - w1[0] - w2[0];
412 }
413
414 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
415 {
416         to[0] = to[1] = to[2] = 0;
417         VECADDMUL ( to, v1, w1 );
418         VECADDMUL ( to, v2, w2 );
419         VECADDMUL ( to, v3, w3 );
420 }
421
422 int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd )
423 {
424         int result = 0;
425         LinkNode *search = NULL;
426         CollPair *collpair = NULL;
427         Cloth *cloth1;
428         float w1, w2, w3, u1, u2, u3;
429         float v1[3], v2[3], relativeVelocity[3];
430         float magrelVel;
431         float epsilon2 = collmd->bvh->epsilon;
432
433         cloth1 = clmd->clothObject;
434
435         search = clmd->coll_parms->collision_list;
436
437         while ( search )
438         {
439                 collpair = search->link;
440
441                 // compute barycentric coordinates for both collision points
442                 collision_compute_barycentric ( collpair->pa,
443                                                 cloth1->verts[collpair->ap1].txold,
444                                                 cloth1->verts[collpair->ap2].txold,
445                                                 cloth1->verts[collpair->ap3].txold,
446                                                 &w1, &w2, &w3 );
447
448                 // was: txold
449                 collision_compute_barycentric ( collpair->pb,
450                                                 collmd->current_x[collpair->bp1].co,
451                                                 collmd->current_x[collpair->bp2].co,
452                                                 collmd->current_x[collpair->bp3].co,
453                                                 &u1, &u2, &u3 );
454
455                 // Calculate relative "velocity".
456                 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
457
458                 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
459
460                 VECSUB ( relativeVelocity, v2, v1 );
461
462                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
463                 magrelVel = INPR ( relativeVelocity, collpair->normal );
464
465                 // printf("magrelVel: %f\n", magrelVel);
466
467                 // Calculate masses of points.
468                 // TODO
469
470                 // If v_n_mag < 0 the edges are approaching each other.
471                 if ( magrelVel > ALMOST_ZERO )
472                 {
473                         // Calculate Impulse magnitude to stop all motion in normal direction.
474                         float magtangent = 0, repulse = 0, d = 0;
475                         double impulse = 0.0;
476                         float vrel_t_pre[3];
477                         float temp[3];
478
479                         // calculate tangential velocity
480                         VECCOPY ( temp, collpair->normal );
481                         VecMulf ( temp, magrelVel );
482                         VECSUB ( vrel_t_pre, relativeVelocity, temp );
483
484                         // Decrease in magnitude of relative tangential velocity due to coulomb friction
485                         // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
486                         magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
487
488                         // Apply friction impulse.
489                         if ( magtangent > ALMOST_ZERO )
490                         {
491                                 Normalize ( vrel_t_pre );
492
493                                 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
494                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
495                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
496                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
497                         }
498
499                         // Apply velocity stopping impulse
500                         // I_c = m * v_N / 2.0
501                         // no 2.0 * magrelVel normally, but looks nicer DG
502                         impulse =  magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
503
504                         VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
505                         cloth1->verts[collpair->ap1].impulse_count++;
506
507                         VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
508                         cloth1->verts[collpair->ap2].impulse_count++;
509
510                         VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
511                         cloth1->verts[collpair->ap3].impulse_count++;
512
513                         // Apply repulse impulse if distance too short
514                         // I_r = -min(dt*kd, m(0,1d/dt - v_n))
515                         d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
516                         if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) )
517                         {
518                                 repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
519
520                                 // stay on the safe side and clamp repulse
521                                 if ( impulse > ALMOST_ZERO )
522                                         repulse = MIN2 ( repulse, 5.0*impulse );
523                                 repulse = MAX2 ( impulse, repulse );
524
525                                 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
526                                 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal,  impulse );
527                                 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal,  impulse );
528                                 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal,  impulse );
529                         }
530
531                         result = 1;
532                 }
533
534                 search = search->next;
535         }
536
537
538         return result;
539 }
540
541 int cloth_collision_response_moving_tris ( ClothModifierData *clmd, ClothModifierData *coll_clmd )
542 {
543         return 1;
544 }
545
546
547 int cloth_collision_response_moving_edges ( ClothModifierData *clmd, ClothModifierData *coll_clmd )
548 {
549         return 1;
550 }
551
552 void cloth_collision_static ( ModifierData *md1, ModifierData *md2, CollisionTree *tree1, CollisionTree *tree2 )
553 {
554         ClothModifierData *clmd = ( ClothModifierData * ) md1;
555         CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
556         CollPair *collpair = NULL;
557         Cloth *cloth1=NULL;
558         MFace *face1=NULL, *face2=NULL;
559         ClothVertex *verts1=NULL;
560         double distance = 0;
561         float epsilon = clmd->coll_parms->epsilon;
562         float epsilon2 = ( ( CollisionModifierData * ) md2 )->bvh->epsilon;
563         unsigned int i = 0;
564
565         for ( i = 0; i < 4; i++ )
566         {
567                 collpair = ( CollPair * ) MEM_callocN ( sizeof ( CollPair ), "cloth coll pair" );
568
569                 cloth1 = clmd->clothObject;
570
571                 verts1 = cloth1->verts;
572
573                 face1 = & ( cloth1->mfaces[tree1->tri_index] );
574                 face2 = & ( collmd->mfaces[tree2->tri_index] );
575
576                 // check all possible pairs of triangles
577                 if ( i == 0 )
578                 {
579                         collpair->ap1 = face1->v1;
580                         collpair->ap2 = face1->v2;
581                         collpair->ap3 = face1->v3;
582
583                         collpair->bp1 = face2->v1;
584                         collpair->bp2 = face2->v2;
585                         collpair->bp3 = face2->v3;
586
587                 }
588
589                 if ( i == 1 )
590                 {
591                         if ( face1->v4 )
592                         {
593                                 collpair->ap1 = face1->v3;
594                                 collpair->ap2 = face1->v4;
595                                 collpair->ap3 = face1->v1;
596
597                                 collpair->bp1 = face2->v1;
598                                 collpair->bp2 = face2->v2;
599                                 collpair->bp3 = face2->v3;
600                         }
601                         else
602                                 i++;
603                 }
604
605                 if ( i == 2 )
606                 {
607                         if ( face2->v4 )
608                         {
609                                 collpair->ap1 = face1->v1;
610                                 collpair->ap2 = face1->v2;
611                                 collpair->ap3 = face1->v3;
612
613                                 collpair->bp1 = face2->v3;
614                                 collpair->bp2 = face2->v4;
615                                 collpair->bp3 = face2->v1;
616                         }
617                         else
618                                 i+=2;
619                 }
620
621                 if ( i == 3 )
622                 {
623                         if ( ( face1->v4 ) && ( face2->v4 ) )
624                         {
625                                 collpair->ap1 = face1->v3;
626                                 collpair->ap2 = face1->v4;
627                                 collpair->ap3 = face1->v1;
628
629                                 collpair->bp1 = face2->v3;
630                                 collpair->bp2 = face2->v4;
631                                 collpair->bp3 = face2->v1;
632                         }
633                         else
634                                 i++;
635                 }
636
637                 // calc SIPcode (?)
638
639                 if ( i < 4 )
640                 {
641                         // calc distance + normal
642 #ifdef WITH_BULLET
643                         distance = plNearestPoints (
644                                        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 );
645 #else
646                         // just be sure that we don't add anything
647                         distance = 2.0 * ( epsilon + epsilon2 + ALMOST_ZERO );
648 #endif
649                         if ( distance <= ( epsilon + epsilon2 + ALMOST_ZERO ) )
650                         {
651                                 // printf("dist: %f\n", (float)distance);
652
653                                 // collpair->face1 = tree1->tri_index;
654                                 // collpair->face2 = tree2->tri_index;
655
656                                 VECCOPY ( collpair->normal, collpair->vector );
657                                 Normalize ( collpair->normal );
658
659                                 collpair->distance = distance;
660                                 BLI_linklist_prepend ( &clmd->coll_parms->collision_list, collpair );
661
662                         }
663                         else
664                         {
665                                 MEM_freeN ( collpair );
666                         }
667                 }
668                 else
669                 {
670                         MEM_freeN ( collpair );
671                 }
672         }
673 }
674
675 int cloth_are_edges_adjacent ( ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair )
676 {
677         Cloth *cloth1 = NULL, *cloth2 = NULL;
678         ClothVertex *verts1 = NULL, *verts2 = NULL;
679         float temp[3];
680
681         cloth1 = clmd->clothObject;
682         cloth2 = coll_clmd->clothObject;
683
684         verts1 = cloth1->verts;
685         verts2 = cloth2->verts;
686
687         VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold );
688         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
689                 return 1;
690
691         VECSUB ( temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].xold );
692         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
693                 return 1;
694
695         VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold );
696         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
697                 return 1;
698
699         VECSUB ( temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold );
700         if ( ABS ( INPR ( temp, temp ) ) < ALMOST_ZERO )
701                 return 1;
702
703         return 0;
704 }
705
706 void cloth_collision_moving_edges ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
707 {
708         EdgeCollPair edgecollpair;
709         Cloth *cloth1=NULL, *cloth2=NULL;
710         MFace *face1=NULL, *face2=NULL;
711         ClothVertex *verts1=NULL, *verts2=NULL;
712         unsigned int i = 0, j = 0, k = 0;
713         int numsolutions = 0;
714         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
715
716         cloth1 = clmd->clothObject;
717         cloth2 = coll_clmd->clothObject;
718
719         verts1 = cloth1->verts;
720         verts2 = cloth2->verts;
721
722         face1 = & ( cloth1->mfaces[tree1->tri_index] );
723         face2 = & ( cloth2->mfaces[tree2->tri_index] );
724
725         for ( i = 0; i < 5; i++ )
726         {
727                 if ( i == 0 )
728                 {
729                         edgecollpair.p11 = face1->v1;
730                         edgecollpair.p12 = face1->v2;
731                 }
732                 else if ( i == 1 )
733                 {
734                         edgecollpair.p11 = face1->v2;
735                         edgecollpair.p12 = face1->v3;
736                 }
737                 else if ( i == 2 )
738                 {
739                         if ( face1->v4 )
740                         {
741                                 edgecollpair.p11 = face1->v3;
742                                 edgecollpair.p12 = face1->v4;
743                         }
744                         else
745                         {
746                                 edgecollpair.p11 = face1->v3;
747                                 edgecollpair.p12 = face1->v1;
748                                 i+=5; // get out of here after this edge pair is handled
749                         }
750                 }
751                 else if ( i == 3 )
752                 {
753                         if ( face1->v4 )
754                         {
755                                 edgecollpair.p11 = face1->v4;
756                                 edgecollpair.p12 = face1->v1;
757                         }
758                         else
759                                 continue;
760                 }
761                 else
762                 {
763                         edgecollpair.p11 = face1->v3;
764                         edgecollpair.p12 = face1->v1;
765                 }
766
767
768                 for ( j = 0; j < 5; j++ )
769                 {
770                         if ( j == 0 )
771                         {
772                                 edgecollpair.p21 = face2->v1;
773                                 edgecollpair.p22 = face2->v2;
774                         }
775                         else if ( j == 1 )
776                         {
777                                 edgecollpair.p21 = face2->v2;
778                                 edgecollpair.p22 = face2->v3;
779                         }
780                         else if ( j == 2 )
781                         {
782                                 if ( face2->v4 )
783                                 {
784                                         edgecollpair.p21 = face2->v3;
785                                         edgecollpair.p22 = face2->v4;
786                                 }
787                                 else
788                                 {
789                                         edgecollpair.p21 = face2->v3;
790                                         edgecollpair.p22 = face2->v1;
791                                 }
792                         }
793                         else if ( j == 3 )
794                         {
795                                 if ( face2->v4 )
796                                 {
797                                         edgecollpair.p21 = face2->v4;
798                                         edgecollpair.p22 = face2->v1;
799                                 }
800                                 else
801                                         continue;
802                         }
803                         else
804                         {
805                                 edgecollpair.p21 = face2->v3;
806                                 edgecollpair.p22 = face2->v1;
807                         }
808
809
810                         if ( !cloth_are_edges_adjacent ( clmd, coll_clmd, &edgecollpair ) )
811                         {
812                                 VECSUB ( a, verts1[edgecollpair.p12].xold, verts1[edgecollpair.p11].xold );
813                                 VECSUB ( b, verts1[edgecollpair.p12].v, verts1[edgecollpair.p11].v );
814                                 VECSUB ( c, verts1[edgecollpair.p21].xold, verts1[edgecollpair.p11].xold );
815                                 VECSUB ( d, verts1[edgecollpair.p21].v, verts1[edgecollpair.p11].v );
816                                 VECSUB ( e, verts2[edgecollpair.p22].xold, verts1[edgecollpair.p11].xold );
817                                 VECSUB ( f, verts2[edgecollpair.p22].v, verts1[edgecollpair.p11].v );
818
819                                 numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution );
820
821                                 for ( k = 0; k < numsolutions; k++ )
822                                 {
823                                         if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) )
824                                         {
825                                                 //float out_collisionTime = solution[k];
826
827                                                 // TODO: check for collisions
828
829                                                 // TODO: put into (edge) collision list
830
831                                                 // printf("Moving edge found!\n");
832                                         }
833                                 }
834                         }
835                 }
836         }
837 }
838
839 void cloth_collision_moving_tris ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
840 {
841         CollPair collpair;
842         Cloth *cloth1=NULL, *cloth2=NULL;
843         MFace *face1=NULL, *face2=NULL;
844         ClothVertex *verts1=NULL, *verts2=NULL;
845         unsigned int i = 0, j = 0, k = 0;
846         int numsolutions = 0;
847         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
848
849         for ( i = 0; i < 2; i++ )
850         {
851                 cloth1 = clmd->clothObject;
852                 cloth2 = coll_clmd->clothObject;
853
854                 verts1 = cloth1->verts;
855                 verts2 = cloth2->verts;
856
857                 face1 = & ( cloth1->mfaces[tree1->tri_index] );
858                 face2 = & ( cloth2->mfaces[tree2->tri_index] );
859
860                 // check all possible pairs of triangles
861                 if ( i == 0 )
862                 {
863                         collpair.ap1 = face1->v1;
864                         collpair.ap2 = face1->v2;
865                         collpair.ap3 = face1->v3;
866
867                         collpair.pointsb[0] = face2->v1;
868                         collpair.pointsb[1] = face2->v2;
869                         collpair.pointsb[2] = face2->v3;
870                         collpair.pointsb[3] = face2->v4;
871                 }
872
873                 if ( i == 1 )
874                 {
875                         if ( face1->v4 )
876                         {
877                                 collpair.ap1 = face1->v3;
878                                 collpair.ap2 = face1->v4;
879                                 collpair.ap3 = face1->v1;
880
881                                 collpair.pointsb[0] = face2->v1;
882                                 collpair.pointsb[1] = face2->v2;
883                                 collpair.pointsb[2] = face2->v3;
884                                 collpair.pointsb[3] = face2->v4;
885                         }
886                         else
887                                 i++;
888                 }
889
890                 // calc SIPcode (?)
891
892                 if ( i < 2 )
893                 {
894                         VECSUB ( a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold );
895                         VECSUB ( b, verts1[collpair.ap2].v, verts1[collpair.ap1].v );
896                         VECSUB ( c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold );
897                         VECSUB ( d, verts1[collpair.ap3].v, verts1[collpair.ap1].v );
898
899                         for ( j = 0; j < 4; j++ )
900                         {
901                                 if ( ( j==3 ) && ! ( face2->v4 ) )
902                                         break;
903
904                                 VECSUB ( e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold );
905                                 VECSUB ( f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v );
906
907                                 numsolutions = cloth_get_collision_time ( a, b, c, d, e, f, solution );
908
909                                 for ( k = 0; k < numsolutions; k++ )
910                                 {
911                                         if ( ( solution[k] >= 0.0 ) && ( solution[k] <= 1.0 ) )
912                                         {
913                                                 //float out_collisionTime = solution[k];
914
915                                                 // TODO: check for collisions
916
917                                                 // TODO: put into (point-face) collision list
918
919                                                 // printf("Moving found!\n");
920
921                                         }
922                                 }
923
924                                 // TODO: check borders for collisions
925                         }
926
927                 }
928         }
929 }
930
931 void cloth_collision_moving ( ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2 )
932 {
933         // TODO: check for adjacent
934         cloth_collision_moving_edges ( clmd, coll_clmd, tree1, tree2 );
935
936         cloth_collision_moving_tris ( clmd, coll_clmd, tree1, tree2 );
937         cloth_collision_moving_tris ( coll_clmd, clmd, tree2, tree1 );
938 }
939
940 void cloth_free_collision_list ( ClothModifierData *clmd )
941 {
942         // free collision list
943         if ( clmd->coll_parms->collision_list )
944         {
945                 LinkNode *search = clmd->coll_parms->collision_list;
946                 while ( search )
947                 {
948                         CollPair *coll_pair = search->link;
949
950                         MEM_freeN ( coll_pair );
951                         search = search->next;
952                 }
953                 BLI_linklist_free ( clmd->coll_parms->collision_list,NULL );
954
955                 clmd->coll_parms->collision_list = NULL;
956         }
957 }
958
959 int cloth_bvh_objcollisions_do ( ClothModifierData * clmd, CollisionModifierData *collmd, float step, float dt )
960 {
961         Cloth *cloth = clmd->clothObject;
962         BVH *cloth_bvh= ( BVH * ) cloth->tree;
963         long i=0, j = 0, numfaces = 0, numverts = 0;
964         ClothVertex *verts = NULL;
965         int ret = 0;
966         unsigned int result = 0;
967         float tnull[3] = {0,0,0};
968
969         numfaces = clmd->clothObject->numfaces;
970         numverts = clmd->clothObject->numverts;
971
972         verts = cloth->verts;
973
974         if ( collmd->bvh )
975         {
976                 /* get pointer to bounding volume hierarchy */
977                 BVH *coll_bvh = collmd->bvh;
978
979                 /* move object to position (step) in time */
980                 collision_move_object ( collmd, step + dt, step );
981
982                 /* search for overlapping collision pairs */
983                 bvh_traverse ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_static, 0 );
984         }
985         else
986         {
987                 if ( G.rt > 0 )
988                         printf ( "cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n" );
989         }
990
991         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
992         result = 1;
993         for ( j = 0; j < 5; j++ ) // 5 is just a value that ensures convergence
994         {
995                 result = 0;
996
997                 if ( collmd->bvh )
998                         result += cloth_collision_response_static ( clmd, collmd );
999
1000                 // apply impulses in parallel
1001                 if ( result )
1002                         for ( i = 0; i < numverts; i++ )
1003                         {
1004                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
1005                                 if ( verts[i].impulse_count )
1006                                 {
1007                                         VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1008                                         VECCOPY ( verts[i].impulse, tnull );
1009                                         verts[i].impulse_count = 0;
1010
1011                                         ret++;
1012                                 }
1013                         }
1014
1015                 if ( !result )
1016                         break;
1017         }
1018
1019         cloth_free_collision_list ( clmd );
1020
1021         return ret;
1022 }
1023
1024 // cloth - object collisions
1025 int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
1026 {
1027         Base *base=NULL;
1028         CollisionModifierData *collmd=NULL;
1029         Cloth *cloth=NULL;
1030         Object *coll_ob=NULL;
1031         BVH *cloth_bvh=NULL;
1032         long i=0, j = 0, numfaces = 0, numverts = 0;
1033         unsigned int result = 0, rounds = 0; // result counts applied collisions; ic is for debug output;
1034         ClothVertex *verts = NULL;
1035         int ret = 0;
1036         ClothModifierData *tclmd;
1037         int collisions = 0, count = 0;
1038
1039         if ( ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ ) || ! ( ( ( Cloth * ) clmd->clothObject )->tree ) )
1040         {
1041                 return 0;
1042         }
1043
1044         cloth = clmd->clothObject;
1045         verts = cloth->verts;
1046         cloth_bvh = ( BVH * ) cloth->tree;
1047         numfaces = clmd->clothObject->numfaces;
1048         numverts = clmd->clothObject->numverts;
1049
1050         ////////////////////////////////////////////////////////////
1051         // static collisions
1052         ////////////////////////////////////////////////////////////
1053
1054         // update cloth bvh
1055         bvh_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1056
1057         do
1058         {
1059                 result = 0;
1060                 clmd->coll_parms->collision_list = NULL;
1061
1062                 // check all collision objects
1063                 for ( base = G.scene->base.first; base; base = base->next )
1064                 {
1065                         coll_ob = base->object;
1066                         collmd = ( CollisionModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Collision );
1067
1068                         if ( !collmd )
1069                         {
1070                                 if ( coll_ob->dup_group )
1071                                 {
1072                                         GroupObject *go;
1073                                         Group *group = coll_ob->dup_group;
1074
1075                                         for ( go= group->gobject.first; go; go= go->next )
1076                                         {
1077                                                 coll_ob = go->ob;
1078
1079                                                 collmd = ( CollisionModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Collision );
1080
1081                                                 if ( !collmd )
1082                                                         continue;
1083
1084                                                 tclmd = ( ClothModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Cloth );
1085                                                 if ( tclmd == clmd )
1086                                                         continue;
1087
1088                                                 ret += cloth_bvh_objcollisions_do ( clmd, collmd, step, dt );
1089                                         }
1090                                 }
1091                         }
1092                         else
1093                         {
1094                                 tclmd = ( ClothModifierData * ) modifiers_findByType ( coll_ob, eModifierType_Cloth );
1095                                 if ( tclmd == clmd )
1096                                         continue;
1097
1098                                 ret += cloth_bvh_objcollisions_do ( clmd, collmd, step, dt );
1099                         }
1100                 }
1101                 rounds++;
1102
1103                 ////////////////////////////////////////////////////////////
1104                 // update positions
1105                 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1106                 ////////////////////////////////////////////////////////////
1107
1108                 // verts come from clmd
1109                 for ( i = 0; i < numverts; i++ )
1110                 {
1111                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1112                         {
1113                                 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1114                                 {
1115                                         continue;
1116                                 }
1117                         }
1118
1119                         VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
1120                 }
1121                 ////////////////////////////////////////////////////////////
1122
1123
1124                 ////////////////////////////////////////////////////////////
1125                 // Test on *simple* selfcollisions
1126                 ////////////////////////////////////////////////////////////
1127                 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
1128                 {
1129                         collisions = 1;
1130                         verts = cloth->verts; // needed for openMP
1131
1132                         for ( count = 0; count < clmd->coll_parms->self_loop_count; count++ )
1133                         {
1134                                 if ( collisions )
1135                                 {
1136                                         collisions = 0;
1137 #pragma omp parallel for private(i,j, collisions) shared(verts, ret)
1138                                         for ( i = 0; i < cloth->numverts; i++ )
1139                                         {
1140                                                 for ( j = i + 1; j < cloth->numverts; j++ )
1141                                                 {
1142                                                         float temp[3];
1143                                                         float length = 0;
1144                                                         float mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
1145
1146                                                         if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1147                                                         {
1148                                                                 if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1149                                                                         && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
1150                                                                 {
1151                                                                         continue;
1152                                                                 }
1153                                                         }
1154
1155                                                         VECSUB ( temp, verts[i].tx, verts[j].tx );
1156
1157                                                         if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
1158
1159                                                         // check for adjacent points (i must be smaller j)
1160                                                         if ( BLI_edgehash_haskey ( cloth->edgehash, i, j ) )
1161                                                         {
1162                                                                 continue;
1163                                                         }
1164
1165                                                         length = Normalize ( temp );
1166
1167                                                         if ( length < mindistance )
1168                                                         {
1169                                                                 float correction = mindistance - length;
1170
1171                                                                 if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1172                                                                 {
1173                                                                         VecMulf ( temp, -correction );
1174                                                                         VECADD ( verts[j].tx, verts[j].tx, temp );
1175                                                                 }
1176                                                                 else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
1177                                                                 {
1178                                                                         VecMulf ( temp, correction );
1179                                                                         VECADD ( verts[i].tx, verts[i].tx, temp );
1180                                                                 }
1181                                                                 else
1182                                                                 {
1183                                                                         VecMulf ( temp, -correction*0.5 );
1184                                                                         VECADD ( verts[j].tx, verts[j].tx, temp );
1185
1186                                                                         VECSUB ( verts[i].tx, verts[i].tx, temp );
1187                                                                 }
1188
1189                                                                 collisions = 1;
1190
1191                                                                 if ( !ret )
1192                                                                 {
1193 #pragma omp critical
1194                                                                         {
1195                                                                                 ret = 1;
1196                                                                         }
1197                                                                 }
1198                                                         }
1199                                                 }
1200                                         }
1201                                 }
1202                         }
1203                         ////////////////////////////////////////////////////////////
1204
1205                         ////////////////////////////////////////////////////////////
1206                         // SELFCOLLISIONS: update velocities
1207                         ////////////////////////////////////////////////////////////
1208                         if ( ret )
1209                         {
1210                                 for ( i = 0; i < cloth->numverts; i++ )
1211                                 {
1212                                         if ( ! ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) )
1213                                                 VECSUB ( verts[i].tv, verts[i].tx, verts[i].txold );
1214                                 }
1215                         }
1216                         ////////////////////////////////////////////////////////////
1217                 }
1218         }
1219         while ( result && ( clmd->coll_parms->loop_count>rounds ) );
1220
1221         return MIN2 ( ret, 1 );
1222 }