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