svn merge -r 12347:12382 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[blender.git] / source / blender / blenkernel / intern / implicit.c
1 /*  implicit.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 #include <math.h>
33 #include <stdlib.h>
34 #include <string.h>
35 #include <stdio.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_modifier_types.h"
45 #include "DNA_meshdata_types.h"
46 #include "DNA_lattice_types.h"
47 #include "DNA_scene_types.h"
48 #include "DNA_modifier_types.h"
49 #include "BLI_blenlib.h"
50 #include "BLI_arithb.h"
51 #include "BLI_threads.h"
52 #include "BKE_collisions.h"
53 #include "BKE_curve.h"
54 #include "BKE_displist.h"
55 #include "BKE_effect.h"
56 #include "BKE_global.h"
57 #include "BKE_key.h"
58 #include "BKE_object.h"
59 #include "BKE_cloth.h"
60 #include "BKE_modifier.h"
61 #include "BKE_utildefines.h"
62 #include "BKE_global.h"
63 #include  "BIF_editdeform.h"
64
65
66 #ifdef _WIN32
67 #include <windows.h>
68 static LARGE_INTEGER _itstart, _itend;
69 static LARGE_INTEGER ifreq;
70 void itstart(void)
71 {
72         static int first = 1;
73         if(first) {
74                 QueryPerformanceFrequency(&ifreq);
75                 first = 0;
76         }
77         QueryPerformanceCounter(&_itstart);
78 }
79 void itend(void)
80 {
81         QueryPerformanceCounter(&_itend);
82 }
83 double itval()
84 {
85         return ((double)_itend.QuadPart -
86                 (double)_itstart.QuadPart)/((double)ifreq.QuadPart);
87 }
88 #else
89 #include <sys/time.h>
90 // intrinsics need better compile flag checking
91 // #include <xmmintrin.h>
92 // #include <pmmintrin.h>
93 // #include <pthread.h>
94
95 static struct timeval _itstart, _itend;
96 static struct timezone itz;
97 void itstart(void)
98 {
99         gettimeofday(&_itstart, &itz);
100 }
101 void itend(void)
102 {
103         gettimeofday(&_itend,&itz);
104 }
105 double itval()
106 {
107         double t1, t2;
108         t1 =  (double)_itstart.tv_sec + (double)_itstart.tv_usec/(1000*1000);
109         t2 =  (double)_itend.tv_sec + (double)_itend.tv_usec/(1000*1000);
110         return t2-t1;
111 }
112 #endif
113 /*
114 #define C99
115 #ifdef C99
116 #defineDO_INLINE inline 
117 #else 
118 #defineDO_INLINE static 
119 #endif
120 */
121 struct Cloth;
122
123 //////////////////////////////////////////
124 /* fast vector / matrix library, enhancements are welcome :) -dg */
125 /////////////////////////////////////////
126
127 /* DEFINITIONS */
128 typedef float lfVector[3];
129 typedef struct fmatrix3x3 {
130         float m[3][3]; /* 4x4 matrix */
131         unsigned int c,r; /* column and row number */
132         int pinned; /* is this vertex allowed to move? */
133         float n1,n2,n3; /* three normal vectors for collision constrains */
134         unsigned int vcount; /* vertex count */
135         unsigned int scount; /* spring count */ 
136 } fmatrix3x3;
137
138 ///////////////////////////
139 // float[3] vector
140 ///////////////////////////
141 /* simple vector code */
142 /* STATUS: verified */
143 DO_INLINE void mul_fvector_S(float to[3], float from[3], float scalar)
144 {
145         to[0] = from[0] * scalar;
146         to[1] = from[1] * scalar;
147         to[2] = from[2] * scalar;
148 }
149 /* simple cross product */
150 /* STATUS: verified */
151 DO_INLINE void cross_fvector(float to[3], float vectorA[3], float vectorB[3])
152 {
153         to[0] = vectorA[1] * vectorB[2] - vectorA[2] * vectorB[1];
154         to[1] = vectorA[2] * vectorB[0] - vectorA[0] * vectorB[2];
155         to[2] = vectorA[0] * vectorB[1] - vectorA[1] * vectorB[0];
156 }
157 /* simple v^T * v product ("outer product") */
158 /* STATUS: HAS TO BE verified (*should* work) */
159 DO_INLINE void mul_fvectorT_fvector(float to[3][3], float vectorA[3], float vectorB[3])
160 {
161         mul_fvector_S(to[0], vectorB, vectorA[0]);
162         mul_fvector_S(to[1], vectorB, vectorA[1]);
163         mul_fvector_S(to[2], vectorB, vectorA[2]);
164 }
165 /* simple v^T * v product with scalar ("outer product") */
166 /* STATUS: HAS TO BE verified (*should* work) */
167 DO_INLINE void mul_fvectorT_fvectorS(float to[3][3], float vectorA[3], float vectorB[3], float aS)
168 {
169         mul_fvector_S(to[0], vectorB, vectorA[0]* aS);
170         mul_fvector_S(to[1], vectorB, vectorA[1]* aS);
171         mul_fvector_S(to[2], vectorB, vectorA[2]* aS);
172 }
173
174 /* printf vector[3] on console: for debug output */
175 void print_fvector(float m3[3])
176 {
177         printf("%f\n%f\n%f\n\n",m3[0],m3[1],m3[2]);
178 }
179
180 ///////////////////////////
181 // long float vector float (*)[3]
182 ///////////////////////////
183 /* print long vector on console: for debug output */
184 DO_INLINE void print_lfvector(float (*fLongVector)[3], unsigned int verts)
185 {
186         unsigned int i = 0;
187         for(i = 0; i < verts; i++)
188         {
189                 print_fvector(fLongVector[i]);
190         }
191 }
192 /* create long vector */
193 DO_INLINE lfVector *create_lfvector(unsigned int verts)
194 {
195         // TODO: check if memory allocation was successfull */
196         return  (lfVector *)MEM_callocN (verts * sizeof(lfVector), "cloth_implicit_alloc_vector");
197         // return (lfVector *)cloth_aligned_malloc(&MEMORY_BASE, verts * sizeof(lfVector));
198 }
199 /* delete long vector */
200 DO_INLINE void del_lfvector(float (*fLongVector)[3])
201 {
202         if (fLongVector != NULL)
203         {
204                 MEM_freeN (fLongVector);
205                 // cloth_aligned_free(&MEMORY_BASE, fLongVector);
206         }
207 }
208 /* copy long vector */
209 DO_INLINE void cp_lfvector(float (*to)[3], float (*from)[3], unsigned int verts)
210 {
211         memcpy(to, from, verts * sizeof(lfVector));
212 }
213 /* init long vector with float[3] */
214 DO_INLINE void init_lfvector(float (*fLongVector)[3], float vector[3], unsigned int verts)
215 {
216         unsigned int i = 0;
217         for(i = 0; i < verts; i++)
218         {
219                 VECCOPY(fLongVector[i], vector);
220         }
221 }
222 /* zero long vector with float[3] */
223 DO_INLINE void zero_lfvector(float (*to)[3], unsigned int verts)
224 {
225         memset(to, 0.0f, verts * sizeof(lfVector));
226 }
227 /* multiply long vector with scalar*/
228 DO_INLINE void mul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float scalar, unsigned int verts)
229 {
230         unsigned int i = 0;
231
232         for(i = 0; i < verts; i++)
233         {
234                 mul_fvector_S(to[i], fLongVector[i], scalar);
235         }
236 }
237 /* multiply long vector with scalar*/
238 /* A -= B * float */
239 DO_INLINE void submul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float scalar, unsigned int verts)
240 {
241         unsigned int i = 0;
242         for(i = 0; i < verts; i++)
243         {
244                 VECSUBMUL(to[i], fLongVector[i], scalar);
245         }
246 }
247 /* dot product for big vector */
248 DO_INLINE float dot_lfvector(float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
249 {
250         unsigned int i = 0;
251         float temp = 0.0;
252 // schedule(guided, 2)
253 #pragma omp parallel for reduction(+: temp)
254         for(i = 0; i < verts; i++)
255         {
256                 temp += INPR(fLongVectorA[i], fLongVectorB[i]);
257         }
258         return temp;
259 }
260 /* A = B + C  --> for big vector */
261 DO_INLINE void add_lfvector_lfvector(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
262 {
263         unsigned int i = 0;
264
265         for(i = 0; i < verts; i++)
266         {
267                 VECADD(to[i], fLongVectorA[i], fLongVectorB[i]);
268         }
269
270 }
271 /* A = B + C * float --> for big vector */
272 DO_INLINE void add_lfvector_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], float bS, unsigned int verts)
273 {
274         unsigned int i = 0;
275
276         for(i = 0; i < verts; i++)
277         {
278                 VECADDS(to[i], fLongVectorA[i], fLongVectorB[i], bS);
279
280         }
281 }
282 /* A = B * float + C * float --> for big vector */
283 DO_INLINE void add_lfvectorS_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float aS, float (*fLongVectorB)[3], float bS, unsigned int verts)
284 {
285         unsigned int i = 0;
286
287         for(i = 0; i < verts; i++)
288         {
289                 VECADDSS(to[i], fLongVectorA[i], aS, fLongVectorB[i], bS);
290         }
291 }
292 /* A = B - C * float --> for big vector */
293 DO_INLINE void sub_lfvector_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], float bS, unsigned int verts)
294 {
295         unsigned int i = 0;
296         for(i = 0; i < verts; i++)
297         {
298                 VECSUBS(to[i], fLongVectorA[i], fLongVectorB[i], bS);
299         }
300
301 }
302 /* A = B - C --> for big vector */
303 DO_INLINE void sub_lfvector_lfvector(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
304 {
305         unsigned int i = 0;
306
307         for(i = 0; i < verts; i++)
308         {
309                 VECSUB(to[i], fLongVectorA[i], fLongVectorB[i]);
310         }
311
312 }
313 ///////////////////////////
314 // 4x4 matrix
315 ///////////////////////////
316 /* printf 4x4 matrix on console: for debug output */
317 void print_fmatrix(float m3[3][3])
318 {
319         printf("%f\t%f\t%f\n",m3[0][0],m3[0][1],m3[0][2]);
320         printf("%f\t%f\t%f\n",m3[1][0],m3[1][1],m3[1][2]);
321         printf("%f\t%f\t%f\n\n",m3[2][0],m3[2][1],m3[2][2]);
322 }
323 /* copy 4x4 matrix */
324 DO_INLINE void cp_fmatrix(float to[3][3], float from[3][3])
325 {
326         // memcpy(to, from, sizeof (float) * 9);
327         VECCOPY(to[0], from[0]);
328         VECCOPY(to[1], from[1]);
329         VECCOPY(to[2], from[2]);
330 }
331 /* calculate determinant of 4x4 matrix */
332 DO_INLINE float det_fmatrix(float m[3][3])
333 {
334         return  m[0][0]*m[1][1]*m[2][2] + m[1][0]*m[2][1]*m[0][2] + m[0][1]*m[1][2]*m[2][0] 
335         -m[0][0]*m[1][2]*m[2][1] - m[0][1]*m[1][0]*m[2][2] - m[2][0]*m[1][1]*m[0][2];
336 }
337 DO_INLINE void inverse_fmatrix(float to[3][3], float from[3][3])
338 {
339         unsigned int i, j;
340         float d;
341
342         if((d=det_fmatrix(from))==0)
343         {
344                 printf("can't build inverse");
345                 exit(0);
346         }
347         for(i=0;i<3;i++) 
348         {
349                 for(j=0;j<3;j++) 
350                 {
351                         int i1=(i+1)%3;
352                         int i2=(i+2)%3;
353                         int j1=(j+1)%3;
354                         int j2=(j+2)%3;
355                         // reverse indexs i&j to take transpose
356                         to[j][i] = (from[i1][j1]*from[i2][j2]-from[i1][j2]*from[i2][j1])/d;
357                         /*
358                         if(i==j)
359                         to[i][j] = 1.0f / from[i][j];
360                         else
361                         to[i][j] = 0;
362                         */
363                 }
364         }
365
366 }
367
368 /* 4x4 matrix multiplied by a scalar */
369 /* STATUS: verified */
370 DO_INLINE void mul_fmatrix_S(float matrix[3][3], float scalar)
371 {
372         mul_fvector_S(matrix[0], matrix[0],scalar);
373         mul_fvector_S(matrix[1], matrix[1],scalar);
374         mul_fvector_S(matrix[2], matrix[2],scalar);
375 }
376
377 /* a vector multiplied by a 4x4 matrix */
378 /* STATUS: verified */
379 DO_INLINE void mul_fvector_fmatrix(float *to, float *from, float matrix[3][3])
380 {
381         to[0] = matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
382         to[1] = matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
383         to[2] = matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
384 }
385
386 /* 4x4 matrix multiplied by a vector */
387 /* STATUS: verified */
388 DO_INLINE void mul_fmatrix_fvector(float *to, float matrix[3][3], float *from)
389 {
390         to[0] = INPR(matrix[0],from);
391         to[1] = INPR(matrix[1],from);
392         to[2] = INPR(matrix[2],from);
393 }
394 /* 4x4 matrix multiplied by a 4x4 matrix */
395 /* STATUS: verified */
396 DO_INLINE void mul_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
397 {
398         mul_fvector_fmatrix(to[0], matrixA[0],matrixB);
399         mul_fvector_fmatrix(to[1], matrixA[1],matrixB);
400         mul_fvector_fmatrix(to[2], matrixA[2],matrixB);
401 }
402 /* 4x4 matrix addition with 4x4 matrix */
403 DO_INLINE void add_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
404 {
405         VECADD(to[0], matrixA[0], matrixB[0]);
406         VECADD(to[1], matrixA[1], matrixB[1]);
407         VECADD(to[2], matrixA[2], matrixB[2]);
408 }
409 /* 4x4 matrix add-addition with 4x4 matrix */
410 DO_INLINE void addadd_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
411 {
412         VECADDADD(to[0], matrixA[0], matrixB[0]);
413         VECADDADD(to[1], matrixA[1], matrixB[1]);
414         VECADDADD(to[2], matrixA[2], matrixB[2]);
415 }
416 /* 4x4 matrix sub-addition with 4x4 matrix */
417 DO_INLINE void addsub_fmatrixS_fmatrixS(float to[3][3], float matrixA[3][3], float aS, float matrixB[3][3], float bS)
418 {
419         VECADDSUBSS(to[0], matrixA[0], aS, matrixB[0], bS);
420         VECADDSUBSS(to[1], matrixA[1], aS, matrixB[1], bS);
421         VECADDSUBSS(to[2], matrixA[2], aS, matrixB[2], bS);
422 }
423 /* A -= B + C (4x4 matrix sub-addition with 4x4 matrix) */
424 DO_INLINE void subadd_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
425 {
426         VECSUBADD(to[0], matrixA[0], matrixB[0]);
427         VECSUBADD(to[1], matrixA[1], matrixB[1]);
428         VECSUBADD(to[2], matrixA[2], matrixB[2]);
429 }
430 /* A -= B*x + C*y (4x4 matrix sub-addition with 4x4 matrix) */
431 DO_INLINE void subadd_fmatrixS_fmatrixS(float to[3][3], float matrixA[3][3], float aS, float matrixB[3][3], float bS)
432 {
433         VECSUBADDSS(to[0], matrixA[0], aS, matrixB[0], bS);
434         VECSUBADDSS(to[1], matrixA[1], aS, matrixB[1], bS);
435         VECSUBADDSS(to[2], matrixA[2], aS, matrixB[2], bS);
436 }
437 /* A = B - C (4x4 matrix subtraction with 4x4 matrix) */
438 DO_INLINE void sub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
439 {
440         VECSUB(to[0], matrixA[0], matrixB[0]);
441         VECSUB(to[1], matrixA[1], matrixB[1]);
442         VECSUB(to[2], matrixA[2], matrixB[2]);
443 }
444 /* A += B - C (4x4 matrix add-subtraction with 4x4 matrix) */
445 DO_INLINE void addsub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
446 {
447         VECADDSUB(to[0], matrixA[0], matrixB[0]);
448         VECADDSUB(to[1], matrixA[1], matrixB[1]);
449         VECADDSUB(to[2], matrixA[2], matrixB[2]);
450 }
451 /////////////////////////////////////////////////////////////////
452 // special functions
453 /////////////////////////////////////////////////////////////////
454 /* a vector multiplied and added to/by a 4x4 matrix */
455 DO_INLINE void muladd_fvector_fmatrix(float to[3], float from[3], float matrix[3][3])
456 {
457         to[0] += matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
458         to[1] += matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
459         to[2] += matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
460 }
461 /* 4x4 matrix multiplied and added  to/by a 4x4 matrix  and added to another 4x4 matrix */
462 DO_INLINE void muladd_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
463 {
464         muladd_fvector_fmatrix(to[0], matrixA[0],matrixB);
465         muladd_fvector_fmatrix(to[1], matrixA[1],matrixB);
466         muladd_fvector_fmatrix(to[2], matrixA[2],matrixB);
467 }
468 /* a vector multiplied and sub'd to/by a 4x4 matrix */
469 DO_INLINE void mulsub_fvector_fmatrix(float to[3], float from[3], float matrix[3][3])
470 {
471         to[0] -= matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
472         to[1] -= matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
473         to[2] -= matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
474 }
475 /* 4x4 matrix multiplied and sub'd  to/by a 4x4 matrix  and added to another 4x4 matrix */
476 DO_INLINE void mulsub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
477 {
478         mulsub_fvector_fmatrix(to[0], matrixA[0],matrixB);
479         mulsub_fvector_fmatrix(to[1], matrixA[1],matrixB);
480         mulsub_fvector_fmatrix(to[2], matrixA[2],matrixB);
481 }
482 /* 4x4 matrix multiplied+added by a vector */
483 /* STATUS: verified */
484 DO_INLINE void muladd_fmatrix_fvector(float to[3], float matrix[3][3], float from[3])
485 {
486         to[0] += INPR(matrix[0],from);
487         to[1] += INPR(matrix[1],from);
488         to[2] += INPR(matrix[2],from);  
489 }
490 /* 4x4 matrix multiplied+sub'ed by a vector */
491 DO_INLINE void mulsub_fmatrix_fvector(float to[3], float matrix[3][3], float from[3])
492 {
493         to[0] -= INPR(matrix[0],from);
494         to[1] -= INPR(matrix[1],from);
495         to[2] -= INPR(matrix[2],from);
496 }
497 /////////////////////////////////////////////////////////////////
498
499 ///////////////////////////
500 // SPARSE SYMMETRIC big matrix with 4x4 matrix entries
501 ///////////////////////////
502 /* printf a big matrix on console: for debug output */
503 void print_bfmatrix(fmatrix3x3 *m3)
504 {
505         unsigned int i = 0;
506
507         for(i = 0; i < m3[0].vcount + m3[0].scount; i++)
508         {
509                 print_fmatrix(m3[i].m);
510         }
511 }
512 /* create big matrix */
513 DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs)
514 {
515         // TODO: check if memory allocation was successfull */
516         fmatrix3x3 *temp = (fmatrix3x3 *)MEM_callocN (sizeof (fmatrix3x3) * (verts + springs), "cloth_implicit_alloc_matrix");
517         temp[0].vcount = verts;
518         temp[0].scount = springs;
519         return temp;
520 }
521 /* delete big matrix */
522 DO_INLINE void del_bfmatrix(fmatrix3x3 *matrix)
523 {
524         if (matrix != NULL)
525         {
526                 MEM_freeN (matrix);
527         }
528 }
529 /* copy big matrix */
530 DO_INLINE void cp_bfmatrix(fmatrix3x3 *to, fmatrix3x3 *from)
531 {       
532         // TODO bounds checking 
533         memcpy(to, from, sizeof(fmatrix3x3) * (from[0].vcount+from[0].scount) );
534 }
535 /* init the diagonal of big matrix */
536 // slow in parallel
537 DO_INLINE void initdiag_bfmatrix(fmatrix3x3 *matrix, float m3[3][3])
538 {
539         unsigned int i,j;
540         float tmatrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
541
542         for(i = 0; i < matrix[0].vcount; i++)
543         {               
544                 cp_fmatrix(matrix[i].m, m3); 
545         }
546         for(j = matrix[0].vcount; j < matrix[0].vcount+matrix[0].scount; j++)
547         {
548                 cp_fmatrix(matrix[j].m, tmatrix); 
549         }
550 }
551 /* init big matrix */
552 DO_INLINE void init_bfmatrix(fmatrix3x3 *matrix, float m3[3][3])
553 {
554         unsigned int i;
555
556         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
557         {
558                 cp_fmatrix(matrix[i].m, m3); 
559         }
560 }
561 /* multiply big matrix with scalar*/
562 DO_INLINE void mul_bfmatrix_S(fmatrix3x3 *matrix, float scalar)
563 {
564         unsigned int i = 0;
565         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
566         {
567                 mul_fmatrix_S(matrix[i].m, scalar);
568         }
569 }
570 /* SPARSE SYMMETRIC multiply big matrix with long vector*/
571 /* STATUS: verified */
572 DO_INLINE void mul_bfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, float (*fLongVector)[3])
573 {
574         unsigned int i = 0;
575         zero_lfvector(to, from[0].vcount);
576         /* process diagonal elements */ 
577         for(i = 0; i < from[0].vcount; i++)
578         {
579                 muladd_fmatrix_fvector(to[from[i].r], from[i].m, fLongVector[from[i].c]);       
580         }
581
582         /* process off-diagonal entries (every off-diagonal entry needs to be symmetric) */
583         // TODO: pragma below is wrong, correct it!
584         // #pragma omp parallel for shared(to,from, fLongVector) private(i) 
585         for(i = from[0].vcount; i < from[0].vcount+from[0].scount; i++)
586         {
587                 // muladd_fmatrix_fvector(to[from[i].c], from[i].m, fLongVector[from[i].r]);
588                 
589                 to[from[i].c][0] += INPR(from[i].m[0],fLongVector[from[i].r]);
590                 to[from[i].c][1] += INPR(from[i].m[1],fLongVector[from[i].r]);
591                 to[from[i].c][2] += INPR(from[i].m[2],fLongVector[from[i].r]);  
592                 
593                 // muladd_fmatrix_fvector(to[from[i].r], from[i].m, fLongVector[from[i].c]);
594                 
595                 to[from[i].r][0] += INPR(from[i].m[0],fLongVector[from[i].c]);
596                 to[from[i].r][1] += INPR(from[i].m[1],fLongVector[from[i].c]);
597                 to[from[i].r][2] += INPR(from[i].m[2],fLongVector[from[i].c]);  
598         }
599 }
600 /* SPARSE SYMMETRIC add big matrix with big matrix: A = B + C*/
601 DO_INLINE void add_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
602 {
603         unsigned int i = 0;
604
605         /* process diagonal elements */
606         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
607         {
608                 add_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
609         }
610
611 }
612 /* SPARSE SYMMETRIC add big matrix with big matrix: A += B + C */
613 DO_INLINE void addadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
614 {
615         unsigned int i = 0;
616
617         /* process diagonal elements */
618         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
619         {
620                 addadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
621         }
622
623 }
624 /* SPARSE SYMMETRIC subadd big matrix with big matrix: A -= B + C */
625 DO_INLINE void subadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
626 {
627         unsigned int i = 0;
628
629         /* process diagonal elements */
630         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
631         {
632                 subadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
633         }
634
635 }
636 /*  A = B - C (SPARSE SYMMETRIC sub big matrix with big matrix) */
637 DO_INLINE void sub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
638 {
639         unsigned int i = 0;
640
641         /* process diagonal elements */
642         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
643         {
644                 sub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
645         }
646
647 }
648 /* SPARSE SYMMETRIC sub big matrix with big matrix S (special constraint matrix with limited entries) */
649 DO_INLINE void sub_bfmatrix_Smatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
650 {
651         unsigned int i = 0;
652
653         /* process diagonal elements */
654         for(i = 0; i < matrix[0].vcount; i++)
655         {
656                 sub_fmatrix_fmatrix(to[matrix[i].c].m, from[matrix[i].c].m, matrix[i].m);       
657         }
658
659 }
660 /* A += B - C (SPARSE SYMMETRIC addsub big matrix with big matrix) */
661 DO_INLINE void addsub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
662 {
663         unsigned int i = 0;
664
665         /* process diagonal elements */
666         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
667         {
668                 addsub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
669         }
670
671 }
672 /* SPARSE SYMMETRIC sub big matrix with big matrix*/
673 /* A -= B * float + C * float --> for big matrix */
674 /* VERIFIED */
675 DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, float aS,  fmatrix3x3 *matrix, float bS)
676 {
677         unsigned int i = 0;
678
679         /* process diagonal elements */
680         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
681         {
682                 subadd_fmatrixS_fmatrixS(to[i].m, from[i].m, aS, matrix[i].m, bS);      
683         }
684
685 }
686
687 ///////////////////////////////////////////////////////////////////
688 // simulator start
689 ///////////////////////////////////////////////////////////////////
690 static float I[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
691 static float ZERO[3][3] = {{0,0,0}, {0,0,0}, {0,0,0}};
692 typedef struct Implicit_Data 
693 {
694         lfVector *X, *V, *Xnew, *Vnew, *olddV, *F, *B, *dV, *z;
695         fmatrix3x3 *A, *dFdV, *dFdX, *S, *P, *Pinv, *bigI; 
696 } Implicit_Data;
697
698 int implicit_init (Object *ob, ClothModifierData *clmd)
699 {
700         unsigned int i = 0;
701         unsigned int pinned = 0;
702         Cloth *cloth = NULL;
703         ClothVertex *verts = NULL;
704         ClothSpring *spring = NULL;
705         Implicit_Data *id = NULL;
706         LinkNode *search = NULL;
707
708         // init memory guard
709         // MEMORY_BASE.first = MEMORY_BASE.last = NULL;
710
711         cloth = (Cloth *)clmd->clothObject;
712         verts = cloth->verts;
713
714         // create implicit base
715         id = (Implicit_Data *)MEM_callocN (sizeof(Implicit_Data), "implicit vecmat");
716         cloth->implicit = id;
717
718         /* process diagonal elements */         
719         id->A = create_bfmatrix(cloth->numverts, cloth->numsprings);
720         id->dFdV = create_bfmatrix(cloth->numverts, cloth->numsprings);
721         id->dFdX = create_bfmatrix(cloth->numverts, cloth->numsprings);
722         id->S = create_bfmatrix(cloth->numverts, 0);
723         id->Pinv = create_bfmatrix(cloth->numverts, cloth->numsprings);
724         id->P = create_bfmatrix(cloth->numverts, cloth->numsprings);
725         id->bigI = create_bfmatrix(cloth->numverts, cloth->numsprings); // TODO 0 springs
726         id->X = create_lfvector(cloth->numverts);
727         id->Xnew = create_lfvector(cloth->numverts);
728         id->V = create_lfvector(cloth->numverts);
729         id->Vnew = create_lfvector(cloth->numverts);
730         id->olddV = create_lfvector(cloth->numverts);
731         zero_lfvector(id->olddV, cloth->numverts);
732         id->F = create_lfvector(cloth->numverts);
733         id->B = create_lfvector(cloth->numverts);
734         id->dV = create_lfvector(cloth->numverts);
735         id->z = create_lfvector(cloth->numverts);
736         
737         for(i=0;i<cloth->numverts;i++) 
738         {
739                 id->A[i].r = id->A[i].c = id->dFdV[i].r = id->dFdV[i].c = id->dFdX[i].r = id->dFdX[i].c = id->P[i].c = id->P[i].r = id->Pinv[i].c = id->Pinv[i].r = id->bigI[i].c = id->bigI[i].r = i;
740
741                 if(verts [i].goal >= SOFTGOALSNAP)
742                 {
743                         id->S[pinned].pinned = 1;
744                         id->S[pinned].c = id->S[pinned].r = i;
745                         pinned++;
746                 }
747         }
748
749         // S is special and needs specific vcount and scount
750         id->S[0].vcount = pinned; id->S[0].scount = 0;
751
752         // init springs */
753         search = cloth->springs;
754         for(i=0;i<cloth->numsprings;i++) 
755         {
756                 spring = search->link;
757                 
758                 // dFdV_start[i].r = big_I[i].r = big_zero[i].r = 
759                 id->A[i+cloth->numverts].r = id->dFdV[i+cloth->numverts].r = id->dFdX[i+cloth->numverts].r = 
760                                 id->P[i+cloth->numverts].r = id->Pinv[i+cloth->numverts].r = id->bigI[i+cloth->numverts].r = spring->ij;
761
762                 // dFdV_start[i].c = big_I[i].c = big_zero[i].c = 
763                 id->A[i+cloth->numverts].c = id->dFdV[i+cloth->numverts].c = id->dFdX[i+cloth->numverts].c = 
764                         id->P[i+cloth->numverts].c = id->Pinv[i+cloth->numverts].c = id->bigI[i+cloth->numverts].c = spring->kl;
765
766                 spring->matrix_index = i + cloth->numverts;
767                 
768                 search = search->next;
769         }
770
771         for(i = 0; i < cloth->numverts; i++)
772         {               
773                 VECCOPY(id->X[i], verts[i].x);
774         }
775
776         return 1;
777 }
778 int     implicit_free (ClothModifierData *clmd)
779 {
780         Implicit_Data *id;
781         Cloth *cloth;
782         cloth = (Cloth *)clmd->clothObject;
783
784         if(cloth)
785         {
786                 id = cloth->implicit;
787
788                 if(id)
789                 {
790                         del_bfmatrix(id->A);
791                         del_bfmatrix(id->dFdV);
792                         del_bfmatrix(id->dFdX);
793                         del_bfmatrix(id->S);
794                         del_bfmatrix(id->P);
795                         del_bfmatrix(id->Pinv);
796                         del_bfmatrix(id->bigI);
797
798                         del_lfvector(id->X);
799                         del_lfvector(id->Xnew);
800                         del_lfvector(id->V);
801                         del_lfvector(id->Vnew);
802                         del_lfvector(id->olddV);
803                         del_lfvector(id->F);
804                         del_lfvector(id->B);
805                         del_lfvector(id->dV);
806                         del_lfvector(id->z);
807
808                         MEM_freeN(id);
809                 }
810         }
811
812         return 1;
813 }
814
815 DO_INLINE float fb(float length, float L)
816 {
817         float x = length/L;
818         return (-11.541f*pow(x,4)+34.193f*pow(x,3)-39.083f*pow(x,2)+23.116f*x-9.713f);
819 }
820
821 DO_INLINE float fbderiv(float length, float L)
822 {
823         float x = length/L;
824
825         return (-46.164f*pow(x,3)+102.579f*pow(x,2)-78.166f*x+23.116f);
826 }
827
828 DO_INLINE float fbstar(float length, float L, float kb, float cb)
829 {
830         float tempfb = kb * fb(length, L);
831
832         float fbstar = cb * (length - L);
833
834         if(tempfb < fbstar)
835                 return fbstar;
836         else
837                 return tempfb;          
838 }
839
840 DO_INLINE float fbstar_jacobi(float length, float L, float kb, float cb)
841 {
842         float tempfb = kb * fb(length, L);
843         float fbstar = cb * (length - L);
844
845         if(tempfb < fbstar)
846         {               
847                 return cb;
848         }
849         else
850         {
851                 return kb * fbderiv(length, L); 
852         }       
853 }
854
855 DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
856 {
857         unsigned int i=0;
858
859         for(i=0;i<S[0].vcount;i++)
860         {
861                 mul_fvector_fmatrix(V[S[i].r], V[S[i].r], S[i].m);
862         }
863 }
864
865 // block diagonalizer
866 void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *S, fmatrix3x3 *bigI)
867 {
868         unsigned int i=0;
869
870         // Take only the diagonal blocks of A
871         for(i=0;i<lA[0].vcount;i++)
872         {
873                 cp_fmatrix(P[i].m, lA[i].m); 
874         }
875         /*
876         // SpecialBigSMul(P, S, P);
877         for(i=0;i<S[0].vcount;i++)
878         {
879         mul_fmatrix_fmatrix(P[S[i].r].m, S[i].m, P[S[i].r].m);
880         }
881         add_bfmatrix_bfmatrix(P, P, bigI);
882         */
883         for(i=0;i<lA[0].vcount;i++)                              
884         {
885                 inverse_fmatrix(Pinv[i].m, P[i].m); 
886         }               
887
888 }
889
890 int  cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S)
891 {
892         // Solves for unknown X in equation AX=B
893         unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
894         float conjgrad_epsilon=0.0001f, conjgrad_lasterror=0;
895         lfVector *q, *d, *tmp, *r; 
896         float s, starget, a, s_prev;
897         unsigned int numverts = lA[0].vcount;
898         q = create_lfvector(numverts);
899         d = create_lfvector(numverts);
900         tmp = create_lfvector(numverts);
901         r = create_lfvector(numverts);
902
903         // zero_lfvector(ldV, CLOTHPARTICLES);
904         filter(ldV, S);
905
906         add_lfvector_lfvector(ldV, ldV, z, numverts);
907
908         // r = B - Mul(tmp,A,X);    // just use B if X known to be zero
909         cp_lfvector(r, lB, numverts);
910         mul_bfmatrix_lfvector(tmp, lA, ldV);
911         sub_lfvector_lfvector(r, r, tmp, numverts);
912
913         filter(r,S);
914
915         cp_lfvector(d, r, numverts);
916
917         s = dot_lfvector(r, r, numverts);
918         starget = s * sqrt(conjgrad_epsilon);
919
920         while((s>starget && conjgrad_loopcount < conjgrad_looplimit))
921         {       
922                 // Mul(q,A,d); // q = A*d;
923                 mul_bfmatrix_lfvector(q, lA, d);
924
925                 filter(q,S);
926
927                 a = s/dot_lfvector(d, q, numverts);
928
929                 // X = X + d*a;
930                 add_lfvector_lfvectorS(ldV, ldV, d, a, numverts);
931
932                 // r = r - q*a;
933                 sub_lfvector_lfvectorS(r, r, q, a, numverts);
934
935                 s_prev = s;
936                 s = dot_lfvector(r, r, numverts);
937
938                 //d = r+d*(s/s_prev);
939                 add_lfvector_lfvectorS(d, r, d, (s/s_prev), numverts);
940
941                 filter(d,S);
942
943                 conjgrad_loopcount++;
944         }
945         conjgrad_lasterror = s;
946
947         del_lfvector(q);
948         del_lfvector(d);
949         del_lfvector(tmp);
950         del_lfvector(r);
951         // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount);
952
953         return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
954 }
955 /*
956 int cg_filtered_pre(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, lfVector *X0, fmatrix3x3 *P, fmatrix3x3 *Pinv, float dt)
957 {
958 // Solves for unknown X in equation AX=B
959 unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
960 float conjgrad_epsilon=0.0001f, conjgrad_lasterror=0;
961 lfVector *q, *c , *tmp, *r, *s, *filterX0, *p_fb, *bhat;
962 float delta0, deltanew, deltaold, alpha=0, epsilon_sqr;
963 unsigned int numverts = lA[0].vcount;
964 int i = 0;
965 q = create_lfvector(numverts);
966 c = create_lfvector(numverts);
967 tmp = create_lfvector(numverts);
968 r = create_lfvector(numverts);
969 s = create_lfvector(numverts);
970 filterX0 = create_lfvector(numverts);
971 p_fb = create_lfvector(numverts);
972 bhat = create_lfvector(numverts);
973
974 // SpecialBigSSub(bigI, S);
975 initdiag_bfmatrix(bigI, I);
976 sub_bfmatrix_Smatrix(bigI, bigI, S); // TODO
977
978 BuildPPinv(lA,P,Pinv,S, bigI);
979
980 //////////////////////////
981 // x = S*x0 + (I-S)*z 
982 //////////////////////////
983 // filterX0 = X0 * 1.0f;
984 cp_lfvector(filterX0, X0, numverts);
985 // filter(filterX0,S);
986 filter(filterX0, S);
987 // X = filterX0 * 1.0f;
988 cp_lfvector(ldV, filterX0, numverts);
989
990 // X = X + Mul(tmp, bigI, z);
991 mul_bfmatrix_lfvector(tmp, bigI, z);
992 add_lfvector_lfvector(ldV, ldV, tmp, numverts);
993 //////////////////////////
994
995
996 //////////////////////////
997 // b_hat = S*(b-A*(I-S)*z) 
998 //////////////////////////      
999 // bhat = bigI * z;
1000 mul_bfmatrix_lfvector(bhat, bigI, z);
1001 // bhat = Mul(tmp, A, bhat);
1002 mul_bfmatrix_lfvector(tmp, lA, bhat);
1003 cp_lfvector(bhat, tmp, numverts);
1004 // bhat = B - bhat;
1005 sub_lfvector_lfvector(bhat, lB, bhat, numverts);
1006 // cp_lfvector(bhat, lB, numverts);
1007 filter(bhat,S);
1008 //////////////////////////
1009
1010 //////////////////////////
1011 // r = S*(b - A*x)  
1012 //////////////////////////
1013 // r = B - Mul(tmp,A,X);    // just use B if X known to be zero
1014 mul_bfmatrix_lfvector(tmp, lA, ldV);
1015 sub_lfvector_lfvector(r, lB, tmp, numverts);
1016 // cp_lfvector(r, lB, numverts);
1017 filter(r,S);
1018 //////////////////////////
1019
1020
1021 //////////////////////////
1022 // (p) = c = S * P^-1 * r
1023 //////////////////////////
1024 // c = Pinv * r;
1025 mul_bfmatrix_lfvector(c, Pinv, r);
1026 filter(c,S);
1027 //////////////////////////      
1028
1029
1030 //////////////////////////
1031 // p_fb = P * bhat
1032 // delta0 = dot(bhat, p_fb)
1033 //////////////////////////
1034 // p_fb = P*bhat;       
1035 mul_bfmatrix_lfvector(p_fb, P, bhat);
1036 delta0 = dot_lfvector(bhat, p_fb, numverts);
1037 //////////////////////////
1038
1039
1040 //////////////////////////
1041 // deltanew = dot(r,c)
1042 //////////////////////////
1043 deltanew = dot_lfvector(r, c, numverts);
1044 //////////////////////////
1045 epsilon_sqr = conjgrad_epsilon*conjgrad_epsilon; // paper mentiones dt * 0.01
1046
1047 while((deltanew>(epsilon_sqr*delta0))&& (conjgrad_loopcount++ < conjgrad_looplimit))
1048 {
1049 //////////////////////////
1050 // (s) = q = S*A*c
1051 //////////////////////////
1052 // q = A*c; 
1053 mul_bfmatrix_lfvector(q, lA, c);
1054 filter(q,S);
1055 //////////////////////////              
1056
1057 //////////////////////////
1058 // alpha = deltanew / (c^T * q)
1059 //////////////////////////
1060 alpha = deltanew/dot_lfvector(c, q, numverts);
1061 //////////////////////////              
1062
1063 //X = X + c*alpha;
1064 add_lfvector_lfvectorS(ldV, ldV, c, alpha, numverts);
1065 //r = r - q*alpha;
1066 sub_lfvector_lfvectorS(r, r, q, alpha, numverts);               
1067
1068 //////////////////////////
1069 // (h) = s = P^-1 * r
1070 //////////////////////////
1071 // s = Pinv * r;
1072 mul_bfmatrix_lfvector(s, Pinv, r);
1073 filter(s,S);
1074 //////////////////////////
1075
1076 deltaold = deltanew;
1077
1078 // deltanew = dot(r,s);
1079 deltanew = dot_lfvector(r, s, numverts);
1080
1081 //////////////////////////
1082 // c = S * (s + (deltanew/deltaold)*c)
1083 //////////////////////////      
1084 // c = s + c * (deltanew/deltaold);
1085 add_lfvector_lfvectorS(c, s, c, (deltanew/deltaold), numverts);
1086 filter(c,S);
1087 //////////////////////////
1088
1089 }
1090 conjgrad_lasterror = deltanew;
1091 del_lfvector(q);
1092 del_lfvector(c);
1093 del_lfvector(tmp);
1094 del_lfvector(r);
1095 del_lfvector(s);
1096 del_lfvector(filterX0);
1097 del_lfvector(p_fb);
1098 del_lfvector(bhat);
1099 printf("Bconjgrad_loopcount: %d\n", conjgrad_loopcount);
1100
1101 return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
1102 }
1103 */
1104
1105 // outer product is NOT cross product!!!
1106 DO_INLINE void dfdx_spring_type1(float to[3][3], float dir[3],float length,float L,float k)
1107 {
1108         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1109         // return  (outerprod(dir,dir)*k + (I - outerprod(dir,dir))*(k - ((k*L)/length)));
1110         float temp[3][3];
1111         mul_fvectorT_fvector(temp, dir, dir);
1112         sub_fmatrix_fmatrix(to, I, temp);
1113         mul_fmatrix_S(to, k* (1.0f-(L/length)));
1114         mul_fmatrix_S(temp, k);
1115         add_fmatrix_fmatrix(to, temp, to);
1116 }
1117
1118 DO_INLINE void dfdx_spring_type2(float to[3][3], float dir[3],float length,float L,float k, float cb)
1119 {
1120         // return  outerprod(dir,dir)*fbstar_jacobi(length, L, k, cb);
1121         mul_fvectorT_fvectorS(to, dir, dir, fbstar_jacobi(length, L, k, cb));
1122 }
1123
1124 DO_INLINE void dfdv_damp(float to[3][3], float dir[3], float damping)
1125 {
1126         // derivative of force wrt velocity.  
1127         // return outerprod(dir,dir) * damping;
1128         mul_fvectorT_fvectorS(to, dir, dir, damping);
1129 }
1130
1131 DO_INLINE void dfdx_spring(float to[3][3],  float dir[3],float length,float L,float k)
1132 {
1133         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1134         //return  ( (I-outerprod(dir,dir))*Min(1.0f,rest/length) - I) * -k;
1135         mul_fvectorT_fvector(to, dir, dir);
1136         sub_fmatrix_fmatrix(to, I, to);
1137         mul_fmatrix_S(to, (((L/length)> 1.0f) ? (1.0f): (L/length))); 
1138         sub_fmatrix_fmatrix(to, to, I);
1139         mul_fmatrix_S(to, -k);
1140 }
1141
1142 DO_INLINE void dfdx_damp(float to[3][3],  float dir[3],float length,const float vel[3],float rest,float damping)
1143 {
1144         // inner spring damping   vel is the relative velocity  of the endpoints.  
1145         //      return (I-outerprod(dir,dir)) * (-damping * -(dot(dir,vel)/Max(length,rest)));
1146         mul_fvectorT_fvector(to, dir, dir);
1147         sub_fmatrix_fmatrix(to, I, to);
1148         mul_fmatrix_S(to,  (-damping * -(INPR(dir,vel)/MAX2(length,rest)))); 
1149
1150 }
1151
1152 DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
1153 {
1154         float extent[3];
1155         float length = 0;
1156         float dir[3] = {0,0,0};
1157         float vel[3];
1158         float k = 0.0f;
1159         float L = s->restlen;
1160         float cb = clmd->sim_parms.structural;
1161
1162         float nullf[3] = {0,0,0};
1163         float stretch_force[3] = {0,0,0};
1164         float bending_force[3] = {0,0,0};
1165         float damping_force[3] = {0,0,0};
1166         float nulldfdx[3][3]={ {0,0,0}, {0,0,0}, {0,0,0}};
1167         Cloth *cloth = clmd->clothObject;
1168         ClothVertex *verts = cloth->verts;
1169         
1170         VECCOPY(s->f, nullf);
1171         cp_fmatrix(s->dfdx, nulldfdx);
1172         cp_fmatrix(s->dfdv, nulldfdx);
1173
1174         // calculate elonglation
1175         VECSUB(extent, X[s->kl], X[s->ij]);
1176         VECSUB(vel, V[s->kl], V[s->ij]);
1177         length = sqrt(INPR(extent, extent));
1178         
1179         s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
1180         
1181         if(length > ABS(ALMOST_ZERO))
1182         {
1183                 /*
1184                 if(length>L)
1185                 {
1186                         if((clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED) 
1187                         && ((((length-L)*100.0f/L) > clmd->sim_parms.maxspringlen))) // cut spring!
1188                         {
1189                                 s->flags |= CSPRING_FLAG_DEACTIVATE;
1190                                 return;
1191                         }
1192                 } 
1193                 */
1194                 mul_fvector_S(dir, extent, 1.0f/length);
1195         }
1196         else    
1197         {
1198                 mul_fvector_S(dir, extent, 0.0f);
1199         }
1200         
1201         
1202         // calculate force of structural + shear springs
1203         if(s->type != CLOTH_SPRING_TYPE_BENDING)
1204         {
1205                 if(length > L) // only on elonglation
1206                 {
1207                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1208
1209                         k = clmd->sim_parms.structural; 
1210
1211                         mul_fvector_S(stretch_force, dir, (k*(length-L))); 
1212
1213                         VECADD(s->f, s->f, stretch_force);
1214
1215                         // Ascher & Boxman, p.21: Damping only during elonglation
1216                         mul_fvector_S(damping_force, extent, clmd->sim_parms.Cdis * ((INPR(vel,extent)/length))); 
1217                         VECADD(s->f, s->f, damping_force);
1218
1219                         dfdx_spring_type1(s->dfdx, dir,length,L,k);
1220
1221                         dfdv_damp(s->dfdv, dir,clmd->sim_parms.Cdis);
1222                 }
1223         }
1224         else // calculate force of bending springs
1225         {
1226                 if(length < L)
1227                 {
1228                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1229                         
1230                         k = clmd->sim_parms.bending;    
1231
1232                         mul_fvector_S(bending_force, dir, fbstar(length, L, k, cb));
1233                         VECADD(s->f, s->f, bending_force);
1234
1235                         dfdx_spring_type2(s->dfdx, dir,length,L,k, cb);
1236                 }
1237         }
1238 }
1239
1240 DO_INLINE void cloth_apply_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
1241 {
1242         if(s->flags & CLOTH_SPRING_FLAG_NEEDED)
1243         {
1244                 if(s->type != CLOTH_SPRING_TYPE_BENDING)
1245                 {
1246                         sub_fmatrix_fmatrix(dFdV[s->ij].m, dFdV[s->ij].m, s->dfdv);
1247                         sub_fmatrix_fmatrix(dFdV[s->kl].m, dFdV[s->kl].m, s->dfdv);
1248                         add_fmatrix_fmatrix(dFdV[s->matrix_index].m, dFdV[s->matrix_index].m, s->dfdv); 
1249                 }
1250
1251                 VECADD(lF[s->ij], lF[s->ij], s->f);
1252                 VECSUB(lF[s->kl], lF[s->kl], s->f);
1253
1254                 sub_fmatrix_fmatrix(dFdX[s->ij].m, dFdX[s->ij].m, s->dfdx);
1255                 sub_fmatrix_fmatrix(dFdX[s->kl].m, dFdX[s->kl].m, s->dfdx);
1256
1257                 add_fmatrix_fmatrix(dFdX[s->matrix_index].m, dFdX[s->matrix_index].m, s->dfdx);
1258         }       
1259 }
1260
1261 DO_INLINE void calculateTriangleNormal(float to[3], lfVector *X, MFace mface)
1262 {
1263         float v1[3], v2[3];
1264
1265         VECSUB(v1, X[mface.v2], X[mface.v1]);
1266         VECSUB(v2, X[mface.v3], X[mface.v1]);
1267         cross_fvector(to, v1, v2);
1268 }
1269
1270 DO_INLINE void calculatQuadNormal(float to[3], lfVector *X, MFace mface)
1271 {
1272         float temp = CalcNormFloat4(X[mface.v1],X[mface.v2],X[mface.v3],X[mface.v4],to);
1273         mul_fvector_S(to, to, temp);
1274 }
1275
1276 void calculateWeightedVertexNormal(ClothModifierData *clmd, MFace *mfaces, float to[3], int index, lfVector *X)
1277 {
1278         float temp[3]; 
1279         int i;
1280         Cloth *cloth = clmd->clothObject;
1281
1282         for(i = 0; i < cloth->numfaces; i++)
1283         {
1284                 // check if this triangle contains the selected vertex
1285                 if(mfaces[i].v1 == index || mfaces[i].v2 == index || mfaces[i].v3 == index || mfaces[i].v4 == index)
1286                 {
1287                         calculatQuadNormal(temp, X, mfaces[i]);
1288                         VECADD(to, to, temp);
1289                 }
1290         }
1291 }
1292 float calculateVertexWindForce(float wind[3], float vertexnormal[3])  
1293 {
1294         return fabs(INPR(wind, vertexnormal) * 0.5f);
1295 }
1296
1297 DO_INLINE void calc_triangle_force(ClothModifierData *clmd, MFace mface, lfVector *F, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors)
1298 {       
1299
1300 }
1301
1302 void cloth_calc_force(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVector *lV, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors, float time)
1303 {
1304         /* Collect forces and derivatives:  F,dFdX,dFdV */
1305         Cloth           *cloth          = clmd->clothObject;
1306         unsigned int    i               = 0;
1307         float           spring_air      = clmd->sim_parms.Cvi * 0.01f; /* viscosity of air scaled in percent */
1308         float           gravity[3];
1309         float           tm2[3][3]       = {{-spring_air,0,0}, {0,-spring_air,0},{0,0,-spring_air}};
1310         ClothVertex *verts = cloth->verts;
1311         MFace           *mfaces         = cloth->mfaces;
1312         float wind_normalized[3];
1313         unsigned int numverts = cloth->numverts;
1314         float auxvect[3], velgoal[3], tvect[3];
1315         float kd, ks;
1316         LinkNode *search = cloth->springs;
1317
1318
1319         VECCOPY(gravity, clmd->sim_parms.gravity);
1320         mul_fvector_S(gravity, gravity, 0.001f); /* scale gravity force */
1321
1322         /* set dFdX jacobi matrix to zero */
1323         init_bfmatrix(dFdX, ZERO);
1324         /* set dFdX jacobi matrix diagonal entries to -spring_air */ 
1325         initdiag_bfmatrix(dFdV, tm2);
1326
1327         init_lfvector(lF, gravity, numverts);
1328
1329         submul_lfvectorS(lF, lV, spring_air, numverts);
1330
1331         /* do goal stuff */
1332         if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
1333         {       
1334                 for(i = 0; i < numverts; i++)
1335                 {                       
1336                         if(verts [i].goal < SOFTGOALSNAP)
1337                         {                       
1338                                 // current_position = xold + t * (newposition - xold)
1339                                 VECSUB(tvect, verts[i].xconst, verts[i].xold);
1340                                 mul_fvector_S(tvect, tvect, time);
1341                                 VECADD(tvect, tvect, verts[i].xold);
1342
1343                                 VECSUB(auxvect, tvect, lX[i]);
1344                                 ks  = 1.0f/(1.0f- verts [i].goal*clmd->sim_parms.goalspring)-1.0f ;
1345                                 VECADDS(lF[i], lF[i], auxvect, -ks);
1346
1347                                 // calulate damping forces generated by goals                           
1348                                 VECSUB(velgoal,verts[i].xold, verts[i].xconst);
1349                                 kd =  clmd->sim_parms.goalfrict * 0.01f; // friction force scale taken from SB
1350                                 VECSUBADDSS(lF[i], velgoal, kd, lV[i], kd);
1351                                 
1352                         }
1353                 }       
1354         }
1355
1356         /* handle external forces like wind */
1357         if(effectors)
1358         {
1359                 float speed[3] = {0.0f, 0.0f,0.0f};
1360                 float force[3]= {0.0f, 0.0f, 0.0f};
1361                 
1362                 #pragma omp parallel for private (i) shared(lF)
1363                 for(i = 0; i < cloth->numverts; i++)
1364                 {
1365                         float vertexnormal[3]={0,0,0};
1366                         float fieldfactor = 1000.0f, windfactor  = 250.0f; // from sb
1367                         
1368                         pdDoEffectors(effectors, lX[i], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);          
1369                         
1370                         // TODO apply forcefields here
1371                         VECADDS(lF[i], lF[i], force, fieldfactor*0.01f);
1372
1373                         VECCOPY(wind_normalized, speed);
1374                         Normalize(wind_normalized);
1375                         
1376                         calculateWeightedVertexNormal(clmd, mfaces, vertexnormal, i, lX);
1377                         VECADDS(lF[i], lF[i], wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
1378                 }
1379         }
1380         
1381         // calculate spring forces
1382         search = cloth->springs;
1383         while(search)
1384         {
1385                 // only handle active springs
1386                 // if(((clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED)){}
1387                 cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX);
1388
1389                 search = search->next;
1390         }
1391         
1392         // apply spring forces
1393         search = cloth->springs;
1394         while(search)
1395         {
1396                 // only handle active springs
1397                 // if(((clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms.flags & CSIMSETT_FLAG_TEARING_ENABLED))    
1398                 cloth_apply_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX);
1399                 search = search->next;
1400         }
1401 }
1402
1403 void simulate_implicit_euler(lfVector *Vnew, lfVector *lX, lfVector *lV, lfVector *lF, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, float dt, fmatrix3x3 *A, lfVector *B, lfVector *dV, fmatrix3x3 *S, lfVector *z, lfVector *olddV, fmatrix3x3 *P, fmatrix3x3 *Pinv)
1404 {
1405         unsigned int numverts = dFdV[0].vcount;
1406
1407         lfVector *dFdXmV = create_lfvector(numverts);
1408         initdiag_bfmatrix(A, I);
1409         zero_lfvector(dV, numverts);
1410
1411         subadd_bfmatrixS_bfmatrixS(A, dFdV, dt, dFdX, (dt*dt));   
1412
1413         mul_bfmatrix_lfvector(dFdXmV, dFdX, lV);
1414
1415         add_lfvectorS_lfvectorS(B, lF, dt, dFdXmV, (dt*dt), numverts);
1416         
1417         itstart();
1418         
1419         cg_filtered(dV, A, B, z, S); /* conjugate gradient algorithm to solve Ax=b */
1420         // cg_filtered_pre(dV, A, B, z, olddV, P, Pinv, dt);
1421         
1422         itend();
1423         // printf("cg_filtered calc time: %f\n", (float)itval());
1424         
1425         cp_lfvector(olddV, dV, numverts);
1426
1427         // advance velocities
1428         add_lfvector_lfvector(Vnew, lV, dV, numverts);
1429         
1430
1431         del_lfvector(dFdXmV);
1432 }
1433
1434 int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
1435 {               
1436         unsigned int i=0, j;
1437         float step=0.0f, tf=1.0f;
1438         Cloth *cloth = clmd->clothObject;
1439         ClothVertex *verts = cloth->verts;
1440         unsigned int numverts = cloth->numverts;
1441         float dt = 1.0f / clmd->sim_parms.stepsPerFrame;
1442         Implicit_Data *id = cloth->implicit;
1443         int result = 0;
1444         
1445         if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL) /* do goal stuff */
1446         {
1447                 for(i = 0; i < numverts; i++)
1448                 {                       
1449                         // update velocities with constrained velocities from pinned verts
1450                         if(verts [i].goal >= SOFTGOALSNAP)
1451                         {                       
1452                                 VECSUB(id->V[i], verts[i].xconst, verts[i].xold);
1453                                 // VecMulf(id->V[i], 1.0 / dt);
1454                         }
1455                 }       
1456         }
1457
1458         while(step < tf)
1459         {               
1460                 effectors= pdInitEffectors(ob,NULL);
1461                 
1462                 // calculate 
1463                 cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step );      
1464                 simulate_implicit_euler(id->Vnew, id->X, id->V, id->F, id->dFdV, id->dFdX, dt, id->A, id->B, id->dV, id->S, id->z, id->olddV, id->P, id->Pinv);
1465                 
1466                 add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
1467                 
1468                 if(clmd->coll_parms.flags & CLOTH_COLLISIONSETTINGS_FLAG_ENABLED)
1469                 {
1470                         // collisions 
1471                         // itstart();
1472                         
1473                         // update verts to current positions
1474                         for(i = 0; i < numverts; i++)
1475                         {               
1476                                 if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL) /* do goal stuff */
1477                                 {                       
1478                                         if(verts [i].goal >= SOFTGOALSNAP)
1479                                         {                       
1480                                                 float tvect[3] = {.0,.0,.0};
1481                                                 // VECSUB(tvect, id->Xnew[i], verts[i].xold);
1482                                                 mul_fvector_S(tvect, id->V[i], step+dt);
1483                                                 VECADD(tvect, tvect, verts[i].xold);
1484                                                 VECCOPY(id->Xnew[i], tvect);
1485                                         }
1486                                                 
1487                                 }
1488                                 
1489                                 VECCOPY(verts[i].tx, id->Xnew[i]);
1490                                 
1491                                 VECSUB(verts[i].tv, verts[i].tx, verts[i].txold);
1492                                 VECCOPY(verts[i].v, verts[i].tv);
1493                         }
1494         
1495                         // call collision function
1496                         result = 0; // cloth_bvh_objcollision(clmd, step + dt, dt);
1497         
1498                         // copy corrected positions back to simulation
1499                         for(i = 0; i < numverts; i++)
1500                         {               
1501                                 if(result)
1502                                 {
1503                                         // VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
1504                                         
1505                                         VECCOPY(verts[i].txold, verts[i].tx);
1506                                         
1507                                         VECCOPY(id->Xnew[i], verts[i].tx);
1508                                         
1509                                         VECCOPY(id->Vnew[i], verts[i].tv);
1510                                         VecMulf(id->Vnew[i], 1.0f / dt);
1511                                 }
1512                                 else
1513                                 {
1514                                         VECCOPY(verts[i].txold, id->Xnew[i]);
1515                                 }
1516                         }
1517                         
1518                         // X = Xnew;
1519                         cp_lfvector(id->X, id->Xnew, numverts);
1520                         
1521                         // if there were collisions, advance the velocity from v_n+1/2 to v_n+1
1522                         if(result)
1523                         {
1524                                 // V = Vnew;
1525                                 cp_lfvector(id->V, id->Vnew, numverts);
1526                                 
1527                                 // calculate 
1528                                 cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step);       
1529                                 simulate_implicit_euler(id->Vnew, id->X, id->V, id->F, id->dFdV, id->dFdX, dt / 2.0f, id->A, id->B, id->dV, id->S, id->z, id->olddV, id->P, id->Pinv);
1530                         }
1531                 }
1532                 else
1533                 {
1534                         // X = Xnew;
1535                         cp_lfvector(id->X, id->Xnew, numverts);
1536                 }
1537                 
1538                 // itend();
1539                 // printf("collision time: %f\n", (float)itval());
1540                 
1541                 // V = Vnew;
1542                 cp_lfvector(id->V, id->Vnew, numverts);
1543
1544                 step += dt;
1545
1546                 if(effectors) pdEndEffectors(effectors);
1547         }
1548
1549         for(i = 0; i < numverts; i++)
1550         {                               
1551                 if(clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_GOAL)
1552                 {
1553                         if(verts [i].goal < SOFTGOALSNAP)
1554                         {
1555                                 VECCOPY(verts[i].txold, id->X[i]);
1556                                 VECCOPY(verts[i].x, id->X[i]);
1557                                 VECCOPY(verts[i].v, id->V[i]);
1558                         }
1559                         else
1560                         {
1561                                 VECCOPY(verts[i].txold, verts[i].xconst);
1562                                 VECCOPY(verts[i].x, verts[i].xconst);
1563                                 VECCOPY(verts[i].v, id->V[i]);
1564                         }
1565                 }
1566                 else
1567                 {
1568                         VECCOPY(verts[i].txold, id->X[i]);
1569                         VECCOPY(verts[i].x, id->X[i]);
1570                         VECCOPY(verts[i].v, id->V[i]);
1571                 }
1572         }
1573         return 1;
1574 }
1575
1576 void implicit_set_positions (ClothModifierData *clmd)
1577 {               
1578         Cloth *cloth = clmd->clothObject;
1579         ClothVertex *verts = cloth->verts;
1580         unsigned int numverts = cloth->numverts, i;
1581         Implicit_Data *id = cloth->implicit;
1582         
1583         for(i = 0; i < numverts; i++)
1584         {                               
1585                 VECCOPY(id->X[i], verts[i].x);
1586                 VECCOPY(id->V[i], verts[i].v);
1587         }       
1588 }
1589
1590
1591 int collisions_collision_response_static(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1592 {
1593         /*
1594         unsigned int i = 0;
1595         int result = 0;
1596         LinkNode *search = NULL;
1597         CollPair *collpair = NULL;
1598         Cloth *cloth1, *cloth2;
1599         float w1, w2, w3, u1, u2, u3;
1600         float v1[3], v2[3], relativeVelocity[3];
1601         float magrelVel;
1602         
1603         cloth1 = clmd->clothObject;
1604         cloth2 = coll_clmd->clothObject;
1605
1606         // search = clmd->coll_parms.collision_list;
1607         
1608         while(search)
1609         {
1610         collpair = search->link;
1611                 
1612                 // compute barycentric coordinates for both collision points
1613         collisions_compute_barycentric(collpair->pa,
1614         cloth1->verts[collpair->ap1].txold,
1615         cloth1->verts[collpair->ap2].txold,
1616         cloth1->verts[collpair->ap3].txold, 
1617         &w1, &w2, &w3);
1618         
1619         collisions_compute_barycentric(collpair->pb,
1620         cloth2->verts[collpair->bp1].txold,
1621         cloth2->verts[collpair->bp2].txold,
1622         cloth2->verts[collpair->bp3].txold,
1623         &u1, &u2, &u3);
1624         
1625                 // Calculate relative "velocity".
1626         interpolateOnTriangle(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3);
1627                 
1628         interpolateOnTriangle(v2, cloth2->verts[collpair->bp1].tv, cloth2->verts[collpair->bp2].tv, cloth2->verts[collpair->bp3].tv, u1, u2, u3);
1629                 
1630         VECSUB(relativeVelocity, v1, v2);
1631                         
1632                 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
1633         magrelVel = INPR(relativeVelocity, collpair->normal);
1634                 
1635                 // printf("magrelVel: %f\n", magrelVel);
1636                                 
1637                 // Calculate masses of points.
1638                 
1639                 // If v_n_mag < 0 the edges are approaching each other.
1640         if(magrelVel < -ALMOST_ZERO) 
1641         {
1642                         // Calculate Impulse magnitude to stop all motion in normal direction.
1643                         // const double I_mag = v_n_mag / (1/m1 + 1/m2);
1644         float magnitude_i = magrelVel / 2.0f; // TODO implement masses
1645         float tangential[3], magtangent, magnormal, collvel[3];
1646         float vrel_t_pre[3];
1647         float vrel_t[3];
1648         double impulse;
1649         float epsilon = clmd->coll_parms.epsilon;
1650         float overlap = (epsilon + ALMOST_ZERO-collpair->distance);
1651                         
1652                         // calculateFrictionImpulse(tangential, relativeVelocity, collpair->normal, magrelVel, clmd->coll_parms.friction*0.01, magrelVel);
1653                         
1654                         // magtangent = INPR(tangential, tangential);
1655                         
1656                         // Apply friction impulse.
1657         if (magtangent < -ALMOST_ZERO) 
1658         {
1659                                 
1660                                 // printf("friction applied: %f\n", magtangent);
1661                                 // TODO check original code 
1662 }
1663                         
1664
1665         impulse = -2.0f * magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
1666                         
1667                         // printf("impulse: %f\n", impulse);
1668                         
1669                         // face A
1670         VECADDMUL(cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse); 
1671         cloth1->verts[collpair->ap1].impulse_count++;
1672                         
1673         VECADDMUL(cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse); 
1674         cloth1->verts[collpair->ap2].impulse_count++;
1675                         
1676         VECADDMUL(cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse); 
1677         cloth1->verts[collpair->ap3].impulse_count++;
1678                         
1679                         // face B
1680         VECADDMUL(cloth2->verts[collpair->bp1].impulse, collpair->normal, u1 * impulse); 
1681         cloth2->verts[collpair->bp1].impulse_count++;
1682                         
1683         VECADDMUL(cloth2->verts[collpair->bp2].impulse, collpair->normal, u2 * impulse); 
1684         cloth2->verts[collpair->bp2].impulse_count++;
1685                         
1686         VECADDMUL(cloth2->verts[collpair->bp3].impulse, collpair->normal, u3 * impulse); 
1687         cloth2->verts[collpair->bp3].impulse_count++;
1688                         
1689                         
1690         result = 1;     
1691                 
1692                         // printf("magnitude_i: %f\n", magnitude_i); // negative before collision in my case
1693                         
1694                         // Apply the impulse and increase impulse counters.
1695         
1696                         
1697 }
1698                 
1699         search = search->next;
1700 }
1701         
1702         
1703         return result;
1704         */
1705         return 0;
1706 }
1707
1708
1709 int collisions_collision_response_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1710 {
1711         
1712 }
1713
1714
1715 int collisions_collision_response_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd)
1716 {
1717         
1718 }
1719
1720 void cloth_collision_static(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
1721 {
1722         /*
1723         CollPair *collpair = NULL;
1724         Cloth *cloth1=NULL, *cloth2=NULL;
1725         MFace *face1=NULL, *face2=NULL;
1726         ClothVertex *verts1=NULL, *verts2=NULL;
1727         double distance = 0;
1728         float epsilon = clmd->coll_parms.epsilon;
1729         unsigned int i = 0;
1730
1731         for(i = 0; i < 4; i++)
1732         {
1733         collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");                
1734                 
1735         cloth1 = clmd->clothObject;
1736         cloth2 = coll_clmd->clothObject;
1737                 
1738         verts1 = cloth1->verts;
1739         verts2 = cloth2->verts;
1740         
1741         face1 = &(cloth1->mfaces[tree1->tri_index]);
1742         face2 = &(cloth2->mfaces[tree2->tri_index]);
1743                 
1744                 // check all possible pairs of triangles
1745         if(i == 0)
1746         {
1747         collpair->ap1 = face1->v1;
1748         collpair->ap2 = face1->v2;
1749         collpair->ap3 = face1->v3;
1750                         
1751         collpair->bp1 = face2->v1;
1752         collpair->bp2 = face2->v2;
1753         collpair->bp3 = face2->v3;
1754                         
1755 }
1756                 
1757         if(i == 1)
1758         {
1759         if(face1->v4)
1760         {
1761         collpair->ap1 = face1->v3;
1762         collpair->ap2 = face1->v4;
1763         collpair->ap3 = face1->v1;
1764                                 
1765         collpair->bp1 = face2->v1;
1766         collpair->bp2 = face2->v2;
1767         collpair->bp3 = face2->v3;
1768 }
1769         else
1770         i++;
1771 }
1772                 
1773         if(i == 2)
1774         {
1775         if(face2->v4)
1776         {
1777         collpair->ap1 = face1->v1;
1778         collpair->ap2 = face1->v2;
1779         collpair->ap3 = face1->v3;
1780                                 
1781         collpair->bp1 = face2->v3;
1782         collpair->bp2 = face2->v4;
1783         collpair->bp3 = face2->v1;
1784 }
1785         else
1786         i+=2;
1787 }
1788                 
1789         if(i == 3)
1790         {
1791         if((face1->v4)&&(face2->v4))
1792         {
1793         collpair->ap1 = face1->v3;
1794         collpair->ap2 = face1->v4;
1795         collpair->ap3 = face1->v1;
1796                                 
1797         collpair->bp1 = face2->v3;
1798         collpair->bp2 = face2->v4;
1799         collpair->bp3 = face2->v1;
1800 }
1801         else
1802         i++;
1803 }
1804                 
1805                 // calc SIPcode (?)
1806                 
1807         if(i < 4)
1808         {
1809                         // calc distance + normal       
1810         distance = plNearestPoints(
1811         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);
1812                         
1813         if (distance <= (epsilon + ALMOST_ZERO))
1814         {
1815                                 // printf("dist: %f\n", (float)distance);
1816                                 
1817                                 // collpair->face1 = tree1->tri_index;
1818                                 // collpair->face2 = tree2->tri_index;
1819                                 
1820                                 // VECCOPY(collpair->normal, collpair->vector);
1821                                 // Normalize(collpair->normal);
1822                                 
1823                                 // collpair->distance = distance;
1824                                 
1825 }
1826         else
1827         {
1828         MEM_freeN(collpair);
1829 }
1830 }
1831         else
1832         {
1833         MEM_freeN(collpair);
1834 }
1835 }
1836         */
1837 }
1838
1839 int collisions_are_edges_adjacent(ClothModifierData *clmd, ClothModifierData *coll_clmd, EdgeCollPair *edgecollpair)
1840 {
1841         Cloth *cloth1, *cloth2;
1842         ClothVertex *verts1, *verts2;
1843         float temp[3];
1844          
1845         cloth1 = clmd->clothObject;
1846         cloth2 = coll_clmd->clothObject;
1847         
1848         verts1 = cloth1->verts;
1849         verts2 = cloth2->verts;
1850         
1851         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p21].xold);
1852         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
1853                 return 1;
1854         
1855         VECSUB(temp, verts1[edgecollpair->p11].xold, verts2[edgecollpair->p22].xold);
1856         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
1857                 return 1;
1858         
1859         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p21].xold);
1860         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
1861                 return 1;
1862         
1863         VECSUB(temp, verts1[edgecollpair->p12].xold, verts2[edgecollpair->p22].xold);
1864         if(ABS(INPR(temp, temp)) < ALMOST_ZERO)
1865                 return 1;
1866                 
1867         return 0;
1868 }
1869
1870
1871 void collisions_collision_moving_edges(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
1872 {
1873         /*
1874         EdgeCollPair edgecollpair;
1875         Cloth *cloth1=NULL, *cloth2=NULL;
1876         MFace *face1=NULL, *face2=NULL;
1877         ClothVertex *verts1=NULL, *verts2=NULL;
1878         double distance = 0;
1879         float epsilon = clmd->coll_parms.epsilon;
1880         unsigned int i = 0, j = 0, k = 0;
1881         int numsolutions = 0;
1882         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
1883         
1884         cloth1 = clmd->clothObject;
1885         cloth2 = coll_clmd->clothObject;
1886         
1887         verts1 = cloth1->verts;
1888         verts2 = cloth2->verts;
1889
1890         face1 = &(cloth1->mfaces[tree1->tri_index]);
1891         face2 = &(cloth2->mfaces[tree2->tri_index]);
1892         
1893         for( i = 0; i < 5; i++)
1894         {
1895         if(i == 0) 
1896         {
1897         edgecollpair.p11 = face1->v1;
1898         edgecollpair.p12 = face1->v2;
1899 }
1900         else if(i == 1) 
1901         {
1902         edgecollpair.p11 = face1->v2;
1903         edgecollpair.p12 = face1->v3;
1904 }
1905         else if(i == 2) 
1906         {
1907         if(face1->v4) 
1908         {
1909         edgecollpair.p11 = face1->v3;
1910         edgecollpair.p12 = face1->v4;
1911 }
1912         else 
1913         {
1914         edgecollpair.p11 = face1->v3;
1915         edgecollpair.p12 = face1->v1;
1916         i+=5; // get out of here after this edge pair is handled
1917 }
1918 }
1919         else if(i == 3) 
1920         {
1921         if(face1->v4) 
1922         {
1923         edgecollpair.p11 = face1->v4;
1924         edgecollpair.p12 = face1->v1;
1925 }       
1926         else
1927         continue;
1928 }
1929         else
1930         {
1931         edgecollpair.p11 = face1->v3;
1932         edgecollpair.p12 = face1->v1;
1933 }
1934
1935                 
1936         for( j = 0; j < 5; j++)
1937         {
1938         if(j == 0)
1939         {
1940         edgecollpair.p21 = face2->v1;
1941         edgecollpair.p22 = face2->v2;
1942 }
1943         else if(j == 1)
1944         {
1945         edgecollpair.p21 = face2->v2;
1946         edgecollpair.p22 = face2->v3;
1947 }
1948         else if(j == 2)
1949         {
1950         if(face2->v4) 
1951         {
1952         edgecollpair.p21 = face2->v3;
1953         edgecollpair.p22 = face2->v4;
1954 }
1955         else 
1956         {
1957         edgecollpair.p21 = face2->v3;
1958         edgecollpair.p22 = face2->v1;
1959 }
1960 }
1961         else if(j == 3)
1962         {
1963         if(face2->v4) 
1964         {
1965         edgecollpair.p21 = face2->v4;
1966         edgecollpair.p22 = face2->v1;
1967 }
1968         else
1969         continue;
1970 }
1971         else
1972         {
1973         edgecollpair.p21 = face2->v3;
1974         edgecollpair.p22 = face2->v1;
1975 }
1976                         
1977                         
1978         if(!collisions_are_edges_adjacent(clmd, coll_clmd, &edgecollpair))
1979         {
1980         VECSUB(a, verts1[edgecollpair.p12].xold, verts1[edgecollpair.p11].xold);
1981         VECSUB(b, verts1[edgecollpair.p12].v, verts1[edgecollpair.p11].v);
1982         VECSUB(c, verts1[edgecollpair.p21].xold, verts1[edgecollpair.p11].xold);
1983         VECSUB(d, verts1[edgecollpair.p21].v, verts1[edgecollpair.p11].v);
1984         VECSUB(e, verts2[edgecollpair.p22].xold, verts1[edgecollpair.p11].xold);
1985         VECSUB(f, verts2[edgecollpair.p22].v, verts1[edgecollpair.p11].v);
1986                                 
1987         numsolutions = collisions_get_collision_time(a, b, c, d, e, f, solution);
1988                                 
1989         for (k = 0; k < numsolutions; k++) 
1990         {                                                               
1991         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
1992         {
1993         float out_collisionTime = solution[k];
1994                                                 
1995                                                 // TODO: check for collisions 
1996                                                 
1997                                                 // TODO: put into (edge) collision list
1998                                                 
1999         printf("Moving edge found!\n");
2000 }
2001 }
2002 }
2003 }
2004 }       
2005         */      
2006 }
2007
2008 void collisions_collision_moving_tris(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
2009 {
2010         /*
2011         CollPair collpair;
2012         Cloth *cloth1=NULL, *cloth2=NULL;
2013         MFace *face1=NULL, *face2=NULL;
2014         ClothVertex *verts1=NULL, *verts2=NULL;
2015         double distance = 0;
2016         float epsilon = clmd->coll_parms.epsilon;
2017         unsigned int i = 0, j = 0, k = 0;
2018         int numsolutions = 0;
2019         float a[3], b[3], c[3], d[3], e[3], f[3], solution[3];
2020
2021         for(i = 0; i < 2; i++)
2022         {               
2023         cloth1 = clmd->clothObject;
2024         cloth2 = coll_clmd->clothObject;
2025                 
2026         verts1 = cloth1->verts;
2027         verts2 = cloth2->verts;
2028         
2029         face1 = &(cloth1->mfaces[tree1->tri_index]);
2030         face2 = &(cloth2->mfaces[tree2->tri_index]);
2031                 
2032                 // check all possible pairs of triangles
2033         if(i == 0)
2034         {
2035         collpair.ap1 = face1->v1;
2036         collpair.ap2 = face1->v2;
2037         collpair.ap3 = face1->v3;
2038                         
2039         collpair.pointsb[0] = face2->v1;
2040         collpair.pointsb[1] = face2->v2;
2041         collpair.pointsb[2] = face2->v3;
2042         collpair.pointsb[3] = face2->v4;
2043 }
2044                 
2045         if(i == 1)
2046         {
2047         if(face1->v4)
2048         {
2049         collpair.ap1 = face1->v3;
2050         collpair.ap2 = face1->v4;
2051         collpair.ap3 = face1->v1;
2052                                 
2053         collpair.pointsb[0] = face2->v1;
2054         collpair.pointsb[1] = face2->v2;
2055         collpair.pointsb[2] = face2->v3;
2056         collpair.pointsb[3] = face2->v4;
2057 }
2058         else
2059         i++;
2060 }
2061                 
2062                 // calc SIPcode (?)
2063                 
2064         if(i < 2)
2065         {
2066         VECSUB(a, verts1[collpair.ap2].xold, verts1[collpair.ap1].xold);
2067         VECSUB(b, verts1[collpair.ap2].v, verts1[collpair.ap1].v);
2068         VECSUB(c, verts1[collpair.ap3].xold, verts1[collpair.ap1].xold);
2069         VECSUB(d, verts1[collpair.ap3].v, verts1[collpair.ap1].v);
2070                                 
2071         for(j = 0; j < 4; j++)
2072         {                                       
2073         if((j==3) && !(face2->v4))
2074         break;
2075                                 
2076         VECSUB(e, verts2[collpair.pointsb[j]].xold, verts1[collpair.ap1].xold);
2077         VECSUB(f, verts2[collpair.pointsb[j]].v, verts1[collpair.ap1].v);
2078                                 
2079         numsolutions = collisions_get_collision_time(a, b, c, d, e, f, solution);
2080                                 
2081         for (k = 0; k < numsolutions; k++) 
2082         {                                                               
2083         if ((solution[k] >= 0.0) && (solution[k] <= 1.0)) 
2084         {
2085         float out_collisionTime = solution[k];
2086                                                 
2087                                                 // TODO: check for collisions 
2088                                                 
2089                                                 // TODO: put into (point-face) collision list
2090                                                 
2091         printf("Moving found!\n");
2092                                                 
2093 }
2094 }
2095                                 
2096                                 // TODO: check borders for collisions
2097 }
2098                         
2099 }
2100 }
2101         */
2102 }
2103
2104
2105 // move collision objects forward in time and update static bounding boxes
2106 void collisions_update_collision_objects(float step)
2107 {
2108         Base *base=NULL;
2109         ClothModifierData *coll_clmd=NULL;
2110         Object *coll_ob=NULL;
2111         unsigned int i=0;
2112         
2113         // search all objects for collision object
2114         for (base = G.scene->base.first; base; base = base->next)
2115         {
2116
2117                 coll_ob = base->object;
2118                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
2119                 if (!coll_clmd)
2120                         continue;
2121
2122                 // if collision object go on
2123                 if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
2124                 {
2125                         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
2126                         {
2127                                 Cloth *coll_cloth = coll_clmd->clothObject;
2128                                 BVH *coll_bvh = coll_clmd->clothObject->tree;
2129                                 unsigned int coll_numverts = coll_cloth->numverts;
2130
2131                                 // update position of collision object
2132                                 for(i = 0; i < coll_numverts; i++)
2133                                 {
2134                                         VECCOPY(coll_cloth->verts[i].txold, coll_cloth->verts[i].tx);
2135
2136                                         VECADDS(coll_cloth->verts[i].tx, coll_cloth->verts[i].xold, coll_cloth->verts[i].v, step);
2137                                         
2138                                         // no dt here because of float rounding errors
2139                                         VECSUB(coll_cloth->verts[i].tv, coll_cloth->verts[i].tx, coll_cloth->verts[i].txold);
2140                                 }
2141                                 
2142                                 // update BVH of collision object
2143                                 // bvh_update(coll_clmd, coll_bvh, 0); // 0 means STATIC, 1 means MOVING 
2144                         }
2145                         else
2146                                 printf ("collisions_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
2147                 }
2148         }
2149 }
2150
2151
2152 void collisions_collision_moving(ClothModifierData *clmd, ClothModifierData *coll_clmd, CollisionTree *tree1, CollisionTree *tree2)
2153 {
2154         /*
2155         // TODO: check for adjacent
2156         collisions_collision_moving_edges(clmd, coll_clmd, tree1, tree2);
2157         
2158         collisions_collision_moving_tris(clmd, coll_clmd, tree1, tree2);
2159         collisions_collision_moving_tris(coll_clmd, clmd, tree2, tree1);
2160         */
2161 }
2162
2163 // cloth - object collisions
2164 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, float dt)
2165 {
2166         /*
2167         Base *base=NULL;
2168         ClothModifierData *coll_clmd=NULL;
2169         Cloth *cloth=NULL;
2170         Object *coll_ob=NULL;
2171         BVH *collisions_bvh=NULL;
2172         unsigned int i=0, j = 0, numfaces = 0, numverts = 0;
2173         unsigned int result = 0, ic = 0, rounds = 0; // result counts applied collisions; ic is for debug output; 
2174         ClothVertex *verts = NULL;
2175         float tnull[3] = {0,0,0};
2176         int ret = 0;
2177         LinkNode *collision_list = NULL; 
2178
2179         if ((clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || !(((Cloth *)clmd->clothObject)->tree))
2180         {
2181                 return 0;
2182         }
2183         cloth = clmd->clothObject;
2184         verts = cloth->verts;
2185         collisions_bvh = (BVH *) cloth->tree;
2186         numfaces = clmd->clothObject->numfaces;
2187         numverts = clmd->clothObject->numverts;
2188         
2189         ////////////////////////////////////////////////////////////
2190         // static collisions
2191         ////////////////////////////////////////////////////////////
2192
2193         // update cloth bvh
2194         // bvh_update(clmd, collisions_bvh, 0); // 0 means STATIC, 1 means MOVING (see later in this function)
2195         
2196         // update collision objects
2197         collisions_update_collision_objects(step);
2198         
2199         do
2200         {
2201                 result = 0;
2202                 ic = 0;
2203                 
2204                 // check all collision objects
2205                 for (base = G.scene->base.first; base; base = base->next)
2206                 {
2207                         coll_ob = base->object;
2208                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
2209                         
2210                         if (!coll_clmd)
2211                                 continue;
2212                         
2213                         // if collision object go on
2214                         if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
2215                         {
2216                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
2217                                 {
2218                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
2219                                         
2220                                         // fill collision list 
2221                                         bvh_traverse(collisions_bvh->root, coll_bvh->root, collision_list);
2222                                         
2223                                         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
2224                                         result = 1;
2225                                         for(j = 0; j < 10; j++) // 10 is just a value that ensures convergence
2226                                         {
2227                                                 result = 0;
2228                                                 
2229                                                 // result += collisions_collision_response_static_tris(clmd, coll_clmd, collision_list, 0);
2230                         
2231                                                 // result += collisions_collision_response_static_tris(coll_clmd, clmd, collision_list, 1);
2232                                         
2233                                                 // apply impulses in parallel
2234                                                 ic=0;
2235                                                 for(i = 0; i < numverts; i++)
2236                                                 {
2237                                                         // calculate "velocities" (just xnew = xold + v; no dt in v)
2238                                                         if(verts[i].impulse_count)
2239                                                         {
2240                                                                 VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
2241                                                                 VECCOPY(verts[i].impulse, tnull);
2242                                                                 verts[i].impulse_count = 0;
2243                                 
2244                                                                 ic++;
2245                                                                 ret++;
2246                                                         }
2247                                                 }
2248                                         }
2249                                         
2250                                         // free collision list
2251                                         if(collision_list)
2252                                         {
2253                                                 LinkNode *search = collision_list;
2254                                                 while(search)
2255                                                 {
2256                                                         CollisionPair *coll_pair = search->link;
2257                                                         
2258                                                         MEM_freeN(coll_pair);
2259                                                         search = search->next;
2260                                                 }
2261                                                 BLI_linklist_free(collision_list,NULL);
2262                         
2263                                                 collision_list = NULL;
2264                                         }
2265                                 }
2266                                 else
2267                                         printf ("collisions_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
2268                         }
2269                 }
2270                 
2271                 printf("ic: %d\n", ic);
2272                 rounds++;
2273         }
2274         while(result && (10>rounds));// CLOTH_MAX_THRESHOLD
2275         
2276         printf("\n");
2277                         
2278         ////////////////////////////////////////////////////////////
2279         // update positions
2280         // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
2281         ////////////////////////////////////////////////////////////
2282         
2283         // verts come from clmd
2284         for(i = 0; i < numverts; i++)
2285         {
2286                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
2287         }
2288         ////////////////////////////////////////////////////////////
2289
2290         ////////////////////////////////////////////////////////////
2291         // moving collisions
2292         ////////////////////////////////////////////////////////////
2293
2294         
2295         // update cloth bvh
2296         // bvh_update(clmd, collisions_bvh, 1);  // 0 means STATIC, 1 means MOVING 
2297         
2298         // update moving bvh for collision object once
2299         for (base = G.scene->base.first; base; base = base->next)
2300         {
2301                 
2302                 coll_ob = base->object;
2303                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
2304                 if (!coll_clmd)
2305                         continue;
2306                 
2307                 if(!coll_clmd->clothObject)
2308                         continue;
2309                 
2310                                 // if collision object go on
2311                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
2312                 {
2313                         BVH *coll_bvh = coll_clmd->clothObject->tree;
2314                         
2315                         // bvh_update(coll_clmd, coll_bvh, 1);  // 0 means STATIC, 1 means MOVING       
2316                 }
2317         }
2318         
2319         
2320         do
2321         {
2322                 result = 0;
2323                 ic = 0;
2324                 
2325                 // check all collision objects
2326                 for (base = G.scene->base.first; base; base = base->next)
2327                 {
2328                         coll_ob = base->object;
2329                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
2330                         
2331                         if (!coll_clmd)
2332                                 continue;
2333                         
2334                         // if collision object go on
2335                         if (coll_clmd->sim_parms.flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ)
2336                         {
2337                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
2338                                 {
2339                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
2340                                         
2341                                         bvh_traverse(collisions_bvh->root, coll_bvh->root, collision_list);
2342                                         
2343                                         // process all collisions (calculate impulses, TODO: also repulses if distance too short)
2344                                         result = 1;
2345                                         for(j = 0; j < 10; j++) // 10 is just a value that ensures convergence
2346                                         {
2347                                                 result = 0;
2348                                 
2349                                                 // handle all collision objects
2350                                                 
2351                                                 
2352                                                 if (coll_clmd->clothObject) 
2353                                                 result += collisions_collision_response_moving_tris(clmd, coll_clmd);
2354                                                 else
2355                                                 printf ("collisions_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
2356                                                 
2357                                                 
2358                                                 // apply impulses in parallel
2359                                                 ic=0;
2360                                                 for(i = 0; i < numverts; i++)
2361                                                 {
2362                                                 // calculate "velocities" (just xnew = xold + v; no dt in v)
2363                                                         if(verts[i].impulse_count)
2364                                                         {
2365                                                                 VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
2366                                                                 VECCOPY(verts[i].impulse, tnull);
2367                                                                 verts[i].impulse_count = 0;
2368                                                         
2369                                                                 ic++;
2370                                                                 ret++;
2371                                                         }
2372                                                 }
2373                                         }
2374                 
2375                 
2376                                         // verts come from clmd
2377                                         for(i = 0; i < numverts; i++)
2378                                         {
2379                                                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
2380                                         }
2381                 
2382                                         // update cloth bvh
2383                                         // bvh_update(clmd, collisions_bvh, 1);  // 0 means STATIC, 1 means MOVING 
2384                 
2385                 
2386                                         // free collision list
2387                                         if(collision_list)
2388                                         {
2389                                                 LinkNode *search = collision_list;
2390                                                 while(search)
2391                                                 {
2392                                                         CollisionPair *coll_pair = search->link;
2393                                                         
2394                                                         MEM_freeN(coll_pair);
2395                                                         search = search->next;
2396                                                 }
2397                                                 BLI_linklist_free(collision_list,NULL);
2398                         
2399                                                 collision_list = NULL;
2400                                         }
2401                                 }
2402                                 else
2403                                         printf ("collisions_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
2404                         }
2405                 }
2406                                 
2407                 printf("ic: %d\n", ic);
2408                 rounds++;
2409         }
2410         while(result && (10>rounds)); // CLOTH_MAX_THRESHOLD
2411         
2412         
2413         ////////////////////////////////////////////////////////////
2414         // update positions + velocities
2415         ////////////////////////////////////////////////////////////
2416         
2417         // verts come from clmd
2418         for(i = 0; i < numverts; i++)
2419         {
2420                 VECADD(verts[i].tx, verts[i].txold, verts[i].tv);
2421         }
2422         ////////////////////////////////////////////////////////////
2423
2424         return MIN2(ret, 1);
2425         */
2426 }