And more UI messages spell check.
[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_fl = kb * fb(length, L);
861         float fbstar_fl = cb * (length - L);
862         
863         if (tempfb_fl < fbstar_fl)
864                 return fbstar_fl;
865         else
866                 return tempfb_fl;
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_fl = kb * fb(length, L);
873         float fbstar_fl = cb * (length - L);
874
875         if (tempfb_fl < fbstar_fl) {
876                 return cb;
877         }
878         else {
879                 return kb * fbderiv(length, L); 
880         }       
881 }
882
883 DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
884 {
885         unsigned int i=0;
886
887         for (i = 0; i < S[0].vcount; i++) {
888                 mul_fvector_fmatrix(V[S[i].r], V[S[i].r], S[i].m);
889         }
890 }
891
892 static int  cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S)
893 {
894         // Solves for unknown X in equation AX=B
895         unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
896         float conjgrad_epsilon=0.0001f /* , conjgrad_lasterror=0 */ /* UNUSED */;
897         lfVector *q, *d, *tmp, *r; 
898         float s, starget, a, s_prev;
899         unsigned int numverts = lA[0].vcount;
900         q = create_lfvector(numverts);
901         d = create_lfvector(numverts);
902         tmp = create_lfvector(numverts);
903         r = create_lfvector(numverts);
904
905         // zero_lfvector(ldV, CLOTHPARTICLES);
906         filter(ldV, S);
907
908         add_lfvector_lfvector(ldV, ldV, z, numverts);
909
910         // r = B - Mul(tmp, A, X);    // just use B if X known to be zero
911         cp_lfvector(r, lB, numverts);
912         mul_bfmatrix_lfvector(tmp, lA, ldV);
913         sub_lfvector_lfvector(r, r, tmp, numverts);
914
915         filter(r, S);
916
917         cp_lfvector(d, r, numverts);
918
919         s = dot_lfvector(r, r, numverts);
920         starget = s * sqrt(conjgrad_epsilon);
921
922         while (s>starget && conjgrad_loopcount < conjgrad_looplimit) {
923                 // Mul(q, A, d); // q = A*d;
924                 mul_bfmatrix_lfvector(q, lA, d);
925
926                 filter(q, S);
927
928                 a = s/dot_lfvector(d, q, numverts);
929
930                 // X = X + d*a;
931                 add_lfvector_lfvectorS(ldV, ldV, d, a, numverts);
932
933                 // r = r - q*a;
934                 sub_lfvector_lfvectorS(r, r, q, a, numverts);
935
936                 s_prev = s;
937                 s = dot_lfvector(r, r, numverts);
938
939                 //d = r+d*(s/s_prev);
940                 add_lfvector_lfvectorS(d, r, d, (s/s_prev), numverts);
941
942                 filter(d, S);
943
944                 conjgrad_loopcount++;
945         }
946         /* conjgrad_lasterror = s; */ /* UNUSED */
947
948         del_lfvector(q);
949         del_lfvector(d);
950         del_lfvector(tmp);
951         del_lfvector(r);
952         // printf("W/O conjgrad_loopcount: %d\n", conjgrad_loopcount);
953
954         return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
955 }
956
957 // block diagonalizer
958 DO_INLINE void BuildPPinv(fmatrix3x3 *lA, fmatrix3x3 *P, fmatrix3x3 *Pinv)
959 {
960         unsigned int i = 0;
961         
962         // Take only the diagonal blocks of A
963 // #pragma omp parallel for private(i) if (lA[0].vcount > CLOTH_OPENMP_LIMIT)
964         for (i = 0; i<lA[0].vcount; i++) {
965                 // block diagonalizer
966                 cp_fmatrix(P[i].m, lA[i].m);
967                 inverse_fmatrix(Pinv[i].m, P[i].m);
968                 
969         }
970 }
971 #if 0
972 /*
973 // version 1.3
974 static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv)
975 {
976         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
977         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0;
978         float conjgrad_epsilon=0.0001; // 0.2 is dt for steps=5
979         lfVector *r = create_lfvector(numverts);
980         lfVector *p = create_lfvector(numverts);
981         lfVector *s = create_lfvector(numverts);
982         lfVector *h = create_lfvector(numverts);
983         
984         BuildPPinv(lA, P, Pinv);
985         
986         filter(dv, S);
987         add_lfvector_lfvector(dv, dv, z, numverts);
988         
989         mul_bfmatrix_lfvector(r, lA, dv);
990         sub_lfvector_lfvector(r, lB, r, numverts);
991         filter(r, S);
992         
993         mul_prevfmatrix_lfvector(p, Pinv, r);
994         filter(p, S);
995         
996         deltaNew = dot_lfvector(r, p, numverts);
997         
998         delta0 = deltaNew * sqrt(conjgrad_epsilon);
999         
1000         // itstart();
1001         
1002         while ((deltaNew > delta0) && (iterations < conjgrad_looplimit))
1003         {
1004                 iterations++;
1005                 
1006                 mul_bfmatrix_lfvector(s, lA, p);
1007                 filter(s, S);
1008                 
1009                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1010                 
1011                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1012                 
1013                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
1014                 
1015                 mul_prevfmatrix_lfvector(h, Pinv, r);
1016                 filter(h, S);
1017                 
1018                 deltaOld = deltaNew;
1019                 
1020                 deltaNew = dot_lfvector(r, h, numverts);
1021                 
1022                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1023                 
1024                 filter(p, S);
1025                 
1026         }
1027         
1028         // itend();
1029         // printf("cg_filtered_pre time: %f\n", (float)itval());
1030         
1031         del_lfvector(h);
1032         del_lfvector(s);
1033         del_lfvector(p);
1034         del_lfvector(r);
1035         
1036         printf("iterations: %d\n", iterations);
1037         
1038         return iterations<conjgrad_looplimit;
1039 }
1040 */
1041 // version 1.4
1042 static int cg_filtered_pre(lfVector *dv, fmatrix3x3 *lA, lfVector *lB, lfVector *z, fmatrix3x3 *S, fmatrix3x3 *P, fmatrix3x3 *Pinv, fmatrix3x3 *bigI)
1043 {
1044         unsigned int numverts = lA[0].vcount, iterations = 0, conjgrad_looplimit=100;
1045         float delta0 = 0, deltaNew = 0, deltaOld = 0, alpha = 0, tol = 0;
1046         lfVector *r = create_lfvector(numverts);
1047         lfVector *p = create_lfvector(numverts);
1048         lfVector *s = create_lfvector(numverts);
1049         lfVector *h = create_lfvector(numverts);
1050         lfVector *bhat = create_lfvector(numverts);
1051         lfVector *btemp = create_lfvector(numverts);
1052         
1053         BuildPPinv(lA, P, Pinv);
1054         
1055         initdiag_bfmatrix(bigI, I);
1056         sub_bfmatrix_Smatrix(bigI, bigI, S);
1057         
1058         // x = Sx_0+(I-S)z
1059         filter(dv, S);
1060         add_lfvector_lfvector(dv, dv, z, numverts);
1061         
1062         // b_hat = S(b-A(I-S)z)
1063         mul_bfmatrix_lfvector(r, lA, z);
1064         mul_bfmatrix_lfvector(bhat, bigI, r);
1065         sub_lfvector_lfvector(bhat, lB, bhat, numverts);
1066         
1067         // r = S(b-Ax)
1068         mul_bfmatrix_lfvector(r, lA, dv);
1069         sub_lfvector_lfvector(r, lB, r, numverts);
1070         filter(r, S);
1071         
1072         // p = SP^-1r
1073         mul_prevfmatrix_lfvector(p, Pinv, r);
1074         filter(p, S);
1075         
1076         // delta0 = bhat^TP^-1bhat
1077         mul_prevfmatrix_lfvector(btemp, Pinv, bhat);
1078         delta0 = dot_lfvector(bhat, btemp, numverts);
1079         
1080         // deltaNew = r^TP
1081         deltaNew = dot_lfvector(r, p, numverts);
1082         
1083         /*
1084         filter(dv, S);
1085         add_lfvector_lfvector(dv, dv, z, numverts);
1086         
1087         mul_bfmatrix_lfvector(r, lA, dv);
1088         sub_lfvector_lfvector(r, lB, r, numverts);
1089         filter(r, S);
1090         
1091         mul_prevfmatrix_lfvector(p, Pinv, r);
1092         filter(p, S);
1093         
1094         deltaNew = dot_lfvector(r, p, numverts);
1095         
1096         delta0 = deltaNew * sqrt(conjgrad_epsilon);
1097         */
1098         
1099         // itstart();
1100         
1101         tol = (0.01*0.2);
1102         
1103         while ((deltaNew > delta0*tol*tol) && (iterations < conjgrad_looplimit))
1104         {
1105                 iterations++;
1106                 
1107                 mul_bfmatrix_lfvector(s, lA, p);
1108                 filter(s, S);
1109                 
1110                 alpha = deltaNew / dot_lfvector(p, s, numverts);
1111                 
1112                 add_lfvector_lfvectorS(dv, dv, p, alpha, numverts);
1113                 
1114                 add_lfvector_lfvectorS(r, r, s, -alpha, numverts);
1115                 
1116                 mul_prevfmatrix_lfvector(h, Pinv, r);
1117                 filter(h, S);
1118                 
1119                 deltaOld = deltaNew;
1120                 
1121                 deltaNew = dot_lfvector(r, h, numverts);
1122                 
1123                 add_lfvector_lfvectorS(p, h, p, deltaNew / deltaOld, numverts);
1124                 
1125                 filter(p, S);
1126                 
1127         }
1128         
1129         // itend();
1130         // printf("cg_filtered_pre time: %f\n", (float)itval());
1131         
1132         del_lfvector(btemp);
1133         del_lfvector(bhat);
1134         del_lfvector(h);
1135         del_lfvector(s);
1136         del_lfvector(p);
1137         del_lfvector(r);
1138         
1139         // printf("iterations: %d\n", iterations);
1140         
1141         return iterations<conjgrad_looplimit;
1142 }
1143 #endif
1144
1145 // outer product is NOT cross product!!!
1146 DO_INLINE void dfdx_spring_type1(float to[3][3], float extent[3], float length, float L, float dot, float k)
1147 {
1148         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1149         // return  (outerprod(dir, dir)*k + (I - outerprod(dir, dir))*(k - ((k*L)/length)));
1150         float temp[3][3];
1151         float temp1 = k*(1.0 - (L/length));     
1152         
1153         mul_fvectorT_fvectorS(temp, extent, extent, 1.0 / dot);
1154         sub_fmatrix_fmatrix(to, I, temp);
1155         mul_fmatrix_S(to, temp1);
1156         
1157         mul_fvectorT_fvectorS(temp, extent, extent, k/ dot);
1158         add_fmatrix_fmatrix(to, to, temp);
1159         
1160         /*
1161         mul_fvectorT_fvector(temp, dir, dir);
1162         sub_fmatrix_fmatrix(to, I, temp);
1163         mul_fmatrix_S(to, k* (1.0f-(L/length)));
1164         mul_fmatrix_S(temp, k);
1165         add_fmatrix_fmatrix(to, temp, to);
1166         */
1167 }
1168
1169 DO_INLINE void dfdx_spring_type2(float to[3][3], float dir[3], float length, float L, float k, float cb)
1170 {
1171         // return  outerprod(dir, dir)*fbstar_jacobi(length, L, k, cb);
1172         mul_fvectorT_fvectorS(to, dir, dir, fbstar_jacobi(length, L, k, cb));
1173 }
1174
1175 DO_INLINE void dfdv_damp(float to[3][3], float dir[3], float damping)
1176 {
1177         // derivative of force wrt velocity.  
1178         mul_fvectorT_fvectorS(to, dir, dir, damping);
1179         
1180 }
1181
1182 DO_INLINE void dfdx_spring(float to[3][3],  float dir[3], float length, float L, float k)
1183 {
1184         // dir is unit length direction, rest is spring's restlength, k is spring constant.
1185         //return  ( (I-outerprod(dir, dir))*Min(1.0f, rest/length) - I) * -k;
1186         mul_fvectorT_fvector(to, dir, dir);
1187         sub_fmatrix_fmatrix(to, I, to);
1188
1189         mul_fmatrix_S(to, (L/length)); 
1190         sub_fmatrix_fmatrix(to, to, I);
1191         mul_fmatrix_S(to, -k);
1192 }
1193
1194 // unused atm
1195 DO_INLINE void dfdx_damp(float to[3][3],  float dir[3], float length, const float vel[3], float rest, float damping)
1196 {
1197         // inner spring damping   vel is the relative velocity  of the endpoints.  
1198         //      return (I-outerprod(dir, dir)) * (-damping * -(dot(dir, vel)/Max(length, rest)));
1199         mul_fvectorT_fvector(to, dir, dir);
1200         sub_fmatrix_fmatrix(to, I, to);
1201         mul_fmatrix_S(to,  (-damping * -(dot_v3v3(dir, vel)/MAX2(length, rest))));
1202
1203 }
1204
1205 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)
1206 {
1207         Cloth *cloth = clmd->clothObject;
1208         ClothVertex *verts = cloth->verts;
1209         float extent[3];
1210         float length = 0, dot = 0;
1211         float dir[3] = {0, 0, 0};
1212         float vel[3];
1213         float k = 0.0f;
1214         float L = s->restlen;
1215         float cb; /* = clmd->sim_parms->structural; */ /*UNUSED*/
1216
1217         float nullf[3] = {0, 0, 0};
1218         float stretch_force[3] = {0, 0, 0};
1219         float bending_force[3] = {0, 0, 0};
1220         float damping_force[3] = {0, 0, 0};
1221         float nulldfdx[3][3]={ {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
1222         
1223         float scaling = 0.0;
1224
1225         int no_compress = clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_NO_SPRING_COMPRESS;
1226         
1227         copy_v3_v3(s->f, nullf);
1228         cp_fmatrix(s->dfdx, nulldfdx);
1229         cp_fmatrix(s->dfdv, nulldfdx);
1230
1231         // calculate elonglation
1232         sub_v3_v3v3(extent, X[s->kl], X[s->ij]);
1233         sub_v3_v3v3(vel, V[s->kl], V[s->ij]);
1234         dot = dot_v3v3(extent, extent);
1235         length = sqrt(dot);
1236         
1237         s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
1238         
1239         if (length > ALMOST_ZERO) {
1240                 /*
1241                 if (length>L)
1242                 {
1243                 if ((clmd->sim_parms->flags & CSIMSETT_FLAG_TEARING_ENABLED) &&
1244                     ((((length-L)*100.0f/L) > clmd->sim_parms->maxspringlen))) // cut spring!
1245                 {
1246                 s->flags |= CSPRING_FLAG_DEACTIVATE;
1247                 return;
1248         }
1249         } 
1250                 */
1251                 mul_fvector_S(dir, extent, 1.0f/length);
1252         }
1253         else {
1254                 mul_fvector_S(dir, extent, 0.0f);
1255         }
1256         
1257         // calculate force of structural + shear springs
1258         if ((s->type & CLOTH_SPRING_TYPE_STRUCTURAL) || (s->type & CLOTH_SPRING_TYPE_SHEAR)) {
1259                 if (length > L || no_compress) {
1260                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1261                         
1262                         k = clmd->sim_parms->structural;
1263
1264                         scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
1265
1266                         k = scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON);
1267
1268                         // TODO: verify, half verified (couldn't see error)
1269                         mul_fvector_S(stretch_force, dir, k*(length-L));
1270
1271                         VECADD(s->f, s->f, stretch_force);
1272
1273                         // Ascher & Boxman, p.21: Damping only during elonglation
1274                         // something wrong with it...
1275                         mul_fvector_S(damping_force, dir, clmd->sim_parms->Cdis * dot_v3v3(vel, dir));
1276                         VECADD(s->f, s->f, damping_force);
1277                         
1278                         /* VERIFIED */
1279                         dfdx_spring(s->dfdx, dir, length, L, k);
1280                         
1281                         /* VERIFIED */
1282                         dfdv_damp(s->dfdv, dir, clmd->sim_parms->Cdis);
1283                         
1284                 }
1285         }
1286         else if (s->type & CLOTH_SPRING_TYPE_GOAL) {
1287                 float tvect[3];
1288                 
1289                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1290                 
1291                 // current_position = xold + t * (newposition - xold)
1292                 sub_v3_v3v3(tvect, verts[s->ij].xconst, verts[s->ij].xold);
1293                 mul_fvector_S(tvect, tvect, time);
1294                 VECADD(tvect, tvect, verts[s->ij].xold);
1295
1296                 sub_v3_v3v3(extent, X[s->ij], tvect);
1297                 
1298                 // SEE MSG BELOW (these are UNUSED)
1299                 // dot = dot_v3v3(extent, extent);
1300                 // length = sqrt(dot);
1301                 
1302                 k = clmd->sim_parms->goalspring;
1303                 
1304                 scaling = k + s->stiffness * ABS(clmd->sim_parms->max_struct-k);
1305                         
1306                 k = verts [s->ij].goal * scaling / (clmd->sim_parms->avg_spring_len + FLT_EPSILON);
1307                 
1308                 VECADDS(s->f, s->f, extent, -k);
1309                 
1310                 mul_fvector_S(damping_force, dir, clmd->sim_parms->goalfrict * 0.01 * dot_v3v3(vel, dir));
1311                 VECADD(s->f, s->f, damping_force);
1312                 
1313                 // HERE IS THE PROBLEM!!!!
1314                 // dfdx_spring(s->dfdx, dir, length, 0.0, k);
1315                 // dfdv_damp(s->dfdv, dir, MIN2(1.0, (clmd->sim_parms->goalfrict/100.0)));
1316         }
1317         else {  /* calculate force of bending springs */
1318                 if (length < L) {
1319                         s->flags |= CLOTH_SPRING_FLAG_NEEDED;
1320                         
1321                         k = clmd->sim_parms->bending;   
1322                         
1323                         scaling = k + s->stiffness * ABS(clmd->sim_parms->max_bend-k);                  
1324                         cb = k = scaling / (20.0*(clmd->sim_parms->avg_spring_len + FLT_EPSILON));
1325
1326                         mul_fvector_S(bending_force, dir, fbstar(length, L, k, cb));
1327                         VECADD(s->f, s->f, bending_force);
1328
1329                         dfdx_spring_type2(s->dfdx, dir, length, L, k, cb);
1330                 }
1331         }
1332 }
1333
1334 DO_INLINE void cloth_apply_spring_force(ClothModifierData *UNUSED(clmd), ClothSpring *s, lfVector *lF, lfVector *UNUSED(X), lfVector *UNUSED(V), fmatrix3x3 *dFdV, fmatrix3x3 *dFdX)
1335 {
1336         if (s->flags & CLOTH_SPRING_FLAG_NEEDED) {
1337                 if (!(s->type & CLOTH_SPRING_TYPE_BENDING)) {
1338                         sub_fmatrix_fmatrix(dFdV[s->ij].m, dFdV[s->ij].m, s->dfdv);
1339                         sub_fmatrix_fmatrix(dFdV[s->kl].m, dFdV[s->kl].m, s->dfdv);
1340                         add_fmatrix_fmatrix(dFdV[s->matrix_index].m, dFdV[s->matrix_index].m, s->dfdv); 
1341                 }
1342
1343                 VECADD(lF[s->ij], lF[s->ij], s->f);
1344                 
1345                 if (!(s->type & CLOTH_SPRING_TYPE_GOAL))
1346                         sub_v3_v3v3(lF[s->kl], lF[s->kl], s->f);
1347                 
1348                 sub_fmatrix_fmatrix(dFdX[s->kl].m, dFdX[s->kl].m, s->dfdx);
1349                 sub_fmatrix_fmatrix(dFdX[s->ij].m, dFdX[s->ij].m, s->dfdx);
1350                 add_fmatrix_fmatrix(dFdX[s->matrix_index].m, dFdX[s->matrix_index].m, s->dfdx);
1351         }       
1352 }
1353
1354
1355 static void CalcFloat( float *v1, float *v2, float *v3, float *n)
1356 {
1357         float n1[3], n2[3];
1358
1359         n1[0]= v1[0]-v2[0];
1360         n2[0]= v2[0]-v3[0];
1361         n1[1]= v1[1]-v2[1];
1362         n2[1]= v2[1]-v3[1];
1363         n1[2]= v1[2]-v2[2];
1364         n2[2]= v2[2]-v3[2];
1365         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1366         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1367         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1368 }
1369
1370 static void CalcFloat4( float *v1, float *v2, float *v3, float *v4, float *n)
1371 {
1372         /* real cross! */
1373         float n1[3], n2[3];
1374
1375         n1[0]= v1[0]-v3[0];
1376         n1[1]= v1[1]-v3[1];
1377         n1[2]= v1[2]-v3[2];
1378
1379         n2[0]= v2[0]-v4[0];
1380         n2[1]= v2[1]-v4[1];
1381         n2[2]= v2[2]-v4[2];
1382
1383         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
1384         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
1385         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
1386 }
1387
1388 static float calculateVertexWindForce(float wind[3], float vertexnormal[3])  
1389 {
1390         return dot_v3v3(wind, vertexnormal);
1391 }
1392
1393 typedef struct HairGridVert {
1394         float velocity[3];
1395         float density;
1396 } HairGridVert;
1397 #define HAIR_GRID_INDEX(vec, min, max, axis) (int)((vec[axis] - min[axis]) / (max[axis] - min[axis]) * 9.99f)
1398 /* Smoothing of hair velocities:
1399  * adapted from
1400  *      Volumetric Methods for Simulation and Rendering of Hair
1401  *      by Lena Petrovic, Mark Henne and John Anderson
1402  *      Pixar Technical Memo #06-08, Pixar Animation Studios
1403  */
1404 static void hair_velocity_smoothing(ClothModifierData *clmd, lfVector *lF, lfVector *lX, lfVector *lV, unsigned int numverts)
1405 {
1406         /* TODO: This is an initial implementation and should be made much better in due time.
1407          * What should at least be implemented is a grid size parameter and a smoothing kernel
1408          * for bigger grids.
1409          */
1410
1411         /* 10x10x10 grid gives nice initial results */
1412         HairGridVert grid[10][10][10];
1413         HairGridVert colg[10][10][10];
1414         ListBase *colliders = get_collider_cache(clmd->scene, NULL, NULL);
1415         ColliderCache *col = NULL;
1416         float gmin[3], gmax[3], density;
1417         /* 2.0f is an experimental value that seems to give good results */
1418         float smoothfac = 2.0f * clmd->sim_parms->velocity_smooth;
1419         float collfac = 2.0f * clmd->sim_parms->collider_friction;
1420         unsigned int    v = 0;
1421         unsigned int    i = 0;
1422         int                             j = 0;
1423         int                             k = 0;
1424
1425         INIT_MINMAX(gmin, gmax);
1426
1427         for (i = 0; i < numverts; i++)
1428                 DO_MINMAX(lX[i], gmin, gmax);
1429
1430         /* initialize grid */
1431         for (i = 0; i < 10; i++) {
1432                 for (j = 0; j < 10; j++) {
1433                         for (k = 0; k < 10; k++) {
1434                                 grid[i][j][k].velocity[0] = 0.0f;
1435                                 grid[i][j][k].velocity[1] = 0.0f;
1436                                 grid[i][j][k].velocity[2] = 0.0f;
1437                                 grid[i][j][k].density = 0.0f;
1438
1439                                 colg[i][j][k].velocity[0] = 0.0f;
1440                                 colg[i][j][k].velocity[1] = 0.0f;
1441                                 colg[i][j][k].velocity[2] = 0.0f;
1442                                 colg[i][j][k].density = 0.0f;
1443                         }
1444                 }
1445         }
1446
1447         /* gather velocities & density */
1448         if (smoothfac > 0.0f) for (v = 0; v < numverts; v++) {
1449                 i = HAIR_GRID_INDEX(lX[v], gmin, gmax, 0);
1450                 j = HAIR_GRID_INDEX(lX[v], gmin, gmax, 1);
1451                 k = HAIR_GRID_INDEX(lX[v], gmin, gmax, 2);
1452                 if (i < 0 || j < 0 || k < 0 || i >= 10 || j >= 10 || k >= 10)
1453                         continue;
1454
1455                 grid[i][j][k].velocity[0] += lV[v][0];
1456                 grid[i][j][k].velocity[1] += lV[v][1];
1457                 grid[i][j][k].velocity[2] += lV[v][2];
1458                 grid[i][j][k].density += 1.0f;
1459         }
1460
1461         /* gather colliders */
1462         if (colliders && collfac > 0.0f) for (col = colliders->first; col; col = col->next) {
1463                 MVert *loc0 = col->collmd->x;
1464                 MVert *loc1 = col->collmd->xnew;
1465                 float vel[3];
1466
1467                 for (v=0; v<col->collmd->numverts; v++, loc0++, loc1++) {
1468                         i = HAIR_GRID_INDEX(loc1->co, gmin, gmax, 0);
1469
1470                         if (i>=0 && i<10) {
1471                                 j = HAIR_GRID_INDEX(loc1->co, gmin, gmax, 1);
1472
1473                                 if (j>=0 && j<10) {
1474                                         k = HAIR_GRID_INDEX(loc1->co, gmin, gmax, 2);
1475
1476                                         if (k>=0 && k<10) {
1477                                                 sub_v3_v3v3(vel, loc1->co, loc0->co);
1478
1479                                                 colg[i][j][k].velocity[0] += vel[0];
1480                                                 colg[i][j][k].velocity[1] += vel[1];
1481                                                 colg[i][j][k].velocity[2] += vel[2];
1482                                                 colg[i][j][k].density += 1.0;
1483                                         }
1484                                 }
1485                         }
1486                 }
1487         }
1488         
1489
1490         /* divide velocity with density */
1491         for (i = 0; i < 10; i++) {
1492                 for (j = 0; j < 10; j++) {
1493                         for (k = 0; k < 10; k++) {
1494                                 density = grid[i][j][k].density;
1495                                 if (density > 0.0f) {
1496                                         grid[i][j][k].velocity[0] /= density;
1497                                         grid[i][j][k].velocity[1] /= density;
1498                                         grid[i][j][k].velocity[2] /= density;
1499                                 }
1500
1501                                 density = colg[i][j][k].density;
1502                                 if (density > 0.0f) {
1503                                         colg[i][j][k].velocity[0] /= density;
1504                                         colg[i][j][k].velocity[1] /= density;
1505                                         colg[i][j][k].velocity[2] /= density;
1506                                 }
1507                         }
1508                 }
1509         }
1510
1511         /* calculate forces */
1512         for (v = 0; v < numverts; v++) {
1513                 i = HAIR_GRID_INDEX(lX[v], gmin, gmax, 0);
1514                 j = HAIR_GRID_INDEX(lX[v], gmin, gmax, 1);
1515                 k = HAIR_GRID_INDEX(lX[v], gmin, gmax, 2);
1516                 if (i < 0 || j < 0 || k < 0 || i > 10 || j >= 10 || k >= 10)
1517                         continue;
1518
1519                 lF[v][0] += smoothfac * (grid[i][j][k].velocity[0] - lV[v][0]);
1520                 lF[v][1] += smoothfac * (grid[i][j][k].velocity[1] - lV[v][1]);
1521                 lF[v][2] += smoothfac * (grid[i][j][k].velocity[2] - lV[v][2]);
1522
1523                 if (colg[i][j][k].density > 0.0f) {
1524                         lF[v][0] += collfac * (colg[i][j][k].velocity[0] - lV[v][0]);
1525                         lF[v][1] += collfac * (colg[i][j][k].velocity[1] - lV[v][1]);
1526                         lF[v][2] += collfac * (colg[i][j][k].velocity[2] - lV[v][2]);
1527                 }
1528         }
1529
1530         free_collider_cache(&colliders);
1531 }
1532
1533 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)
1534 {
1535         /* Collect forces and derivatives:  F, dFdX, dFdV */
1536         Cloth           *cloth          = clmd->clothObject;
1537         unsigned int i  = 0;
1538         float           spring_air      = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */
1539         float           gravity[3] = {0.0f, 0.0f, 0.0f};
1540         float           tm2[3][3]       = {{0}};
1541         MFace           *mfaces         = cloth->mfaces;
1542         unsigned int numverts = cloth->numverts;
1543         LinkNode *search;
1544         lfVector *winvec;
1545         EffectedPoint epoint;
1546
1547         tm2[0][0]= tm2[1][1]= tm2[2][2]= -spring_air;
1548         
1549         /* global acceleration (gravitation) */
1550         if (clmd->scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
1551                 copy_v3_v3(gravity, clmd->scene->physics_settings.gravity);
1552                 mul_fvector_S(gravity, gravity, 0.001f * clmd->sim_parms->effector_weights->global_gravity); /* scale gravity force */
1553         }
1554
1555         /* set dFdX jacobi matrix to zero */
1556         init_bfmatrix(dFdX, ZERO);
1557         /* set dFdX jacobi matrix diagonal entries to -spring_air */ 
1558         initdiag_bfmatrix(dFdV, tm2);
1559
1560         init_lfvector(lF, gravity, numverts);
1561         
1562         if (clmd->sim_parms->velocity_smooth > 0.0f || clmd->sim_parms->collider_friction > 0.0f)
1563                 hair_velocity_smoothing(clmd, lF, lX, lV, numverts);
1564
1565         /* multiply lF with mass matrix
1566          * force = mass * acceleration (in this case: gravity)
1567          */
1568         for (i = 0; i < numverts; i++) {
1569                 float temp[3];
1570                 copy_v3_v3(temp, lF[i]);
1571                 mul_fmatrix_fvector(lF[i], M[i].m, temp);
1572         }
1573
1574         submul_lfvectorS(lF, lV, spring_air, numverts);
1575         
1576         /* handle external forces like wind */
1577         if (effectors) {
1578                 // 0 = force, 1 = normalized force
1579                 winvec = create_lfvector(cloth->numverts);
1580                 
1581                 if (!winvec)
1582                         printf("winvec: out of memory in implicit.c\n");
1583                 
1584                 // precalculate wind forces
1585                 for (i = 0; i < cloth->numverts; i++) {
1586                         pd_point_from_loc(clmd->scene, (float*)lX[i], (float*)lV[i], i, &epoint);
1587                         pdDoEffectors(effectors, NULL, clmd->sim_parms->effector_weights, &epoint, winvec[i], NULL);
1588                 }
1589                 
1590                 for (i = 0; i < cloth->numfaces; i++) {
1591                         float trinormal[3]={0, 0, 0}; // normalized triangle normal
1592                         float triunnormal[3]={0, 0, 0}; // not-normalized-triangle normal
1593                         float tmp[3]={0, 0, 0};
1594                         float factor = (mfaces[i].v4) ? 0.25 : 1.0 / 3.0;
1595                         factor *= 0.02;
1596                         
1597                         // calculate face normal
1598                         if (mfaces[i].v4)
1599                                 CalcFloat4(lX[mfaces[i].v1], lX[mfaces[i].v2], lX[mfaces[i].v3], lX[mfaces[i].v4], triunnormal);
1600                         else
1601                                 CalcFloat(lX[mfaces[i].v1], lX[mfaces[i].v2], lX[mfaces[i].v3], triunnormal);
1602
1603                         normalize_v3_v3(trinormal, triunnormal);
1604                         
1605                         // add wind from v1
1606                         copy_v3_v3(tmp, trinormal);
1607                         mul_v3_fl(tmp, calculateVertexWindForce(winvec[mfaces[i].v1], triunnormal));
1608                         VECADDS(lF[mfaces[i].v1], lF[mfaces[i].v1], tmp, factor);
1609                         
1610                         // add wind from v2
1611                         copy_v3_v3(tmp, trinormal);
1612                         mul_v3_fl(tmp, calculateVertexWindForce(winvec[mfaces[i].v2], triunnormal));
1613                         VECADDS(lF[mfaces[i].v2], lF[mfaces[i].v2], tmp, factor);
1614                         
1615                         // add wind from v3
1616                         copy_v3_v3(tmp, trinormal);
1617                         mul_v3_fl(tmp, calculateVertexWindForce(winvec[mfaces[i].v3], triunnormal));
1618                         VECADDS(lF[mfaces[i].v3], lF[mfaces[i].v3], tmp, factor);
1619                         
1620                         // add wind from v4
1621                         if (mfaces[i].v4) {
1622                                 copy_v3_v3(tmp, trinormal);
1623                                 mul_v3_fl(tmp, calculateVertexWindForce(winvec[mfaces[i].v4], triunnormal));
1624                                 VECADDS(lF[mfaces[i].v4], lF[mfaces[i].v4], tmp, factor);
1625                         }
1626                 }
1627
1628                 /* Hair has only edges */
1629                 if (cloth->numfaces == 0) {
1630                         ClothSpring *spring;
1631                         float edgevec[3]={0, 0, 0}; //edge vector
1632                         float edgeunnormal[3]={0, 0, 0}; // not-normalized-edge normal
1633                         float tmp[3]={0, 0, 0};
1634                         float factor = 0.01;
1635
1636                         search = cloth->springs;
1637                         while (search) {
1638                                 spring = search->link;
1639                                 
1640                                 if (spring->type == CLOTH_SPRING_TYPE_STRUCTURAL) {
1641                                         sub_v3_v3v3(edgevec, (float*)lX[spring->ij], (float*)lX[spring->kl]);
1642
1643                                         project_v3_v3v3(tmp, winvec[spring->ij], edgevec);
1644                                         sub_v3_v3v3(edgeunnormal, winvec[spring->ij], tmp);
1645                                         /* hair doesn't stretch too much so we can use restlen pretty safely */
1646                                         VECADDS(lF[spring->ij], lF[spring->ij], edgeunnormal, spring->restlen * factor);
1647
1648                                         project_v3_v3v3(tmp, winvec[spring->kl], edgevec);
1649                                         sub_v3_v3v3(edgeunnormal, winvec[spring->kl], tmp);
1650                                         VECADDS(lF[spring->kl], lF[spring->kl], edgeunnormal, spring->restlen * factor);
1651                                 }
1652
1653                                 search = search->next;
1654                         }
1655                 }
1656
1657                 del_lfvector(winvec);
1658         }
1659                 
1660         // calculate spring forces
1661         search = cloth->springs;
1662         while (search) {
1663                 // only handle active springs
1664                 ClothSpring *spring = search->link;
1665                 if( !(spring->flags & CLOTH_SPRING_FLAG_DEACTIVATE))
1666                         cloth_calc_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX, time);
1667
1668                 search = search->next;
1669         }
1670         
1671         // apply spring forces
1672         search = cloth->springs;
1673         while (search) {
1674                 // only handle active springs
1675                 ClothSpring *spring = search->link;
1676                 if (!(spring->flags & CLOTH_SPRING_FLAG_DEACTIVATE))
1677                         cloth_apply_spring_force(clmd, search->link, lF, lX, lV, dFdV, dFdX);
1678                 search = search->next;
1679         }
1680         // printf("\n");
1681 }
1682
1683 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))
1684 {
1685         unsigned int numverts = dFdV[0].vcount;
1686
1687         lfVector *dFdXmV = create_lfvector(numverts);
1688         zero_lfvector(dV, numverts);
1689         
1690         cp_bfmatrix(A, M);
1691         
1692         subadd_bfmatrixS_bfmatrixS(A, dFdV, dt, dFdX, (dt*dt));
1693
1694         mul_bfmatrix_lfvector(dFdXmV, dFdX, lV);
1695
1696         add_lfvectorS_lfvectorS(B, lF, dt, dFdXmV, (dt*dt), numverts);
1697
1698         // itstart();
1699
1700         cg_filtered(dV, A, B, z, S); /* conjugate gradient algorithm to solve Ax=b */
1701         // cg_filtered_pre(dV, A, B, z, S, P, Pinv, bigI);
1702
1703         // itend();
1704         // printf("cg_filtered calc time: %f\n", (float)itval());
1705         
1706         cp_lfvector(olddV, dV, numverts);
1707
1708         // advance velocities
1709         add_lfvector_lfvector(Vnew, lV, dV, numverts);
1710         
1711
1712         del_lfvector(dFdXmV);
1713 }
1714
1715 /* computes where the cloth would be if it were subject to perfectly stiff edges
1716  * (edge distance constraints) in a lagrangian solver.  then add forces to help
1717  * guide the implicit solver to that state.  this function is called after
1718  * collisions*/
1719 static int UNUSED_FUNCTION(cloth_calc_helper_forces)(Object *UNUSED(ob), ClothModifierData * clmd, float (*initial_cos)[3], float UNUSED(step), float dt)
1720 {
1721         Cloth *cloth= clmd->clothObject;
1722         float (*cos)[3] = MEM_callocN(sizeof(float)*3*cloth->numverts, "cos cloth_calc_helper_forces");
1723         float *masses = MEM_callocN(sizeof(float)*cloth->numverts, "cos cloth_calc_helper_forces");
1724         LinkNode *node;
1725         ClothSpring *spring;
1726         ClothVertex *cv;
1727         int i, steps;
1728         
1729         cv = cloth->verts;
1730         for (i=0; i<cloth->numverts; i++, cv++) {
1731                 copy_v3_v3(cos[i], cv->tx);
1732                 
1733                 if (cv->goal == 1.0f || len_v3v3(initial_cos[i], cv->tx) != 0.0) {
1734                         masses[i] = 1e+10;      
1735                 }
1736                 else {
1737                         masses[i] = cv->mass;
1738                 }
1739         }
1740         
1741         steps = 55;
1742         for (i=0; i<steps; i++) {
1743                 for (node=cloth->springs; node; node=node->next) {
1744                         /* ClothVertex *cv1, *cv2; */ /* UNUSED */
1745                         int v1, v2;
1746                         float len, c, l, vec[3];
1747                         
1748                         spring = node->link;
1749                         if (spring->type != CLOTH_SPRING_TYPE_STRUCTURAL && spring->type != CLOTH_SPRING_TYPE_SHEAR) 
1750                                 continue;
1751                         
1752                         v1 = spring->ij; v2 = spring->kl;
1753                         /* cv1 = cloth->verts + v1; */ /* UNUSED */
1754                         /* cv2 = cloth->verts + v2; */ /* UNUSED */
1755                         len = len_v3v3(cos[v1], cos[v2]);
1756                         
1757                         sub_v3_v3v3(vec, cos[v1], cos[v2]);
1758                         normalize_v3(vec);
1759                         
1760                         c = (len - spring->restlen);
1761                         if (c == 0.0)
1762                                 continue;
1763                         
1764                         l = c / ((1.0/masses[v1]) + (1.0/masses[v2]));
1765                         
1766                         mul_v3_fl(vec, -(1.0/masses[v1])*l);
1767                         add_v3_v3(cos[v1], vec);
1768         
1769                         sub_v3_v3v3(vec, cos[v2], cos[v1]);
1770                         normalize_v3(vec);
1771                         
1772                         mul_v3_fl(vec, -(1.0/masses[v2])*l);
1773                         add_v3_v3(cos[v2], vec);
1774                 }
1775         }
1776         
1777         cv = cloth->verts;
1778         for (i=0; i<cloth->numverts; i++, cv++) {
1779                 float vec[3];
1780                 
1781                 /*compute forces*/
1782                 sub_v3_v3v3(vec, cos[i], cv->tx);
1783                 mul_v3_fl(vec, cv->mass*dt*20.0f);
1784                 add_v3_v3(cv->tv, vec);
1785                 //copy_v3_v3(cv->tx, cos[i]);
1786         }
1787         
1788         MEM_freeN(cos);
1789         MEM_freeN(masses);
1790         
1791         return 1;
1792 }
1793 int implicit_solver(Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
1794 {
1795         unsigned int i=0;
1796         float step=0.0f, tf=clmd->sim_parms->timescale;
1797         Cloth *cloth = clmd->clothObject;
1798         ClothVertex *verts = cloth->verts, *cv;
1799         unsigned int numverts = cloth->numverts;
1800         float dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame;
1801         float spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
1802         float (*initial_cos)[3] = MEM_callocN(sizeof(float)*3*cloth->numverts, "initial_cos implicit.c");
1803         Implicit_Data *id = cloth->implicit;
1804         int do_extra_solve;
1805
1806         if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { /* do goal stuff */
1807                 
1808                 /* Update vertex constraints for pinned vertices */
1809                 update_matrixS(verts, cloth->numverts, id->S);
1810
1811                 for (i = 0; i < numverts; i++) {
1812                         // update velocities with constrained velocities from pinned verts
1813                         if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
1814                                 sub_v3_v3v3(id->V[i], verts[i].xconst, verts[i].xold);
1815                                 // mul_v3_fl(id->V[i], clmd->sim_parms->stepsPerFrame);
1816                         }
1817                 }       
1818         }
1819         
1820         while (step < tf) {
1821                 // damping velocity for artistic reasons
1822                 mul_lfvectorS(id->V, id->V, clmd->sim_parms->vel_damping, numverts);
1823
1824                 // calculate forces
1825                 cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step, id->M);
1826                 
1827                 // calculate new velocity
1828                 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);
1829                 
1830                 // advance positions
1831                 add_lfvector_lfvectorS(id->Xnew, id->X, id->Vnew, dt, numverts);
1832                 
1833                 /* move pinned verts to correct position */
1834                 for (i = 0; i < numverts; i++) {
1835                         if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) {
1836                                 if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
1837                                         float tvect[3] = {0.0f, 0.0f, 0.0f};
1838                                         sub_v3_v3v3(tvect, verts[i].xconst, verts[i].xold);
1839                                         mul_fvector_S(tvect, tvect, step+dt);
1840                                         VECADD(tvect, tvect, verts[i].xold);
1841                                         copy_v3_v3(id->Xnew[i], tvect);
1842                                 }       
1843                         }
1844                         
1845                         copy_v3_v3(verts[i].txold, id->X[i]);
1846                 }
1847
1848                 if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED && clmd->clothObject->bvhtree) {
1849                         // collisions 
1850                         // itstart();
1851                         
1852                         // update verts to current positions
1853                         for (i = 0; i < numverts; i++) {
1854                                 copy_v3_v3(verts[i].tx, id->Xnew[i]);
1855
1856                                 sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold);
1857                                 copy_v3_v3(verts[i].v, verts[i].tv);
1858                         }
1859
1860                         for (i=0, cv=cloth->verts; i<cloth->numverts; i++, cv++) {
1861                                 copy_v3_v3(initial_cos[i], cv->tx);
1862                         }
1863
1864                         // call collision function
1865                         // TODO: check if "step" or "step+dt" is correct - dg
1866                         do_extra_solve = cloth_bvh_objcollision(ob, clmd, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale);
1867
1868                         // copy corrected positions back to simulation
1869                         for (i = 0; i < numverts; i++) {
1870                                 // correct velocity again, just to be sure we had to change it due to adaptive collisions
1871                                 sub_v3_v3v3(verts[i].tv, verts[i].tx, id->X[i]);
1872                         }
1873
1874                         //if (do_extra_solve)
1875                         //      cloth_calc_helper_forces(ob, clmd, initial_cos, step/clmd->sim_parms->timescale, dt/clmd->sim_parms->timescale);
1876                         
1877                         for (i = 0; i < numverts; i++) {
1878
1879                                 if (do_extra_solve) {
1880                                         
1881                                         if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED))
1882                                                 continue;
1883
1884                                         copy_v3_v3(id->Xnew[i], verts[i].tx);
1885                                         copy_v3_v3(id->Vnew[i], verts[i].tv);
1886                                         mul_v3_fl(id->Vnew[i], spf);
1887                                 }
1888                         }
1889                         
1890                         // X = Xnew;
1891                         cp_lfvector(id->X, id->Xnew, numverts);
1892
1893                         // if there were collisions, advance the velocity from v_n+1/2 to v_n+1
1894                         
1895                         if (do_extra_solve) {
1896                                 // V = Vnew;
1897                                 cp_lfvector(id->V, id->Vnew, numverts);
1898
1899                                 // calculate 
1900                                 cloth_calc_force(clmd, frame, id->F, id->X, id->V, id->dFdV, id->dFdX, effectors, step+dt, id->M);      
1901                                 
1902                                 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);
1903                         }
1904                 }
1905                 else {
1906                         // X = Xnew;
1907                         cp_lfvector(id->X, id->Xnew, numverts);
1908                 }
1909                 
1910                 // itend();
1911                 // printf("collision time: %f\n", (float)itval());
1912                 
1913                 // V = Vnew;
1914                 cp_lfvector(id->V, id->Vnew, numverts);
1915                 
1916                 step += dt;
1917         }
1918
1919         for (i = 0; i < numverts; i++) {
1920                 if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) && (verts [i].flags & CLOTH_VERT_FLAG_PINNED)) {
1921                         copy_v3_v3(verts[i].txold, verts[i].xconst); // TODO: test --> should be .x
1922                         copy_v3_v3(verts[i].x, verts[i].xconst);
1923                         copy_v3_v3(verts[i].v, id->V[i]);
1924                 }
1925                 else {
1926                         copy_v3_v3(verts[i].txold, id->X[i]);
1927                         copy_v3_v3(verts[i].x, id->X[i]);
1928                         copy_v3_v3(verts[i].v, id->V[i]);
1929                 }
1930         }
1931         
1932         MEM_freeN(initial_cos);
1933         
1934         return 1;
1935 }
1936
1937 void implicit_set_positions(ClothModifierData *clmd)
1938 {
1939         Cloth *cloth = clmd->clothObject;
1940         ClothVertex *verts = cloth->verts;
1941         unsigned int numverts = cloth->numverts, i;
1942         Implicit_Data *id = cloth->implicit;
1943         
1944         for (i = 0; i < numverts; i++) {
1945                 copy_v3_v3(id->X[i], verts[i].x);
1946                 copy_v3_v3(id->V[i], verts[i].v);
1947         }
1948         if (G.debug_value > 0)
1949                 printf("implicit_set_positions\n");     
1950 }
1951