svn merge -r 14561:14627 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 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 void itstart(void)
47 {
48         static int first = 1;
49         if(first) {
50                 QueryPerformanceFrequency(&ifreq);
51                 first = 0;
52         }
53         QueryPerformanceCounter(&_itstart);
54 }
55 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 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 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 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 void print_bfmatrix(fmatrix3x3 *m3)
500 {
501         unsigned int i = 0;
502
503         for(i = 0; i < m3[0].vcount + m3[0].scount; i++)
504         {
505                 print_fmatrix(m3[i].m);
506         }
507 }
508 /* create big matrix */
509 DO_INLINE fmatrix3x3 *create_bfmatrix(unsigned int verts, unsigned int springs)
510 {
511         // TODO: check if memory allocation was successfull */
512         fmatrix3x3 *temp = (fmatrix3x3 *)MEM_callocN (sizeof (fmatrix3x3) * (verts + springs), "cloth_implicit_alloc_matrix");
513         temp[0].vcount = verts;
514         temp[0].scount = springs;
515         return temp;
516 }
517 /* delete big matrix */
518 DO_INLINE void del_bfmatrix(fmatrix3x3 *matrix)
519 {
520         if (matrix != NULL)
521         {
522                 MEM_freeN (matrix);
523         }
524 }
525
526 /* copy big matrix */
527 DO_INLINE void cp_bfmatrix(fmatrix3x3 *to, fmatrix3x3 *from)
528 {       
529         // TODO bounds checking 
530         memcpy(to, from, sizeof(fmatrix3x3) * (from[0].vcount+from[0].scount) );
531 }
532
533 /* init big matrix */
534 // slow in parallel
535 DO_INLINE void init_bfmatrix(fmatrix3x3 *matrix, float m3[3][3])
536 {
537         unsigned int i;
538
539         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
540         {               
541                 cp_fmatrix(matrix[i].m, m3); 
542         }
543 }
544
545 /* init the diagonal of big matrix */
546 // slow in parallel
547 DO_INLINE void initdiag_bfmatrix(fmatrix3x3 *matrix, float m3[3][3])
548 {
549         unsigned int i,j;
550         float tmatrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
551
552         for(i = 0; i < matrix[0].vcount; i++)
553         {               
554                 cp_fmatrix(matrix[i].m, m3); 
555         }
556         for(j = matrix[0].vcount; j < matrix[0].vcount+matrix[0].scount; j++)
557         {
558                 cp_fmatrix(matrix[j].m, tmatrix); 
559         }
560 }
561
562 /* multiply big matrix with scalar*/
563 DO_INLINE void mul_bfmatrix_S(fmatrix3x3 *matrix, float scalar)
564 {
565         unsigned int i = 0;
566         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
567         {
568                 mul_fmatrix_S(matrix[i].m, scalar);
569         }
570 }
571
572 /* SPARSE SYMMETRIC multiply big matrix with long vector*/
573 /* STATUS: verified */
574 DO_INLINE void mul_bfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector *fLongVector)
575 {
576         unsigned int i = 0;
577         lfVector *temp = create_lfvector(from[0].vcount);
578         
579         zero_lfvector(to, from[0].vcount);
580
581 #pragma omp parallel sections private(i)
582         {
583 #pragma omp section
584                 {
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                 }       
590 #pragma omp section
591                 {
592                         for(i = 0; i < from[0].vcount+from[0].scount; i++)
593                         {
594                                 muladd_fmatrix_fvector(temp[from[i].r], from[i].m, fLongVector[from[i].c]);
595                         }
596                 }
597         }
598         add_lfvector_lfvector(to, to, temp, from[0].vcount);
599         
600         del_lfvector(temp);
601         
602         
603 }
604
605 /* SPARSE SYMMETRIC multiply big matrix with long vector (for diagonal preconditioner) */
606 /* STATUS: verified */
607 DO_INLINE void mul_prevfmatrix_lfvector( float (*to)[3], fmatrix3x3 *from, lfVector *fLongVector)
608 {
609         unsigned int i = 0;
610         
611         for(i = 0; i < from[0].vcount; i++)
612         {
613                 mul_fmatrix_fvector(to[from[i].r], from[i].m, fLongVector[from[i].c]);
614         }
615 }
616
617 /* SPARSE SYMMETRIC add big matrix with big matrix: A = B + C*/
618 DO_INLINE void add_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
619 {
620         unsigned int i = 0;
621
622         /* process diagonal elements */
623         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
624         {
625                 add_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
626         }
627
628 }
629 /* SPARSE SYMMETRIC add big matrix with big matrix: A += B + C */
630 DO_INLINE void addadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
631 {
632         unsigned int i = 0;
633
634         /* process diagonal elements */
635         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
636         {
637                 addadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
638         }
639
640 }
641 /* SPARSE SYMMETRIC subadd big matrix with big matrix: A -= B + C */
642 DO_INLINE void subadd_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
643 {
644         unsigned int i = 0;
645
646         /* process diagonal elements */
647         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
648         {
649                 subadd_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
650         }
651
652 }
653 /*  A = B - C (SPARSE SYMMETRIC sub big matrix with big matrix) */
654 DO_INLINE void sub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
655 {
656         unsigned int i = 0;
657
658         /* process diagonal elements */
659         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
660         {
661                 sub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);   
662         }
663
664 }
665 /* SPARSE SYMMETRIC sub big matrix with big matrix S (special constraint matrix with limited entries) */
666 DO_INLINE void sub_bfmatrix_Smatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
667 {
668         unsigned int i = 0;
669
670         /* process diagonal elements */
671         for(i = 0; i < matrix[0].vcount; i++)
672         {
673                 sub_fmatrix_fmatrix(to[matrix[i].c].m, from[matrix[i].c].m, matrix[i].m);       
674         }
675
676 }
677 /* A += B - C (SPARSE SYMMETRIC addsub big matrix with big matrix) */
678 DO_INLINE void addsub_bfmatrix_bfmatrix( fmatrix3x3 *to, fmatrix3x3 *from,  fmatrix3x3 *matrix)
679 {
680         unsigned int i = 0;
681
682         /* process diagonal elements */
683         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
684         {
685                 addsub_fmatrix_fmatrix(to[i].m, from[i].m, matrix[i].m);        
686         }
687
688 }
689 /* SPARSE SYMMETRIC sub big matrix with big matrix*/
690 /* A -= B * float + C * float --> for big matrix */
691 /* VERIFIED */
692 DO_INLINE void subadd_bfmatrixS_bfmatrixS( fmatrix3x3 *to, fmatrix3x3 *from, float aS,  fmatrix3x3 *matrix, float bS)
693 {
694         unsigned int i = 0;
695
696         /* process diagonal elements */
697         for(i = 0; i < matrix[0].vcount+matrix[0].scount; i++)
698         {
699                 subadd_fmatrixS_fmatrixS(to[i].m, from[i].m, aS, matrix[i].m, bS);      
700         }
701
702 }
703
704 ///////////////////////////////////////////////////////////////////
705 // simulator start
706 ///////////////////////////////////////////////////////////////////
707 typedef struct Implicit_Data 
708 {
709         lfVector *X, *V, *Xnew, *Vnew, *olddV, *F, *B, *dV, *z;
710         fmatrix3x3 *A, *dFdV, *dFdX, *S, *P, *Pinv, *bigI, *M; 
711 } Implicit_Data;
712
713 int implicit_init (Object *ob, ClothModifierData *clmd)
714 {
715         unsigned int i = 0;
716         unsigned int pinned = 0;
717         Cloth *cloth = NULL;
718         ClothVertex *verts = NULL;
719         ClothSpring *spring = NULL;
720         Implicit_Data *id = NULL;
721         LinkNode *search = NULL;
722         
723         if(G.rt > 0)
724                 printf("implicit_init\n");
725
726         // init memory guard
727         // MEMORY_BASE.first = MEMORY_BASE.last = NULL;
728
729         cloth = (Cloth *)clmd->clothObject;
730         verts = cloth->verts;
731
732         // create implicit base
733         id = (Implicit_Data *)MEM_callocN (sizeof(Implicit_Data), "implicit vecmat");
734         cloth->implicit = id;
735
736         /* process diagonal elements */         
737         id->A = create_bfmatrix(cloth->numverts, cloth->numsprings);
738         id->dFdV = create_bfmatrix(cloth->numverts, cloth->numsprings);
739         id->dFdX = create_bfmatrix(cloth->numverts, cloth->numsprings);
740         id->S = create_bfmatrix(cloth->numverts, 0);
741         id->Pinv = create_bfmatrix(cloth->numverts, cloth->numsprings);
742         id->P = create_bfmatrix(cloth->numverts, cloth->numsprings);
743         id->bigI = create_bfmatrix(cloth->numverts, cloth->numsprings); // TODO 0 springs
744         id->M = create_bfmatrix(cloth->numverts, cloth->numsprings);
745         id->X = create_lfvector(cloth->numverts);
746         id->Xnew = create_lfvector(cloth->numverts);
747         id->V = create_lfvector(cloth->numverts);
748         id->Vnew = create_lfvector(cloth->numverts);
749         id->olddV = create_lfvector(cloth->numverts);
750         zero_lfvector(id->olddV, cloth->numverts);
751         id->F = create_lfvector(cloth->numverts);
752         id->B = create_lfvector(cloth->numverts);
753         id->dV = create_lfvector(cloth->numverts);
754         id->z = create_lfvector(cloth->numverts);
755         
756         for(i=0;i<cloth->numverts;i++) 
757         {
758                 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;
759
760                 if(verts [i].flags & CLOTH_VERT_FLAG_PINNED)
761                 {
762                         id->S[pinned].pinned = 1;
763                         id->S[pinned].c = id->S[pinned].r = i;
764                         pinned++;
765                 }
766                 
767                 initdiag_fmatrixS(id->M[i].m, verts[i].mass);
768         }
769
770         // S is special and needs specific vcount and scount
771         id->S[0].vcount = pinned; id->S[0].scount = 0;
772
773         // init springs 
774         search = cloth->springs;
775         for(i=0;i<cloth->numsprings;i++) 
776         {
777                 spring = search->link;
778                 
779                 // dFdV_start[i].r = big_I[i].r = big_zero[i].r = 
780                 id->A[i+cloth->numverts].r = id->dFdV[i+cloth->numverts].r = id->dFdX[i+cloth->numverts].r = 
781                                 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;
782
783                 // dFdV_start[i].c = big_I[i].c = big_zero[i].c = 
784                 id->A[i+cloth->numverts].c = id->dFdV[i+cloth->numverts].c = id->dFdX[i+cloth->numverts].c = 
785                                 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;
786
787                 spring->matrix_index = i + cloth->numverts;
788                 
789                 search = search->next;
790         }
791         
792         initdiag_bfmatrix(id->bigI, I);
793
794         for(i = 0; i < cloth->numverts; i++)
795         {               
796                 VECCOPY(id->X[i], verts[i].x);
797         }
798
799         return 1;
800 }
801 int     implicit_free (ClothModifierData *clmd)
802 {
803         Implicit_Data *id;
804         Cloth *cloth;
805         cloth = (Cloth *)clmd->clothObject;
806
807         if(cloth)
808         {
809                 id = cloth->implicit;
810
811                 if(id)
812                 {
813                         del_bfmatrix(id->A);
814                         del_bfmatrix(id->dFdV);
815                         del_bfmatrix(id->dFdX);
816                         del_bfmatrix(id->S);
817                         del_bfmatrix(id->P);
818                         del_bfmatrix(id->Pinv);
819                         del_bfmatrix(id->bigI);
820                         del_bfmatrix(id->M);
821
822                         del_lfvector(id->X);
823                         del_lfvector(id->Xnew);
824                         del_lfvector(id->V);
825                         del_lfvector(id->Vnew);
826                         del_lfvector(id->olddV);
827                         del_lfvector(id->F);
828                         del_lfvector(id->B);
829                         del_lfvector(id->dV);
830                         del_lfvector(id->z);
831
832                         MEM_freeN(id);
833                 }
834         }
835
836         return 1;
837 }
838
839 DO_INLINE float fb(float length, float L)
840 {
841         float x = length/L;
842         return (-11.541f*pow(x,4)+34.193f*pow(x,3)-39.083f*pow(x,2)+23.116f*x-9.713f);
843 }
844
845 DO_INLINE float fbderiv(float length, float L)
846 {
847         float x = length/L;
848
849         return (-46.164f*pow(x,3)+102.579f*pow(x,2)-78.166f*x+23.116f);
850 }
851
852 DO_INLINE float fbstar(float length, float L, float kb, float cb)
853 {
854         float tempfb = kb * fb(length, L);
855
856         float fbstar = cb * (length - L);
857         
858         if(tempfb < fbstar)
859                 return fbstar;
860         else
861                 return tempfb;          
862 }
863
864 // function to calculae bending spring force (taken from Choi & Co)
865 DO_INLINE float fbstar_jacobi(float length, float L, float kb, float cb)
866 {
867         float tempfb = kb * fb(length, L);
868         float fbstar = cb * (length - L);
869
870         if(tempfb < fbstar)
871         {               
872                 return cb;
873         }
874         else
875         {
876                 return kb * fbderiv(length, L); 
877         }       
878 }
879
880 DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
881 {
882         unsigned int i=0;
883
884         for(i=0;i<S[0].vcount;i++)
885         {
886                 mul_fvector_fmatrix(V[S[i].r], V[S[i].r], S[i].m);
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 // block diagonalizer
957 DO_INLINE void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv)
958 {
959         unsigned int i = 0;
960         
961         // Take only the diagonal blocks of A
962 // #pragma omp parallel for private(i)
963         for(i = 0; i<lA[0].vcount; i++)
964         {
965                 // block diagonalizer
966                 cp_fmatrix(P[i].m, lA[i].m);
967                 inverse_fmatrix(Pinv[i].m, P[i].m);
968                 
969         }
970 }
971 /*
972 // version 1.3
973 int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
974 {
975         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
976         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0;
977         float conjgrad_epsilon=0.0001; // 0.2 is dt for steps=5
978         lfVector *r = create_lfvector(numverts);
979         lfVector *p = create_lfvector(numverts);
980         lfVector *s = create_lfvector(numverts);
981         lfVector *h = create_lfvector(numverts);
982         
983         BuildPPinv(lA, P, Pinv);
984         
985         filter(dv, S);
986         add_lfvector_lfvector(dv, dv, z, numverts);
987         
988         mul_bfmatrix_lfvector(r, lA, dv);
989         sub_lfvector_lfvector(r, lB, r, numverts);
990         filter(r, S);
991         
992         mul_prevfmatrix_lfvector(p, Pinv, r);
993         filter(p, S);
994         
995         deltaNew = dot_lfvector(r, p, numverts);
996         
997         delta0 = deltaNew * sqrt(conjgrad_epsilon);
998         
999         // itstart();
1000         
1001         while ((deltaNew > delta0) && (iterations < conjgrad_looplimit))
1002         {
1003                 iterations++;
1004                 
1005                 mul_bfmatrix_lfvector(s, lA, p);
1006                 filter(s, S);
1007                 
1008                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1009                 
1010                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1011                 
1012                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
1013                 
1014                 mul_prevfmatrix_lfvector(h, Pinv, r);
1015                 filter(h, S);
1016                 
1017                 deltaOld = deltaNew;
1018                 
1019                 deltaNew = dot_lfvector(r, h, numverts);
1020                 
1021                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1022                 
1023                 filter(p, S);
1024                 
1025         }
1026         
1027         // itend();
1028         // printf("cg_filtered_pre time: %f\n", (float)itval());
1029         
1030         del_lfvector(h);
1031         del_lfvector(s);
1032         del_lfvector(p);
1033         del_lfvector(r);
1034         
1035         printf("iterations: %d\n", iterations);
1036         
1037         return iterations<conjgrad_looplimit;
1038 }
1039 */
1040 // version 1.4
1041 int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *bigI)
1042 {
1043         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
1044         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0, tol = 0;
1045         lfVector *r = create_lfvector(numverts);
1046         lfVector *p = create_lfvector(numverts);
1047         lfVector *s = create_lfvector(numverts);
1048         lfVector *h = create_lfvector(numverts);
1049         lfVector *bhat = create_lfvector(numverts);
1050         lfVector *btemp = create_lfvector(numverts);
1051         
1052         BuildPPinv(lA, P, Pinv);
1053         
1054         initdiag_bfmatrix(bigI, I);
1055         sub_bfmatrix_Smatrix(bigI, bigI, S);
1056         
1057         // x = Sx_0+(I-S)z
1058         filter(dv, S);
1059         add_lfvector_lfvector(dv, dv, z, numverts);
1060         
1061         // b_hat = S(b-A(I-S)z)
1062         mul_bfmatrix_lfvector(r, lA, z);
1063         mul_bfmatrix_lfvector(bhat, bigI, r);
1064         sub_lfvector_lfvector(bhat, lB, bhat, numverts);
1065         
1066         // r = S(b-Ax)
1067         mul_bfmatrix_lfvector(r, lA, dv);
1068         sub_lfvector_lfvector(r, lB, r, numverts);
1069         filter(r, S);
1070         
1071         // p = SP^-1r
1072         mul_prevfmatrix_lfvector(p, Pinv, r);
1073         filter(p, S);
1074         
1075         // delta0 = bhat^TP^-1bhat
1076         mul_prevfmatrix_lfvector(btemp, Pinv, bhat);
1077         delta0 = dot_lfvector(bhat, btemp, numverts);
1078         
1079         // deltaNew = r^TP
1080         deltaNew = dot_lfvector(r, p, numverts);
1081         
1082         /*
1083         filter(dv, S);
1084         add_lfvector_lfvector(dv, dv, z, numverts);
1085         
1086         mul_bfmatrix_lfvector(r, lA, dv);
1087         sub_lfvector_lfvector(r, lB, r, numverts);
1088         filter(r, S);
1089         
1090         mul_prevfmatrix_lfvector(p, Pinv, r);
1091         filter(p, S);
1092         
1093         deltaNew = dot_lfvector(r, p, numverts);
1094         
1095         delta0 = deltaNew * sqrt(conjgrad_epsilon);
1096         */
1097         
1098         // itstart();
1099         
1100         tol = (0.01*0.2);
1101         
1102         while ((deltaNew > delta0*tol*tol) && (iterations < conjgrad_looplimit))
1103         {
1104                 iterations++;
1105                 
1106                 mul_bfmatrix_lfvector(s, lA, p);
1107                 filter(s, S);
1108                 
1109                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1110                 
1111                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1112                 
1113                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
1114                 
1115                 mul_prevfmatrix_lfvector(h, Pinv, r);
1116                 filter(h, S);
1117                 
1118                 deltaOld = deltaNew;
1119                 
1120                 deltaNew = dot_lfvector(r, h, numverts);
1121                 
1122                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1123                 
1124                 filter(p, S);
1125                 
1126         }
1127         
1128         // itend();
1129         // printf("cg_filtered_pre time: %f\n", (float)itval());
1130         
1131         del_lfvector(btemp);
1132         del_lfvector(bhat);
1133         del_lfvector(h);
1134         del_lfvector(s);
1135         del_lfvector(p);
1136         del_lfvector(r);
1137         
1138         // printf("iterations: %d\n", iterations);
1139         
1140         return iterations<conjgrad_looplimit;
1141 }
1142
1143 // outer product is NOT cross product!!!
1144 DO_INLINE void dfdx_spring_type1(float to[3][3], float extent[3], float length, float L, float dot, float k)
1145 {
1146         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1147         // return  (outerprod(dir,dir)*k + (I - outerprod(dir,dir))*(k - ((k*L)/length)));
1148         float temp[3][3];
1149         float temp1 = k*(1.0 - (L/length));     
1150         
1151         mul_fvectorT_fvectorS(temp, extent, extent, 1.0 / dot);
1152         sub_fmatrix_fmatrix(to, I, temp);
1153         mul_fmatrix_S(to, temp1);
1154         
1155         mul_fvectorT_fvectorS(temp, extent, extent, k/ dot);
1156         add_fmatrix_fmatrix(to, to, temp);
1157         
1158         /*
1159         mul_fvectorT_fvector(temp, dir, dir);
1160         sub_fmatrix_fmatrix(to, I, temp);
1161         mul_fmatrix_S(to, k* (1.0f-(L/length)));
1162         mul_fmatrix_S(temp, k);
1163         add_fmatrix_fmatrix(to, temp, to);
1164         */
1165 }
1166
1167 DO_INLINE void dfdx_spring_type2(float to[3][3], float dir[3], float length, float L, float k, float cb)
1168 {
1169         // return  outerprod(dir,dir)*fbstar_jacobi(length, L, k, cb);
1170         mul_fvectorT_fvectorS(to, dir, dir, fbstar_jacobi(length, L, k, cb));
1171 }
1172
1173 DO_INLINE void dfdv_damp(float to[3][3], float dir[3], float damping)
1174 {
1175         // derivative of force wrt velocity.  
1176         mul_fvectorT_fvectorS(to, dir, dir, damping);
1177         
1178 }
1179
1180 DO_INLINE void dfdx_spring(float to[3][3],  float dir[3],float length,float L,float k)
1181 {
1182         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1183         //return  ( (I-outerprod(dir,dir))*Min(1.0f,rest/length) - I) * -k;
1184         mul_fvectorT_fvector(to, dir, dir);
1185         sub_fmatrix_fmatrix(to, I, to);
1186         mul_fmatrix_S(to, (((L/length)> 1.0f) ? (1.0f): (L/length))); 
1187         sub_fmatrix_fmatrix(to, to, I);
1188         mul_fmatrix_S(to, -k);
1189 }
1190
1191 // unused atm
1192 DO_INLINE void dfdx_damp(float to[3][3],  float dir[3],float length,const float vel[3],float rest,float damping)
1193 {
1194         // inner spring damping   vel is the relative velocity  of the endpoints.  
1195         //      return (I-outerprod(dir,dir)) * (-damping * -(dot(dir,vel)/Max(length,rest)));
1196         mul_fvectorT_fvector(to, dir, dir);
1197         sub_fmatrix_fmatrix(to, I, to);
1198         mul_fmatrix_S(to,  (-damping * -(INPR(dir,vel)/MAX2(length,rest)))); 
1199
1200 }
1201
1202 DO_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, float time)
1203 {
1204         Cloth *cloth = clmd->clothObject;
1205         ClothVertex *verts = cloth->verts;
1206         float extent[3];
1207         float length = 0, dot = 0;
1208         float dir[3] = {0,0,0};
1209         float vel[3];
1210         float k = 0.0f;
1211         float L = s->restlen;
1212         float cb = clmd->sim_parms->structural;
1213
1214         float nullf[3] = {0,0,0};
1215         float stretch_force[3] = {0,0,0};
1216         float bending_force[3] = {0,0,0};
1217         float damping_force[3] = {0,0,0};
1218         float nulldfdx[3][3]={ {0,0,0}, {0,0,0}, {0,0,0}};
1219         
1220         float scaling = 0.0;
1221         
1222         VECCOPY(s->f, nullf);
1223         cp_fmatrix(s->dfdx, nulldfdx);
1224         cp_fmatrix(s->dfdv, nulldfdx);
1225
1226         // calculate elonglation
1227         VECSUB(extent, X[s->kl], X[s->ij]);
1228         VECSUB(vel, V[s->kl], V[s->ij]);
1229         dot = INPR(extent, extent);
1230         length = sqrt(dot);
1231         
1232         s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
1233         
1234         if(length > ALMOST_ZERO)
1235         {
1236                 /*
1237                 if(length>L)
1238                 {
1239                 if((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) 
1240                 && ((((length-L)*100.0f/L) > clmd->sim_parms->maxspringlen))) // cut spring!
1241                 {
1242                 s->flags |= CSPRING_FLAG_DEACTIVATE;
1243                 return;
1244         }
1245         } 
1246                 */
1247                 mul_fvector_S(dir, extent, 1.0f/length);
1248         }
1249         else    
1250         {
1251                 mul_fvector_S(dir, extent, 0.0f);
1252         }
1253         
1254         // calculate force of structural + shear springs
1255         if((s->type & CLOTH_SPRING_TYPE_STRUCTURAL) || (s->type & CLOTH_SPRING_TYPE_SHEAR))
1256         {
1257                 if(length > L) // only on elonglation
1258                 {
1259                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1260                         
1261                         k = clmd->sim_parms->structural;
1262                                 
1263                         scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
1264                         
1265                         k = scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON);
1266                         
1267                         // TODO: verify, half verified (couldn't see error)
1268                         mul_fvector_S(stretch_force, dir, k*(length-L)); 
1269
1270                         VECADD(s->f, s->f, stretch_force);
1271
1272                         // Ascher & Boxman, p.21: Damping only during elonglation
1273                         // something wrong with it...
1274                         mul_fvector_S(damping_force, dir, clmd->sim_parms->Cdis * INPR(vel,dir));
1275                         VECADD(s->f, s->f, damping_force);
1276                         
1277                         /* VERIFIED */
1278                         dfdx_spring(s->dfdx, dir, length, L, k);
1279                         
1280                         /* VERIFIED */
1281                         dfdv_damp(s->dfdv, dir, clmd->sim_parms->Cdis);
1282                         
1283                 }
1284         }
1285         else if(s->type & CLOTH_SPRING_TYPE_GOAL)
1286         {
1287                 float tvect[3];
1288                 
1289                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1290                 
1291                 // current_position = xold + t * (newposition - xold)
1292                 VECSUB(tvect, verts[s->ij].xconst, verts[s->ij].xold);
1293                 mul_fvector_S(tvect, tvect, time);
1294                 VECADD(tvect, tvect, verts[s->ij].xold);
1295
1296                 VECSUB(extent, X[s->ij], tvect);
1297                 
1298                 dot = INPR(extent, extent);
1299                 length = sqrt(dot);
1300                 
1301                 k = clmd->sim_parms->goalspring;
1302                 
1303                 scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
1304                         
1305                 k = verts [s->ij].goal * scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON);
1306                 
1307                 VECADDS(s->f, s->f, extent, -k);
1308                 
1309                 mul_fvector_S(damping_force, dir, clmd->sim_parms->goalfrict * 0.01 * INPR(vel,dir));
1310                 VECADD(s->f, s->f, damping_force);
1311                 
1312                 // HERE IS THE PROBLEM!!!!
1313                 // dfdx_spring(s->dfdx, dir, length, 0.0, k);
1314                 // dfdv_damp(s->dfdv, dir, MIN2(1.0, (clmd->sim_parms->goalfrict/100.0)));
1315         }
1316         else // calculate force of bending springs
1317         {
1318                 if(length < L)
1319                 {
1320                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1321                         
1322                         k = clmd->sim_parms->bending;   
1323                         
1324                         scaling = k + s->stiffness * ABS(clmd->sim_parms->max_bend-k);                  
1325                         cb = k = scaling / (20.0*(clmd->sim_parms->avg_spring_len + FLT_EPSILON));
1326
1327                         mul_fvector_S(bending_force, dir, fbstar(length, L, k, cb));
1328                         VECADD(s->f, s->f, bending_force);
1329
1330                         dfdx_spring_type2(s->dfdx, dir, length,L, k, cb);
1331                 }
1332         }
1333 }
1334
1335 DO_INLINE void cloth_apply_spring_force(ClothModifierData *clmd, ClothSpring *s, lfVector *lF, lfVector *X, lfVector *V, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
1336 {
1337         if(s->flags & CLOTH_SPRING_FLAG_NEEDED)
1338         {
1339                 if(!(s->type & CLOTH_SPRING_TYPE_BENDING))
1340                 {
1341                         sub_fmatrix_fmatrix(dFdV[s->ij].m, dFdV[s->ij].m, s->dfdv);
1342                         sub_fmatrix_fmatrix(dFdV[s->kl].m, dFdV[s->kl].m, s->dfdv);
1343                         add_fmatrix_fmatrix(dFdV[s->matrix_index].m, dFdV[s->matrix_index].m, s->dfdv); 
1344                 }
1345
1346                 VECADD(lF[s->ij], lF[s->ij], s->f);
1347                 
1348                 if(!(s->type & CLOTH_SPRING_TYPE_GOAL))
1349                         VECSUB(lF[s->kl], lF[s->kl], s->f);
1350                 
1351                 sub_fmatrix_fmatrix(dFdX[s->kl].m, dFdX[s->kl].m, s->dfdx);
1352                 sub_fmatrix_fmatrix(dFdX[s->ij].m, dFdX[s->ij].m, s->dfdx);
1353                 add_fmatrix_fmatrix(dFdX[s->matrix_index].m, dFdX[s->matrix_index].m, s->dfdx);
1354         }       
1355 }
1356
1357 float calculateVertexWindForce(float wind[3], float vertexnormal[3])  
1358 {
1359         return fabs(INPR(wind, vertexnormal));
1360 }
1361
1362 void cloth_calc_force(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVector *lV, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, ListBase *effectors, float time, fmatrix3x3 *M)
1363 {
1364         /* Collect forces and derivatives:  F,dFdX,dFdV */
1365         Cloth           *cloth          = clmd->clothObject;
1366         long            i               = 0;
1367         float           spring_air      = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */
1368         float           gravity[3];
1369         float           tm2[3][3]       = {{-spring_air,0,0}, {0,-spring_air,0},{0,0,-spring_air}};
1370         MFace           *mfaces         = cloth->mfaces;
1371         //ClothVertex   *verts          = cloth->verts;
1372         float wind_normalized[3];
1373         unsigned int numverts = cloth->numverts;
1374         LinkNode *search = cloth->springs;
1375
1376
1377         VECCOPY(gravity, clmd->sim_parms->gravity);
1378         mul_fvector_S(gravity, gravity, 0.001f); /* scale gravity force */
1379
1380         /* set dFdX jacobi matrix to zero */
1381         init_bfmatrix(dFdX, ZERO);
1382         /* set dFdX jacobi matrix diagonal entries to -spring_air */ 
1383         initdiag_bfmatrix(dFdV, tm2);
1384
1385         init_lfvector(lF, gravity, numverts);
1386         
1387         /* multiply lF with mass matrix
1388         // force = mass * acceleration (in this case: gravity)
1389         */
1390         for(i = 0; i < (long)numverts; i++)
1391         {
1392                 float temp[3];
1393                 VECCOPY(temp, lF[i]);
1394                 mul_fmatrix_fvector(lF[i], M[i].m, temp);
1395         }
1396
1397         submul_lfvectorS(lF, lV, spring_air, numverts);
1398         
1399         /* handle external forces like wind */
1400         if(effectors)
1401         {       
1402                 for(i = 0; i < cloth->numfaces; i++)
1403                 {
1404                         float vertexnormal[3]={0,0,0};
1405                         float speed[3] = {0.0f, 0.0f,0.0f};
1406                         float force[3]= {0.0f, 0.0f, 0.0f};
1407                         
1408                         if(mfaces[i].v4)
1409                                 CalcNormFloat4(lX[mfaces[i].v1],lX[mfaces[i].v2],lX[mfaces[i].v3],lX[mfaces[i].v4],vertexnormal);
1410                         else
1411                                 CalcNormFloat(lX[mfaces[i].v1],lX[mfaces[i].v2],lX[mfaces[i].v3],vertexnormal);
1412                         
1413                         pdDoEffectors(effectors, lX[mfaces[i].v1], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
1414                         VECCOPY(wind_normalized, speed);
1415                         Normalize(wind_normalized);
1416                         VecMulf(wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
1417                         
1418                         if(mfaces[i].v4)
1419                         {
1420                                 VECADDS(lF[mfaces[i].v1], lF[mfaces[i].v1], wind_normalized, 0.25);
1421                         }
1422                         else
1423                         {
1424                                 VECADDS(lF[mfaces[i].v1], lF[mfaces[i].v1], wind_normalized, 1.0 / 3.0);
1425                         }
1426                         
1427                         speed[0] = speed[1] = speed[2] = 0.0;
1428                         pdDoEffectors(effectors, lX[mfaces[i].v2], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
1429                         VECCOPY(wind_normalized, speed);
1430                         Normalize(wind_normalized);
1431                         VecMulf(wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
1432                         if(mfaces[i].v4)
1433                         {
1434                                 VECADDS(lF[mfaces[i].v2], lF[mfaces[i].v2], wind_normalized, 0.25);
1435                         }
1436                         else
1437                         {
1438                                 VECADDS(lF[mfaces[i].v2], lF[mfaces[i].v2], wind_normalized, 1.0 / 3.0);
1439                         }
1440                         
1441                         speed[0] = speed[1] = speed[2] = 0.0;
1442                         pdDoEffectors(effectors, lX[mfaces[i].v3], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
1443                         VECCOPY(wind_normalized, speed);
1444                         Normalize(wind_normalized);
1445                         VecMulf(wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
1446                         if(mfaces[i].v4)
1447                         {
1448                                 VECADDS(lF[mfaces[i].v3], lF[mfaces[i].v3], wind_normalized, 0.25);
1449                         }
1450                         else
1451                         {
1452                                 VECADDS(lF[mfaces[i].v3], lF[mfaces[i].v3], wind_normalized, 1.0 / 3.0);
1453                         }
1454                         
1455                         speed[0] = speed[1] = speed[2] = 0.0;
1456                         if(mfaces[i].v4)
1457                         {
1458                                 pdDoEffectors(effectors, lX[mfaces[i].v4], force, speed, (float)G.scene->r.cfra, 0.0f, PE_WIND_AS_SPEED);
1459                                 VECCOPY(wind_normalized, speed);
1460                                 Normalize(wind_normalized);
1461                                 VecMulf(wind_normalized, -calculateVertexWindForce(speed, vertexnormal));
1462                                 VECADDS(lF[mfaces[i].v4], lF[mfaces[i].v4], wind_normalized, 0.25);
1463                         }
1464                         
1465                 }
1466         }
1467                 
1468         // calculate spring forces
1469         search = cloth->springs;
1470         while(search)
1471         {
1472                 // only handle active springs
1473                 // if(((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED)){}
1474                 cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX, time);
1475
1476                 search = search->next;
1477         }
1478         
1479         // apply spring forces
1480         search = cloth->springs;
1481         while(search)
1482         {
1483                 // only handle active springs
1484                 // if(((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) && !(springs[i].flags & CSPRING_FLAG_DEACTIVATE))|| !(clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED))  
1485                 cloth_apply_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX);
1486                 search = search->next;
1487         }
1488         // printf("\n");
1489 }
1490
1491 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)
1492 {
1493         unsigned int numverts = dFdV[0].vcount;
1494
1495         lfVector *dFdXmV = create_lfvector(numverts);
1496         zero_lfvector(dV, numverts);
1497         
1498         cp_bfmatrix(A, M);
1499         
1500         subadd_bfmatrixS_bfmatrixS(A, dFdV, dt, dFdX, (dt*dt));
1501
1502         mul_bfmatrix_lfvector(dFdXmV, dFdX, lV);
1503
1504         add_lfvectorS_lfvectorS(B, lF, dt, dFdXmV, (dt*dt), numverts);
1505         
1506         itstart();
1507         
1508         cg_filtered(dV, A, B, z, S); /* conjugate gradient algorithm to solve Ax=b */
1509         // cg_filtered_pre(dV, A, B, z, S, P, Pinv, bigI);
1510         
1511         itend();
1512         // printf("cg_filtered calc time: %f\n", (float)itval());
1513         
1514         cp_lfvector(olddV, dV, numverts);
1515
1516         // advance velocities
1517         add_lfvector_lfvector(Vnew, lV, dV, numverts);
1518         
1519
1520         del_lfvector(dFdXmV);
1521 }
1522
1523 int implicit_solver (Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
1524 {               
1525         unsigned int i=0;
1526         float step=0.0f, tf=clmd->sim_parms->timescale;
1527         Cloth *cloth = clmd->clothObject;
1528         ClothVertex *verts = cloth->verts;
1529         unsigned int numverts = cloth->numverts;
1530         float dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame;
1531         Implicit_Data *id = cloth->implicit;
1532         int result = 0;
1533         
1534         if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) /* do goal stuff */
1535         {
1536                 for(i = 0; i < numverts; i++)
1537                 {                       
1538                         // update velocities with constrained velocities from pinned verts
1539                         if(verts [i].flags & CLOTH_VERT_FLAG_PINNED)
1540                         {                       
1541                                 VECSUB(id->V[i], verts[i].xconst, verts[i].xold);
1542                                 // VecMulf(id->V[i], clmd->sim_parms->stepsPerFrame);
1543                         }
1544                 }       
1545         }
1546         
1547         while(step < tf)
1548         {       
1549                 // calculate forces
1550                 effectors= pdInitEffectors(ob,NULL);
1551                 cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M);
1552                 if(effectors) pdEndEffectors(effectors);
1553                 
1554                 // calculate new velocity
1555                 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);
1556                 
1557                 // advance positions
1558                 add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
1559                 
1560                 /* move pinned verts to correct position */
1561                 for(i = 0; i < numverts; i++)
1562                 {       
1563                         if(clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) 
1564                         {                       
1565                                 if(verts [i].flags & CLOTH_VERT_FLAG_PINNED)
1566                                 {                       
1567                                         float tvect[3] = {.0,.0,.0};
1568                                         VECSUB(tvect, verts[i].xconst, verts[i].xold);
1569                                         mul_fvector_S(tvect, tvect, step+dt);
1570                                         VECADD(tvect, tvect, verts[i].xold);
1571                                         VECCOPY(id->Xnew[i], tvect);
1572                                 }       
1573                         }
1574                         
1575                         VECCOPY(verts[i].txold, id->X[i]);
1576                 }
1577                 
1578                 if(clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED)
1579                 {
1580                         // collisions 
1581                         // itstart();
1582                         
1583                         // update verts to current positions
1584                         for(i = 0; i < numverts; i++)
1585                         {       
1586                                 VECCOPY(verts[i].tx, id->Xnew[i]);
1587                                 
1588                                 VECSUB(verts[i].tv, verts[i].tx, verts[i].txold);
1589                                 VECCOPY(verts[i].v, verts[i].tv);
1590                         }
1591         
1592                         // call collision function
1593                         result = cloth_bvh_objcollision(clmd, step + dt, dt);
1594         
1595                         // copy corrected positions back to simulation
1596                         for(i = 0; i < numverts; i++)
1597                         {               
1598                                 if(result)
1599                                 {
1600                                         
1601                                         if((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED))
1602                                                 continue;
1603                                         
1604                                         VECCOPY(id->Xnew[i], verts[i].tx);
1605                                         VECCOPY(id->Vnew[i], verts[i].tv);
1606                                         VecMulf(id->Vnew[i], clmd->sim_parms->stepsPerFrame);
1607                                 }
1608                         }
1609                         
1610                         // X = Xnew;
1611                         cp_lfvector(id->X, id->Xnew, numverts);
1612                         
1613                         // if there were collisions, advance the velocity from v_n+1/2 to v_n+1
1614                         
1615                         if(result)
1616                         {
1617                                 // V = Vnew;
1618                                 cp_lfvector(id->V, id->Vnew, numverts);
1619                                 
1620                                 // calculate 
1621                                 effectors= pdInitEffectors(ob,NULL);
1622                                 cloth_calc_force(clmd, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step+dt, id->M);     
1623                                 if(effectors) pdEndEffectors(effectors);
1624                                 
1625                                 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);
1626                         }
1627                         
1628                 }
1629                 else
1630                 {
1631                         // X = Xnew;
1632                         cp_lfvector(id->X, id->Xnew, numverts);
1633                 }
1634                 
1635                 // itend();
1636                 // printf("collision time: %f\n", (float)itval());
1637                 
1638                 // V = Vnew;
1639                 cp_lfvector(id->V, id->Vnew, numverts);
1640                 
1641                 step += dt;
1642                 
1643         }
1644
1645         for(i = 0; i < numverts; i++)
1646         {                               
1647                 if((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED))
1648                 {
1649                         VECCOPY(verts[i].txold, verts[i].xconst); // TODO: test --> should be .x 
1650                         VECCOPY(verts[i].x, verts[i].xconst);
1651                         VECCOPY(verts[i].v, id->V[i]);
1652                 }
1653                 else
1654                 {
1655                         VECCOPY(verts[i].txold, id->X[i]);
1656                         VECCOPY(verts[i].x, id->X[i]);
1657                         VECCOPY(verts[i].v, id->V[i]);
1658                 }
1659         }
1660         
1661         return 1;
1662 }
1663
1664 void implicit_set_positions (ClothModifierData *clmd)
1665 {               
1666         Cloth *cloth = clmd->clothObject;
1667         ClothVertex *verts = cloth->verts;
1668         unsigned int numverts = cloth->numverts, i;
1669         Implicit_Data *id = cloth->implicit;
1670         
1671         for(i = 0; i < numverts; i++)
1672         {                               
1673                 VECCOPY(id->X[i], verts[i].x);
1674                 VECCOPY(id->V[i], verts[i].v);
1675         }
1676         if(G.rt > 0)
1677                 printf("implicit_set_positions\n");     
1678 }
1679