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