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