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