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