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