Warning fixes for blenkernel and editors.
[blender.git] / source / blender / blenkernel / intern / implicit.c
1 /*  implicit.c      
2
3 *
4 * ***** BEGIN GPL 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.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software Foundation,
18 * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
19 *
20 * The Original Code is Copyright (C) Blender Foundation
21 * All rights reserved.
22 *
23 * The Original Code is: all of this file.
24 *
25 * Contributor(s): none yet.
26 *
27 * ***** END GPL LICENSE BLOCK *****
28 */
29
30 #include "MEM_guardedalloc.h"
31
32 #include "BKE_cloth.h"
33
34 #include "DNA_cloth_types.h"    
35 #include "DNA_scene_types.h"
36
37 #include "BKE_effect.h"
38 #include "BKE_global.h"
39 #include "BKE_cloth.h"
40 #include "BKE_utildefines.h"
41
42 #ifdef _WIN32
43 #include <windows.h>
44 static LARGE_INTEGER _itstart, _itend;
45 static LARGE_INTEGER ifreq;
46 static void itstart(void)
47 {
48         static int first = 1;
49         if(first) {
50                 QueryPerformanceFrequency(&ifreq);
51                 first = 0;
52         }
53         QueryPerformanceCounter(&_itstart);
54 }
55 static void itend(void)
56 {
57         QueryPerformanceCounter(&_itend);
58 }
59 double itval()
60 {
61         return ((double)_itend.QuadPart -
62                         (double)_itstart.QuadPart)/((double)ifreq.QuadPart);
63 }
64 #else
65 #include <sys/time.h>
66 // intrinsics need better compile flag checking
67 // #include <xmmintrin.h>
68 // #include <pmmintrin.h>
69 // #include <pthread.h>
70
71                          static struct timeval _itstart, _itend;
72          static struct timezone itz;
73          void itstart(void)
74 {
75         gettimeofday(&_itstart, &itz);
76 }
77 static void itend(void)
78 {
79         gettimeofday(&_itend,&itz);
80 }
81 double itval()
82 {
83         double t1, t2;
84         t1 =  (double)_itstart.tv_sec + (double)_itstart.tv_usec/(1000*1000);
85         t2 =  (double)_itend.tv_sec + (double)_itend.tv_usec/(1000*1000);
86         return t2-t1;
87 }
88 #endif
89
90 static float I[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
91 static float ZERO[3][3] = {{0,0,0}, {0,0,0}, {0,0,0}};
92
93 /*
94 #define C99
95 #ifdef C99
96 #defineDO_INLINE inline 
97 #else 
98 #defineDO_INLINE static 
99 #endif
100 */
101 struct Cloth;
102
103 //////////////////////////////////////////
104 /* fast vector / matrix library, enhancements are welcome :) -dg */
105 /////////////////////////////////////////
106
107 /* DEFINITIONS */
108 typedef float lfVector[3];
109 typedef struct fmatrix3x3 {
110         float m[3][3]; /* 3x3 matrix */
111         unsigned int c,r; /* column and row number */
112         int pinned; /* is this vertex allowed to move? */
113         float n1,n2,n3; /* three normal vectors for collision constrains */
114         unsigned int vcount; /* vertex count */
115         unsigned int scount; /* spring count */ 
116 } fmatrix3x3;
117
118 ///////////////////////////
119 // float[3] vector
120 ///////////////////////////
121 /* simple vector code */
122 /* STATUS: verified */
123 DO_INLINE void mul_fvector_S(float to[3], float from[3], float scalar)
124 {
125         to[0] = from[0] * scalar;
126         to[1] = from[1] * scalar;
127         to[2] = from[2] * scalar;
128 }
129 /* simple cross product */
130 /* STATUS: verified */
131 DO_INLINE void cross_fvector(float to[3], float vectorA[3], float vectorB[3])
132 {
133         to[0] = vectorA[1] * vectorB[2] - vectorA[2] * vectorB[1];
134         to[1] = vectorA[2] * vectorB[0] - vectorA[0] * vectorB[2];
135         to[2] = vectorA[0] * vectorB[1] - vectorA[1] * vectorB[0];
136 }
137 /* simple v^T * v product ("outer product") */
138 /* STATUS: HAS TO BE verified (*should* work) */
139 DO_INLINE void mul_fvectorT_fvector(float to[3][3], float vectorA[3], float vectorB[3])
140 {
141         mul_fvector_S(to[0], vectorB, vectorA[0]);
142         mul_fvector_S(to[1], vectorB, vectorA[1]);
143         mul_fvector_S(to[2], vectorB, vectorA[2]);
144 }
145 /* simple v^T * v product with scalar ("outer product") */
146 /* STATUS: HAS TO BE verified (*should* work) */
147 DO_INLINE void mul_fvectorT_fvectorS(float to[3][3], float vectorA[3], float vectorB[3], float aS)
148 {       
149         mul_fvectorT_fvector(to, vectorA, vectorB);
150         
151         mul_fvector_S(to[0], to[0], aS);
152         mul_fvector_S(to[1], to[1], aS);
153         mul_fvector_S(to[2], to[2], aS);
154 }
155
156
157 /* printf vector[3] on console: for debug output */
158 static void print_fvector(float m3[3])
159 {
160         printf("%f\n%f\n%f\n\n",m3[0],m3[1],m3[2]);
161 }
162
163 ///////////////////////////
164 // long float vector float (*)[3]
165 ///////////////////////////
166 /* print long vector on console: for debug output */
167 DO_INLINE void print_lfvector(float (*fLongVector)[3], unsigned int verts)
168 {
169         unsigned int i = 0;
170         for(i = 0; i < verts; i++)
171         {
172                 print_fvector(fLongVector[i]);
173         }
174 }
175 /* create long vector */
176 DO_INLINE lfVector *create_lfvector(unsigned int verts)
177 {
178         // TODO: check if memory allocation was successfull */
179         return  (lfVector *)MEM_callocN (verts * sizeof(lfVector), "cloth_implicit_alloc_vector");
180         // return (lfVector *)cloth_aligned_malloc(&MEMORY_BASE, verts * sizeof(lfVector));
181 }
182 /* delete long vector */
183 DO_INLINE void del_lfvector(float (*fLongVector)[3])
184 {
185         if (fLongVector != NULL)
186         {
187                 MEM_freeN (fLongVector);
188                 // cloth_aligned_free(&MEMORY_BASE, fLongVector);
189         }
190 }
191 /* copy long vector */
192 DO_INLINE void cp_lfvector(float (*to)[3], float (*from)[3], unsigned int verts)
193 {
194         memcpy(to, from, verts * sizeof(lfVector));
195 }
196 /* init long vector with float[3] */
197 DO_INLINE void init_lfvector(float (*fLongVector)[3], float vector[3], unsigned int verts)
198 {
199         unsigned int i = 0;
200         for(i = 0; i < verts; i++)
201         {
202                 VECCOPY(fLongVector[i], vector);
203         }
204 }
205 /* zero long vector with float[3] */
206 DO_INLINE void zero_lfvector(float (*to)[3], unsigned int verts)
207 {
208         memset(to, 0.0f, verts * sizeof(lfVector));
209 }
210 /* multiply long vector with scalar*/
211 DO_INLINE void mul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float scalar, unsigned int verts)
212 {
213         unsigned int i = 0;
214
215         for(i = 0; i < verts; i++)
216         {
217                 mul_fvector_S(to[i], fLongVector[i], scalar);
218         }
219 }
220 /* multiply long vector with scalar*/
221 /* A -= B * float */
222 DO_INLINE void submul_lfvectorS(float (*to)[3], float (*fLongVector)[3], float scalar, unsigned int verts)
223 {
224         unsigned int i = 0;
225         for(i = 0; i < verts; i++)
226         {
227                 VECSUBMUL(to[i], fLongVector[i], scalar);
228         }
229 }
230 /* dot product for big vector */
231 DO_INLINE float dot_lfvector(float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
232 {
233         long i = 0;
234         float temp = 0.0;
235 // schedule(guided, 2)
236 #pragma omp parallel for reduction(+: temp)
237         for(i = 0; i < (long)verts; i++)
238         {
239                 temp += INPR(fLongVectorA[i], fLongVectorB[i]);
240         }
241         return temp;
242 }
243 /* A = B + C  --> for big vector */
244 DO_INLINE void add_lfvector_lfvector(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
245 {
246         unsigned int i = 0;
247
248         for(i = 0; i < verts; i++)
249         {
250                 VECADD(to[i], fLongVectorA[i], fLongVectorB[i]);
251         }
252
253 }
254 /* A = B + C * float --> for big vector */
255 DO_INLINE void add_lfvector_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], float bS, unsigned int verts)
256 {
257         unsigned int i = 0;
258
259         for(i = 0; i < verts; i++)
260         {
261                 VECADDS(to[i], fLongVectorA[i], fLongVectorB[i], bS);
262
263         }
264 }
265 /* A = B * float + C * float --> for big vector */
266 DO_INLINE void add_lfvectorS_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float aS, float (*fLongVectorB)[3], float bS, unsigned int verts)
267 {
268         unsigned int i = 0;
269
270         for(i = 0; i < verts; i++)
271         {
272                 VECADDSS(to[i], fLongVectorA[i], aS, fLongVectorB[i], bS);
273         }
274 }
275 /* A = B - C * float --> for big vector */
276 DO_INLINE void sub_lfvector_lfvectorS(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], float bS, unsigned int verts)
277 {
278         unsigned int i = 0;
279         for(i = 0; i < verts; i++)
280         {
281                 VECSUBS(to[i], fLongVectorA[i], fLongVectorB[i], bS);
282         }
283
284 }
285 /* A = B - C --> for big vector */
286 DO_INLINE void sub_lfvector_lfvector(float (*to)[3], float (*fLongVectorA)[3], float (*fLongVectorB)[3], unsigned int verts)
287 {
288         unsigned int i = 0;
289
290         for(i = 0; i < verts; i++)
291         {
292                 VECSUB(to[i], fLongVectorA[i], fLongVectorB[i]);
293         }
294
295 }
296 ///////////////////////////
297 // 3x3 matrix
298 ///////////////////////////
299 /* printf 3x3 matrix on console: for debug output */
300 static void print_fmatrix(float m3[3][3])
301 {
302         printf("%f\t%f\t%f\n",m3[0][0],m3[0][1],m3[0][2]);
303         printf("%f\t%f\t%f\n",m3[1][0],m3[1][1],m3[1][2]);
304         printf("%f\t%f\t%f\n\n",m3[2][0],m3[2][1],m3[2][2]);
305 }
306
307 /* copy 3x3 matrix */
308 DO_INLINE void cp_fmatrix(float to[3][3], float from[3][3])
309 {
310         // memcpy(to, from, sizeof (float) * 9);
311         VECCOPY(to[0], from[0]);
312         VECCOPY(to[1], from[1]);
313         VECCOPY(to[2], from[2]);
314 }
315
316 /* copy 3x3 matrix */
317 DO_INLINE void initdiag_fmatrixS(float to[3][3], float aS)
318 {
319         cp_fmatrix(to, ZERO);
320         
321         to[0][0] = aS;
322         to[1][1] = aS;
323         to[2][2] = aS;
324 }
325
326 /* calculate determinant of 3x3 matrix */
327 DO_INLINE float det_fmatrix(float m[3][3])
328 {
329         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] 
330                         -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];
331 }
332
333 DO_INLINE void inverse_fmatrix(float to[3][3], float from[3][3])
334 {
335         unsigned int i, j;
336         float d;
337
338         if((d=det_fmatrix(from))==0)
339         {
340                 printf("can't build inverse");
341                 exit(0);
342         }
343         for(i=0;i<3;i++) 
344         {
345                 for(j=0;j<3;j++) 
346                 {
347                         int i1=(i+1)%3;
348                         int i2=(i+2)%3;
349                         int j1=(j+1)%3;
350                         int j2=(j+2)%3;
351                         // reverse indexs i&j to take transpose
352                         to[j][i] = (from[i1][j1]*from[i2][j2]-from[i1][j2]*from[i2][j1])/d;
353                         /*
354                         if(i==j)
355                         to[i][j] = 1.0f / from[i][j];
356                         else
357                         to[i][j] = 0;
358                         */
359                 }
360         }
361
362 }
363
364 /* 3x3 matrix multiplied by a scalar */
365 /* STATUS: verified */
366 DO_INLINE void mul_fmatrix_S(float matrix[3][3], float scalar)
367 {
368         mul_fvector_S(matrix[0], matrix[0],scalar);
369         mul_fvector_S(matrix[1], matrix[1],scalar);
370         mul_fvector_S(matrix[2], matrix[2],scalar);
371 }
372
373 /* a vector multiplied by a 3x3 matrix */
374 /* STATUS: verified */
375 DO_INLINE void mul_fvector_fmatrix(float *to, float *from, float matrix[3][3])
376 {
377         to[0] = matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
378         to[1] = matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
379         to[2] = matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
380 }
381
382 /* 3x3 matrix multiplied by a vector */
383 /* STATUS: verified */
384 DO_INLINE void mul_fmatrix_fvector(float *to, float matrix[3][3], float *from)
385 {
386         to[0] = INPR(matrix[0],from);
387         to[1] = INPR(matrix[1],from);
388         to[2] = INPR(matrix[2],from);
389 }
390 /* 3x3 matrix multiplied by a 3x3 matrix */
391 /* STATUS: verified */
392 DO_INLINE void mul_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
393 {
394         mul_fvector_fmatrix(to[0], matrixA[0],matrixB);
395         mul_fvector_fmatrix(to[1], matrixA[1],matrixB);
396         mul_fvector_fmatrix(to[2], matrixA[2],matrixB);
397 }
398 /* 3x3 matrix addition with 3x3 matrix */
399 DO_INLINE void add_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
400 {
401         VECADD(to[0], matrixA[0], matrixB[0]);
402         VECADD(to[1], matrixA[1], matrixB[1]);
403         VECADD(to[2], matrixA[2], matrixB[2]);
404 }
405 /* 3x3 matrix add-addition with 3x3 matrix */
406 DO_INLINE void addadd_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
407 {
408         VECADDADD(to[0], matrixA[0], matrixB[0]);
409         VECADDADD(to[1], matrixA[1], matrixB[1]);
410         VECADDADD(to[2], matrixA[2], matrixB[2]);
411 }
412 /* 3x3 matrix sub-addition with 3x3 matrix */
413 DO_INLINE void addsub_fmatrixS_fmatrixS(float to[3][3], float matrixA[3][3], float aS, float matrixB[3][3], float bS)
414 {
415         VECADDSUBSS(to[0], matrixA[0], aS, matrixB[0], bS);
416         VECADDSUBSS(to[1], matrixA[1], aS, matrixB[1], bS);
417         VECADDSUBSS(to[2], matrixA[2], aS, matrixB[2], bS);
418 }
419 /* A -= B + C (3x3 matrix sub-addition with 3x3 matrix) */
420 DO_INLINE void subadd_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
421 {
422         VECSUBADD(to[0], matrixA[0], matrixB[0]);
423         VECSUBADD(to[1], matrixA[1], matrixB[1]);
424         VECSUBADD(to[2], matrixA[2], matrixB[2]);
425 }
426 /* A -= B*x + C*y (3x3 matrix sub-addition with 3x3 matrix) */
427 DO_INLINE void subadd_fmatrixS_fmatrixS(float to[3][3], float matrixA[3][3], float aS, float matrixB[3][3], float bS)
428 {
429         VECSUBADDSS(to[0], matrixA[0], aS, matrixB[0], bS);
430         VECSUBADDSS(to[1], matrixA[1], aS, matrixB[1], bS);
431         VECSUBADDSS(to[2], matrixA[2], aS, matrixB[2], bS);
432 }
433 /* A = B - C (3x3 matrix subtraction with 3x3 matrix) */
434 DO_INLINE void sub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
435 {
436         VECSUB(to[0], matrixA[0], matrixB[0]);
437         VECSUB(to[1], matrixA[1], matrixB[1]);
438         VECSUB(to[2], matrixA[2], matrixB[2]);
439 }
440 /* A += B - C (3x3 matrix add-subtraction with 3x3 matrix) */
441 DO_INLINE void addsub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
442 {
443         VECADDSUB(to[0], matrixA[0], matrixB[0]);
444         VECADDSUB(to[1], matrixA[1], matrixB[1]);
445         VECADDSUB(to[2], matrixA[2], matrixB[2]);
446 }
447 /////////////////////////////////////////////////////////////////
448 // special functions
449 /////////////////////////////////////////////////////////////////
450 /* a vector multiplied and added to/by a 3x3 matrix */
451 DO_INLINE void muladd_fvector_fmatrix(float to[3], float from[3], float matrix[3][3])
452 {
453         to[0] += matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
454         to[1] += matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
455         to[2] += matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
456 }
457 /* 3x3 matrix multiplied and added  to/by a 3x3 matrix  and added to another 3x3 matrix */
458 DO_INLINE void muladd_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
459 {
460         muladd_fvector_fmatrix(to[0], matrixA[0],matrixB);
461         muladd_fvector_fmatrix(to[1], matrixA[1],matrixB);
462         muladd_fvector_fmatrix(to[2], matrixA[2],matrixB);
463 }
464 /* a vector multiplied and sub'd to/by a 3x3 matrix */
465 DO_INLINE void mulsub_fvector_fmatrix(float to[3], float from[3], float matrix[3][3])
466 {
467         to[0] -= matrix[0][0]*from[0] + matrix[1][0]*from[1] + matrix[2][0]*from[2];
468         to[1] -= matrix[0][1]*from[0] + matrix[1][1]*from[1] + matrix[2][1]*from[2];
469         to[2] -= matrix[0][2]*from[0] + matrix[1][2]*from[1] + matrix[2][2]*from[2];
470 }
471 /* 3x3 matrix multiplied and sub'd  to/by a 3x3 matrix  and added to another 3x3 matrix */
472 DO_INLINE void mulsub_fmatrix_fmatrix(float to[3][3], float matrixA[3][3], float matrixB[3][3])
473 {
474         mulsub_fvector_fmatrix(to[0], matrixA[0],matrixB);
475         mulsub_fvector_fmatrix(to[1], matrixA[1],matrixB);
476         mulsub_fvector_fmatrix(to[2], matrixA[2],matrixB);
477 }
478 /* 3x3 matrix multiplied+added by a vector */
479 /* STATUS: verified */
480 DO_INLINE void muladd_fmatrix_fvector(float to[3], float matrix[3][3], float from[3])
481 {
482         to[0] += INPR(matrix[0],from);
483         to[1] += INPR(matrix[1],from);
484         to[2] += INPR(matrix[2],from);  
485 }
486 /* 3x3 matrix multiplied+sub'ed by a vector */
487 DO_INLINE void mulsub_fmatrix_fvector(float to[3], float matrix[3][3], float from[3])
488 {
489         to[0] -= INPR(matrix[0],from);
490         to[1] -= INPR(matrix[1],from);
491         to[2] -= INPR(matrix[2],from);
492 }
493 /////////////////////////////////////////////////////////////////
494
495 ///////////////////////////
496 // SPARSE SYMMETRIC big matrix with 3x3 matrix entries
497 ///////////////////////////
498 /* printf a big matrix on console: for debug output */
499 #if 0
500 static void print_bfmatrix(fmatrix3x3 *m3)
501 {
502         unsigned int i = 0;
503
504         for(i = 0; i < m3[0].vcount + m3[0].scount; i++)
505         {
506                 print_fmatrix(m3[i].m);
507         }
508 }
509 #endif
510
511 /* create big matrix */
512 DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs)
513 {
514         // TODO: check if memory allocation was successfull */
515         fmatrix3x3 *temp = (fmatrix3x3 *)MEM_callocN (sizeof (fmatrix3x3) * (verts + springs), "cloth_implicit_alloc_matrix");
516         temp[0].vcount = verts;
517         temp[0].scount = springs;
518         return temp;
519 }
520 /* delete big matrix */
521 DO_INLINE void del_bfmatrix(fmatrix3x3 *matrix)
522 {
523         if (matrix != NULL)
524         {
525                 MEM_freeN (matrix);
526         }
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
536 /* init big matrix */
537 // slow in parallel
538 DO_INLINE void init_bfmatrix(fmatrix3x3 *matrix, float m3[3][3])
539 {
540         unsigned int i;
541
542         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
543         {               
544                 cp_fmatrix(matrix[i].m, m3); 
545         }
546 }
547
548 /* init the diagonal of big matrix */
549 // slow in parallel
550 DO_INLINE void initdiag_bfmatrix(fmatrix3x3 *matrix, float m3[3][3])
551 {
552         unsigned int i,j;
553         float tmatrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
554
555         for(i = 0; i < matrix[0].vcount; i++)
556         {               
557                 cp_fmatrix(matrix[i].m, m3); 
558         }
559         for(j = matrix[0].vcount; j < matrix[0].vcount+matrix[0].scount; j++)
560         {
561                 cp_fmatrix(matrix[j].m, tmatrix); 
562         }
563 }
564
565 /* multiply big matrix with scalar*/
566 DO_INLINE void mul_bfmatrix_S(fmatrix3x3 *matrix, float scalar)
567 {
568         unsigned int i = 0;
569         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
570         {
571                 mul_fmatrix_S(matrix[i].m, scalar);
572         }
573 }
574
575 /* SPARSE SYMMETRIC multiply big matrix with long vector*/
576 /* STATUS: verified */
577 DO_INLINE void mul_bfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector *fLongVector)
578 {
579         unsigned int i = 0;
580         lfVector *temp = create_lfvector(from[0].vcount);
581         
582         zero_lfvector(to, from[0].vcount);
583
584 #pragma omp parallel sections private(i)
585         {
586 #pragma omp section
587                 {
588                         for(i = from[0].vcount; i < from[0].vcount+from[0].scount; i++)
589                         {
590                                 muladd_fmatrix_fvector(to[from[i].c], from[i].m, fLongVector[from[i].r]);
591                         }
592                 }       
593 #pragma omp section
594                 {
595                         for(i = 0; i < from[0].vcount+from[0].scount; i++)
596                         {
597                                 muladd_fmatrix_fvector(temp[from[i].r], from[i].m, fLongVector[from[i].c]);
598                         }
599                 }
600         }
601         add_lfvector_lfvector(to, to, temp, from[0].vcount);
602         
603         del_lfvector(temp);
604         
605         
606 }
607
608 /* SPARSE SYMMETRIC multiply big matrix with long vector (for diagonal preconditioner) */
609 /* STATUS: verified */
610 DO_INLINE void mul_prevfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector *fLongVector)
611 {
612         unsigned int i = 0;
613         
614         for(i = 0; i < from[0].vcount; i++)
615         {
616                 mul_fmatrix_fvector(to[from[i].r], from[i].m, fLongVector[from[i].c]);
617         }
618 }
619
620 /* SPARSE SYMMETRIC add big matrix with big matrix: A = B + C*/
621 DO_INLINE void add_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
622 {
623         unsigned int i = 0;
624
625         /* process diagonal elements */
626         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
627         {
628                 add_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
629         }
630
631 }
632 /* SPARSE SYMMETRIC add big matrix with big matrix: A += B + C */
633 DO_INLINE void addadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
634 {
635         unsigned int i = 0;
636
637         /* process diagonal elements */
638         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
639         {
640                 addadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
641         }
642
643 }
644 /* SPARSE SYMMETRIC subadd big matrix with big matrix: A -= B + C */
645 DO_INLINE void subadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
646 {
647         unsigned int i = 0;
648
649         /* process diagonal elements */
650         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
651         {
652                 subadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
653         }
654
655 }
656 /*  A = B - C (SPARSE SYMMETRIC sub big matrix with big matrix) */
657 DO_INLINE void sub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
658 {
659         unsigned int i = 0;
660
661         /* process diagonal elements */
662         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
663         {
664                 sub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
665         }
666
667 }
668 /* SPARSE SYMMETRIC sub big matrix with big matrix S (special constraint matrix with limited entries) */
669 DO_INLINE void sub_bfmatrix_Smatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
670 {
671         unsigned int i = 0;
672
673         /* process diagonal elements */
674         for(i = 0; i < matrix[0].vcount; i++)
675         {
676                 sub_fmatrix_fmatrix(to[matrix[i].c].m, from[matrix[i].c].m, matrix[i].m);       
677         }
678
679 }
680 /* A += B - C (SPARSE SYMMETRIC addsub big matrix with big matrix) */
681 DO_INLINE void addsub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
682 {
683         unsigned int i = 0;
684
685         /* process diagonal elements */
686         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
687         {
688                 addsub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
689         }
690
691 }
692 /* SPARSE SYMMETRIC sub big matrix with big matrix*/
693 /* A -= B * float + C * float --> for big matrix */
694 /* VERIFIED */
695 DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, float aS,  fmatrix3x3 *matrix, float bS)
696 {
697         unsigned int i = 0;
698
699         /* process diagonal elements */
700         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
701         {
702                 subadd_fmatrixS_fmatrixS(to[i].m, from[i].m, aS, matrix[i].m, bS);      
703         }
704
705 }
706
707 ///////////////////////////////////////////////////////////////////
708 // simulator start
709 ///////////////////////////////////////////////////////////////////
710 typedef struct Implicit_Data 
711 {
712         lfVector *X, *V, *Xnew, *Vnew, *olddV, *F, *B, *dV, *z;
713         fmatrix3x3 *A, *dFdV, *dFdX, *S, *P, *Pinv, *bigI, *M; 
714 } Implicit_Data;
715
716 int implicit_init (Object *ob, ClothModifierData *clmd)
717 {
718         unsigned int i = 0;
719         unsigned int pinned = 0;
720         Cloth *cloth = NULL;
721         ClothVertex *verts = NULL;
722         ClothSpring *spring = NULL;
723         Implicit_Data *id = NULL;
724         LinkNode *search = NULL;
725         
726         if(G.rt > 0)
727                 printf("implicit_init\n");
728
729         // init memory guard
730         // MEMORY_BASE.first = MEMORY_BASE.last = NULL;
731
732         cloth = (Cloth *)clmd->clothObject;
733         verts = cloth->verts;
734
735         // create implicit base
736         id = (Implicit_Data *)MEM_callocN (sizeof(Implicit_Data), "implicit vecmat");
737         cloth->implicit = id;
738
739         /* process diagonal elements */         
740         id->A = create_bfmatrix(cloth->numverts, cloth->numsprings);
741         id->dFdV = create_bfmatrix(cloth->numverts, cloth->numsprings);
742         id->dFdX = create_bfmatrix(cloth->numverts, cloth->numsprings);
743         id->S = create_bfmatrix(cloth->numverts, 0);
744         id->Pinv = create_bfmatrix(cloth->numverts, cloth->numsprings);
745         id->P = create_bfmatrix(cloth->numverts, cloth->numsprings);
746         id->bigI = create_bfmatrix(cloth->numverts, cloth->numsprings); // TODO 0 springs
747         id->M = create_bfmatrix(cloth->numverts, cloth->numsprings);
748         id->X = create_lfvector(cloth->numverts);
749         id->Xnew = create_lfvector(cloth->numverts);
750         id->V = create_lfvector(cloth->numverts);
751         id->Vnew = create_lfvector(cloth->numverts);
752         id->olddV = create_lfvector(cloth->numverts);
753         zero_lfvector(id->olddV, cloth->numverts);
754         id->F = create_lfvector(cloth->numverts);
755         id->B = create_lfvector(cloth->numverts);
756         id->dV = create_lfvector(cloth->numverts);
757         id->z = create_lfvector(cloth->numverts);
758         
759         for(i=0;i<cloth->numverts;i++) 
760         {
761                 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 = id->M[i].r = id->M[i].c = i;
762
763                 if(verts [i].flags & CLOTH_VERT_FLAG_PINNED)
764                 {
765                         id->S[pinned].pinned = 1;
766                         id->S[pinned].c = id->S[pinned].r = i;
767                         pinned++;
768                 }
769                 
770                 initdiag_fmatrixS(id->M[i].m, verts[i].mass);
771         }
772
773         // S is special and needs specific vcount and scount
774         id->S[0].vcount = pinned; id->S[0].scount = 0;
775
776         // init springs 
777         search = cloth->springs;
778         for(i=0;i<cloth->numsprings;i++) 
779         {
780                 spring = search->link;
781                 
782                 // dFdV_start[i].r = big_I[i].r = big_zero[i].r = 
783                 id->A[i+cloth->numverts].r = id->dFdV[i+cloth->numverts].r = id->dFdX[i+cloth->numverts].r = 
784                                 id->P[i+cloth->numverts].r = id->Pinv[i+cloth->numverts].r = id->bigI[i+cloth->numverts].r = id->M[i+cloth->numverts].r = spring->ij;
785
786                 // dFdV_start[i].c = big_I[i].c = big_zero[i].c = 
787                 id->A[i+cloth->numverts].c = id->dFdV[i+cloth->numverts].c = id->dFdX[i+cloth->numverts].c = 
788                                 id->P[i+cloth->numverts].c = id->Pinv[i+cloth->numverts].c = id->bigI[i+cloth->numverts].c = id->M[i+cloth->numverts].c = spring->kl;
789
790                 spring->matrix_index = i + cloth->numverts;
791                 
792                 search = search->next;
793         }
794         
795         initdiag_bfmatrix(id->bigI, I);
796
797         for(i = 0; i < cloth->numverts; i++)
798         {               
799                 VECCOPY(id->X[i], verts[i].x);
800         }
801
802         return 1;
803 }
804 int     implicit_free (ClothModifierData *clmd)
805 {
806         Implicit_Data *id;
807         Cloth *cloth;
808         cloth = (Cloth *)clmd->clothObject;
809
810         if(cloth)
811         {
812                 id = cloth->implicit;
813
814                 if(id)
815                 {
816                         del_bfmatrix(id->A);
817                         del_bfmatrix(id->dFdV);
818                         del_bfmatrix(id->dFdX);
819                         del_bfmatrix(id->S);
820                         del_bfmatrix(id->P);
821                         del_bfmatrix(id->Pinv);
822                         del_bfmatrix(id->bigI);
823                         del_bfmatrix(id->M);
824
825                         del_lfvector(id->X);
826                         del_lfvector(id->Xnew);
827                         del_lfvector(id->V);
828                         del_lfvector(id->Vnew);
829                         del_lfvector(id->olddV);
830                         del_lfvector(id->F);
831                         del_lfvector(id->B);
832                         del_lfvector(id->dV);
833                         del_lfvector(id->z);
834
835                         MEM_freeN(id);
836                 }
837         }
838
839         return 1;
840 }
841
842 DO_INLINE float fb(float length, float L)
843 {
844         float x = length/L;
845         return (-11.541f*pow(x,4)+34.193f*pow(x,3)-39.083f*pow(x,2)+23.116f*x-9.713f);
846 }
847
848 DO_INLINE float fbderiv(float length, float L)
849 {
850         float x = length/L;
851
852         return (-46.164f*pow(x,3)+102.579f*pow(x,2)-78.166f*x+23.116f);
853 }
854
855 DO_INLINE float fbstar(float length, float L, float kb, float cb)
856 {
857         float tempfb = kb * fb(length, L);
858
859         float fbstar = cb * (length - L);
860         
861         if(tempfb < fbstar)
862                 return fbstar;
863         else
864                 return tempfb;          
865 }
866
867 // function to calculae bending spring force (taken from Choi & Co)
868 DO_INLINE float fbstar_jacobi(float length, float L, float kb, float cb)
869 {
870         float tempfb = kb * fb(length, L);
871         float fbstar = cb * (length - L);
872
873         if(tempfb < fbstar)
874         {               
875                 return cb;
876         }
877         else
878         {
879                 return kb * fbderiv(length, L); 
880         }       
881 }
882
883 DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
884 {
885         unsigned int i=0;
886
887         for(i=0;i<S[0].vcount;i++)
888         {
889                 mul_fvector_fmatrix(V[S[i].r], V[S[i].r], S[i].m);
890         }
891 }
892
893 static int  cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S)
894 {
895         // Solves for unknown X in equation AX=B
896         unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
897         float conjgrad_epsilon=0.0001f, conjgrad_lasterror=0;
898         lfVector *q, *d, *tmp, *r; 
899         float s, starget, a, s_prev;
900         unsigned int numverts = lA[0].vcount;
901         q = create_lfvector(numverts);
902         d = create_lfvector(numverts);
903         tmp = create_lfvector(numverts);
904         r = create_lfvector(numverts);
905
906         // zero_lfvector(ldV, CLOTHPARTICLES);
907         filter(ldV, S);
908
909         add_lfvector_lfvector(ldV, ldV, z, numverts);
910
911         // r = B - Mul(tmp,A,X);    // just use B if X known to be zero
912         cp_lfvector(r, lB, numverts);
913         mul_bfmatrix_lfvector(tmp, lA, ldV);
914         sub_lfvector_lfvector(r, r, tmp, numverts);
915
916         filter(r,S);
917
918         cp_lfvector(d, r, numverts);
919
920         s = dot_lfvector(r, r, numverts);
921         starget = s * sqrt(conjgrad_epsilon);
922
923         while((s>starget && conjgrad_loopcount < conjgrad_looplimit))
924         {       
925                 // Mul(q,A,d); // q = A*d;
926                 mul_bfmatrix_lfvector(q, lA, d);
927
928                 filter(q,S);
929
930                 a = s/dot_lfvector(d, q, numverts);
931
932                 // X = X + d*a;
933                 add_lfvector_lfvectorS(ldV, ldV, d, a, numverts);
934
935                 // r = r - q*a;
936                 sub_lfvector_lfvectorS(r, r, q, a, numverts);
937
938                 s_prev = s;
939                 s = dot_lfvector(r, r, numverts);
940
941                 //d = r+d*(s/s_prev);
942                 add_lfvector_lfvectorS(d, r, d, (s/s_prev), numverts);
943
944                 filter(d,S);
945
946                 conjgrad_loopcount++;
947         }
948         conjgrad_lasterror = s;
949
950         del_lfvector(q);
951         del_lfvector(d);
952         del_lfvector(tmp);
953         del_lfvector(r);
954         // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount);
955
956         return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
957 }
958
959 // block diagonalizer
960 DO_INLINE void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv)
961 {
962         unsigned int i = 0;
963         
964         // Take only the diagonal blocks of A
965 // #pragma omp parallel for private(i)
966         for(i = 0; i<lA[0].vcount; i++)
967         {
968                 // block diagonalizer
969                 cp_fmatrix(P[i].m, lA[i].m);
970                 inverse_fmatrix(Pinv[i].m, P[i].m);
971                 
972         }
973 }
974 /*
975 // version 1.3
976 static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
977 {
978         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
979         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0;
980         float conjgrad_epsilon=0.0001; // 0.2 is dt for steps=5
981         lfVector *r = create_lfvector(numverts);
982         lfVector *p = create_lfvector(numverts);
983         lfVector *s = create_lfvector(numverts);
984         lfVector *h = create_lfvector(numverts);
985         
986         BuildPPinv(lA, P, Pinv);
987         
988         filter(dv, S);
989         add_lfvector_lfvector(dv, dv, z, numverts);
990         
991         mul_bfmatrix_lfvector(r, lA, dv);
992         sub_lfvector_lfvector(r, lB, r, numverts);
993         filter(r, S);
994         
995         mul_prevfmatrix_lfvector(p, Pinv, r);
996         filter(p, S);
997         
998         deltaNew = dot_lfvector(r, p, numverts);
999         
1000         delta0 = deltaNew * sqrt(conjgrad_epsilon);
1001         
1002         // itstart();
1003         
1004         while ((deltaNew > delta0) && (iterations < conjgrad_looplimit))
1005         {
1006                 iterations++;
1007                 
1008                 mul_bfmatrix_lfvector(s, lA, p);
1009                 filter(s, S);
1010                 
1011                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1012                 
1013                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1014                 
1015                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
1016                 
1017                 mul_prevfmatrix_lfvector(h, Pinv, r);
1018                 filter(h, S);
1019                 
1020                 deltaOld = deltaNew;
1021                 
1022                 deltaNew = dot_lfvector(r, h, numverts);
1023                 
1024                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1025                 
1026                 filter(p, S);
1027                 
1028         }
1029         
1030         // itend();
1031         // printf("cg_filtered_pre time: %f\n", (float)itval());
1032         
1033         del_lfvector(h);
1034         del_lfvector(s);
1035         del_lfvector(p);
1036         del_lfvector(r);
1037         
1038         printf("iterations: %d\n", iterations);
1039         
1040         return iterations<conjgrad_looplimit;
1041 }
1042 */
1043 // version 1.4
1044 static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *bigI)
1045 {
1046         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
1047         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0, tol = 0;
1048         lfVector *r = create_lfvector(numverts);
1049         lfVector *p = create_lfvector(numverts);
1050         lfVector *s = create_lfvector(numverts);
1051         lfVector *h = create_lfvector(numverts);
1052         lfVector *bhat = create_lfvector(numverts);
1053         lfVector *btemp = create_lfvector(numverts);
1054         
1055         BuildPPinv(lA, P, Pinv);
1056         
1057         initdiag_bfmatrix(bigI, I);
1058         sub_bfmatrix_Smatrix(bigI, bigI, S);
1059         
1060         // x = Sx_0+(I-S)z
1061         filter(dv, S);
1062         add_lfvector_lfvector(dv, dv, z, numverts);
1063         
1064         // b_hat = S(b-A(I-S)z)
1065         mul_bfmatrix_lfvector(r, lA, z);
1066         mul_bfmatrix_lfvector(bhat, bigI, r);
1067         sub_lfvector_lfvector(bhat, lB, bhat, numverts);
1068         
1069         // r = S(b-Ax)
1070         mul_bfmatrix_lfvector(r, lA, dv);
1071         sub_lfvector_lfvector(r, lB, r, numverts);
1072         filter(r, S);
1073         
1074         // p = SP^-1r
1075         mul_prevfmatrix_lfvector(p, Pinv, r);
1076         filter(p, S);
1077         
1078         // delta0 = bhat^TP^-1bhat
1079         mul_prevfmatrix_lfvector(btemp, Pinv, bhat);
1080         delta0 = dot_lfvector(bhat, btemp, numverts);
1081         
1082         // deltaNew = r^TP
1083         deltaNew = dot_lfvector(r, p, numverts);
1084         
1085         /*
1086         filter(dv, S);
1087         add_lfvector_lfvector(dv, dv, z, numverts);
1088         
1089         mul_bfmatrix_lfvector(r, lA, dv);
1090         sub_lfvector_lfvector(r, lB, r, numverts);
1091         filter(r, S);
1092         
1093         mul_prevfmatrix_lfvector(p, Pinv, r);
1094         filter(p, S);
1095         
1096         deltaNew = dot_lfvector(r, p, numverts);
1097         
1098         delta0 = deltaNew * sqrt(conjgrad_epsilon);
1099         */
1100         
1101         // itstart();
1102         
1103         tol = (0.01*0.2);
1104         
1105         while ((deltaNew > delta0*tol*tol) && (iterations < conjgrad_looplimit))
1106         {
1107                 iterations++;
1108                 
1109                 mul_bfmatrix_lfvector(s, lA, p);
1110                 filter(s, S);
1111                 
1112                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1113                 
1114                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1115                 
1116                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
1117                 
1118                 mul_prevfmatrix_lfvector(h, Pinv, r);
1119                 filter(h, S);
1120                 
1121                 deltaOld = deltaNew;
1122                 
1123                 deltaNew = dot_lfvector(r, h, numverts);
1124                 
1125                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1126                 
1127                 filter(p, S);
1128                 
1129         }
1130         
1131         // itend();
1132         // printf("cg_filtered_pre time: %f\n", (float)itval());
1133         
1134         del_lfvector(btemp);
1135         del_lfvector(bhat);
1136         del_lfvector(h);
1137         del_lfvector(s);
1138         del_lfvector(p);
1139         del_lfvector(r);
1140         
1141         // printf("iterations: %d\n", iterations);
1142         
1143         return iterations<conjgrad_looplimit;
1144 }
1145
1146 // outer product is NOT cross product!!!
1147 DO_INLINE void dfdx_spring_type1(float to[3][3], float extent[3], float length, float L, float dot, float k)
1148 {
1149         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1150         // return  (outerprod(dir,dir)*k + (I - outerprod(dir,dir))*(k - ((k*L)/length)));
1151         float temp[3][3];
1152         float temp1 = k*(1.0 - (L/length));     
1153         
1154         mul_fvectorT_fvectorS(temp, extent, extent, 1.0 / dot);
1155         sub_fmatrix_fmatrix(to, I, temp);
1156         mul_fmatrix_S(to, temp1);
1157         
1158         mul_fvectorT_fvectorS(temp, extent, extent, k/ dot);
1159         add_fmatrix_fmatrix(to, to, temp);
1160         
1161         /*
1162         mul_fvectorT_fvector(temp, dir, dir);
1163         sub_fmatrix_fmatrix(to, I, temp);
1164         mul_fmatrix_S(to, k* (1.0f-(L/length)));
1165         mul_fmatrix_S(temp, k);
1166         add_fmatrix_fmatrix(to, temp, to);
1167         */
1168 }
1169
1170 DO_INLINE void dfdx_spring_type2(float to[3][3], float dir[3], float length, float L, float k, float cb)
1171 {
1172         // return  outerprod(dir,dir)*fbstar_jacobi(length, L, k, cb);
1173         mul_fvectorT_fvectorS(to, dir, dir, fbstar_jacobi(length, L, k, cb));
1174 }
1175
1176 DO_INLINE void dfdv_damp(float to[3][3], float dir[3], float damping)
1177 {
1178         // derivative of force wrt velocity.  
1179         mul_fvectorT_fvectorS(to, dir, dir, damping);
1180         
1181 }
1182
1183 DO_INLINE void dfdx_spring(float to[3][3],  float dir[3],float length,float L,float k)
1184 {
1185         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1186         //return  ( (I-outerprod(dir,dir))*Min(1.0f,rest/length) - I) * -k;
1187         mul_fvectorT_fvector(to, dir, dir);
1188         sub_fmatrix_fmatrix(to, I, to);
1189
1190         mul_fmatrix_S(to, (L/length)); 
1191         sub_fmatrix_fmatrix(to, to, I);
1192         mul_fmatrix_S(to, -k);
1193 }
1194
1195 // unused atm
1196 DO_INLINE void dfdx_damp(float to[3][3],  float dir[3],float length,const float vel[3],float rest,float damping)
1197 {
1198         // inner spring damping   vel is the relative velocity  of the endpoints.  
1199         //      return (I-outerprod(dir,dir)) * (-damping * -(dot(dir,vel)/Max(length,rest)));
1200         mul_fvectorT_fvector(to, dir, dir);
1201         sub_fmatrix_fmatrix(to, I, to);
1202         mul_fmatrix_S(to,  (-damping * -(INPR(dir,vel)/MAX2(length,rest)))); 
1203
1204 }
1205
1206 DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, float time)
1207 {
1208         Cloth *cloth = clmd->clothObject;
1209         ClothVertex *verts = cloth->verts;
1210         float extent[3];
1211         float length = 0, dot = 0;
1212         float dir[3] = {0,0,0};
1213         float vel[3];
1214         float k = 0.0f;
1215         float L = s->restlen;
1216         float cb = clmd->sim_parms->structural;
1217
1218         float nullf[3] = {0,0,0};
1219         float stretch_force[3] = {0,0,0};
1220         float bending_force[3] = {0,0,0};
1221         float damping_force[3] = {0,0,0};
1222         float nulldfdx[3][3]={ {0,0,0}, {0,0,0}, {0,0,0}};
1223         
1224         float scaling = 0.0;
1225
1226         int no_compress = clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_NO_SPRING_COMPRESS;
1227         
1228         VECCOPY(s->f, nullf);
1229         cp_fmatrix(s->dfdx, nulldfdx);
1230         cp_fmatrix(s->dfdv, nulldfdx);
1231
1232         // calculate elonglation
1233         VECSUB(extent, X[s->kl], X[s->ij]);
1234         VECSUB(vel, V[s->kl], V[s->ij]);
1235         dot = INPR(extent, extent);
1236         length = sqrt(dot);
1237         
1238         s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
1239         
1240         if(length > ALMOST_ZERO)
1241         {
1242                 /*
1243                 if(length>L)
1244                 {
1245                 if((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) 
1246                 && ((((length-L)*100.0f/L) > clmd->sim_parms->maxspringlen))) // cut spring!
1247                 {
1248                 s->flags |= CSPRING_FLAG_DEACTIVATE;
1249                 return;
1250         }
1251         } 
1252                 */
1253                 mul_fvector_S(dir, extent, 1.0f/length);
1254         }
1255         else    
1256         {
1257                 mul_fvector_S(dir, extent, 0.0f);
1258         }
1259         
1260         // calculate force of structural + shear springs
1261         if((s->type & CLOTH_SPRING_TYPE_STRUCTURAL) || (s->type & CLOTH_SPRING_TYPE_SHEAR))
1262         {
1263                 if(length > L || no_compress)
1264                 {
1265                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1266                         
1267                         k = clmd->sim_parms->structural;
1268                                 
1269                         scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
1270                         
1271                         k = scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON);
1272                         
1273                         // TODO: verify, half verified (couldn't see error)
1274                         mul_fvector_S(stretch_force, dir, k*(length-L)); 
1275
1276                         VECADD(s->f, s->f, stretch_force);
1277
1278                         // Ascher & Boxman, p.21: Damping only during elonglation
1279                         // something wrong with it...
1280                         mul_fvector_S(damping_force, dir, clmd->sim_parms->Cdis * INPR(vel,dir));
1281                         VECADD(s->f, s->f, damping_force);
1282                         
1283                         /* VERIFIED */
1284                         dfdx_spring(s->dfdx, dir, length, L, k);
1285                         
1286                         /* VERIFIED */
1287                         dfdv_damp(s->dfdv, dir, clmd->sim_parms->Cdis);
1288                         
1289                 }
1290         }
1291         else if(s->type & CLOTH_SPRING_TYPE_GOAL)
1292         {
1293                 float tvect[3];
1294                 
1295                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1296                 
1297                 // current_position = xold + t * (newposition - xold)
1298                 VECSUB(tvect, verts[s->ij].xconst, verts[s->ij].xold);
1299                 mul_fvector_S(tvect, tvect, time);
1300                 VECADD(tvect, tvect, verts[s->ij].xold);
1301
1302                 VECSUB(extent, X[s->ij], tvect);
1303                 
1304                 dot = INPR(extent, extent);
1305                 length = sqrt(dot);
1306                 
1307                 k = clmd->sim_parms->goalspring;
1308                 
1309                 scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
1310                         
1311                 k = verts [s->ij].goal * scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON);
1312                 
1313                 VECADDS(s->f, s->f, extent, -k);
1314                 
1315                 mul_fvector_S(damping_force, dir, clmd->sim_parms->goalfrict * 0.01 * INPR(vel,dir));
1316                 VECADD(s->f, s->f, damping_force);
1317                 
1318                 // HERE IS THE PROBLEM!!!!
1319                 // dfdx_spring(s->dfdx, dir, length, 0.0, k);
1320                 // dfdv_damp(s->dfdv, dir, MIN2(1.0, (clmd->sim_parms->goalfrict/100.0)));
1321         }
1322         else // calculate force of bending springs
1323         {
1324                 if(length < L)
1325                 {
1326                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1327                         
1328                         k = clmd->sim_parms->bending;   
1329                         
1330                         scaling = k + s->stiffness * ABS(clmd->sim_parms->max_bend-k);                  
1331                         cb = k = scaling / (20.0*(clmd->sim_parms->avg_spring_len + FLT_EPSILON));
1332
1333                         mul_fvector_S(bending_force, dir, fbstar(length, L, k, cb));
1334                         VECADD(s->f, s->f, bending_force);
1335
1336                         dfdx_spring_type2(s->dfdx, dir, length,L, k, cb);
1337                 }
1338         }
1339 }
1340
1341 DO_INLINE void cloth_apply_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
1342 {
1343         if(s->flags & CLOTH_SPRING_FLAG_NEEDED)
1344         {
1345                 if(!(s->type & CLOTH_SPRING_TYPE_BENDING))
1346                 {
1347                         sub_fmatrix_fmatrix(dFdV[s->ij].m, dFdV[s->ij].m, s->dfdv);
1348                         sub_fmatrix_fmatrix(dFdV[s->kl].m, dFdV[s->kl].m, s->dfdv);
1349                         add_fmatrix_fmatrix(dFdV[s->matrix_index].m, dFdV[s->matrix_index].m, s->dfdv); 
1350                 }
1351
1352                 VECADD(lF[s->ij], lF[s->ij], s->f);
1353                 
1354                 if(!(s->type & CLOTH_SPRING_TYPE_GOAL))
1355                         VECSUB(lF[s->kl], lF[s->kl], s->f);
1356                 
1357                 sub_fmatrix_fmatrix(dFdX[s->kl].m, dFdX[s->kl].m, s->dfdx);
1358                 sub_fmatrix_fmatrix(dFdX[s->ij].m, dFdX[s->ij].m, s->dfdx);
1359                 add_fmatrix_fmatrix(dFdX[s->matrix_index].m, dFdX[s->matrix_index].m, s->dfdx);
1360         }       
1361 }
1362
1363
1364 static void CalcFloat( float *v1, float *v2, float *v3, float *n)
1365 {
1366         float n1[3],n2[3];
1367
1368         n1[0]= v1[0]-v2[0];
1369         n2[0]= v2[0]-v3[0];
1370         n1[1]= v1[1]-v2[1];
1371         n2[1]= v2[1]-v3[1];
1372         n1[2]= v1[2]-v2[2];
1373         n2[2]= v2[2]-v3[2];
1374         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1375         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1376         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1377 }
1378
1379 static void CalcFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
1380 {
1381         /* real cross! */
1382         float n1[3],n2[3];
1383
1384         n1[0]= v1[0]-v3[0];
1385         n1[1]= v1[1]-v3[1];
1386         n1[2]= v1[2]-v3[2];
1387
1388         n2[0]= v2[0]-v4[0];
1389         n2[1]= v2[1]-v4[1];
1390         n2[2]= v2[2]-v4[2];
1391
1392         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1393         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1394         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1395 }
1396
1397 static float calculateVertexWindForce(float wind[3], float vertexnormal[3])  
1398 {
1399         return (INPR(wind, vertexnormal));
1400 }
1401
1402 typedef struct HairGridVert {
1403         float velocity[3];
1404         float density;
1405 } HairGridVert;
1406 /* Smoothing of hair velocities:
1407  * adapted from
1408                 Volumetric Methods for Simulation and Rendering of Hair
1409                 by Lena Petrovic, Mark Henne and John Anderson
1410  *              Pixar Technical Memo #06-08, Pixar Animation Studios
1411  */
1412 static void hair_velocity_smoothing(float smoothfac, lfVector *lF, lfVector *lX, lfVector *lV, int numverts)
1413 {
1414         /* TODO: this is an initial implementation and should be made much better in due time */
1415
1416         /* 10x10x10 grid gives nice initial results */
1417         HairGridVert grid[10][10][10];
1418         float gmin[3], gmax[3], density;
1419         int     v = 0;
1420         int     i = 0;
1421         int     j = 0;
1422         int     k = 0;
1423
1424         INIT_MINMAX(gmin, gmax);
1425
1426         for(i = 0; i < numverts; i++)
1427                 DO_MINMAX(lX[i], gmin, gmax);
1428
1429         /* initialize grid */
1430         for(i = 0; i < 10; i++) {
1431                 for(j = 0; j < 10; j++) {
1432                         for(k = 0; k < 10; k++) {
1433                                 grid[i][j][k].velocity[0] = 0.0f;
1434                                 grid[i][j][k].velocity[1] = 0.0f;
1435                                 grid[i][j][k].velocity[2] = 0.0f;
1436                                 grid[i][j][k].density = 0.0f;
1437                         }
1438                 }
1439         }
1440
1441         /* gather velocities & density */
1442         for(v = 0; v < numverts; v++) {
1443                 i = (int)( (lX[v][0] - gmin[0]) / (gmax[0] - gmin[0]) * 9.99f );
1444                 j = (int)( (lX[v][1] - gmin[1]) / (gmax[1] - gmin[1]) * 9.99f );
1445                 k = (int)( (lX[v][2] - gmin[2]) / (gmax[2] - gmin[2]) * 9.99f );
1446
1447                 grid[i][j][k].velocity[0] += lV[v][0];
1448                 grid[i][j][k].velocity[1] += lV[v][1];
1449                 grid[i][j][k].velocity[2] += lV[v][2];
1450                 grid[i][j][k].density += 1.0f;
1451         }
1452
1453         /* divide velocity with density */
1454         for(i = 0; i < 10; i++) {
1455                 for(j = 0; j < 10; j++) {
1456                         for(k = 0; k < 10; k++) {
1457                                 density = grid[i][j][k].density;
1458                                 if(density > 0.0f) {
1459                                         grid[i][j][k].velocity[0] /= density;
1460                                         grid[i][j][k].velocity[1] /= density;
1461                                         grid[i][j][k].velocity[2] /= density;
1462                                 }
1463                         }
1464                 }
1465         }
1466
1467         /* calculate forces */
1468         for(v = 0; v < numverts; v++) {
1469                 i = (int)( (lX[v][0] - gmin[0]) / (gmax[0] - gmin[0]) * 9.99f );
1470                 j = (int)( (lX[v][1] - gmin[1]) / (gmax[1] - gmin[1]) * 9.99f );
1471                 k = (int)( (lX[v][2] - gmin[2]) / (gmax[2] - gmin[2]) * 9.99f );
1472
1473                 /* 2.0f is an experimental value that seems to give good results */
1474                 lF[v][0] += 2.0f * smoothfac * (grid[i][j][k].velocity[0] - lV[v][0]);
1475                 lF[v][1] += 2.0f * smoothfac * (grid[i][j][k].velocity[1] - lV[v][1]);
1476                 lF[v][2] += 2.0f * smoothfac * (grid[i][j][k].velocity[2] - lV[v][2]);
1477         }
1478 }
1479 static void cloth_calc_force(ClothModifierData *clmd, float frame, lfVector *lF, lfVector *lX, lfVector *lV, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors, float time, fmatrix3x3 *M)
1480 {
1481         /* Collect forces and derivatives:  F,dFdX,dFdV */
1482         Cloth           *cloth          = clmd->clothObject;
1483         int             i               = 0;
1484         float           spring_air      = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */
1485         float           gravity[3];
1486         float           tm2[3][3]       = {{-spring_air,0,0}, {0,-spring_air,0},{0,0,-spring_air}};
1487         MFace           *mfaces         = cloth->mfaces;
1488         unsigned int numverts = cloth->numverts;
1489         LinkNode *search = cloth->springs;
1490         lfVector *winvec;
1491
1492         VECCOPY(gravity, clmd->sim_parms->gravity);
1493         mul_fvector_S(gravity, gravity, 0.001f); /* scale gravity force */
1494
1495         /* set dFdX jacobi matrix to zero */
1496         init_bfmatrix(dFdX, ZERO);
1497         /* set dFdX jacobi matrix diagonal entries to -spring_air */ 
1498         initdiag_bfmatrix(dFdV, tm2);
1499
1500         init_lfvector(lF, gravity, numverts);
1501         
1502         if(clmd->sim_parms->velocity_smooth > 0.0f)
1503                 hair_velocity_smoothing(clmd->sim_parms->velocity_smooth, lF, lX, lV, numverts);
1504
1505         /* multiply lF with mass matrix
1506         // force = mass * acceleration (in this case: gravity)
1507         */
1508         for(i = 0; i < numverts; i++)
1509         {
1510                 float temp[3];
1511                 VECCOPY(temp, lF[i]);
1512                 mul_fmatrix_fvector(lF[i], M[i].m, temp);
1513         }
1514
1515         submul_lfvectorS(lF, lV, spring_air, numverts);
1516         
1517         /* handle external forces like wind */
1518         if(effectors)
1519         {       
1520                 // 0 = force, 1 = normalized force
1521                 winvec = create_lfvector(cloth->numverts);
1522                 
1523                 if(!winvec)
1524                         printf("winvec: out of memory in implicit.c\n");
1525                 
1526                 // precalculate wind forces
1527                 for(i = 0; i < cloth->numverts; i++)
1528                 {
1529                         float speed[3] = {0.0f, 0.0f,0.0f};
1530                         
1531                         pdDoEffectors(clmd->scene, effectors, lX[i], winvec[i], speed, frame, 0.0f, 0);
1532                 }
1533                 
1534                 for(i = 0; i < cloth->numfaces; i++)
1535                 {
1536                         float trinormal[3]={0,0,0}; // normalized triangle normal
1537                         float triunnormal[3]={0,0,0}; // not-normalized-triangle normal
1538                         float tmp[3]={0,0,0};
1539                         float factor = (mfaces[i].v4) ? 0.25 : 1.0 / 3.0;
1540                         factor *= 0.02;
1541                         
1542                         // calculate face normal
1543                         if(mfaces[i].v4)
1544                                 CalcFloat4(lX[mfaces[i].v1],lX[mfaces[i].v2],lX[mfaces[i].v3],lX[mfaces[i].v4],triunnormal);
1545                         else
1546                                 CalcFloat(lX[mfaces[i].v1],lX[mfaces[i].v2],lX[mfaces[i].v3],triunnormal);
1547                         
1548                         VECCOPY(trinormal, triunnormal);
1549                         Normalize(trinormal);
1550                         
1551                         // add wind from v1
1552                         VECCOPY(tmp, trinormal);
1553                         VecMulf(tmp, calculateVertexWindForce(winvec[mfaces[i].v1], triunnormal));
1554                         VECADDS(lF[mfaces[i].v1], lF[mfaces[i].v1], tmp, factor);
1555                         
1556                         // add wind from v2
1557                         VECCOPY(tmp, trinormal);
1558                         VecMulf(tmp, calculateVertexWindForce(winvec[mfaces[i].v2], triunnormal));
1559                         VECADDS(lF[mfaces[i].v2], lF[mfaces[i].v2], tmp, factor);
1560                         
1561                         // add wind from v3
1562                         VECCOPY(tmp, trinormal);
1563                         VecMulf(tmp, calculateVertexWindForce(winvec[mfaces[i].v3], triunnormal));
1564                         VECADDS(lF[mfaces[i].v3], lF[mfaces[i].v3], tmp, factor);
1565                         
1566                         // add wind from v4
1567                         if(mfaces[i].v4)
1568                         {
1569                                 VECCOPY(tmp, trinormal);
1570                                 VecMulf(tmp, calculateVertexWindForce(winvec[mfaces[i].v4], triunnormal));
1571                                 VECADDS(lF[mfaces[i].v4], lF[mfaces[i].v4], tmp, factor);
1572                         }
1573                 }
1574                 del_lfvector(winvec);
1575         }
1576                 
1577         // calculate spring forces
1578         search = cloth->springs;
1579         while(search)
1580         {
1581                 // only handle active springs
1582                 // if(((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED)){}
1583                 cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX, time);
1584
1585                 search = search->next;
1586         }
1587         
1588         // apply spring forces
1589         search = cloth->springs;
1590         while(search)
1591         {
1592                 // only handle active springs
1593                 // if(((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED))  
1594                 cloth_apply_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX);
1595                 search = search->next;
1596         }
1597         // printf("\n");
1598 }
1599
1600 static 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, fmatrix3x3 *M, fmatrix3x3 *bigI)
1601 {
1602         unsigned int numverts = dFdV[0].vcount;
1603
1604         lfVector *dFdXmV = create_lfvector(numverts);
1605         zero_lfvector(dV, numverts);
1606         
1607         cp_bfmatrix(A, M);
1608         
1609         subadd_bfmatrixS_bfmatrixS(A, dFdV, dt, dFdX, (dt*dt));
1610
1611         mul_bfmatrix_lfvector(dFdXmV, dFdX, lV);
1612
1613         add_lfvectorS_lfvectorS(B, lF, dt, dFdXmV, (dt*dt), numverts);
1614         
1615         itstart();
1616         
1617         cg_filtered(dV, A, B, z, S); /* conjugate gradient algorithm to solve Ax=b */
1618         // cg_filtered_pre(dV, A, B, z, S, P, Pinv, bigI);
1619         
1620         itend();
1621         // printf("cg_filtered calc time: %f\n", (float)itval());
1622         
1623         cp_lfvector(olddV, dV, numverts);
1624
1625         // advance velocities
1626         add_lfvector_lfvector(Vnew, lV, dV, numverts);
1627         
1628
1629         del_lfvector(dFdXmV);
1630 }
1631
1632 int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
1633 {               
1634         unsigned int i=0;
1635         float step=0.0f, tf=clmd->sim_parms->timescale;
1636         Cloth *cloth = clmd->clothObject;
1637         ClothVertex *verts = cloth->verts;
1638         unsigned int numverts = cloth->numverts;
1639         float dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame;
1640         Implicit_Data *id = cloth->implicit;
1641         int result = 0;
1642         
1643         if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) /* do goal stuff */
1644         {
1645                 for(i = 0; i < numverts; i++)
1646                 {                       
1647                         // update velocities with constrained velocities from pinned verts
1648                         if(verts [i].flags & CLOTH_VERT_FLAG_PINNED)
1649                         {                       
1650                                 VECSUB(id->V[i], verts[i].xconst, verts[i].xold);
1651                                 // VecMulf(id->V[i], clmd->sim_parms->stepsPerFrame);
1652                         }
1653                 }       
1654         }
1655         
1656         while(step < tf)
1657         {       
1658                 // calculate forces
1659                 effectors= pdInitEffectors(clmd->scene, ob, NULL);
1660                 cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M);
1661                 if(effectors) pdEndEffectors(effectors);
1662                 
1663                 // calculate new velocity
1664                 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, id->M, id->bigI);
1665                 
1666                 // advance positions
1667                 add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
1668                 
1669                 /* move pinned verts to correct position */
1670                 for(i = 0; i < numverts; i++)
1671                 {       
1672                         if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
1673                         {                       
1674                                 if(verts [i].flags & CLOTH_VERT_FLAG_PINNED)
1675                                 {                       
1676                                         float tvect[3] = {.0,.0,.0};
1677                                         VECSUB(tvect, verts[i].xconst, verts[i].xold);
1678                                         mul_fvector_S(tvect, tvect, step+dt);
1679                                         VECADD(tvect, tvect, verts[i].xold);
1680                                         VECCOPY(id->Xnew[i], tvect);
1681                                 }       
1682                         }
1683                         
1684                         VECCOPY(verts[i].txold, id->X[i]);
1685                 }
1686                 
1687                 if(clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED)
1688                 {
1689                         float temp = clmd->sim_parms->stepsPerFrame;
1690                         /* not too nice hack, but collisions need this correction -jahka */
1691                         clmd->sim_parms->stepsPerFrame /= clmd->sim_parms->timescale;
1692
1693                         // collisions 
1694                         // itstart();
1695                         
1696                         // update verts to current positions
1697                         for(i = 0; i < numverts; i++)
1698                         {       
1699                                 VECCOPY(verts[i].tx, id->Xnew[i]);
1700                                 
1701                                 VECSUB(verts[i].tv, verts[i].tx, verts[i].txold);
1702                                 VECCOPY(verts[i].v, verts[i].tv);
1703                         }
1704                         
1705                         // call collision function
1706                         // TODO: check if "step" or "step+dt" is correct - dg
1707                         result = cloth_bvh_objcollision(ob, clmd, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale);
1708                         
1709                         // correct velocity again, just to be sure we had to change it due to adaptive collisions
1710                         for(i = 0; i < numverts; i++)
1711                         {
1712                                 VECSUB(verts[i].tv, verts[i].tx, id->X[i]);
1713                         }
1714                         
1715                         // copy corrected positions back to simulation
1716                         for(i = 0; i < numverts; i++)
1717                         {               
1718                                 if(result)
1719                                 {
1720                                         
1721                                         if((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED))
1722                                                 continue;
1723                                         
1724                                         VECCOPY(id->Xnew[i], verts[i].tx);
1725                                         VECCOPY(id->Vnew[i], verts[i].tv);
1726                                         VecMulf(id->Vnew[i], clmd->sim_parms->stepsPerFrame);
1727                                 }
1728                         }
1729                         
1730                         /* restore original stepsPerFrame */
1731                         clmd->sim_parms->stepsPerFrame = temp;
1732                         
1733                         // X = Xnew;
1734                         cp_lfvector(id->X, id->Xnew, numverts);
1735                         
1736                         // if there were collisions, advance the velocity from v_n+1/2 to v_n+1
1737                         
1738                         if(result)
1739                         {
1740                                 // V = Vnew;
1741                                 cp_lfvector(id->V, id->Vnew, numverts);
1742                                 
1743                                 // calculate 
1744                                 effectors= pdInitEffectors(clmd->scene, ob, NULL);
1745                                 cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step+dt, id->M);      
1746                                 if(effectors) pdEndEffectors(effectors);
1747                                 
1748                                 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, id->M, id->bigI);
1749                         }
1750                 }
1751                 else
1752                 {
1753                         // X = Xnew;
1754                         cp_lfvector(id->X, id->Xnew, numverts);
1755                 }
1756                 
1757                 // itend();
1758                 // printf("collision time: %f\n", (float)itval());
1759                 
1760                 // V = Vnew;
1761                 cp_lfvector(id->V, id->Vnew, numverts);
1762                 
1763                 step += dt;
1764                 
1765         }
1766
1767         for(i = 0; i < numverts; i++)
1768         {                               
1769                 if((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED))
1770                 {
1771                         VECCOPY(verts[i].txold, verts[i].xconst); // TODO: test --> should be .x 
1772                         VECCOPY(verts[i].x, verts[i].xconst);
1773                         VECCOPY(verts[i].v, id->V[i]);
1774                 }
1775                 else
1776                 {
1777                         VECCOPY(verts[i].txold, id->X[i]);
1778                         VECCOPY(verts[i].x, id->X[i]);
1779                         VECCOPY(verts[i].v, id->V[i]);
1780                 }
1781         }
1782         
1783         return 1;
1784 }
1785
1786 void implicit_set_positions (ClothModifierData *clmd)
1787 {               
1788         Cloth *cloth = clmd->clothObject;
1789         ClothVertex *verts = cloth->verts;
1790         unsigned int numverts = cloth->numverts, i;
1791         Implicit_Data *id = cloth->implicit;
1792         
1793         for(i = 0; i < numverts; i++)
1794         {                               
1795                 VECCOPY(id->X[i], verts[i].x);
1796                 VECCOPY(id->V[i], verts[i].v);
1797         }
1798         if(G.rt > 0)
1799                 printf("implicit_set_positions\n");     
1800 }
1801