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