New: Collision detection for inter-timestep-collisions for triangle-point 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 void cloth_collision_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree *tree1, Tree *tree2)
631 {
632         CollPair collpair;
633         Cloth *cloth1=NULL, *cloth2=NULL;
634         MFace *face1=NULL, *face2=NULL;
635         ClothVertex *verts1=NULL, *verts2=NULL;
636         double distance = 0;
637         float epsilon = clmd->coll_parms.epsilon;
638         unsigned int i = 0, j = 0, k = 0;
639         int numsolutions = 0;
640         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
641
642         for(i = 0; i < 2; i++)
643         {               
644                 cloth1 = clmd->clothObject;
645                 cloth2 = coll_clmd->clothObject;
646                 
647                 verts1 = cloth1->verts;
648                 verts2 = cloth2->verts;
649         
650                 face1 = &(cloth1->mfaces[tree1->tri_index]);
651                 face2 = &(cloth2->mfaces[tree2->tri_index]);
652                 
653                 // check all possible pairs of triangles
654                 if(i == 0)
655                 {
656                         collpair.ap1 = face1->v1;
657                         collpair.ap2 = face1->v2;
658                         collpair.ap3 = face1->v3;
659                         
660                         collpair.pointsb[0] = face2->v1;
661                         collpair.pointsb[1] = face2->v2;
662                         collpair.pointsb[2] = face2->v3;
663                         collpair.pointsb[3] = face2->v4;
664                 }
665                 
666                 if(i == 1)
667                 {
668                         if(face1->v4)
669                         {
670                                 collpair.ap1 = face1->v3;
671                                 collpair.ap2 = face1->v4;
672                                 collpair.ap3 = face1->v1;
673                                 
674                                 collpair.pointsb[0] = face2->v1;
675                                 collpair.pointsb[1] = face2->v2;
676                                 collpair.pointsb[2] = face2->v3;
677                                 collpair.pointsb[3] = face2->v4;
678                         }
679                         else
680                                 i++;
681                 }
682                 
683                 // calc SIPcode (?)
684                 
685                 if(i < 2)
686                 {
687                         VECSUB(a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold);
688                         VECSUB(b, verts1[collpair.ap2].v, verts1[collpair.ap1].v);
689                         VECSUB(c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold);
690                         VECSUB(d, verts1[collpair.ap3].v, verts1[collpair.ap1].v);
691                                 
692                         for(j = 0; j < 4; j++)
693                                 {                                       
694                                         if((j==3) && !(face2->v4))
695                                                 break;
696                                         
697                                         VECSUB(e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold);
698                                         VECSUB(f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v);
699                                         
700                                         numsolutions = cloth_get_collision_time(a, b, c, d, e, f, solution);
701                                         
702                                         for (k = 0; k < numsolutions; k++) 
703                                         {                                                               
704                                                 if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
705                                                 {
706                                                         float out_collisionTime = solution[k];
707                                                         
708                                                         // TODO: check for collisions 
709                                                         
710                                                         // TODO: put into collision list
711                                                         
712                                                         printf("Moving found!\n");
713                                                 }
714                                         }
715                                         
716                                         // TODO: check borders for collisions
717                                 }
718                         
719                 }
720         }
721 }
722
723 // move collision objects forward in time and update static bounding boxes
724 void cloth_update_collision_objects(float step)
725 {
726         Base *base=NULL;
727         ClothModifierData *coll_clmd=NULL;
728         Object *coll_ob=NULL;
729         unsigned int i=0;
730         
731         // search all objects for collision object
732         for (base = G.scene->base.first; base; base = base->next)
733         {
734
735                 coll_ob = base->object;
736                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
737                 if (!coll_clmd)
738                         continue;
739
740                 // if collision object go on
741                 if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
742                 {
743                         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
744                         {
745                                 Cloth *coll_cloth = coll_clmd->clothObject;
746                                 BVH *coll_bvh = coll_clmd->clothObject->tree;
747                                 unsigned int coll_numverts = coll_cloth->numverts;
748
749                                 // update position of collision object
750                                 for(i = 0; i < coll_numverts; i++)
751                                 {
752                                         VECCOPY(coll_cloth->verts[i].txold, coll_cloth->verts[i].tx);
753
754                                         VECADDS(coll_cloth->verts[i].tx, coll_cloth->verts[i].xold, coll_cloth->verts[i].v, step);
755                                         
756                                         // no dt here because of float rounding errors
757                                         VECSUB(coll_cloth->verts[i].tv, coll_cloth->verts[i].tx, coll_cloth->verts[i].txold);
758                                 }
759                                 
760                                 // update BVH of collision object
761                                 bvh_update(coll_clmd, coll_bvh, 0); // 0 means STATIC, 1 means MOVING 
762                         }
763                         else
764                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
765                 }
766         }
767 }
768
769 // CLOTH_MAX_THRESHOLD defines how much collision rounds/loops should be taken
770 #define CLOTH_MAX_THRESHOLD 10
771
772 // cloth - object collisions
773 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, float dt)
774 {
775         Base *base=NULL;
776         ClothModifierData *coll_clmd=NULL;
777         Cloth *cloth=NULL;
778         Object *coll_ob=NULL;
779         BVH *cloth_bvh=NULL;
780         unsigned int i=0, j = 0, numfaces = 0, numverts = 0;
781         unsigned int result = 0, ic = 0, rounds = 0; // result counts applied collisions; ic is for debug output; 
782         ClothVertex *verts = NULL;
783         float tnull[3] = {0,0,0};
784         int ret = 0;
785
786         if ((clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ) || !(((Cloth *)clmd->clothObject)->tree))
787         {
788                 return 0;
789         }
790         cloth = clmd->clothObject;
791         verts = cloth->verts;
792         cloth_bvh = (BVH *) cloth->tree;
793         numfaces = clmd->clothObject->numfaces;
794         numverts = clmd->clothObject->numverts;
795         
796         ////////////////////////////////////////////////////////////
797         // static collisions
798         ////////////////////////////////////////////////////////////
799
800         // update cloth bvh
801         bvh_update(clmd, cloth_bvh, 0); // 0 means STATIC, 1 means MOVING (see later in this function)
802         
803         // update collision objects
804         cloth_update_collision_objects(step);
805         
806         do
807         {
808                 result = 0;
809                 ic = 0;
810                 clmd->coll_parms.collision_list = NULL; 
811                 
812                 // check all collision objects
813                 for (base = G.scene->base.first; base; base = base->next)
814                 {
815                         coll_ob = base->object;
816                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
817                         
818                         if (!coll_clmd)
819                                 continue;
820                         
821                         // if collision object go on
822                         if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
823                         {
824                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
825                                 {
826                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
827                                         
828                                         bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_static);
829                                 }
830                                 else
831                                         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
832                         }
833                 }
834                 
835                 // process all collisions (calculate impulses, TODO: also repulses if distance too short)
836                 result = 1;
837                 for(j = 0; j < 50; j++) // 50 is just a value that ensures convergence
838                 {
839                         result = 0;
840                         
841                         // handle all collision objects
842                         for (base = G.scene->base.first; base; base = base->next)
843                         {
844                 
845                                 coll_ob = base->object;
846                                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
847                                 if (!coll_clmd)
848                                         continue;
849                 
850                                 // if collision object go on
851                                 if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
852                                 {
853                                         if (coll_clmd->clothObject) 
854                                                 result += cloth_collision_response_static(clmd, coll_clmd);
855                                         else
856                                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
857                                 }
858                         }
859                         
860                         // apply impulses in parallel
861                         ic=0;
862                         for(i = 0; i < numverts; i++)
863                         {
864                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
865                                 if(verts[i].impulse_count)
866                                 {
867                                         VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
868                                         VECCOPY(verts[i].impulse, tnull);
869                                         verts[i].impulse_count = 0;
870                                 
871                                         ic++;
872                                         ret++;
873                                 }
874                         }
875                 }
876                 
877                 // free collision list
878                 if(clmd->coll_parms.collision_list)
879                 {
880                         LinkNode *search = clmd->coll_parms.collision_list;
881                         while(search)
882                         {
883                                 CollPair *coll_pair = search->link;
884                                                         
885                                 MEM_freeN(coll_pair);
886                                 search = search->next;
887                         }
888                         BLI_linklist_free(clmd->coll_parms.collision_list,NULL);
889                         
890                         clmd->coll_parms.collision_list = NULL;
891                 }
892                 
893                 printf("ic: %d\n", ic);
894                 rounds++;
895         }
896         while(result && (CLOTH_MAX_THRESHOLD>rounds));
897         
898         printf("\n");
899                         
900         ////////////////////////////////////////////////////////////
901         // update positions
902         // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
903         ////////////////////////////////////////////////////////////
904         
905         // verts come from clmd
906         for(i = 0; i < numverts; i++)
907         {
908                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
909         }
910         ////////////////////////////////////////////////////////////
911
912         ////////////////////////////////////////////////////////////
913         // moving collisions
914         ////////////////////////////////////////////////////////////
915
916         
917         // update cloth bvh
918         bvh_update(clmd, cloth_bvh, 1);  // 0 means STATIC, 1 means MOVING 
919         
920         // update moving bvh for collision object once
921         for (base = G.scene->base.first; base; base = base->next)
922         {
923                 
924                 coll_ob = base->object;
925                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
926                 if (!coll_clmd)
927                         continue;
928                 
929                 if(!coll_clmd->clothObject)
930                         continue;
931                 
932                                 // if collision object go on
933                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
934                 {
935                         BVH *coll_bvh = coll_clmd->clothObject->tree;
936                         
937                         bvh_update(coll_clmd, coll_bvh, 1);  // 0 means STATIC, 1 means MOVING  
938                 }
939         }
940         
941         
942         do
943         {
944                 result = 0;
945                 ic = 0;
946                 clmd->coll_parms.collision_list = NULL; 
947                 
948                 // check all collision objects
949                 for (base = G.scene->base.first; base; base = base->next)
950                 {
951                         coll_ob = base->object;
952                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
953                         
954                         if (!coll_clmd)
955                                 continue;
956                         
957                         // if collision object go on
958                         if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
959                         {
960                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
961                                 {
962                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
963                                         
964                                         bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_moving_tris);
965                                 }
966                                 else
967                                         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
968                         }
969                 }
970                 /*
971                 // process all collisions (calculate impulses, TODO: also repulses if distance too short)
972                 result = 1;
973                 for(j = 0; j < 50; j++) // 50 is just a value that ensures convergence
974                 {
975                 result = 0;
976                         
977                         // handle all collision objects
978                 for (base = G.scene->base.first; base; base = base->next)
979                 {
980                 
981                 coll_ob = base->object;
982                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
983                                 
984                 if (!coll_clmd)
985                 continue;
986                 
987                                 // if collision object go on
988                 if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
989                 {
990                 if (coll_clmd->clothObject) 
991                 result += cloth_collision_response_moving_tris(clmd, coll_clmd);
992                 else
993                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
994         }
995         }
996                         
997                         // apply impulses in parallel
998                 ic=0;
999                 for(i = 0; i < numverts; i++)
1000                 {
1001                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
1002                 if(verts[i].impulse_count)
1003                 {
1004                 VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
1005                 VECCOPY(verts[i].impulse, tnull);
1006                 verts[i].impulse_count = 0;
1007                                 
1008                 ic++;
1009                 ret++;
1010         }
1011         }
1012         }
1013                 */
1014                 
1015                 // verts come from clmd
1016                 for(i = 0; i < numverts; i++)
1017                 {
1018                         VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1019                 }
1020                 
1021                 // update cloth bvh
1022                 bvh_update(clmd, cloth_bvh, 1);  // 0 means STATIC, 1 means MOVING 
1023                 
1024                 
1025                 // free collision list
1026                 if(clmd->coll_parms.collision_list)
1027                 {
1028                         LinkNode *search = clmd->coll_parms.collision_list;
1029                         while(search)
1030                         {
1031                                 CollPair *coll_pair = search->link;
1032                                                         
1033                                 MEM_freeN(coll_pair);
1034                                 search = search->next;
1035                         }
1036                         BLI_linklist_free(clmd->coll_parms.collision_list,NULL);
1037                         
1038                         clmd->coll_parms.collision_list = NULL;
1039                 }
1040                 
1041                 printf("ic: %d\n", ic);
1042                 rounds++;
1043         }
1044         while(result && (CLOTH_MAX_THRESHOLD>rounds));
1045         
1046         
1047         ////////////////////////////////////////////////////////////
1048         // update positions + velocities
1049         ////////////////////////////////////////////////////////////
1050         
1051         // verts come from clmd
1052         for(i = 0; i < numverts; i++)
1053         {
1054                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1055         }
1056         ////////////////////////////////////////////////////////////
1057
1058         return MIN2(ret, 1);
1059 }