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