makes bullet independant from gameengine for cmake, introduces esc-key during sim...
[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 #if WITH_BULLET == 1
684                         distance = plNearestPoints(
685                                         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);
686 #else
687                         // just be sure that we don't add anything
688                         distance = 2.0 * (epsilon + ALMOST_ZERO);
689 #endif  
690                         if (distance <= (epsilon + ALMOST_ZERO))
691                         {
692                                 // printf("dist: %f\n", (float)distance);
693                                 
694                                 // collpair->face1 = tree1->tri_index;
695                                 // collpair->face2 = tree2->tri_index;
696                                 
697                                 VECCOPY(collpair->normal, collpair->vector);
698                                 Normalize(collpair->normal);
699                                 
700                                 collpair->distance = distance;
701                                 BLI_linklist_prepend(&clmd->coll_parms->collision_list, collpair);
702                                 
703                         }
704                         else
705                         {
706                                 MEM_freeN(collpair);
707                         }
708                 }
709                 else
710                 {
711                         MEM_freeN(collpair);
712                 }
713         }
714 }
715
716 int cloth_are_edges_adjacent(ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair)
717 {
718         Cloth *cloth1 = NULL, *cloth2 = NULL;
719         ClothVertex *verts1 = NULL, *verts2 = NULL;
720         float temp[3];
721          
722         cloth1 = clmd->clothObject;
723         cloth2 = coll_clmd->clothObject;
724         
725         verts1 = cloth1->verts;
726         verts2 = cloth2->verts;
727         
728         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold);
729         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
730                 return 1;
731         
732         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].xold);
733         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
734                 return 1;
735         
736         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold);
737         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
738                 return 1;
739         
740         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold);
741         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
742                 return 1;
743                 
744         return 0;
745 }
746
747 void cloth_collision_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
748 {
749         EdgeCollPair edgecollpair;
750         Cloth *cloth1=NULL, *cloth2=NULL;
751         MFace *face1=NULL, *face2=NULL;
752         ClothVertex *verts1=NULL, *verts2=NULL;
753         double distance = 0;
754         float epsilon = clmd->coll_parms->epsilon;
755         unsigned int i = 0, j = 0, k = 0;
756         int numsolutions = 0;
757         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
758         
759         cloth1 = clmd->clothObject;
760         cloth2 = coll_clmd->clothObject;
761         
762         verts1 = cloth1->verts;
763         verts2 = cloth2->verts;
764
765         face1 = &(cloth1->mfaces[tree1->tri_index]);
766         face2 = &(cloth2->mfaces[tree2->tri_index]);
767         
768         for( i = 0; i < 5; i++)
769         {
770                 if(i == 0) 
771                 {
772                         edgecollpair.p11 = face1->v1;
773                         edgecollpair.p12 = face1->v2;
774                 }
775                 else if(i == 1) 
776                 {
777                         edgecollpair.p11 = face1->v2;
778                         edgecollpair.p12 = face1->v3;
779                 }
780                 else if(i == 2) 
781                 {
782                         if(face1->v4) 
783                         {
784                                 edgecollpair.p11 = face1->v3;
785                                 edgecollpair.p12 = face1->v4;
786                         }
787                         else 
788                         {
789                                 edgecollpair.p11 = face1->v3;
790                                 edgecollpair.p12 = face1->v1;
791                                 i+=5; // get out of here after this edge pair is handled
792                         }
793                 }
794                 else if(i == 3) 
795                 {
796                         if(face1->v4) 
797                         {
798                                 edgecollpair.p11 = face1->v4;
799                                 edgecollpair.p12 = face1->v1;
800                         }       
801                         else
802                                 continue;
803                 }
804                 else
805                 {
806                         edgecollpair.p11 = face1->v3;
807                         edgecollpair.p12 = face1->v1;
808                 }
809
810                 
811                 for( j = 0; j < 5; j++)
812                 {
813                         if(j == 0)
814                         {
815                                 edgecollpair.p21 = face2->v1;
816                                 edgecollpair.p22 = face2->v2;
817                         }
818                         else if(j == 1)
819                         {
820                                 edgecollpair.p21 = face2->v2;
821                                 edgecollpair.p22 = face2->v3;
822                         }
823                         else if(j == 2)
824                         {
825                                 if(face2->v4) 
826                                 {
827                                         edgecollpair.p21 = face2->v3;
828                                         edgecollpair.p22 = face2->v4;
829                                 }
830                                 else 
831                                 {
832                                         edgecollpair.p21 = face2->v3;
833                                         edgecollpair.p22 = face2->v1;
834                                 }
835                         }
836                         else if(j == 3)
837                         {
838                                 if(face2->v4) 
839                                 {
840                                         edgecollpair.p21 = face2->v4;
841                                         edgecollpair.p22 = face2->v1;
842                                 }
843                                 else
844                                         continue;
845                         }
846                         else
847                         {
848                                 edgecollpair.p21 = face2->v3;
849                                 edgecollpair.p22 = face2->v1;
850                         }
851                         
852                         
853                         if(!cloth_are_edges_adjacent(clmd, coll_clmd, &edgecollpair))
854                         {
855                                 VECSUB(a, verts1[edgecollpair.p12].xold, verts1[edgecollpair.p11].xold);
856                                 VECSUB(b, verts1[edgecollpair.p12].v, verts1[edgecollpair.p11].v);
857                                 VECSUB(c, verts1[edgecollpair.p21].xold, verts1[edgecollpair.p11].xold);
858                                 VECSUB(d, verts1[edgecollpair.p21].v, verts1[edgecollpair.p11].v);
859                                 VECSUB(e, verts2[edgecollpair.p22].xold, verts1[edgecollpair.p11].xold);
860                                 VECSUB(f, verts2[edgecollpair.p22].v, verts1[edgecollpair.p11].v);
861                                 
862                                 numsolutions = cloth_get_collision_time(a, b, c, d, e, f, solution);
863                                 
864                                 for (k = 0; k < numsolutions; k++) 
865                                 {                                                               
866                                         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
867                                         {
868                                                 float out_collisionTime = solution[k];
869                                                 
870                                                 // TODO: check for collisions 
871                                                 
872                                                 // TODO: put into (edge) collision list
873                                                 
874                                                 // printf("Moving edge found!\n");
875                                         }
876                                 }
877                         }
878                 }
879         }               
880 }
881
882 void cloth_collision_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
883 {
884         CollPair collpair;
885         Cloth *cloth1=NULL, *cloth2=NULL;
886         MFace *face1=NULL, *face2=NULL;
887         ClothVertex *verts1=NULL, *verts2=NULL;
888         double distance = 0;
889         float epsilon = clmd->coll_parms->epsilon;
890         unsigned int i = 0, j = 0, k = 0;
891         int numsolutions = 0;
892         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
893
894         for(i = 0; i < 2; i++)
895         {               
896                 cloth1 = clmd->clothObject;
897                 cloth2 = coll_clmd->clothObject;
898                 
899                 verts1 = cloth1->verts;
900                 verts2 = cloth2->verts;
901         
902                 face1 = &(cloth1->mfaces[tree1->tri_index]);
903                 face2 = &(cloth2->mfaces[tree2->tri_index]);
904                 
905                 // check all possible pairs of triangles
906                 if(i == 0)
907                 {
908                         collpair.ap1 = face1->v1;
909                         collpair.ap2 = face1->v2;
910                         collpair.ap3 = face1->v3;
911                         
912                         collpair.pointsb[0] = face2->v1;
913                         collpair.pointsb[1] = face2->v2;
914                         collpair.pointsb[2] = face2->v3;
915                         collpair.pointsb[3] = face2->v4;
916                 }
917                 
918                 if(i == 1)
919                 {
920                         if(face1->v4)
921                         {
922                                 collpair.ap1 = face1->v3;
923                                 collpair.ap2 = face1->v4;
924                                 collpair.ap3 = face1->v1;
925                                 
926                                 collpair.pointsb[0] = face2->v1;
927                                 collpair.pointsb[1] = face2->v2;
928                                 collpair.pointsb[2] = face2->v3;
929                                 collpair.pointsb[3] = face2->v4;
930                         }
931                         else
932                                 i++;
933                 }
934                 
935                 // calc SIPcode (?)
936                 
937                 if(i < 2)
938                 {
939                         VECSUB(a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold);
940                         VECSUB(b, verts1[collpair.ap2].v, verts1[collpair.ap1].v);
941                         VECSUB(c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold);
942                         VECSUB(d, verts1[collpair.ap3].v, verts1[collpair.ap1].v);
943                                 
944                         for(j = 0; j < 4; j++)
945                         {                                       
946                                 if((j==3) && !(face2->v4))
947                                         break;
948                                 
949                                 VECSUB(e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold);
950                                 VECSUB(f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v);
951                                 
952                                 numsolutions = cloth_get_collision_time(a, b, c, d, e, f, solution);
953                                 
954                                 for (k = 0; k < numsolutions; k++) 
955                                 {                                                               
956                                         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
957                                         {
958                                                 float out_collisionTime = solution[k];
959                                                 
960                                                 // TODO: check for collisions 
961                                                 
962                                                 // TODO: put into (point-face) collision list
963                                                 
964                                                 // printf("Moving found!\n");
965                                                 
966                                         }
967                                 }
968                                 
969                                 // TODO: check borders for collisions
970                         }
971                         
972                 }
973         }
974 }
975
976 void cloth_collision_moving(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
977 {
978         // TODO: check for adjacent
979         cloth_collision_moving_edges(clmd, coll_clmd, tree1, tree2);
980         
981         cloth_collision_moving_tris(clmd, coll_clmd, tree1, tree2);
982         cloth_collision_moving_tris(coll_clmd, clmd, tree2, tree1);
983 }
984
985 // cloth - object collisions
986 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, float dt)
987 {
988         Base *base=NULL;
989         CollisionModifierData *collmd=NULL;
990         Cloth *cloth=NULL;
991         Object *coll_ob=NULL;
992         BVH *cloth_bvh=NULL;
993         unsigned int i=0, j = 0, numfaces = 0, numverts = 0;
994         unsigned int result = 0, ic = 0, rounds = 0; // result counts applied collisions; ic is for debug output; 
995         ClothVertex *verts = NULL;
996         float tnull[3] = {0,0,0};
997         int ret = 0;
998         ClothModifierData *tclmd;
999
1000         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || !(((Cloth *)clmd->clothObject)->tree))
1001         {
1002                 return 0;
1003         }
1004         
1005         cloth = clmd->clothObject;
1006         verts = cloth->verts;
1007         cloth_bvh = (BVH *) cloth->tree;
1008         numfaces = clmd->clothObject->numfaces;
1009         numverts = clmd->clothObject->numverts;
1010         
1011         ////////////////////////////////////////////////////////////
1012         // static collisions
1013         ////////////////////////////////////////////////////////////
1014
1015         // update cloth bvh
1016         bvh_update_from_cloth(clmd, 0); // 0 means STATIC, 1 means MOVING (see later in this function)
1017         
1018         do
1019         {
1020                 result = 0;
1021                 ic = 0;
1022                 clmd->coll_parms->collision_list = NULL; 
1023                 
1024                 // check all collision objects
1025                 for (base = G.scene->base.first; base; base = base->next)
1026                 {
1027                         coll_ob = base->object;
1028                         collmd = (CollisionModifierData *) modifiers_findByType (coll_ob, eModifierType_Collision);
1029                         
1030                         if (!collmd)
1031                                 continue;
1032                         
1033                         tclmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1034                         if(tclmd == clmd)
1035                                 continue;
1036                         
1037                         if (collmd->tree) 
1038                         {
1039                                 BVH *coll_bvh = collmd->tree;
1040                                 
1041                                 collision_move_object(collmd, step + dt, step);
1042                                         
1043                                 bvh_traverse(clmd, collmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_static);
1044                         }
1045                         else
1046                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
1047                 
1048                 
1049                         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
1050                         result = 1;
1051                         for(j = 0; j < 10; j++) // 10 is just a value that ensures convergence
1052                         {
1053                                 result = 0;
1054                                 
1055                                 if (collmd->tree) 
1056                                         result += cloth_collision_response_static(clmd, collmd);
1057                         
1058                                 
1059                                 // apply impulses in parallel
1060                                 ic=0;
1061                                 for(i = 0; i < numverts; i++)
1062                                 {
1063                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
1064                                         if(verts[i].impulse_count)
1065                                         {
1066                                                 VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
1067                                                 VECCOPY(verts[i].impulse, tnull);
1068                                                 verts[i].impulse_count = 0;
1069                                         
1070                                                 ic++;
1071                                                 ret++;
1072                                         }
1073                                 }
1074                         }
1075                         
1076                         // free collision list
1077                         if(clmd->coll_parms->collision_list)
1078                         {
1079                                 LinkNode *search = clmd->coll_parms->collision_list;
1080                                 while(search)
1081                                 {
1082                                         CollPair *coll_pair = search->link;
1083                                                         
1084                                         MEM_freeN(coll_pair);
1085                                         search = search->next;
1086                                 }
1087                                 BLI_linklist_free(clmd->coll_parms->collision_list,NULL);
1088                         
1089                                 clmd->coll_parms->collision_list = NULL;
1090                         }
1091                 }
1092                 rounds++;
1093         }
1094         while(result && (clmd->coll_parms->loop_count>rounds));
1095         
1096         ////////////////////////////////////////////////////////////
1097         // update positions
1098         // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1099         ////////////////////////////////////////////////////////////
1100         
1101         // verts come from clmd
1102         for(i = 0; i < numverts; i++)
1103         {
1104                 if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
1105                 {                       
1106                         if(verts [i].goal >= SOFTGOALSNAP)
1107                         {
1108                                 continue;
1109                         }
1110                 }
1111                 
1112                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1113         }
1114         ////////////////////////////////////////////////////////////
1115
1116         ////////////////////////////////////////////////////////////
1117         // moving collisions
1118         //
1119         // response code is just missing itm 
1120         ////////////////////////////////////////////////////////////
1121
1122         /*
1123         // update cloth bvh
1124         bvh_update_from_cloth(clmd, 1);  // 0 means STATIC, 1 means MOVING 
1125         
1126         // update moving bvh for collision object once
1127         for (base = G.scene->base.first; base; base = base->next)
1128         {
1129                 
1130         coll_ob = base->object;
1131         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1132         if (!coll_clmd)
1133         continue;
1134                 
1135         if(!coll_clmd->clothObject)
1136         continue;
1137                 
1138                                 // if collision object go on
1139         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
1140         {
1141         BVH *coll_bvh = coll_clmd->clothObject->tree;
1142                         
1143         bvh_update_from_cloth(coll_clmd, 1);  // 0 means STATIC, 1 means MOVING         
1144 }
1145 }
1146         
1147         
1148         do
1149         {
1150         result = 0;
1151         ic = 0;
1152         clmd->coll_parms->collision_list = NULL; 
1153                 
1154                 // check all collision objects
1155         for (base = G.scene->base.first; base; base = base->next)
1156         {
1157         coll_ob = base->object;
1158         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1159                         
1160         if (!coll_clmd)
1161         continue;
1162                         
1163                         // if collision object go on
1164         if (coll_clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
1165         {
1166         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
1167         {
1168         BVH *coll_bvh = coll_clmd->clothObject->tree;
1169                                         
1170         bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_moving);
1171 }
1172         else
1173         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
1174 }
1175 }
1176                 
1177                 // process all collisions (calculate impulses, TODO: also repulses if distance too short)
1178         result = 1;
1179         for(j = 0; j < 10; j++) // 10 is just a value that ensures convergence
1180         {
1181         result = 0;
1182                                 
1183                         // handle all collision objects
1184         for (base = G.scene->base.first; base; base = base->next)
1185         {
1186                         
1187         coll_ob = base->object;
1188         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1189                                                 
1190         if (!coll_clmd)
1191         continue;
1192                                 
1193                                 // if collision object go on
1194         if (coll_clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
1195         {
1196         if (coll_clmd->clothObject) 
1197         result += cloth_collision_response_moving_tris(clmd, coll_clmd);
1198         else
1199         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
1200 }
1201 }
1202                                                 
1203                         // apply impulses in parallel
1204         ic=0;
1205         for(i = 0; i < numverts; i++)
1206         {
1207                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
1208         if(verts[i].impulse_count)
1209         {
1210         VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
1211         VECCOPY(verts[i].impulse, tnull);
1212         verts[i].impulse_count = 0;
1213                                                         
1214         ic++;
1215         ret++;
1216 }
1217 }
1218 }
1219                 
1220                 
1221                 // verts come from clmd
1222         for(i = 0; i < numverts; i++)
1223         {
1224         VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1225 }
1226                 
1227                 // update cloth bvh
1228         bvh_update_from_cloth(clmd, 1);  // 0 means STATIC, 1 means MOVING 
1229                 
1230                 
1231                 // free collision list
1232         if(clmd->coll_parms->collision_list)
1233         {
1234         LinkNode *search = clmd->coll_parms->collision_list;
1235         while(search)
1236         {
1237         CollPair *coll_pair = search->link;
1238                                                         
1239         MEM_freeN(coll_pair);
1240         search = search->next;
1241 }
1242         BLI_linklist_free(clmd->coll_parms->collision_list,NULL);
1243                         
1244         clmd->coll_parms->collision_list = NULL;
1245 }
1246                 
1247                 // printf("ic: %d\n", ic);
1248         rounds++;
1249 }
1250         while(result && (CLOTH_MAX_THRESHOLD>rounds));
1251         
1252         ////////////////////////////////////////////////////////////
1253         // update positions + velocities
1254         ////////////////////////////////////////////////////////////
1255         
1256         // verts come from clmd
1257         for(i = 0; i < numverts; i++)
1258         {
1259         VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1260 }
1261         ////////////////////////////////////////////////////////////
1262         */
1263         
1264         return MIN2(ret, 1);
1265 }