Springs are in a dynamic list now, New function cloth_add_spring() for easier access...
[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) { float tmp = b ; b = a ; a = tmp ; }
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 // unused in the moment, has some bug in
365 DO_INLINE void calculateFrictionImpulse(float to[3], float vrel[3], float normal[3], double normalVelocity,
366                                         double frictionConstant, double delta_V_n) 
367 {
368         float vrel_t_pre[3];
369         float vrel_t[3];
370         VECSUBS(vrel_t_pre, vrel, normal, normalVelocity);
371         VECCOPY(to, vrel_t_pre);
372         VecMulf(to, MAX2(1.0f - frictionConstant * delta_V_n / INPR(vrel_t_pre,vrel_t_pre), 0.0f));
373 }
374
375 int cloth_collision_response_static(ClothModifierData *clmd, ClothModifierData *coll_clmd)
376 {
377         unsigned int i = 0;
378         int result = 0;
379         LinkNode *search = NULL;
380         CollPair *collpair = NULL;
381         Cloth *cloth1, *cloth2;
382         float w1, w2, w3, u1, u2, u3;
383         float v1[3], v2[3], relativeVelocity[3];
384         float magrelVel;
385         
386         cloth1 = clmd->clothObject;
387         cloth2 = coll_clmd->clothObject;
388
389         search = clmd->coll_parms.collision_list;
390         
391         while(search)
392         {
393                 collpair = search->link;
394                 
395                 // compute barycentric coordinates for both collision points
396                 cloth_compute_barycentric(collpair->pa,
397                                         cloth1->verts[collpair->ap1].txold,
398                                         cloth1->verts[collpair->ap2].txold,
399                                         cloth1->verts[collpair->ap3].txold, 
400                                         &w1, &w2, &w3);
401         
402                 cloth_compute_barycentric(collpair->pb,
403                                         cloth2->verts[collpair->bp1].txold,
404                                         cloth2->verts[collpair->bp2].txold,
405                                         cloth2->verts[collpair->bp3].txold,
406                                         &u1, &u2, &u3);
407         
408                 // Calculate relative "velocity".
409                 interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
410                 
411                 interpolateOnTriangle(v2, cloth2->verts[collpair->bp1].tv, cloth2->verts[collpair->bp2].tv, cloth2->verts[collpair->bp3].tv, u1, u2, u3);
412                 
413                 VECSUB(relativeVelocity, v1, v2);
414                         
415                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
416                 magrelVel = INPR(relativeVelocity, collpair->normal);
417                 
418                 // printf("magrelVel: %f\n", magrelVel);
419                                 
420                 // Calculate masses of points.
421                 
422                 // If v_n_mag < 0 the edges are approaching each other.
423                 if(magrelVel < -ALMOST_ZERO) 
424                 {
425                         // Calculate Impulse magnitude to stop all motion in normal direction.
426                         // const double I_mag = v_n_mag / (1/m1 + 1/m2);
427                         float magnitude_i = magrelVel / 2.0f; // TODO implement masses
428                         float tangential[3], magtangent, magnormal, collvel[3];
429                         float vrel_t_pre[3];
430                         float vrel_t[3];
431                         double impulse;
432                         float epsilon = clmd->coll_parms.epsilon;
433                         float overlap = (epsilon + ALMOST_ZERO-collpair->distance);
434                         
435                         // calculateFrictionImpulse(tangential, relativeVelocity, collpair->normal, magrelVel, clmd->coll_parms.friction*0.01, magrelVel);
436                         
437                         // magtangent = INPR(tangential, tangential);
438                         
439                         // Apply friction impulse.
440                         if (magtangent < -ALMOST_ZERO) 
441                         {
442                                 
443                                 // printf("friction applied: %f\n", magtangent);
444                                 // TODO check original code 
445                                 /*
446                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,tangential);
447                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v2].tv,tangential);
448                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v3].tv,tangential);
449                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v4].tv,tangential);
450                                 */
451                         }
452                         
453
454                         impulse = -2.0f * magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
455                         
456                         // printf("impulse: %f\n", impulse);
457                         
458                         VECADDMUL(cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse); 
459                         cloth1->verts[collpair->ap1].impulse_count++;
460                         
461                         VECADDMUL(cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse); 
462                         cloth1->verts[collpair->ap2].impulse_count++;
463                         
464                         VECADDMUL(cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse); 
465                         cloth1->verts[collpair->ap3].impulse_count++;
466                         
467                         result = 1;
468                         
469                         /*
470                         if (overlap > ALMOST_ZERO) {
471                         double I_mag  = overlap * 0.1;
472                                 
473                         impulse = -I_mag / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
474                                 
475                         VECADDMUL(cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse); 
476                         cloth1->verts[collpair->ap1].impulse_count++;
477                                                         
478                         VECADDMUL(cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse); 
479                         cloth1->verts[collpair->ap2].impulse_count++;
480                         
481                         VECADDMUL(cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse); 
482                         cloth1->verts[collpair->ap3].impulse_count++;
483                 }
484                         */
485                 
486                         // printf("magnitude_i: %f\n", magnitude_i); // negative before collision in my case
487                         
488                         // Apply the impulse and increase impulse counters.
489
490                         /*                      
491                         // calculateFrictionImpulse(tangential, collvel, collpair->normal, magtangent, clmd->coll_parms.friction*0.01, magtangent);
492                         VECSUBS(vrel_t_pre, collvel, collpair->normal, magnormal);
493                         // VecMulf(vrel_t_pre, clmd->coll_parms.friction*0.01f/INPR(vrel_t_pre,vrel_t_pre));
494                         magtangent = Normalize(vrel_t_pre);
495                         VecMulf(vrel_t_pre, MIN2(clmd->coll_parms.friction*0.01f*magnormal,magtangent));
496                         
497                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,vrel_t_pre);
498                         */
499                         
500                         
501                         
502                 }
503                 
504                 search = search->next;
505         }
506         
507                 
508         return result;
509 }
510
511 int cloth_collision_response_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd)
512 {
513         
514 }
515
516
517 int cloth_collision_response_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd)
518 {
519         
520 }
521
522 void cloth_collision_static(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree *tree1, Tree *tree2)
523 {
524         CollPair *collpair = NULL;
525         Cloth *cloth1=NULL, *cloth2=NULL;
526         MFace *face1=NULL, *face2=NULL;
527         ClothVertex *verts1=NULL, *verts2=NULL;
528         double distance = 0;
529         float epsilon = clmd->coll_parms.epsilon;
530         unsigned int i = 0;
531
532         for(i = 0; i < 4; i++)
533         {
534                 collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");                
535                 
536                 cloth1 = clmd->clothObject;
537                 cloth2 = coll_clmd->clothObject;
538                 
539                 verts1 = cloth1->verts;
540                 verts2 = cloth2->verts;
541         
542                 face1 = &(cloth1->mfaces[tree1->tri_index]);
543                 face2 = &(cloth2->mfaces[tree2->tri_index]);
544                 
545                 // check all possible pairs of triangles
546                 if(i == 0)
547                 {
548                         collpair->ap1 = face1->v1;
549                         collpair->ap2 = face1->v2;
550                         collpair->ap3 = face1->v3;
551                         
552                         collpair->bp1 = face2->v1;
553                         collpair->bp2 = face2->v2;
554                         collpair->bp3 = face2->v3;
555                         
556                 }
557                 
558                 if(i == 1)
559                 {
560                         if(face1->v4)
561                         {
562                                 collpair->ap1 = face1->v3;
563                                 collpair->ap2 = face1->v4;
564                                 collpair->ap3 = face1->v1;
565                                 
566                                 collpair->bp1 = face2->v1;
567                                 collpair->bp2 = face2->v2;
568                                 collpair->bp3 = face2->v3;
569                         }
570                         else
571                                 i++;
572                 }
573                 
574                 if(i == 2)
575                 {
576                         if(face2->v4)
577                         {
578                                 collpair->ap1 = face1->v1;
579                                 collpair->ap2 = face1->v2;
580                                 collpair->ap3 = face1->v3;
581                                 
582                                 collpair->bp1 = face2->v3;
583                                 collpair->bp2 = face2->v4;
584                                 collpair->bp3 = face2->v1;
585                         }
586                         else
587                                 i+=2;
588                 }
589                 
590                 if(i == 3)
591                 {
592                         if((face1->v4)&&(face2->v4))
593                         {
594                                 collpair->ap1 = face1->v3;
595                                 collpair->ap2 = face1->v4;
596                                 collpair->ap3 = face1->v1;
597                                 
598                                 collpair->bp1 = face2->v3;
599                                 collpair->bp2 = face2->v4;
600                                 collpair->bp3 = face2->v1;
601                         }
602                         else
603                                 i++;
604                 }
605                 
606                 // calc SIPcode (?)
607                 
608                 if(i < 4)
609                 {
610                         // calc distance + normal       
611                         distance = plNearestPoints(
612                                         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);
613                         
614                         if (distance <= (epsilon + ALMOST_ZERO))
615                         {
616                                 // printf("dist: %f\n", (float)distance);
617                                 
618                                 // collpair->face1 = tree1->tri_index;
619                                 // collpair->face2 = tree2->tri_index;
620                                 
621                                 VECCOPY(collpair->normal, collpair->vector);
622                                 Normalize(collpair->normal);
623                                 
624                                 collpair->distance = distance;
625                                 BLI_linklist_append(&clmd->coll_parms.collision_list, collpair);
626                         }
627                         else
628                         {
629                                 MEM_freeN(collpair);
630                         }
631                 }
632                 else
633                 {
634                         MEM_freeN(collpair);
635                 }
636         }
637 }
638
639 int cloth_are_edges_adjacent(ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair)
640 {
641         Cloth *cloth1, *cloth2;
642         ClothVertex *verts1, *verts2;
643         float temp[3];
644          
645         cloth1 = clmd->clothObject;
646         cloth2 = coll_clmd->clothObject;
647         
648         verts1 = cloth1->verts;
649         verts2 = cloth2->verts;
650         
651         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold);
652         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
653                 return 1;
654         
655         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].xold);
656         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
657                 return 1;
658         
659         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold);
660         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
661                 return 1;
662         
663         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold);
664         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
665                 return 1;
666                 
667         return 0;
668 }
669
670 void cloth_collision_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree *tree1, Tree *tree2)
671 {
672         EdgeCollPair edgecollpair;
673         Cloth *cloth1=NULL, *cloth2=NULL;
674         MFace *face1=NULL, *face2=NULL;
675         ClothVertex *verts1=NULL, *verts2=NULL;
676         double distance = 0;
677         float epsilon = clmd->coll_parms.epsilon;
678         unsigned int i = 0, j = 0, k = 0;
679         int numsolutions = 0;
680         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
681         
682         cloth1 = clmd->clothObject;
683         cloth2 = coll_clmd->clothObject;
684         
685         verts1 = cloth1->verts;
686         verts2 = cloth2->verts;
687
688         face1 = &(cloth1->mfaces[tree1->tri_index]);
689         face2 = &(cloth2->mfaces[tree2->tri_index]);
690         
691         for( i = 0; i < 5; i++)
692         {
693                 if(i == 0) 
694                 {
695                         edgecollpair.p11 = face1->v1;
696                         edgecollpair.p12 = face1->v2;
697                 }
698                 else if(i == 1) 
699                 {
700                         edgecollpair.p11 = face1->v2;
701                         edgecollpair.p12 = face1->v3;
702                 }
703                 else if(i == 2) 
704                 {
705                         if(face1->v4) 
706                         {
707                                 edgecollpair.p11 = face1->v3;
708                                 edgecollpair.p12 = face1->v4;
709                         }
710                         else 
711                         {
712                                 edgecollpair.p11 = face1->v3;
713                                 edgecollpair.p12 = face1->v1;
714                                 i+=5; // get out of here after this edge pair is handled
715                         }
716                 }
717                 else if(i == 3) 
718                 {
719                         if(face1->v4) 
720                         {
721                                 edgecollpair.p11 = face1->v4;
722                                 edgecollpair.p12 = face1->v1;
723                         }       
724                         else
725                                 continue;
726                 }
727                 else
728                 {
729                         edgecollpair.p11 = face1->v3;
730                         edgecollpair.p12 = face1->v1;
731                 }
732
733                 
734                 for( j = 0; j < 5; j++)
735                 {
736                         if(j == 0)
737                         {
738                                 edgecollpair.p21 = face2->v1;
739                                 edgecollpair.p22 = face2->v2;
740                         }
741                         else if(j == 1)
742                         {
743                                 edgecollpair.p21 = face2->v2;
744                                 edgecollpair.p22 = face2->v3;
745                         }
746                         else if(j == 2)
747                         {
748                                 if(face2->v4) 
749                                 {
750                                         edgecollpair.p21 = face2->v3;
751                                         edgecollpair.p22 = face2->v4;
752                                 }
753                                 else 
754                                 {
755                                         edgecollpair.p21 = face2->v3;
756                                         edgecollpair.p22 = face2->v1;
757                                 }
758                         }
759                         else if(j == 3)
760                         {
761                                 if(face2->v4) 
762                                 {
763                                         edgecollpair.p21 = face2->v4;
764                                         edgecollpair.p22 = face2->v1;
765                                 }
766                                 else
767                                         continue;
768                         }
769                         else
770                         {
771                                 edgecollpair.p21 = face2->v3;
772                                 edgecollpair.p22 = face2->v1;
773                         }
774                         
775                         
776                         if(!cloth_are_edges_adjacent(clmd, coll_clmd, &edgecollpair))
777                         {
778                                 VECSUB(a, verts1[edgecollpair.p12].xold, verts1[edgecollpair.p11].xold);
779                                 VECSUB(b, verts1[edgecollpair.p12].v, verts1[edgecollpair.p11].v);
780                                 VECSUB(c, verts1[edgecollpair.p21].xold, verts1[edgecollpair.p11].xold);
781                                 VECSUB(d, verts1[edgecollpair.p21].v, verts1[edgecollpair.p11].v);
782                                 VECSUB(e, verts2[edgecollpair.p22].xold, verts1[edgecollpair.p11].xold);
783                                 VECSUB(f, verts2[edgecollpair.p22].v, verts1[edgecollpair.p11].v);
784                                 
785                                 numsolutions = cloth_get_collision_time(a, b, c, d, e, f, solution);
786                                 
787                                 for (k = 0; k < numsolutions; k++) 
788                                 {                                                               
789                                         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
790                                         {
791                                                 float out_collisionTime = solution[k];
792                                                 
793                                                 // TODO: check for collisions 
794                                                 
795                                                 // TODO: put into (edge) collision list
796                                                 
797                                                 printf("Moving edge found!\n");
798                                         }
799                                 }
800                         }
801                 }
802         }               
803 }
804
805 void cloth_collision_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree *tree1, Tree *tree2)
806 {
807         CollPair collpair;
808         Cloth *cloth1=NULL, *cloth2=NULL;
809         MFace *face1=NULL, *face2=NULL;
810         ClothVertex *verts1=NULL, *verts2=NULL;
811         double distance = 0;
812         float epsilon = clmd->coll_parms.epsilon;
813         unsigned int i = 0, j = 0, k = 0;
814         int numsolutions = 0;
815         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
816
817         for(i = 0; i < 2; i++)
818         {               
819                 cloth1 = clmd->clothObject;
820                 cloth2 = coll_clmd->clothObject;
821                 
822                 verts1 = cloth1->verts;
823                 verts2 = cloth2->verts;
824         
825                 face1 = &(cloth1->mfaces[tree1->tri_index]);
826                 face2 = &(cloth2->mfaces[tree2->tri_index]);
827                 
828                 // check all possible pairs of triangles
829                 if(i == 0)
830                 {
831                         collpair.ap1 = face1->v1;
832                         collpair.ap2 = face1->v2;
833                         collpair.ap3 = face1->v3;
834                         
835                         collpair.pointsb[0] = face2->v1;
836                         collpair.pointsb[1] = face2->v2;
837                         collpair.pointsb[2] = face2->v3;
838                         collpair.pointsb[3] = face2->v4;
839                 }
840                 
841                 if(i == 1)
842                 {
843                         if(face1->v4)
844                         {
845                                 collpair.ap1 = face1->v3;
846                                 collpair.ap2 = face1->v4;
847                                 collpair.ap3 = face1->v1;
848                                 
849                                 collpair.pointsb[0] = face2->v1;
850                                 collpair.pointsb[1] = face2->v2;
851                                 collpair.pointsb[2] = face2->v3;
852                                 collpair.pointsb[3] = face2->v4;
853                         }
854                         else
855                                 i++;
856                 }
857                 
858                 // calc SIPcode (?)
859                 
860                 if(i < 2)
861                 {
862                         VECSUB(a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold);
863                         VECSUB(b, verts1[collpair.ap2].v, verts1[collpair.ap1].v);
864                         VECSUB(c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold);
865                         VECSUB(d, verts1[collpair.ap3].v, verts1[collpair.ap1].v);
866                                 
867                         for(j = 0; j < 4; j++)
868                         {                                       
869                                 if((j==3) && !(face2->v4))
870                                         break;
871                                 
872                                 VECSUB(e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold);
873                                 VECSUB(f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v);
874                                 
875                                 numsolutions = cloth_get_collision_time(a, b, c, d, e, f, solution);
876                                 
877                                 for (k = 0; k < numsolutions; k++) 
878                                 {                                                               
879                                         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
880                                         {
881                                                 float out_collisionTime = solution[k];
882                                                 
883                                                 // TODO: check for collisions 
884                                                 
885                                                 // TODO: put into (point-face) collision list
886                                                 
887                                                 printf("Moving found!\n");
888                                                 
889                                         }
890                                 }
891                                 
892                                 // TODO: check borders for collisions
893                         }
894                         
895                 }
896         }
897 }
898
899 void cloth_collision_moving(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree *tree1, Tree *tree2)
900 {
901         // TODO: check for adjacent
902         cloth_collision_moving_edges(clmd, coll_clmd, tree1, tree2);
903         
904         cloth_collision_moving_tris(clmd, coll_clmd, tree1, tree2);
905         cloth_collision_moving_tris(coll_clmd, clmd, tree2, tree1);
906 }
907
908 // move collision objects forward in time and update static bounding boxes
909 void cloth_update_collision_objects(float step)
910 {
911         Base *base=NULL;
912         ClothModifierData *coll_clmd=NULL;
913         Object *coll_ob=NULL;
914         unsigned int i=0;
915         
916         // search all objects for collision object
917         for (base = G.scene->base.first; base; base = base->next)
918         {
919
920                 coll_ob = base->object;
921                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
922                 if (!coll_clmd)
923                         continue;
924
925                 // if collision object go on
926                 if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
927                 {
928                         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
929                         {
930                                 Cloth *coll_cloth = coll_clmd->clothObject;
931                                 BVH *coll_bvh = coll_clmd->clothObject->tree;
932                                 unsigned int coll_numverts = coll_cloth->numverts;
933
934                                 // update position of collision object
935                                 for(i = 0; i < coll_numverts; i++)
936                                 {
937                                         VECCOPY(coll_cloth->verts[i].txold, coll_cloth->verts[i].tx);
938
939                                         VECADDS(coll_cloth->verts[i].tx, coll_cloth->verts[i].xold, coll_cloth->verts[i].v, step);
940                                         
941                                         // no dt here because of float rounding errors
942                                         VECSUB(coll_cloth->verts[i].tv, coll_cloth->verts[i].tx, coll_cloth->verts[i].txold);
943                                 }
944                                 
945                                 // update BVH of collision object
946                                 bvh_update(coll_clmd, coll_bvh, 0); // 0 means STATIC, 1 means MOVING 
947                         }
948                         else
949                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
950                 }
951         }
952 }
953
954 // CLOTH_MAX_THRESHOLD defines how much collision rounds/loops should be taken
955 #define CLOTH_MAX_THRESHOLD 10
956
957 // cloth - object collisions
958 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, float dt)
959 {
960         Base *base=NULL;
961         ClothModifierData *coll_clmd=NULL;
962         Cloth *cloth=NULL;
963         Object *coll_ob=NULL;
964         BVH *cloth_bvh=NULL;
965         unsigned int i=0, j = 0, numfaces = 0, numverts = 0;
966         unsigned int result = 0, ic = 0, rounds = 0; // result counts applied collisions; ic is for debug output; 
967         ClothVertex *verts = NULL;
968         float tnull[3] = {0,0,0};
969         int ret = 0;
970
971         if ((clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || !(((Cloth *)clmd->clothObject)->tree))
972         {
973                 return 0;
974         }
975         cloth = clmd->clothObject;
976         verts = cloth->verts;
977         cloth_bvh = (BVH *) cloth->tree;
978         numfaces = clmd->clothObject->numfaces;
979         numverts = clmd->clothObject->numverts;
980         
981         ////////////////////////////////////////////////////////////
982         // static collisions
983         ////////////////////////////////////////////////////////////
984
985         // update cloth bvh
986         bvh_update(clmd, cloth_bvh, 0); // 0 means STATIC, 1 means MOVING (see later in this function)
987         
988         // update collision objects
989         cloth_update_collision_objects(step);
990         
991         do
992         {
993                 result = 0;
994                 ic = 0;
995                 clmd->coll_parms.collision_list = NULL; 
996                 
997                 // check all collision objects
998                 for (base = G.scene->base.first; base; base = base->next)
999                 {
1000                         coll_ob = base->object;
1001                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1002                         
1003                         if (!coll_clmd)
1004                                 continue;
1005                         
1006                         // if collision object go on
1007                         if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
1008                         {
1009                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
1010                                 {
1011                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
1012                                         
1013                                         bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_static);
1014                                 }
1015                                 else
1016                                         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
1017                         }
1018                 }
1019                 
1020                 // process all collisions (calculate impulses, TODO: also repulses if distance too short)
1021                 result = 1;
1022                 for(j = 0; j < 10; j++) // 10 is just a value that ensures convergence
1023                 {
1024                         result = 0;
1025                         
1026                         // handle all collision objects
1027                         for (base = G.scene->base.first; base; base = base->next)
1028                         {
1029                 
1030                                 coll_ob = base->object;
1031                                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1032                                 if (!coll_clmd)
1033                                         continue;
1034                 
1035                                 // if collision object go on
1036                                 if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
1037                                 {
1038                                         if (coll_clmd->clothObject) 
1039                                                 result += cloth_collision_response_static(clmd, coll_clmd);
1040                                         else
1041                                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
1042                                 }
1043                         }
1044                         
1045                         // apply impulses in parallel
1046                         ic=0;
1047                         for(i = 0; i < numverts; i++)
1048                         {
1049                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
1050                                 if(verts[i].impulse_count)
1051                                 {
1052                                         VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
1053                                         VECCOPY(verts[i].impulse, tnull);
1054                                         verts[i].impulse_count = 0;
1055                                 
1056                                         ic++;
1057                                         ret++;
1058                                 }
1059                         }
1060                 }
1061                 
1062                 // free collision list
1063                 if(clmd->coll_parms.collision_list)
1064                 {
1065                         LinkNode *search = clmd->coll_parms.collision_list;
1066                         while(search)
1067                         {
1068                                 CollPair *coll_pair = search->link;
1069                                                         
1070                                 MEM_freeN(coll_pair);
1071                                 search = search->next;
1072                         }
1073                         BLI_linklist_free(clmd->coll_parms.collision_list,NULL);
1074                         
1075                         clmd->coll_parms.collision_list = NULL;
1076                 }
1077                 
1078                 printf("ic: %d\n", ic);
1079                 rounds++;
1080         }
1081         while(result && (CLOTH_MAX_THRESHOLD>rounds));
1082         
1083         printf("\n");
1084                         
1085         ////////////////////////////////////////////////////////////
1086         // update positions
1087         // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1088         ////////////////////////////////////////////////////////////
1089         
1090         // verts come from clmd
1091         for(i = 0; i < numverts; i++)
1092         {
1093                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1094         }
1095         ////////////////////////////////////////////////////////////
1096
1097         ////////////////////////////////////////////////////////////
1098         // moving collisions
1099         ////////////////////////////////////////////////////////////
1100
1101         
1102         // update cloth bvh
1103         bvh_update(clmd, cloth_bvh, 1);  // 0 means STATIC, 1 means MOVING 
1104         
1105         // update moving bvh for collision object once
1106         for (base = G.scene->base.first; base; base = base->next)
1107         {
1108                 
1109                 coll_ob = base->object;
1110                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1111                 if (!coll_clmd)
1112                         continue;
1113                 
1114                 if(!coll_clmd->clothObject)
1115                         continue;
1116                 
1117                                 // if collision object go on
1118                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
1119                 {
1120                         BVH *coll_bvh = coll_clmd->clothObject->tree;
1121                         
1122                         bvh_update(coll_clmd, coll_bvh, 1);  // 0 means STATIC, 1 means MOVING  
1123                 }
1124         }
1125         
1126         
1127         do
1128         {
1129                 result = 0;
1130                 ic = 0;
1131                 clmd->coll_parms.collision_list = NULL; 
1132                 
1133                 // check all collision objects
1134                 for (base = G.scene->base.first; base; base = base->next)
1135                 {
1136                         coll_ob = base->object;
1137                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1138                         
1139                         if (!coll_clmd)
1140                                 continue;
1141                         
1142                         // if collision object go on
1143                         if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
1144                         {
1145                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
1146                                 {
1147                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
1148                                         
1149                                         bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, cloth_collision_moving);
1150                                 }
1151                                 else
1152                                         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
1153                         }
1154                 }
1155                 
1156                 // process all collisions (calculate impulses, TODO: also repulses if distance too short)
1157                 result = 1;
1158                 for(j = 0; j < 10; j++) // 10 is just a value that ensures convergence
1159                 {
1160                         result = 0;
1161                                 
1162                         // handle all collision objects
1163                         for (base = G.scene->base.first; base; base = base->next)
1164                         {
1165                         
1166                                 coll_ob = base->object;
1167                                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
1168                                                 
1169                                 if (!coll_clmd)
1170                                 continue;
1171                                 
1172                                 // if collision object go on
1173                                 if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
1174                                 {
1175                                         if (coll_clmd->clothObject) 
1176                                         result += cloth_collision_response_moving_tris(clmd, coll_clmd);
1177                                         else
1178                                         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
1179                                 }
1180                         }
1181                                                 
1182                         // apply impulses in parallel
1183                         ic=0;
1184                         for(i = 0; i < numverts; i++)
1185                         {
1186                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
1187                                 if(verts[i].impulse_count)
1188                                 {
1189                                         VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
1190                                         VECCOPY(verts[i].impulse, tnull);
1191                                         verts[i].impulse_count = 0;
1192                                                         
1193                                         ic++;
1194                                         ret++;
1195                                 }
1196                         }
1197                 }
1198                 
1199                 
1200                 // verts come from clmd
1201                 for(i = 0; i < numverts; i++)
1202                 {
1203                         VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1204                 }
1205                 
1206                 // update cloth bvh
1207                 bvh_update(clmd, cloth_bvh, 1);  // 0 means STATIC, 1 means MOVING 
1208                 
1209                 
1210                 // free collision list
1211                 if(clmd->coll_parms.collision_list)
1212                 {
1213                         LinkNode *search = clmd->coll_parms.collision_list;
1214                         while(search)
1215                         {
1216                                 CollPair *coll_pair = search->link;
1217                                                         
1218                                 MEM_freeN(coll_pair);
1219                                 search = search->next;
1220                         }
1221                         BLI_linklist_free(clmd->coll_parms.collision_list,NULL);
1222                         
1223                         clmd->coll_parms.collision_list = NULL;
1224                 }
1225                 
1226                 printf("ic: %d\n", ic);
1227                 rounds++;
1228         }
1229         while(result && (CLOTH_MAX_THRESHOLD>rounds));
1230         
1231         
1232         ////////////////////////////////////////////////////////////
1233         // update positions + velocities
1234         ////////////////////////////////////////////////////////////
1235         
1236         // verts come from clmd
1237         for(i = 0; i < numverts; i++)
1238         {
1239                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1240         }
1241         ////////////////////////////////////////////////////////////
1242
1243         return MIN2(ret, 1);
1244 }