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